From 90f800fa1d3d6078b2483e52849cf47f68cdf69a Mon Sep 17 00:00:00 2001 From: DC Date: Sat, 27 Apr 2024 17:49:03 -0700 Subject: [PATCH] Fixed issue with data packing and unit conversion for MMC5983MA --- common/include/mmc5983ma.h | 2 ++ common/mmc5983ma.c | 52 ++++++++++++++++++++++++-------- src/f0/app_adcs/source/adcs.c | 57 ++++++++++++++++++----------------- 3 files changed, 70 insertions(+), 41 deletions(-) diff --git a/common/include/mmc5983ma.h b/common/include/mmc5983ma.h index 7335af37..6cdad137 100644 --- a/common/include/mmc5983ma.h +++ b/common/include/mmc5983ma.h @@ -183,9 +183,11 @@ extern "C" { void mmc5983maObjectInit(MMC5983MADriver *devp); bool mmc5983maStart(MMC5983MADriver *devp, const MMC5983MAConfig *config); void mmc5983maStop(MMC5983MADriver *devp); +int32_t mmc5983maRawToMilliGauss(const int16_t raw); bool mmc5983maReadData(MMC5983MADriver *devp, mmc5983ma_data_t *dest); bool mmc5983maI2CReadRegister3(I2CDriver *i2cp, const uint8_t i2c_address, const uint8_t reg_number, uint8_t *dest_value); bool mmc5983maI2CReadRegister4(I2CDriver *i2cp, const uint8_t i2c_address, const uint8_t reg_number, uint16_t *dest_value); +float mmc5983maRawToGauss(const int16_t raw); #ifdef __cplusplus } #endif diff --git a/common/mmc5983ma.c b/common/mmc5983ma.c index 8198f9a4..a069831d 100644 --- a/common/mmc5983ma.c +++ b/common/mmc5983ma.c @@ -21,6 +21,7 @@ /* Driver local functions. */ /*===========================================================================*/ #define MMC5983MA_DEFAULT_I2C_TIMEOUT 50 +#define MMC5983MA_DATA_READ_I2C_TIMEOUT 250 #if (MMC5983MA_USE_I2C) || defined(__DOXYGEN__) @@ -247,7 +248,23 @@ void mmc5983maStop(MMC5983MADriver *devp) { +const char* msg_t_to_str(const msg_t v) { + switch (v) { + case MSG_OK: + return ("MSG_OK"); + case MSG_TIMEOUT: + return ("MSG_TIMEOUT"); + case MSG_RESET: + return ("MSG_RESET"); + default: + return ("MSG_???"); + } +} +int32_t mmc5983maRawToMilliGauss(const int16_t raw) { + float gauss = 1000.0 * ((float) raw) / 4096.0; + return(gauss); +} bool mmc5983maReadData(MMC5983MADriver *devp, mmc5983ma_data_t *dest) { osalDbgCheck((devp != NULL)); @@ -286,22 +303,24 @@ bool mmc5983maReadData(MMC5983MADriver *devp, mmc5983ma_data_t *dest) { // chprintf(DEBUG_SD, "MMC5983MA status register = 0x%X\r\n", rx[0]);chThdSleepMilliseconds(10); memset(rx, 0, sizeof(rx)); - uint8_t reg = MMC5983MA_XOUT_0; +// uint8_t reg = MMC5983MA_XOUT_0; + uint8_t tx[2]; + tx[0] = MMC5983MA_XOUT_0; + tx[1] = 0; // chprintf(DEBUG_SD, "MMC5983MA reading data registers\r\n");chThdSleepMilliseconds(10); i2cStart(devp->config->i2cp, devp->config->i2ccfg); - msg_t r = i2cMasterTransmitTimeout(devp->config->i2cp, MMC5983MA_I2C_ADDRESS_READ, ®, 1, rx, 6, MMC5983MA_DEFAULT_I2C_TIMEOUT); + msg_t r = i2cMasterTransmitTimeout(devp->config->i2cp, MMC5983MA_I2C_ADDRESS_READ, tx, 1, rx, 7, MMC5983MA_DATA_READ_I2C_TIMEOUT); i2cStop(devp->config->i2cp); - - dest->mx = (rx[0] << 8) | rx[1]; - dest->my = (rx[2] << 8) | rx[3]; - dest->mz = (rx[4] << 8) | rx[5]; - -// chprintf(DEBUG_SD, "MMC5983MA done reading data registers: (%d, %d, %d)\r\n", dest->mx, dest->my, dest->mz);chThdSleepMilliseconds(10); + chprintf(DEBUG_SD, "MMC5983MA done reading data registers: (%d, %d, %d), r=%d (%s), state=%d\r\n", dest->mx, dest->my, dest->mz, r, msg_t_to_str(r), devp->state);chThdSleepMilliseconds(10); if( r == MSG_OK ) { + //Note The data sheet says this is 4096 counts/gauss + dest->mx = saturate_int32_t(((uint32_t) (rx[0] << 8) | rx[1]) - 32768, INT16_MIN, INT16_MAX); + dest->my = saturate_int32_t(((uint32_t) (rx[2] << 8) | rx[3]) - 32768, INT16_MIN, INT16_MAX); + dest->mz = saturate_int32_t(((uint32_t) (rx[4] << 8) | rx[5]) - 32768, INT16_MIN, INT16_MAX); ret = true; if( devp->read_call_count > 20 ) { @@ -315,16 +334,17 @@ bool mmc5983maReadData(MMC5983MADriver *devp, mmc5983ma_data_t *dest) { i2cStop(devp->config->i2cp); chThdSleepMilliseconds(1); - + tx[0] = MMC5983MA_XOUT_0; + tx[1] = 0; memset(rx, 0, sizeof(rx)); i2cStart(devp->config->i2cp, devp->config->i2ccfg); - msg_t r = i2cMasterTransmitTimeout(devp->config->i2cp, MMC5983MA_I2C_ADDRESS_READ, ®, 1, rx, 6, MMC5983MA_DEFAULT_I2C_TIMEOUT); + msg_t r = i2cMasterTransmitTimeout(devp->config->i2cp, MMC5983MA_I2C_ADDRESS_READ, tx, 1, rx, 7, MMC5983MA_DATA_READ_I2C_TIMEOUT); i2cStop(devp->config->i2cp); mmc5983ma_data_t dest_temp; - dest_temp.mx = (rx[0] << 8) | rx[1]; - dest_temp.my = (rx[2] << 8) | rx[3]; - dest_temp.mz = (rx[4] << 8) | rx[5]; + dest_temp.mx = saturate_int32_t(((uint32_t) (rx[0] << 8) | rx[1]) - 32768, INT16_MIN, INT16_MAX); + dest_temp.my = saturate_int32_t(((uint32_t) (rx[2] << 8) | rx[3]) - 32768, INT16_MIN, INT16_MAX); + dest_temp.mz = saturate_int32_t(((uint32_t) (rx[4] << 8) | rx[5]) - 32768, INT16_MIN, INT16_MAX); const int32_t delta_set_reset_x = dest_temp.mx - dest->mx; @@ -352,6 +372,12 @@ bool mmc5983maReadData(MMC5983MADriver *devp, mmc5983ma_data_t *dest) { dest->mx += devp->bridge_offset_estimate_x; dest->my += devp->bridge_offset_estimate_y; dest->mz += devp->bridge_offset_estimate_z; + } else { + dest->mx = 0; + dest->my = 0; + dest->mz = 0; + + ret = false; } diff --git a/src/f0/app_adcs/source/adcs.c b/src/f0/app_adcs/source/adcs.c index f040d532..9bf70ab7 100644 --- a/src/f0/app_adcs/source/adcs.c +++ b/src/f0/app_adcs/source/adcs.c @@ -245,6 +245,7 @@ const char* end_card_magnetometoer_t_to_str(const end_card_magnetometoer_t ecm) } + int32_t saturate_int32_t(const int32_t v, const int32_t min, const int32_t max) { if (v >= max) return (max); @@ -322,9 +323,9 @@ void handle_can_open_data(void) { if (g_adcs_data.magetometer_data[EC_MAG_2_PZ_1].is_working) { - OD_RAM.x4003_pos_z_magnetometer_1.x = g_adcs_data.magetometer_data[EC_MAG_2_PZ_1].data.mx; - OD_RAM.x4003_pos_z_magnetometer_1.y = g_adcs_data.magetometer_data[EC_MAG_2_PZ_1].data.my; - OD_RAM.x4003_pos_z_magnetometer_1.z = g_adcs_data.magetometer_data[EC_MAG_2_PZ_1].data.mz; + OD_RAM.x4003_pos_z_magnetometer_1.x = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_2_PZ_1].data.mx); + OD_RAM.x4003_pos_z_magnetometer_1.y = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_2_PZ_1].data.my); + OD_RAM.x4003_pos_z_magnetometer_1.z = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_2_PZ_1].data.mz); } else { OD_RAM.x4003_pos_z_magnetometer_1.x = INT16_MAX; OD_RAM.x4003_pos_z_magnetometer_1.y = INT16_MAX; @@ -332,9 +333,9 @@ void handle_can_open_data(void) { } if (g_adcs_data.magetometer_data[EC_MAG_3_PZ_2].is_working) { - OD_RAM.x4004_pos_z_magnetometer_2.x = g_adcs_data.magetometer_data[EC_MAG_3_PZ_2].data.mx; - OD_RAM.x4004_pos_z_magnetometer_2.y = g_adcs_data.magetometer_data[EC_MAG_3_PZ_2].data.my; - OD_RAM.x4004_pos_z_magnetometer_2.z = g_adcs_data.magetometer_data[EC_MAG_3_PZ_2].data.mz; + OD_RAM.x4004_pos_z_magnetometer_2.x = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_3_PZ_2].data.mx); + OD_RAM.x4004_pos_z_magnetometer_2.y = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_3_PZ_2].data.my); + OD_RAM.x4004_pos_z_magnetometer_2.z = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_3_PZ_2].data.mz); } else { OD_RAM.x4004_pos_z_magnetometer_2.x = INT16_MAX; OD_RAM.x4004_pos_z_magnetometer_2.y = INT16_MAX; @@ -343,9 +344,9 @@ void handle_can_open_data(void) { if (g_adcs_data.magetometer_data[EC_MAG_0_MZ_1].is_working) { - OD_RAM.x4005_min_z_magnetometer_1.x = g_adcs_data.magetometer_data[EC_MAG_0_MZ_1].data.mx; - OD_RAM.x4005_min_z_magnetometer_1.y = g_adcs_data.magetometer_data[EC_MAG_0_MZ_1].data.my; - OD_RAM.x4005_min_z_magnetometer_1.z = g_adcs_data.magetometer_data[EC_MAG_0_MZ_1].data.mz; + OD_RAM.x4005_min_z_magnetometer_1.x = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_0_MZ_1].data.mx); + OD_RAM.x4005_min_z_magnetometer_1.y = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_0_MZ_1].data.my); + OD_RAM.x4005_min_z_magnetometer_1.z = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_0_MZ_1].data.mz); } else { OD_RAM.x4005_min_z_magnetometer_1.x = INT16_MAX; OD_RAM.x4005_min_z_magnetometer_1.y = INT16_MAX; @@ -354,9 +355,9 @@ void handle_can_open_data(void) { if (g_adcs_data.magetometer_data[EC_MAG_1_MZ_2].is_working) { - OD_RAM.x4006_min_z_magnetometer_2.x = g_adcs_data.magetometer_data[EC_MAG_1_MZ_2].data.mx; - OD_RAM.x4006_min_z_magnetometer_2.y = g_adcs_data.magetometer_data[EC_MAG_1_MZ_2].data.my; - OD_RAM.x4006_min_z_magnetometer_2.z = g_adcs_data.magetometer_data[EC_MAG_1_MZ_2].data.mz; + OD_RAM.x4006_min_z_magnetometer_2.x = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_1_MZ_2].data.mx); + OD_RAM.x4006_min_z_magnetometer_2.y = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_1_MZ_2].data.my); + OD_RAM.x4006_min_z_magnetometer_2.z = mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[EC_MAG_1_MZ_2].data.mz); } else { OD_RAM.x4006_min_z_magnetometer_2.x = INT16_MAX; OD_RAM.x4006_min_z_magnetometer_2.y = INT16_MAX; @@ -538,11 +539,13 @@ bool select_and_read_magnetometer(const end_card_magnetometoer_t ecm) { chThdSleepMilliseconds(5); if( mmc5983maReadData(&g_adcs_data.magetometer_data[ecm].driver, &g_adcs_data.magetometer_data[ecm].data) ) { - chprintf(DEBUG_SD, " mx=%d, my=%d, mz=%d\r\n", g_adcs_data.magetometer_data[ecm].data.mx, - g_adcs_data.magetometer_data[ecm].data.my, - g_adcs_data.magetometer_data[ecm].data.mz); + chprintf(DEBUG_SD, " mx=%d (%d mG), my=%d (%d mG), mz=%d (%d mG)\r\n", + g_adcs_data.magetometer_data[ecm].data.mx, mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[ecm].data.mx), + g_adcs_data.magetometer_data[ecm].data.my, mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[ecm].data.my), + g_adcs_data.magetometer_data[ecm].data.mz, mmc5983maRawToMilliGauss(g_adcs_data.magetometer_data[ecm].data.mz)); r = true; } else { + chprintf(DEBUG_SD, "\r\nERROR Failed to read from %s\r\n", end_card_magnetometoer_t_to_str(ecm)); //FIXME should this error be cleard if/when comms works again on a subsequent call? static systime_t last_report_time = 0; CO_errorReportRateLimited(CO->em, CO_EM_GENERIC_ERROR, CO_EMC_COMMUNICATION, ADCS_OD_ERROR_INFO_CODE_MAGNETOMETER_0_COMM_FAILURE + ecm, &last_report_time); @@ -639,7 +642,7 @@ void set_pwm_output(void) { //Updates will come in periodically via CANOpen, this will apply those updates to the PWM outputs. if( g_adcs_data.mt_pwm_data[i].last_update_time == 0 || chTimeDiffX(g_adcs_data.mt_pwm_data[i].last_update_time, now_time) > 10 ) { if( g_adcs_data.mt_pwm_data[i].current_pwm_percent != g_adcs_data.mt_pwm_data[i].target_pwm_percent ) { - chprintf(DEBUG_SD, "target_pwm_percent = %u\r\n", g_adcs_data.mt_pwm_data[i].target_pwm_percent); + chprintf(DEBUG_SD, "target_pwm_percent = %d\r\n", g_adcs_data.mt_pwm_data[i].target_pwm_percent); pwmDisableChannel(&PWMD1, g_adcs_data.mt_pwm_data[i].pwm_channel_number); @@ -892,13 +895,9 @@ THD_FUNCTION(adcs, arg) bmi088ObjectInit(&imudev); - - chprintf(DEBUG_SD, "Starting BMI088...\r\n"); - chThdSleepMilliseconds(50); + chprintf(DEBUG_SD, "Starting BMI088...\r\n");chThdSleepMilliseconds(50); bmi088Start(&imudev, &imucfg); - - - + chprintf(DEBUG_SD, "BMI088 started...\r\n"); chprintf(DEBUG_SD, "BMI088 state = %u, error_flags=0x%X\r\n", imudev.state, imudev.error_flags); @@ -907,19 +906,21 @@ THD_FUNCTION(adcs, arg) static systime_t last_report_time = 0; CO_errorReportRateLimited(CO->em, CO_EM_GENERIC_ERROR, CO_EMC_HARDWARE, ADCS_OD_ERROR_INFO_CODE_IMU_INIT_FAILURE, &last_report_time); } else { - uint8_t bmi088_chip_id = 0; - if( (r = bmi088ReadAccelerometerChipId(&imudev, &bmi088_chip_id)) == MSG_OK ) { - chprintf(DEBUG_SD, "BMI088 accelerometer chip ID is 0x%X, expected to be (0x%X or 0x%X)\r\n", bmi088_chip_id, BMI088_ACC_CHIP_ID_EXPECTED, BMI090L_ACC_CHIP_ID_EXPECTED); + uint8_t bmi_chip_id = 0; + if( (r = bmi088ReadAccelerometerChipId(&imudev, &bmi_chip_id)) == MSG_OK ) { + chprintf(DEBUG_SD, "BMI088 accelerometer chip ID is 0x%X, expected to be (0x%X or 0x%X)\r\n", bmi_chip_id, BMI088_ACC_CHIP_ID_EXPECTED, BMI090L_ACC_CHIP_ID_EXPECTED); } else { chprintf(DEBUG_SD, "Failed to read accel chip ID from BMI088 ACCEL, r = %d\r\n", r); } - if( bmi088_chip_id != BMI088_ACC_CHIP_ID_EXPECTED && bmi088_chip_id != BMI090L_ACC_CHIP_ID_EXPECTED ) { + if( bmi_chip_id != BMI088_ACC_CHIP_ID_EXPECTED && bmi_chip_id != BMI090L_ACC_CHIP_ID_EXPECTED ) { chprintf(DEBUG_SD, "ERROR: BMI088 ACCEL FAIL: didn't find BMI088!\r\n"); static systime_t last_report_time = 0; CO_errorReportRateLimited(CO->em, CO_EM_GENERIC_ERROR, CO_EMC_HARDWARE, ADCS_OD_ERROR_INFO_CODE_ACCL_CHIP_ID_MISMATCH, &last_report_time); - } else { - chprintf(DEBUG_SD, "BMI088 ACCEL SUCCESS: found BMI088!\r\n"); + } else if (bmi_chip_id == BMI088_ACC_CHIP_ID_EXPECTED ){ + chprintf(DEBUG_SD, "BMI088 ACCEL SUCCESS: found BMI088!, chip_id = 0x%X\r\n", bmi_chip_id); + } else if (bmi_chip_id == BMI090L_ACC_CHIP_ID_EXPECTED ){ + chprintf(DEBUG_SD, "BMI088 ACCEL SUCCESS: found BMI090L!, chip_id = 0x%X\r\n", bmi_chip_id); }