Commit 1a5133fa authored by Pawel Spychalski (DzikuVx)'s avatar Pawel Spychalski (DzikuVx)
Browse files

Fix mag gain computation for negative mag zero

parent 0a284d39
......@@ -350,7 +350,7 @@ void compassUpdate(timeUs_t currentTimeUs)
static sensorCalibrationState_t calState;
static timeUs_t calStartedAt = 0;
static int16_t magPrev[XYZ_AXIS_COUNT];
static int magGain[XYZ_AXIS_COUNT] = {-4096, -4096, -4096};
static int magAxisDeviation[XYZ_AXIS_COUNT];
// Check magZero
if (
......@@ -381,6 +381,7 @@ void compassUpdate(timeUs_t currentTimeUs)
compassConfigMutable()->magZero.raw[axis] = 0;
compassConfigMutable()->magGain[axis] = 1024;
magPrev[axis] = 0;
magAxisDeviation[axis] = 0; // Gain is based on the biggest absolute deviation from the mag zero point. Gain computation starts at 0
}
beeper(BEEPER_ACTION_SUCCESS);
......@@ -400,9 +401,9 @@ void compassUpdate(timeUs_t currentTimeUs)
diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]);
avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f;
const int32_t sample = ABS(mag.magADC[axis]);
if (sample > magGain[axis]) {
magGain[axis] = sample;
// Find the biggest sample deviation together with sample' sign
if (ABS(mag.magADC[axis]) > ABS(magAxisDeviation[axis])) {
magAxisDeviation[axis] = mag.magADC[axis];
}
}
......@@ -429,7 +430,7 @@ void compassUpdate(timeUs_t currentTimeUs)
* It is dirty, but worth checking if this will solve the problem of changing mag vector when UAV is tilted
*/
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
compassConfigMutable()->magGain[axis] = magGain[axis] - compassConfig()->magZero.raw[axis];
compassConfigMutable()->magGain[axis] = ABS(magAxisDeviation[axis] - compassConfig()->magZero.raw[axis]);
}
calStartedAt = 0;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment