Skip to content

Commit

Permalink
Fixed issue with data packing and unit conversion for MMC5983MA
Browse files Browse the repository at this point in the history
  • Loading branch information
DC committed Apr 28, 2024
1 parent 962b77a commit 90f800f
Show file tree
Hide file tree
Showing 3 changed files with 70 additions and 41 deletions.
2 changes: 2 additions & 0 deletions common/include/mmc5983ma.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
52 changes: 39 additions & 13 deletions common/mmc5983ma.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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__)

Expand Down Expand Up @@ -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));
Expand Down Expand Up @@ -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, &reg, 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 ) {
Expand All @@ -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, &reg, 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;
Expand Down Expand Up @@ -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;
}


Expand Down
57 changes: 29 additions & 28 deletions src/f0/app_adcs/source/adcs.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -322,19 +323,19 @@ 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;
OD_RAM.x4003_pos_z_magnetometer_1.z = INT16_MAX;
}

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;
Expand All @@ -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;
Expand All @@ -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;
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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);
Expand All @@ -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);
}


Expand Down

0 comments on commit 90f800f

Please sign in to comment.