diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b68895bbebe..380184eec5b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -264,6 +264,18 @@ groups: condition: USE_DUAL_MAG min: 0 max: 1 + - name: align_mag_roll + field: rollDeciDegrees + min: -1800 + max: 3600 + - name: align_mag_pitch + field: pitchDeciDegrees + min: -1800 + max: 3600 + - name: align_mag_yaw + field: yawDeciDegrees + min: -1800 + max: 3600 - name: PG_BAROMETER_CONFIG type: barometerConfig_t diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 47ec73288ce..1f37e5dfabb 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -72,7 +72,10 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig, .mag_hardware = MAG_HARDWARE_DEFAULT, .mag_declination = 0, .mag_to_use = 0, - .magCalibrationTimeLimit = 30 + .magCalibrationTimeLimit = 30, + .rollDeciDegrees = 0, + .pitchDeciDegrees = 0, + .yawDeciDegrees = 0, ); #ifdef USE_MAG @@ -368,8 +371,34 @@ void compassUpdate(timeUs_t currentTimeUs) } } - applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign); - applyBoardAlignment(mag.magADC); + if (compassConfig()->rollDeciDegrees != 0 || + compassConfig()->pitchDeciDegrees != 0 || + compassConfig()->yawDeciDegrees != 0) { + + // Externally aligned compass + struct fp_vector v = { + .X = mag.magADC[X], + .Y = mag.magADC[Y], + .Z = mag.magADC[Z], + }; + + fp_angles_t r = { + .angles.roll = DECIDEGREES_TO_RADIANS(compassConfig()->rollDeciDegrees), + .angles.pitch = DECIDEGREES_TO_RADIANS(compassConfig()->pitchDeciDegrees), + .angles.yaw = DECIDEGREES_TO_RADIANS(compassConfig()->yawDeciDegrees), + }; + + rotateV(&v, &r); + + mag.magADC[X] = v.X; + mag.magADC[Y] = v.Y; + mag.magADC[Z] = v.Z; + + } else { + // On-board compass + applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign); + applyBoardAlignment(mag.magADC); + } magUpdatedAtLeastOnce = 1; } diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index ac05bfa7b44..3a268deba49 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -52,13 +52,16 @@ typedef struct mag_s { extern mag_t mag; typedef struct compassConfig_s { - int16_t mag_declination; // Get your magnetic decliniation from here : http://magnetic-declination.com/ + int16_t mag_declination; // Get your magnetic declination from here : http://magnetic-declination.com/ // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. - sensor_align_e mag_align; // mag alignment + sensor_align_e mag_align; // on-board mag alignment. Ignored if externally aligned via *DeciDegrees. uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device flightDynamicsTrims_t magZero; uint8_t mag_to_use; uint8_t magCalibrationTimeLimit; // Time for compass calibration (seconds) + int16_t rollDeciDegrees; // Alignment for external mag on the roll (X) axis (0.1deg) + int16_t pitchDeciDegrees; // Alignment for external mag on the pitch (Y) axis (0.1deg) + int16_t yawDeciDegrees; // Alignment for external mag on the yaw (Z) axis (0.1deg) } compassConfig_t; PG_DECLARE(compassConfig_t, compassConfig);