Skip to content

Act on compass read return value #511

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

Merged
digitalentity merged 3 commits into from
Aug 28, 2016
Merged

Act on compass read return value #511

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d3c01ae
Act on compass read return value
martinbudden Aug 27, 2016
c0709f7
Set magADC to zero when mag.read fails
martinbudden Aug 28, 2016
bfa89ff
Improved handling of failed reads in initialisation
martinbudden Aug 28, 2016
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
5 changes: 5 additions & 0 deletions src/main/drivers/compass_ak8963.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -258,6 +258,11 @@ bool ak8963Read(int16_t *magData)
bool ack = false;
uint8_t buf[7];

// set magData to zero for case of failed read
magData[X] = 0;
magData[Y] = 0;
magData[Z] = 0;

#if defined(USE_SPI) && defined(MPU9250_SPI_INSTANCE)

// we currently need a different approach for the MPU9250 connected via SPI.
Expand Down
19 changes: 7 additions & 12 deletions src/main/drivers/compass_ak8975.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -112,12 +112,15 @@ void ak8975Init()

bool ak8975Read(int16_t *magData)
{
bool ack;
UNUSED(ack);
uint8_t status;
uint8_t buf[6];

ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
// set magData to zero for case of failed read
magData[X] = 0;
magData[Y] = 0;
magData[Z] = 0;

bool ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS1, 1, &status);
if (!ack || (status & BIT_STATUS1_REG_DATA_READY) == 0) {
return false;
}
Expand All @@ -134,15 +137,7 @@ bool ak8975Read(int16_t *magData)
#endif

ack = i2cRead(MAG_I2C_INSTANCE, AK8975_MAG_I2C_ADDRESS, AK8975_MAG_REG_STATUS2, 1, &status);
if (!ack) {
return false;
}

if (status & BIT_STATUS2_REG_DATA_ERROR) {
return false;
}

