mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 17:55:28 +03:00
Merge pull request #172 from iNavFlight/imu-fw-heading-improvements
Prefer GPS heading on airplanes, fallback to MAG if available
This commit is contained in:
commit
af78f06ad2
1 changed files with 42 additions and 18 deletions
|
@ -70,8 +70,10 @@
|
||||||
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
|
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
|
||||||
// which results in false gyro drift. See
|
// which results in false gyro drift. See
|
||||||
// http://gentlenav.googlecode.com/files/fastRotations.pdf
|
// http://gentlenav.googlecode.com/files/fastRotations.pdf
|
||||||
#define SPIN_RATE_LIMIT 20
|
|
||||||
#define MAX_ACC_SQ_NEARNESS 25 // 25% or G^2, accepted acceleration of (0.87 - 1.12G)
|
#define SPIN_RATE_LIMIT 20
|
||||||
|
#define MAX_ACC_SQ_NEARNESS 25 // 25% or G^2, accepted acceleration of (0.87 - 1.12G)
|
||||||
|
#define MAX_GPS_HEADING_ERROR_DEG 60 // Amount of error between GPS CoG and estimated Yaw at witch we stop trusting GPS and fallback to MAG
|
||||||
|
|
||||||
t_fp_vector imuAccelInBodyFrame;
|
t_fp_vector imuAccelInBodyFrame;
|
||||||
t_fp_vector imuMeasuredGravityBF;
|
t_fp_vector imuMeasuredGravityBF;
|
||||||
|
@ -427,30 +429,52 @@ static bool isMagnetometerHealthy(void)
|
||||||
|
|
||||||
static void imuCalculateEstimatedAttitude(float dT)
|
static void imuCalculateEstimatedAttitude(float dT)
|
||||||
{
|
{
|
||||||
float courseOverGround = 0;
|
const bool canUseMAG = sensors(SENSOR_MAG) && isMagnetometerHealthy();
|
||||||
|
const int accWeight = imuCalculateAccelerometerConfidence();
|
||||||
|
|
||||||
int accWeight = 0;
|
float courseOverGround = 0;
|
||||||
bool useMag = false;
|
bool useMag = false;
|
||||||
bool useCOG = false;
|
bool useCOG = false;
|
||||||
|
|
||||||
accWeight = imuCalculateAccelerometerConfidence();
|
|
||||||
|
|
||||||
if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
|
|
||||||
useMag = true;
|
|
||||||
}
|
|
||||||
#if defined(GPS)
|
#if defined(GPS)
|
||||||
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
|
if (STATE(FIXED_WING)) {
|
||||||
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
|
bool canUseCOG = sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300;
|
||||||
if (gpsHeadingInitialized) {
|
|
||||||
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
|
if (canUseCOG) {
|
||||||
useCOG = true;
|
if (gpsHeadingInitialized) {
|
||||||
|
// Use GPS heading if error is acceptable or if it's the only source of heading
|
||||||
|
if (ABS(gpsSol.groundCourse - attitude.values.yaw) < DEGREES_TO_DECIDEGREES(MAX_GPS_HEADING_ERROR_DEG) || !canUseMAG) {
|
||||||
|
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
|
||||||
|
useCOG = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// Re-initialize quaternion from known Roll, Pitch and GPS heading
|
||||||
|
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
|
||||||
|
gpsHeadingInitialized = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we can't use COG and there's MAG available - fallback
|
||||||
|
if (!useCOG && canUseMAG) {
|
||||||
|
useMag = true;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
else {
|
else if (canUseMAG) {
|
||||||
// Re-initialize quaternion from known Roll, Pitch and GPS heading
|
useMag = true;
|
||||||
imuComputeQuaternionFromRPY(attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
|
gpsHeadingInitialized = true; // GPS heading initialised from MAG, continue on GPS if possible
|
||||||
gpsHeadingInitialized = true;
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
else {
|
||||||
|
// Multicopters don't use GPS heading
|
||||||
|
if (canUseMAG) {
|
||||||
|
useMag = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
// In absence of GPS MAG is the only option
|
||||||
|
if (canUseMAG) {
|
||||||
|
useMag = true;
|
||||||
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
imuMahonyAHRSupdate(dT, imuMeasuredRotationBF.A[X], imuMeasuredRotationBF.A[Y], imuMeasuredRotationBF.A[Z],
|
imuMahonyAHRSupdate(dT, imuMeasuredRotationBF.A[X], imuMeasuredRotationBF.A[Y], imuMeasuredRotationBF.A[Z],
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue