From 9ada5638a314c6aa358d90412586cc0af6cf6443 Mon Sep 17 00:00:00 2001 From: pichim <93905657+pichim@users.noreply.github.com> Date: Wed, 27 Sep 2023 00:01:22 +0100 Subject: [PATCH] Add magnetic heading as debug and magnetic declination for the Mahony filter (#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 * 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 Co-authored-by: Mark Haslinghuis --- src/main/cli/settings.c | 12 +++-- src/main/common/vector.h | 20 +++++++ src/main/drivers/compass/compass_hmc5883l.c | 3 +- src/main/drivers/compass/compass_qmc5883l.c | 2 +- src/main/fc/parameter_names.h | 8 +++ src/main/fc/tasks.c | 2 +- src/main/flight/imu.c | 60 ++++++++++++++------- src/main/flight/imu.h | 9 ++-- src/main/sensors/compass.c | 10 +++- src/main/sensors/compass.h | 2 + 10 files changed, 96 insertions(+), 32 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 4feeaa7b215..b6ca858c755 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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) }, diff --git a/src/main/common/vector.h b/src/main/common/vector.h index 4a0d5219831..c8df5cefda2 100644 --- a/src/main/common/vector.h +++ b/src/main/common/vector.h @@ -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; diff --git a/src/main/drivers/compass/compass_hmc5883l.c b/src/main/drivers/compass/compass_hmc5883l.c index ed0e69e7621..1614208870f 100644 --- a/src/main/drivers/compass/compass_hmc5883l.c +++ b/src/main/drivers/compass/compass_hmc5883l.c @@ -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 diff --git a/src/main/drivers/compass/compass_qmc5883l.c b/src/main/drivers/compass/compass_qmc5883l.c index 79cdb8080ce..6f827cd4c09 100644 --- a/src/main/drivers/compass/compass_qmc5883l.c +++ b/src/main/drivers/compass/compass_qmc5883l.c @@ -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; } diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 59134d76a21..0c7d7db1808 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -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 diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index e81ecae0d7c..096a332ba7a 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -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 diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 34bc43acced..aa295dcb1b2 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -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; } } diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index e3c51a01ea6..9e7c6627a2c 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -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); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index d49402a4c92..5d4d4c69ffe 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -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 diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 30e0dd79142..40203c47b5d 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -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;