if (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW) {
if (!ack || (status & BIT_STATUS2_REG_DATA_ERROR) || (status & BIT_STATUS2_REG_MAG_SENSOR_OVERFLOW)) {
return false;
}

Expand Down
69 changes: 39 additions & 30 deletions src/main/drivers/compass_hmc5883l.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -169,14 +169,13 @@ static void hmc5883lConfigureDataReadyInterruptHandling(void)

bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
{
bool ack = false;
uint8_t sig = 0;

hmc5883Config = hmc5883ConfigToUse;

ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
if (!ack || sig != 'H')
uint8_t sig = 0;
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, 0x0A, 1, &sig);
if (!ack || sig != 'H') {
return false;
}

mag->init = hmc5883lInit;
mag->read = hmc5883lRead;
Expand All @@ -187,7 +186,6 @@ bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
void hmc5883lInit(void)
{
int16_t magADC[3];
int i;
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
bool bret = true; // Error indicator

Expand All @@ -199,40 +197,48 @@ void hmc5883lInit(void)
delay(100);
hmc5883lRead(magADC);

for (i = 0; i < 10; i++) { // Collect 10 samples
int validSamples = 0;
int failedSamples = 0;
while (validSamples < 10 && failedSamples < 5) { // Collect 10 samples
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
delay(50);
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.

// Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[X] += magADC[X];
xyz_total[Y] += magADC[Y];
xyz_total[Z] += magADC[Z];

// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
if (hmc5883lRead(magADC)) { // Get the raw values in case the scales have already been changed.
++validSamples;
// Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[X] += magADC[X];
xyz_total[Y] += magADC[Y];
xyz_total[Z] += magADC[Z];
// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
} else {
++failedSamples;
}
LED1_TOGGLE;
}

// Apply the negative bias. (Same gain)
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
for (i = 0; i < 10; i++) {
validSamples = 0;
failedSamples = 0;
while (validSamples < 10 && failedSamples < 5) { // Collect 10 samples
i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
delay(50);
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.

// Since the measurements are noisy, they should be averaged.
xyz_total[X] -= magADC[X];
xyz_total[Y] -= magADC[Y];
xyz_total[Z] -= magADC[Z];

// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
if (hmc5883lRead(magADC)) { // Get the raw values in case the scales have already been changed.
++validSamples;
// Since the measurements are noisy, they should be averaged.
xyz_total[X] -= magADC[X];
xyz_total[Y] -= magADC[Y];
xyz_total[Z] -= magADC[Z];
// Detect saturation.
if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
} else {
++failedSamples;
}
LED1_TOGGLE;
}
Expand Down Expand Up @@ -262,6 +268,9 @@ bool hmc5883lRead(int16_t *magData)

bool ack = i2cRead(MAG_I2C_INSTANCE, MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
if (!ack) {
magData[X] = 0;
magData[Y] = 0;
magData[Z] = 0;
return false;
}
// During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
Expand Down
20 changes: 11 additions & 9 deletions src/main/drivers/compass_mag3110.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,10 +61,9 @@

bool mag3110detect(mag_t *mag)
{
bool ack = false;
uint8_t sig = 0;

ack = i2cRead(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_WHO_AM_I, 1, &sig);
bool ack = i2cRead(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_WHO_AM_I, 1, &sig);
if (!ack || sig != 0xC4)
return false;

Expand All @@ -76,10 +75,7 @@ bool mag3110detect(mag_t *mag)

void mag3110Init()
{
bool ack;
UNUSED(ack);

ack = i2cWrite(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_CTRL_REG1, 0x01); // active mode 80 Hz ODR with OSR = 1
bool ack = i2cWrite(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_CTRL_REG1, 0x01); // active mode 80 Hz ODR with OSR = 1
delay(20);

ack = i2cWrite(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_CTRL_REG2, 0xA0); // AUTO_MRST_EN + RAW
Expand All @@ -90,17 +86,23 @@ void mag3110Init()

bool mag3110Read(int16_t *magData)
{
bool ack;
UNUSED(ack);
uint8_t status;
uint8_t buf[6];

ack = i2cRead(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_STATUS, 1, &status);
// set magData to zero for case of failed read
magData[X] = 0;
magData[Y] = 0;
magData[Z] = 0;

bool ack = i2cRead(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_STATUS, 1, &status);
if (!ack || (status & BIT_STATUS_REG_DATA_READY) == 0) {
return false;
}

ack = i2cRead(MAG_I2C_INSTANCE, MAG3110_MAG_I2C_ADDRESS, MAG3110_MAG_REG_HXL, 6, buf);
if (!ack) {
return false;
}

magData[X] = (int16_t)(buf[0] << 8 | buf[1]);
magData[Y] = (int16_t)(buf[2] << 8 | buf[3]);
Expand Down
20 changes: 13 additions & 7 deletions src/main/sensors/compass.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,15 +73,21 @@ void updateCompass(flightDynamicsTrims_t *magZero)
{
static uint32_t calStartedAt = 0;
static int16_t magPrev[XYZ_AXIS_COUNT];
uint32_t axis;

mag.read(magADCRaw);
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) magADC[axis] = magADCRaw[axis]; // int32_t copy to work with
if (!mag.read(magADCRaw)) {
magADC[X] = 0;
magADC[Y] = 0;
magADC[Z] = 0;
return;
Copy link
Copy Markdown
Member

@digitalentity digitalentity Aug 28, 2016

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We still need to return zero vector here so IMU can detect invalid compass and act accordingly.

}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
magADC[axis] = magADCRaw[axis]; // int32_t copy to work with
}

if (STATE(CALIBRATE_MAG)) {
calStartedAt = currentTime;

for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = 0;
magPrev[axis] = 0;
}
Expand All @@ -103,7 +109,7 @@ void updateCompass(flightDynamicsTrims_t *magZero)
float diffMag = 0;
float avgMag = 0;

for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
diffMag += (magADC[axis] - magPrev[axis]) * (magADC[axis] - magPrev[axis]);
avgMag += (magADC[axis] + magPrev[axis]) * (magADC[axis] + magPrev[axis]) / 4.0f;
}
Expand All @@ -112,15 +118,15 @@ void updateCompass(flightDynamicsTrims_t *magZero)
if ((avgMag > 0.01f) && ((diffMag / avgMag) > (0.14f * 0.14f))) {
sensorCalibrationPushSampleForOffsetCalculation(&calState, magADC);

for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
magPrev[axis] = magADC[axis];
}
}
} else {
float magZerof[3];
sensorCalibrationSolveForOffset(&calState, magZerof);

for (axis = 0; axis < 3; axis++) {
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = lrintf(magZerof[axis]);
}

Expand Down