From 948f2aedef59d708ee2deee8218205819fafcae8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sat, 19 Aug 2017 13:48:09 +0200 Subject: [PATCH 1/2] Allow aligning the mag to any rotation Introduce 3 new variables which allow setting the decidegrees for the mag sensor alignment. When any of these 3 variables are non-zero, mag is assumed to be mounted off-board and "align_mag" as well as the board alignment are ignored. Settings are named align_mag_roll, align_mag_pitch and align_mag_yaw. Fixes #86 Fixes #1029 --- src/main/fc/settings.yaml | 12 ++++++++++++ src/main/sensors/compass.c | 35 ++++++++++++++++++++++++++++++++--- src/main/sensors/compass.h | 7 +++++-- 3 files changed, 49 insertions(+), 5 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b97da8674a2..2a1462a0a1c 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..1900e5a74b5 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 @@ -363,8 +366,34 @@ void compassUpdate(timeUs_t currentTimeUs) mag.magADC[Z] -= compassConfig()->magZero.raw[Z]; } - 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 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); From fb08194398c81c7b6226d12b3a05ceee231036e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sun, 25 Feb 2018 12:34:53 +0000 Subject: [PATCH 2/2] Precalculate external mag rotation matrix Introduce rotateByMatrix(), which rotates a vector by the given matrix. Initialize the matrix on mag initialization, so it's not recalculated on every mag reading. --- src/main/drivers/compass/compass.h | 10 ++++- src/main/sensors/compass.c | 69 +++++++++++++++++------------- 2 files changed, 48 insertions(+), 31 deletions(-) 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/sensors/compass.c b/src/main/sensors/compass.c index 1900e5a74b5..566c6147373 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -87,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: @@ -97,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; @@ -113,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; @@ -129,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; @@ -145,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; @@ -161,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; @@ -177,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; @@ -193,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; @@ -209,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; @@ -272,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; @@ -366,32 +383,24 @@ void compassUpdate(timeUs_t currentTimeUs) mag.magADC[Z] -= compassConfig()->magZero.raw[Z]; } - 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], + if (mag.dev.magAlign.useExternal) { + const fpVector3_t 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), - }; + fpVector3_t rotated; - rotateV(&v, &r); + rotationMatrixRotateVector(&rotated, &v, &mag.dev.magAlign.externalRotation); - mag.magADC[X] = v.X; - mag.magADC[Y] = v.Y; - mag.magADC[Z] = v.Z; + 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); + applySensorAlignment(mag.magADC, mag.magADC, mag.dev.magAlign.onBoard); applyBoardAlignment(mag.magADC); }