From b0f00a54c2fe569e5cb363614c27b7b36c143f6c Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sat, 18 Jul 2026 23:34:46 -0700 Subject: [PATCH 1/2] Fix BMM150 blocking magnetometer read --- imu/imu.c | 82 +++++++++++++++++++++++++++++++++++++++---------------- 1 file changed, 58 insertions(+), 24 deletions(-) diff --git a/imu/imu.c b/imu/imu.c index 64a0c61..3750200 100644 --- a/imu/imu.c +++ b/imu/imu.c @@ -516,11 +516,12 @@ IMU_STATUS imu_get_mag_xyz /*------------------------------------------------------------------------------ Local variables ------------------------------------------------------------------------------*/ -uint8_t regMag[6]; /* Magnetometer register bytes */ -uint16_t mag_x_raw; /* Raw magnetometer sensor readouts */ -uint16_t mag_y_raw; -uint16_t mag_z_raw; -IMU_STATUS imu_status; /* IMU status return codes */ +uint8_t regMag[8]; /* Magnetometer register bytes */ +int16_t mag_x_raw; /* Raw magnetometer sensor readouts */ +int16_t mag_y_raw; +int16_t mag_z_raw; +uint16_t mag_hall_raw = 0; /* Rev 1 does not provide RHALL */ +IMU_STATUS imu_status; /* IMU status return codes */ /*------------------------------------------------------------------------------ @@ -528,34 +529,63 @@ IMU_STATUS imu_status; /* IMU status return codes */ ------------------------------------------------------------------------------*/ /* Read MAG_X, MAG_Y, MAG_Z high byte and low byte registers */ -#if defined( A0002_REV1 ) - imu_status = read_mag_regs( IMU_REG_MAG_XOUT_H, - ®Mag[0] , - sizeof( regMag ) ); -#elif defined( A0002_REV2 ) - imu_status = read_mag_regs( MAG_REG_DATAX_L, - ®Mag[0] , - sizeof( regMag ) ); +#if defined(A0002_REV1) + imu_status = read_mag_regs(IMU_REG_MAG_XOUT_H, + ®Mag[0], + 6); +#elif defined(A0002_REV2) + imu_status = read_mag_regs(MAG_REG_DATAX_L, + ®Mag[0], + sizeof(regMag)); #endif /* Check for HAL IMU error */ -if ( imu_status == IMU_TIMEOUT ) - { - return IMU_TIMEOUT; - } +if ( imu_status != IMU_OK ) + { + return imu_status; + } /* Combine high byte and low byte to 16 bit data */ #if defined( A0002_REV1 ) mag_x_raw = ( (uint16_t) regMag[1] ) << 8 | regMag[0]; mag_y_raw = ( (uint16_t) regMag[3] ) << 8 | regMag[2]; mag_z_raw = ( (uint16_t) regMag[5] ) << 8 | regMag[4]; -#elif defined( A0002_REV2 ) - mag_x_raw = ( (uint16_t) regMag[1] << MAG_XY_MSB_BITSHIFT ) | - ( ( (uint16_t) regMag[0] && MAG_XY_LSB_BITMASK ) >> MAG_XY_LSB_BITSHIFT ); - mag_y_raw = ( (uint16_t) regMag[3] << MAG_XY_MSB_BITSHIFT ) | - ( ( (uint16_t) regMag[2] && MAG_XY_LSB_BITMASK ) >> MAG_XY_LSB_BITSHIFT ); - mag_z_raw = ( (uint16_t) regMag[5] << MAG_Z_MSB_BITSHIFT ) | - ( ( (uint16_t) regMag[4] && MAG_Z_LSB_BITMASK ) >> MAG_Z_LSB_BITSHIFT ); +#elif defined(A0002_REV2) + mag_x_raw = + (int16_t)(((uint16_t)regMag[1] << MAG_XY_MSB_BITSHIFT) | + (((uint16_t)regMag[0] & MAG_XY_LSB_BITMASK) >> + MAG_XY_LSB_BITSHIFT)); + + mag_y_raw = + (int16_t)(((uint16_t)regMag[3] << MAG_XY_MSB_BITSHIFT) | + (((uint16_t)regMag[2] & MAG_XY_LSB_BITMASK) >> + MAG_XY_LSB_BITSHIFT)); + + mag_z_raw = + (int16_t)(((uint16_t)regMag[5] << MAG_Z_MSB_BITSHIFT) | + (((uint16_t)regMag[4] & MAG_Z_LSB_BITMASK) >> + MAG_Z_LSB_BITSHIFT)); + + mag_hall_raw = + (uint16_t)(((uint16_t)regMag[7] << MAG_RHALL_MSB_BITSHIFT) | + (((uint16_t)regMag[6] & MAG_RHALL_LSB_BITMASK) >> + MAG_RHALL_LSB_BITSHIFT)); + + /* Sign-extend the packed BMM150 measurements. */ + if (mag_x_raw & (1 << 12)) + { + mag_x_raw |= (int16_t)~((1 << 13) - 1); + } + + if (mag_y_raw & (1 << 12)) + { + mag_y_raw |= (int16_t)~((1 << 13) - 1); + } + + if (mag_z_raw & (1 << 14)) + { + mag_z_raw |= (int16_t)~((1 << 15) - 1); + } #endif /* Export sensor data */ @@ -563,6 +593,10 @@ pIMU->mag_x = mag_x_raw; pIMU->mag_y = mag_y_raw; pIMU->mag_z = mag_z_raw; +#if defined(A0002_REV2) +pIMU->mag_hall = mag_hall_raw; +#endif + return IMU_OK; } /* imu_get_mag_xyz */ From b9feeed81656ef7a057eb9825fec3b4aaeff423f Mon Sep 17 00:00:00 2001 From: Bjorn Bengtsson Date: Sun, 19 Jul 2026 12:33:19 -0700 Subject: [PATCH 2/2] Address BMM150 read review comments --- imu/imu.c | 103 +++++++++++++++++++++--------------------------------- 1 file changed, 40 insertions(+), 63 deletions(-) diff --git a/imu/imu.c b/imu/imu.c index 3750200..82345c0 100644 --- a/imu/imu.c +++ b/imu/imu.c @@ -504,8 +504,8 @@ return IMU_OK; * imu_get_mag_xyz * * * * DESCRIPTION: * -* Return the pointer to structure that updates the x,y,z magnetometer * -* values from the IMU * +* Return the pointer to structure that updates the x, y, z, and RHALL * +* magnetometer values from the BMM150. * * * *******************************************************************************/ IMU_STATUS imu_get_mag_xyz @@ -516,86 +516,63 @@ IMU_STATUS imu_get_mag_xyz /*------------------------------------------------------------------------------ Local variables ------------------------------------------------------------------------------*/ -uint8_t regMag[8]; /* Magnetometer register bytes */ -int16_t mag_x_raw; /* Raw magnetometer sensor readouts */ +uint8_t regMag[8]; /* Magnetometer register bytes */ +int16_t mag_x_raw; /* Raw magnetometer sensor readouts */ int16_t mag_y_raw; int16_t mag_z_raw; -uint16_t mag_hall_raw = 0; /* Rev 1 does not provide RHALL */ -IMU_STATUS imu_status; /* IMU status return codes */ +uint16_t mag_hall_raw; +IMU_STATUS imu_status; /* IMU status return codes */ /*------------------------------------------------------------------------------ API function implementation ------------------------------------------------------------------------------*/ -/* Read MAG_X, MAG_Y, MAG_Z high byte and low byte registers */ -#if defined(A0002_REV1) - imu_status = read_mag_regs(IMU_REG_MAG_XOUT_H, - ®Mag[0], - 6); -#elif defined(A0002_REV2) - imu_status = read_mag_regs(MAG_REG_DATAX_L, - ®Mag[0], - sizeof(regMag)); -#endif +/* Read BMM150 X, Y, Z, and RHALL registers */ +imu_status = read_mag_regs( MAG_REG_DATAX_L, + ®Mag[0], + sizeof( regMag ) ); -/* Check for HAL IMU error */ +/* Check for magnetometer read error */ if ( imu_status != IMU_OK ) { return imu_status; } -/* Combine high byte and low byte to 16 bit data */ -#if defined( A0002_REV1 ) - mag_x_raw = ( (uint16_t) regMag[1] ) << 8 | regMag[0]; - mag_y_raw = ( (uint16_t) regMag[3] ) << 8 | regMag[2]; - mag_z_raw = ( (uint16_t) regMag[5] ) << 8 | regMag[4]; -#elif defined(A0002_REV2) - mag_x_raw = - (int16_t)(((uint16_t)regMag[1] << MAG_XY_MSB_BITSHIFT) | - (((uint16_t)regMag[0] & MAG_XY_LSB_BITMASK) >> - MAG_XY_LSB_BITSHIFT)); - - mag_y_raw = - (int16_t)(((uint16_t)regMag[3] << MAG_XY_MSB_BITSHIFT) | - (((uint16_t)regMag[2] & MAG_XY_LSB_BITMASK) >> - MAG_XY_LSB_BITSHIFT)); - - mag_z_raw = - (int16_t)(((uint16_t)regMag[5] << MAG_Z_MSB_BITSHIFT) | - (((uint16_t)regMag[4] & MAG_Z_LSB_BITMASK) >> - MAG_Z_LSB_BITSHIFT)); - - mag_hall_raw = - (uint16_t)(((uint16_t)regMag[7] << MAG_RHALL_MSB_BITSHIFT) | - (((uint16_t)regMag[6] & MAG_RHALL_LSB_BITMASK) >> - MAG_RHALL_LSB_BITSHIFT)); - - /* Sign-extend the packed BMM150 measurements. */ - if (mag_x_raw & (1 << 12)) - { - mag_x_raw |= (int16_t)~((1 << 13) - 1); - } +/* Combine and scale the packed BMM150 register values */ +mag_x_raw = (int16_t)( (uint16_t) regMag[1] << MAG_XY_MSB_BITSHIFT ) | + ( ( (uint16_t) regMag[0] & MAG_XY_LSB_BITMASK ) >> MAG_XY_LSB_BITSHIFT ); - if (mag_y_raw & (1 << 12)) - { - mag_y_raw |= (int16_t)~((1 << 13) - 1); - } +mag_y_raw = (int16_t)( (uint16_t) regMag[3] << MAG_XY_MSB_BITSHIFT ) | + ( ( (uint16_t) regMag[2] & MAG_XY_LSB_BITMASK ) >> MAG_XY_LSB_BITSHIFT ); - if (mag_z_raw & (1 << 14)) - { - mag_z_raw |= (int16_t)~((1 << 15) - 1); - } -#endif +mag_z_raw = (int16_t)( (uint16_t) regMag[5] << MAG_Z_MSB_BITSHIFT ) | + ( ( (uint16_t) regMag[4] & MAG_Z_LSB_BITMASK ) >> MAG_Z_LSB_BITSHIFT ); -/* Export sensor data */ -pIMU->mag_x = mag_x_raw; -pIMU->mag_y = mag_y_raw; -pIMU->mag_z = mag_z_raw; +mag_hall_raw = (uint16_t)( (uint16_t) regMag[7] << MAG_RHALL_MSB_BITSHIFT ) | + ( ( (uint16_t) regMag[6] & MAG_RHALL_LSB_BITMASK ) >> MAG_RHALL_LSB_BITSHIFT ); -#if defined(A0002_REV2) +/* Sign-extend the packed 13-bit X/Y and 15-bit Z measurements */ +if ( mag_x_raw & ( 1 << 12 ) ) + { + mag_x_raw |= (int16_t) ~( ( 1 << 13 ) - 1 ); + } + +if ( mag_y_raw & ( 1 << 12 ) ) + { + mag_y_raw |= (int16_t) ~( ( 1 << 13 ) - 1 ); + } + +if ( mag_z_raw & ( 1 << 14 ) ) + { + mag_z_raw |= (int16_t) ~( ( 1 << 15 ) - 1 ); + } + +/* Export sensor data */ +pIMU->mag_x = mag_x_raw; +pIMU->mag_y = mag_y_raw; +pIMU->mag_z = mag_z_raw; pIMU->mag_hall = mag_hall_raw; -#endif return IMU_OK; } /* imu_get_mag_xyz */