Skip to content

Commit

Permalink
Add magnetic heading as debug and magnetic declination for the Mahony…
Browse files Browse the repository at this point in the history
… filter (betaflight#13073)

* Add mag heading to GPS Rescue heading debug

* Roll and pitch compensated magnetic yaw

* Changes according to PR comments

* Encapsulate yawMag calculations

* Corrected naming

* Changes according to PR comments

* Changes so that Checks don't fail

* Added PARAM_NAME list

* Changes so that Checks don't fail

* Changes according to PR comments

* Update src/main/fc/parameter_names.h

Co-authored-by: Mark Haslinghuis <mark@numloq.nl>

* Changes according to PR comments

* 200Hz compass task

* fix wait status flag

* increase default ODR of HMC5883L to 75Hz

* fix spikes in MagYaw debug by re-calc only on new data

---------

Co-authored-by: ctzsnooze <chris.thompson@sydney.edu.au>
Co-authored-by: Mark Haslinghuis <mark@numloq.nl>
3 people authored Sep 26, 2023
1 parent 1856d6f commit 9ada563
Showing 10 changed files with 96 additions and 32 deletions.
12 changes: 7 additions & 5 deletions src/main/cli/settings.c
Original file line number Diff line number Diff line change
@@ -1003,16 +1003,18 @@ const clivalue_t valueTable[] = {
{ "serial_update_rate_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 100, 2000 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, serial_update_rate_hz) },

// PG_IMU_CONFIG
{ "imu_dcm_kp", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_kp) },
{ "imu_dcm_ki", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, dcm_ki) },
{ "small_angle", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 180 }, PG_IMU_CONFIG, offsetof(imuConfig_t, small_angle) },
{ "imu_process_denom", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 4 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_process_denom) },
{ PARAM_NAME_IMU_DCM_KP, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_dcm_kp) },
{ PARAM_NAME_IMU_DCM_KI, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 32000 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_dcm_ki) },
{ PARAM_NAME_IMU_SMALL_ANGLE, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 180 }, PG_IMU_CONFIG, offsetof(imuConfig_t, small_angle) },
{ PARAM_NAME_IMU_PROCESS_DENOM, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 4 }, PG_IMU_CONFIG, offsetof(imuConfig_t, imu_process_denom) },
#ifdef USE_MAG
{ PARAM_NAME_IMU_MAG_DECLINATION, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 3599 }, PG_IMU_CONFIG, offsetof(imuConfig_t, mag_declination) },
#endif

// PG_ARMING_CONFIG
{ "auto_disarm_delay", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 60 }, PG_ARMING_CONFIG, offsetof(armingConfig_t, auto_disarm_delay) },
{ PARAM_NAME_GYRO_CAL_ON_FIRST_ARM, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_ARMING_CONFIG, offsetof(armingConfig_t, gyro_cal_on_first_arm) },


// PG_GPS_CONFIG
#ifdef USE_GPS
{ PARAM_NAME_GPS_PROVIDER, VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GPS_PROVIDER }, PG_GPS_CONFIG, offsetof(gpsConfig_t, provider) },
20 changes: 20 additions & 0 deletions src/main/common/vector.h
Original file line number Diff line number Diff line change
@@ -121,6 +121,26 @@ static inline fpVector3_t * matrixVectorMul(fpVector3_t * result, const fpMat33_
return result;
}

static inline fpMat33_t * yawToRotationMatrixZ(fpMat33_t * result, const float yaw)
{
fpMat33_t r;
const float sinYaw = sin_approx(yaw);
const float cosYaw = cos_approx(yaw);

r.m[0][0] = cosYaw;
r.m[1][0] = sinYaw;
r.m[2][0] = 0.0f;
r.m[0][1] = -sinYaw;
r.m[1][1] = cosYaw;
r.m[2][1] = 0.0f;
r.m[0][2] = 0.0f;
r.m[1][2] = 0.0f;
r.m[2][2] = 1.0f;

*result = r;
return result;
}

