Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Modified MMC5883MA driver to set and reset the magnetometer to handle bias and track bridge offset. #88

Merged
merged 4 commits into from
Jan 10, 2024
Merged

Modified MMC5883MA driver to set and reset the magnetometer to handle bias and track bridge offset. #88

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
10f323c
Modified MMC5883MA driver to perodicly set/reset the magnetometer to …
Dec 11, 2023
ef92034
Modified set reset handling code in magnetometer to estimate and trac…
Dec 11, 2023
e7cdb8c
Refactored bridge offset handling to work on a per driver instance ba…
Dec 12, 2023
b49a7e6
Added missing i2c start stop calls for mag set/reset
Dec 18, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 10 additions & 6 deletions common/include/mmc5883ma.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -372,12 +372,16 @@ struct MMC5883MAVMT {
/**
* @brief @p MMC5883MADriver specific data.
*/
#define _mmc5883ma_data \
_base_object_data \
/* Driver state.*/ \
mmc5883ma_state_t state; \
/* Current configuration data.*/ \
const MMC5883MAConfig *config;
#define _mmc5883ma_data \
_base_object_data \
/* Driver state.*/ \
mmc5883ma_state_t state; \
/* Current configuration data.*/ \
const MMC5883MAConfig *config; \
float bridge_offset_estimate_x; \
float bridge_offset_estimate_y; \
float bridge_offset_estimate_z; \
uint32_t read_call_count;

/**
* @brief MMC5883MA Power Monitor class.
Expand Down
62 changes: 60 additions & 2 deletions common/mmc5883ma.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
#include "string.h"
#include "chprintf.h"


#define DEBUG_SD (BaseSequentialStream*) &SD2

/*===========================================================================*/
Expand Down Expand Up @@ -125,6 +126,11 @@ bool mmc5883maReadData(MMC5883MADriver *devp, mmc5883ma_data_t *dest) {
return(false);
}


devp->read_call_count++;



bool ret = false;

/* Configuring common registers.*/
Expand All @@ -149,6 +155,55 @@ bool mmc5883maReadData(MMC5883MADriver *devp, mmc5883ma_data_t *dest) {

if( r == MSG_OK ) {
ret = true;

if( devp->read_call_count > 20 ) {
//This will periodically clear any bias on the magnetometer
devp->read_call_count = 0;

i2cStart(devp->config->i2cp, devp->config->i2ccfg);
if( ! mmc5883maI2CWriteRegister2(devp->config->i2cp, MMC5883MA_AD_INTRNLCTRL0, MMC5883MA_INTRNLCTRL0_SET)) {

}
i2cStop(devp->config->i2cp);
chThdSleepMilliseconds(1);


memset(rx, 0, sizeof(rx));
i2cStart(devp->config->i2cp, devp->config->i2ccfg);
msg_t r = i2cMasterTransmitTimeout(devp->config->i2cp, MMC5883MA_I2C_ADDRESS_READ, &reg, 1, rx, 6, MMC5883MA_DEFAULT_I2C_TIMEOUT);
i2cStop(devp->config->i2cp);

mmc5883ma_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];


const int32_t delta_set_reset_x = dest_temp.mx - dest->mx;
const int32_t delta_set_reset_y = dest_temp.my - dest->my;
const int32_t delta_set_reset_z = dest_temp.mz - dest->mz;

const float bridge_offset_midpoint_x = ((float) delta_set_reset_x) / 2.0;
const float bridge_offset_midpoint_y = ((float) delta_set_reset_y) / 2.0;
const float bridge_offset_midpoint_z = ((float) delta_set_reset_z) / 2.0;

//Modify tracked bridge offset estimate, but filter the data slightly.
const float filter_coefficient = 4.0;
devp->bridge_offset_estimate_x = devp->bridge_offset_estimate_x + ((bridge_offset_midpoint_x - devp->bridge_offset_estimate_x) / filter_coefficient);
devp->bridge_offset_estimate_y = devp->bridge_offset_estimate_y + ((bridge_offset_midpoint_y - devp->bridge_offset_estimate_y) / filter_coefficient);
devp->bridge_offset_estimate_z = devp->bridge_offset_estimate_z + ((bridge_offset_midpoint_z - devp->bridge_offset_estimate_z) / filter_coefficient);

i2cStart(devp->config->i2cp, devp->config->i2ccfg);
if( ! mmc5883maI2CWriteRegister2(devp->config->i2cp, MMC5883MA_AD_INTRNLCTRL0, MMC5883MA_INTRNLCTRL0_RST)) {

}
i2cStop(devp->config->i2cp);
chThdSleepMilliseconds(1);
}

dest->mx += devp->bridge_offset_estimate_x;
dest->my += devp->bridge_offset_estimate_y;
dest->mz += devp->bridge_offset_estimate_z;
}

#if MMC5883MA_SHARED_I2C
Expand All @@ -172,6 +227,11 @@ void mmc5883maObjectInit(MMC5883MADriver *devp) {
devp->config = NULL;

devp->state = MMC5883MA_STOP;

devp->bridge_offset_estimate_x = 0;
devp->bridge_offset_estimate_y = 0;
devp->bridge_offset_estimate_z = 0;
devp->read_call_count = 0;
}

bool mmc5883maSoftReset(MMC5883MADriver *devp) {
Expand All @@ -192,8 +252,6 @@ bool mmc5883maSoftReset(MMC5883MADriver *devp) {
uint8_t mmc_product_id_readback;

bool mmc5883maStart(MMC5883MADriver *devp, const MMC5883MAConfig *config) {
// i2cbuf_t buf;

osalDbgCheck((devp != NULL) && (config != NULL));
osalDbgAssert((devp->state == MMC5883MA_STOP) ||
(devp->state == MMC5883MA_READY),
Expand Down