Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Merge pull request #2833 from iNavFlight/agh_free_mag_alignment

Browse source 
Allow aligning the mag to any rotation
This commit is contained in:
Konstantin Sharlaimov 2018-03-23 20:39:39 +10:00 • committed by GitHub
commit 07342d2f1d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 78 additions and 17 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/drivers/compass/compass.h
View file

@ -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;

12
src/main/fc/settings.yaml
View file

@ -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

66
src/main/sensors/compass.c
View file

@ -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;
}

7
src/main/sensors/compass.h
View file

@ -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);