static inline fpVector3_t * matrixTrnVectorMul(fpVector3_t * result, const fpMat33_t * mat, const fpVector3_t * a)
{
fpVector3_t r;
3 changes: 2 additions & 1 deletion src/main/drivers/compass/compass_hmc5883l.c
Original file line number Diff line number Diff line change
@@ -125,6 +125,7 @@
#define HMC_CONFA_POS_BIAS 0x01
#define HMC_CONFA_NEG_BIAS 0x02
#define HMC_CONFA_DOR_15HZ 0X10
#define HMC_CONFA_DOR_75HZ 0x06
#define HMC_CONFA_8_SAMLES 0X60
#define HMC_CONFB_GAIN_2_5GA 0X60
#define HMC_CONFB_GAIN_1_3GA 0X20
@@ -227,7 +228,7 @@ static bool hmc5883lInit(magDev_t *mag)
extDevice_t *dev = &mag->dev;

// leave test mode
busWriteRegister(dev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
busWriteRegister(dev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_75HZ | HMC_CONFA_NORMAL); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
busWriteRegister(dev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
busWriteRegister(dev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS); // Mode register -- 000000 00 continuous Conversion Mode

2 changes: 1 addition & 1 deletion src/main/drivers/compass/compass_qmc5883l.c
Original file line number Diff line number Diff line change
@@ -109,7 +109,7 @@ static bool qmc5883lRead(magDev_t *magDev, int16_t *magData)
return false;

case STATE_WAIT_STATUS:
if ((status & 0x04) == 0) {
if ((status & 0x01) == 0) {
state = STATE_READ_STATUS;
return false;
}
8 changes: 8 additions & 0 deletions src/main/fc/parameter_names.h
Original file line number Diff line number Diff line change
@@ -197,3 +197,11 @@
#define PARAM_NAME_GPS_LAP_TIMER_GATE_TOLERANCE "gps_lap_timer_gate_tolerance_m"
#endif // USE_GPS_LAP_TIMER
#endif

#define PARAM_NAME_IMU_DCM_KP "imu_dcm_kp"
#define PARAM_NAME_IMU_DCM_KI "imu_dcm_ki"
#define PARAM_NAME_IMU_SMALL_ANGLE "small_angle"
#define PARAM_NAME_IMU_PROCESS_DENOM "imu_process_denom"
#ifdef USE_MAG
#define PARAM_NAME_IMU_MAG_DECLINATION "mag_declination"
#endif
2 changes: 1 addition & 1 deletion src/main/fc/tasks.c
Original file line number Diff line number Diff line change
@@ -383,7 +383,7 @@ task_attribute_t task_attributes[TASK_COUNT] = {
#endif

#ifdef USE_MAG
[TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, taskUpdateMag, TASK_PERIOD_HZ(10), TASK_PRIORITY_LOW),
[TASK_COMPASS] = DEFINE_TASK("COMPASS", NULL, NULL, taskUpdateMag, TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ), TASK_PRIORITY_LOW),
#endif

#ifdef USE_BARO
60 changes: 42 additions & 18 deletions src/main/flight/imu.c
Original file line number Diff line number Diff line change
@@ -100,6 +100,7 @@ static float smallAngleCosZ = 0;
static imuRuntimeConfig_t imuRuntimeConfig;

float rMat[3][3];
static fpVector2_t north_ef;

#if defined(USE_ACC)
STATIC_UNIT_TESTED bool attitudeIsEstablished = false;
@@ -115,13 +116,14 @@ quaternion offset = QUATERNION_INITIALIZE;
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
attitudeEulerAngles_t attitude = EULER_INITIALIZE;

PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 3);

PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
.dcm_kp = 2500, // 1.0 * 10000
.dcm_ki = 0, // 0.003 * 10000
.imu_dcm_kp = 2500, // 1.0 * 10000
.imu_dcm_ki = 0, // 0.003 * 10000
.small_angle = 25,
.imu_process_denom = 2
.imu_process_denom = 2,
.mag_declination = 0
);

static void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd)
@@ -167,8 +169,12 @@ static float calculateThrottleAngleScale(uint16_t throttle_correction_angle)

void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value)
{
imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f;
imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f;
imuRuntimeConfig.imuDcmKp = imuConfig()->imu_dcm_kp / 10000.0f;
imuRuntimeConfig.imuDcmKi = imuConfig()->imu_dcm_ki / 10000.0f;
// magnetic declination has negative sign (positive clockwise when seen from top)
const float imuMagneticDeclinationRad = DEGREES_TO_RADIANS(imuConfig()->mag_declination / 10.0f);
north_ef.x = cos_approx(imuMagneticDeclinationRad);
north_ef.y = -sin_approx(imuMagneticDeclinationRad);

smallAngleCosZ = cos_approx(degreesToRadians(imuConfig()->small_angle));

@@ -252,21 +258,39 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
#ifdef USE_MAG
// Use measured magnetic field vector
fpVector3_t mag_bf = {{mag.magADC[X], mag.magADC[Y], mag.magADC[Z]}};
float recipMagNorm = vectorNormSquared(&mag_bf);
if (useMag && recipMagNorm > 0.01f) {
float magNormSquared = vectorNormSquared(&mag_bf);
fpVector3_t mag_ef;
matrixVectorMul(&mag_ef, (const fpMat33_t*)&rMat, &mag_bf); // BF->EF true north

// Encapsulate additional operations in a block so that it is only executed when the according debug mode is used
// Only re-calculate magYaw when there is a new Mag data reading, to avoid spikes
if (debugMode == DEBUG_GPS_RESCUE_HEADING && mag.isNewMagADCFlag) {
fpMat33_t rMatZTrans;
yawToRotationMatrixZ(&rMatZTrans, -atan2_approx(rMat[1][0], rMat[0][0]));
fpVector3_t mag_ef_yawed;
matrixVectorMul(&mag_ef_yawed, &rMatZTrans, &mag_ef); // EF->EF yawed
// Magnetic yaw is the angle between true north and the X axis of the body frame
int16_t magYaw = lrintf((atan2_approx(mag_ef_yawed.y, mag_ef_yawed.x) * (1800.0f / M_PIf)));
if (magYaw < 0) {
magYaw += 3600;
}
DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 4, magYaw); // mag heading in degrees * 10
// reset new mag data flag to false to initiate monitoring for new Mag data.
// note that if the debug doesn't run, this reset will not occur, and we won't waste cycles on the comparison
mag.isNewMagADCFlag = false;
}

if (useMag && magNormSquared > 0.01f) {
// Normalise magnetometer measurement
vectorNormalize(&mag_bf, &mag_bf);
vectorNormalize(&mag_ef, &mag_ef);

// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in EF).
// This way magnetic field will only affect heading and wont mess roll/pitch angles

// (hx; hy; 0) - measured mag field vector in EF (forcing Z-component to zero)
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
fpVector3_t mag_ef;
matrixVectorMul(&mag_ef, (const fpMat33_t*)&rMat, &mag_bf); // BF->EF
mag_ef.z = 0.0f; // project to XY plane (optimized away)

fpVector2_t north_ef = {{ 1.0f, 0.0f }};
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
// increase gain on large misalignment
const float dot = vector2Dot((fpVector2_t*)&mag_ef, &north_ef);
@@ -297,10 +321,10 @@ STATIC_UNIT_TESTED void imuMahonyAHRSupdate(float dt, float gx, float gy, float
}

// Compute and apply integral feedback if enabled
if (imuRuntimeConfig.dcm_ki > 0.0f) {
if (imuRuntimeConfig.imuDcmKi > 0.0f) {
// Stop integrating if spinning beyond the certain limit
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
const float dcmKiGain = imuRuntimeConfig.dcm_ki;
const float dcmKiGain = imuRuntimeConfig.imuDcmKi;
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt;
@@ -380,7 +404,7 @@ static bool imuIsAccelerometerHealthy(float *accAverage)
return (0.81f < accMagnitudeSq) && (accMagnitudeSq < 1.21f);
}

// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.dcm_kp * 1.0 scaling.
// Calculate the dcmKpGain to use. When armed, the gain is imuRuntimeConfig.imuDcmKp * 1.0 scaling.
// When disarmed after initial boot, the scaling is set to 10.0 for the first 20 seconds to speed up initial convergence.
// After disarming we want to quickly reestablish convergence to deal with the attitude estimation being incorrect due to a crash.
// - wait for a 250ms period of low gyro activity to ensure the craft is not moving
@@ -425,7 +449,7 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera
arState = stReset;
}
// low gain during quiet phase
return imuRuntimeConfig.dcm_kp;
return imuRuntimeConfig.imuDcmKp;
case stReset:
if (cmpTimeUs(currentTimeUs, stateTimeout) >= 0) {
arState = stDisarmed;
@@ -434,11 +458,11 @@ static float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAvera
return ATTITUDE_RESET_KP_GAIN;
case stDisarmed:
// Scale the kP to generally converge faster when disarmed.
return imuRuntimeConfig.dcm_kp * 10.0f;
return imuRuntimeConfig.imuDcmKp * 10.0f;
}
} else {
arState = stArmed;
return imuRuntimeConfig.dcm_kp;
return imuRuntimeConfig.imuDcmKp;
}
}

9 changes: 5 additions & 4 deletions src/main/flight/imu.h
Original file line number Diff line number Diff line change
@@ -53,17 +53,18 @@ extern attitudeEulerAngles_t attitude;
extern float rMat[3][3];

typedef struct imuConfig_s {
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
uint16_t imu_dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t imu_dcm_ki; // DCM filter integral gain ( x 10000)
uint8_t small_angle;
uint8_t imu_process_denom;
uint16_t mag_declination; // Magnetic declination in degrees * 10
} imuConfig_t;

PG_DECLARE(imuConfig_t, imuConfig);

typedef struct imuRuntimeConfig_s {
float dcm_ki;
float dcm_kp;
float imuDcmKi;
float imuDcmKp;
} imuRuntimeConfig_t;

void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value);
10 changes: 8 additions & 2 deletions src/main/sensors/compass.c
Original file line number Diff line number Diff line change
@@ -117,6 +117,7 @@ void pgResetFn_compassConfig(compassConfig_t *compassConfig)
}

static int16_t magADCRaw[XYZ_AXIS_COUNT];
static int16_t magADCRawPrevious[XYZ_AXIS_COUNT];
static uint8_t magInit = 0;

void compassPreInit(void)
@@ -375,12 +376,17 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
if (busBusy(&magDev.dev, NULL) || !magDev.read(&magDev, magADCRaw)) {
// No action was taken as the read has not completed
schedulerIgnoreTaskExecRate();
return 1000; // Wait 1ms between states
return 500; // Wait 500us between states
}

for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (magADCRaw[axis] != magADCRawPrevious[axis]) {
// this test, and the isNewMagADCFlag itself, is only needed if we calculate magYaw in imu.c
mag.isNewMagADCFlag = true;
}
mag.magADC[axis] = magADCRaw[axis];
}

if (magDev.magAlignment == ALIGN_CUSTOM) {
alignSensorViaMatrix(mag.magADC, &magDev.rotationMatrix);
} else {
@@ -413,6 +419,6 @@ uint32_t compassUpdate(timeUs_t currentTimeUs)
}
}

return TASK_PERIOD_HZ(10);
return TASK_PERIOD_HZ(TASK_COMPASS_RATE_HZ);
}
#endif // USE_MAG
2 changes: 2 additions & 0 deletions src/main/sensors/compass.h
Original file line number Diff line number Diff line change
@@ -27,6 +27,7 @@
#include "pg/pg.h"
#include "sensors/sensors.h"

#define TASK_COMPASS_RATE_HZ 200

// Type of magnetometer used/detected
typedef enum {
@@ -42,6 +43,7 @@ typedef enum {
} magSensor_e;

typedef struct mag_s {
bool isNewMagADCFlag;
float magADC[XYZ_AXIS_COUNT];
} mag_t;

0 comments on commit 9ada563

Please sign in to comment.