diff --git a/src/main/drivers/compass/compass.h b/src/main/drivers/compass/compass.h index 18db9c45138..655021c7709 100644 --- a/src/main/drivers/compass/compass.h +++ b/src/main/drivers/compass/compass.h @@ -17,13 +17,21 @@ #pragma once +#include "common/vector.h" + #include "drivers/sensor.h" typedef struct magDev_s { busDevice_t * busDev; sensorMagInitFuncPtr init; // initialize function sensorMagReadFuncPtr read; // read 3 axis data function - sensor_align_e magAlign; + struct { + bool useExternal; + union { + fpMat3_t externalRotation; + sensor_align_e onBoard; + }; + } magAlign; uint8_t magSensorToUse; int16_t magADCRaw[XYZ_AXIS_COUNT]; } magDev_t; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5493d4afd19..1f523c969a2 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 808a8ae2199..566c6147373 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 @@ -84,7 +87,8 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) magSensor_e magHardware = MAG_NONE; requestedSensors[SENSOR_INDEX_MAG] = magHardwareToUse; - dev->magAlign = ALIGN_DEFAULT; + dev->magAlign.useExternal = false; + dev->magAlign.onBoard = ALIGN_DEFAULT; switch (magHardwareToUse) { case MAG_AUTODETECT: @@ -94,7 +98,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) #ifdef USE_MAG_QMC5883 if (qmc5883Detect(dev)) { #ifdef MAG_QMC5883L_ALIGN - dev->magAlign = MAG_QMC5883L_ALIGN; + dev->magAlign.onBoard = MAG_QMC5883L_ALIGN; #endif magHardware = MAG_QMC5883; break; @@ -110,7 +114,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(dev)) { #ifdef MAG_HMC5883_ALIGN - dev->magAlign = MAG_HMC5883_ALIGN; + dev->magAlign.onBoard = MAG_HMC5883_ALIGN; #endif magHardware = MAG_HMC5883; break; @@ -126,7 +130,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) #ifdef USE_MAG_AK8975 if (ak8975Detect(dev)) { #ifdef MAG_AK8975_ALIGN - dev->magAlign = MAG_AK8975_ALIGN; + dev->magAlign.onBoard = MAG_AK8975_ALIGN; #endif magHardware = MAG_AK8975; break; @@ -142,7 +146,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) #ifdef USE_MAG_AK8963 if (ak8963Detect(dev)) { #ifdef MAG_AK8963_ALIGN - dev->magAlign = MAG_AK8963_ALIGN; + dev->magAlign.onBoard = MAG_AK8963_ALIGN; #endif magHardware = MAG_AK8963; break; @@ -158,7 +162,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) #ifdef USE_GPS if (gpsMagDetect(dev)) { #ifdef MAG_GPS_ALIGN - dev->magAlign = MAG_GPS_ALIGN; + dev->magAlign.onBoard = MAG_GPS_ALIGN; #endif magHardware = MAG_GPS; break; @@ -174,7 +178,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) #ifdef USE_MAG_MAG3110 if (mag3110detect(dev)) { #ifdef MAG_MAG3110_ALIGN - dev->magAlign = MAG_MAG3110_ALIGN; + dev->magAlign.onBoard = MAG_MAG3110_ALIGN; #endif magHardware = MAG_MAG3110; break; @@ -190,7 +194,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) #ifdef USE_MAG_IST8310 if (ist8310Detect(dev)) { #ifdef MAG_IST8310_ALIGN - dev->magAlign = MAG_IST8310_ALIGN; + dev->magAlign.onBoard = MAG_IST8310_ALIGN; #endif magHardware = MAG_IST8310; break; @@ -206,7 +210,7 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) #ifdef USE_MAG_MPU9250 if (mpu9250CompassDetect(dev)) { #ifdef MAG_MPU9250_ALIGN - dev->magAlign = MAG_MPU9250_ALIGN; + dev->magAlign.onBoard = MAG_MPU9250_ALIGN; #endif magHardware = MAG_MPU9250; break; @@ -269,8 +273,24 @@ bool compassInit(void) sensorsClear(SENSOR_MAG); } - if (compassConfig()->mag_align != ALIGN_DEFAULT) { - mag.dev.magAlign = compassConfig()->mag_align; + if (compassConfig()->rollDeciDegrees != 0 || + compassConfig()->pitchDeciDegrees != 0 || + compassConfig()->yawDeciDegrees != 0) { + + // Externally aligned compass + mag.dev.magAlign.useExternal = true; + + fp_angles_t compassAngles = { + .angles.roll = DECIDEGREES_TO_RADIANS(compassConfig()->rollDeciDegrees), + .angles.pitch = DECIDEGREES_TO_RADIANS(compassConfig()->pitchDeciDegrees), + .angles.yaw = DECIDEGREES_TO_RADIANS(compassConfig()->yawDeciDegrees), + }; + rotationMatrixFromAngles(&mag.dev.magAlign.externalRotation, &compassAngles); + } else { + mag.dev.magAlign.useExternal = false; + if (compassConfig()->mag_align != ALIGN_DEFAULT) { + mag.dev.magAlign.onBoard = compassConfig()->mag_align; + } } return ret; @@ -363,8 +383,26 @@ void compassUpdate(timeUs_t currentTimeUs) mag.magADC[Z] -= compassConfig()->magZero.raw[Z]; } - applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign); - applyBoardAlignment(mag.magADC); + if (mag.dev.magAlign.useExternal) { + const fpVector3_t v = { + .x = mag.magADC[X], + .y = mag.magADC[Y], + .z = mag.magADC[Z], + }; + + fpVector3_t rotated; + + rotationMatrixRotateVector(&rotated, &v, &mag.dev.magAlign.externalRotation); + + mag.magADC[X] = rotated.x; + mag.magADC[Y] = rotated.y; + mag.magADC[Z] = rotated.z; + + } else { + // On-board compass + applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign.onBoard); + applyBoardAlignment(mag.magADC); + } magUpdatedAtLeastOnce = 1; } diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 80a3ce586b9..23224112eab 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -51,13 +51,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);