mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-26 09:45:37 +03:00
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 <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>
This commit is contained in:
pichim
GitHub
parent
1856d6f7ef
commit
9ada5638a3
10 changed files with 96 additions and 32 deletions
|
|
@ -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) },
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue