Unverified Commit ba3d0ab5 authored by Paweł Spychalski's avatar Paweł Spychalski Committed by GitHub
Browse files

Merge pull request #6636 from iNavFlight/dzikuvx-fix-mag-gain-calibration

Fix mag gain computation for negative mag zero
parents 0a284d39 1a5133fa
...@@ -350,7 +350,7 @@ void compassUpdate(timeUs_t currentTimeUs) ...@@ -350,7 +350,7 @@ void compassUpdate(timeUs_t currentTimeUs)
static sensorCalibrationState_t calState; static sensorCalibrationState_t calState;
static timeUs_t calStartedAt = 0; static timeUs_t calStartedAt = 0;
static int16_t magPrev[XYZ_AXIS_COUNT]; static int16_t magPrev[XYZ_AXIS_COUNT];
static int magGain[XYZ_AXIS_COUNT] = {-4096, -4096, -4096}; static int magAxisDeviation[XYZ_AXIS_COUNT];
// Check magZero // Check magZero
if ( if (
...@@ -381,6 +381,7 @@ void compassUpdate(timeUs_t currentTimeUs) ...@@ -381,6 +381,7 @@ void compassUpdate(timeUs_t currentTimeUs)
compassConfigMutable()->magZero.raw[axis] = 0; compassConfigMutable()->magZero.raw[axis] = 0;
compassConfigMutable()->magGain[axis] = 1024; compassConfigMutable()->magGain[axis] = 1024;
magPrev[axis] = 0; 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); beeper(BEEPER_ACTION_SUCCESS);
...@@ -400,9 +401,9 @@ void compassUpdate(timeUs_t currentTimeUs) ...@@ -400,9 +401,9 @@ void compassUpdate(timeUs_t currentTimeUs)
diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]); diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]);
avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f; avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f;
const int32_t sample = ABS(mag.magADC[axis]); // Find the biggest sample deviation together with sample' sign
if (sample > magGain[axis]) { if (ABS(mag.magADC[axis]) > ABS(magAxisDeviation[axis])) {
magGain[axis] = sample; magAxisDeviation[axis] = mag.magADC[axis];
} }
} }
...@@ -429,7 +430,7 @@ void compassUpdate(timeUs_t currentTimeUs) ...@@ -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 * 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++) { 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; 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