rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #5753 from s0up/gps_rescue 

Use GPS course (when available) for correcting attitude.values.yaw
This commit is contained in:
Konstantin Sharlaimov 2018-04-24 16:51:55 +10:00 committed by GitHub
parent d492c57bdc feba4c329d
commit b892de88b1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 120 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
src/main/flight/imu.c
View File

@ -81,12 +81,14 @@ static bool imuUpdated = false;
#define ATTITUDE_RESET_GYRO_LIMIT 15 // 15 deg/sec - gyro limit for quiet period
#define ATTITUDE_RESET_KP_GAIN 25.0 // dcmKpGain value to use during attitude reset
#define ATTITUDE_RESET_ACTIVE_TIME 500000 // 500ms - Time to wait for attitude to converge at high gain
#define GPS_COG_MIN_GROUNDSPEED 500 // 500cm/s minimum groundspeed for a gps heading to be considered valid
int32_t accSum[XYZ_AXIS_COUNT];
uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
float accVelScale;
bool canUseGPSHeading = true;
static float throttleAngleScale;
static float fc_acc;
@ -166,6 +168,12 @@ void imuInit(void)
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig.small_angle));
accVelScale = 9.80665f / acc.dev.acc_1G / 10000.0f;
#ifdef USE_GPS
canUseGPSHeading = true;
#else
canUseGPSHeading = false;
#endif
imuComputeRotationMatrix();
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
@ -244,7 +252,7 @@ static float invSqrt(float x)
static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
bool useAcc, float ax, float ay, float az,
bool useMag, float mx, float my, float mz,
bool useYaw, float yawError, const float dcmKpGain)
bool useCOG, float courseOverGround, const float dcmKpGain)
{
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
@ -253,10 +261,20 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
// Use raw heading error (from GPS or whatever else)
float ex = 0, ey = 0, ez = 0;
if (useYaw) {
while (yawError > M_PIf) yawError -= (2.0f * M_PIf);
while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
ez += sin_approx(yawError / 2.0f);
if (useCOG) {
while (courseOverGround > M_PIf) {
courseOverGround -= (2.0f * M_PIf);
}
while (courseOverGround < -M_PIf) {
courseOverGround += (2.0f * M_PIf);
}
const float ez_ef = (- sin_approx(courseOverGround) * rMat[0][0] - cos_approx(courseOverGround) * rMat[1][0]);
ex = rMat[2][0] * ez_ef;
ey = rMat[2][1] * ez_ef;
ez = rMat[2][2] * ez_ef;
}
#ifdef USE_MAG
@ -464,10 +482,10 @@ float imuCalcKpGain(timeUs_t currentTimeUs, bool useAcc, float *gyroAverage)
static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
{
static timeUs_t previousIMUUpdateTime;
float rawYawError = 0;
bool useAcc = false;
bool useMag = false;
bool useYaw = false;
bool useCOG = false; // Whether or not correct yaw via imuMahonyAHRSupdate from our ground course
float courseOverGround = 0; // To be used when useCOG is true. Stored in Radians
const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
previousIMUUpdateTime = currentTimeUs;
@ -478,10 +496,29 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
}
#endif
#if defined(USE_GPS)
if (!useMag && STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) {
// In case of a fixed-wing aircraft we can use GPS course over ground to correct heading
rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse);
useYaw = true;
if (!useMag && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= GPS_COG_MIN_GROUNDSPEED) {
// Use GPS course over ground to correct attitude.values.yaw
if (STATE(FIXED_WING)) {
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
} else {
// If GPS rescue mode is active and we can use it, go for it. When we're close to home we will
// probably stop re calculating GPS heading data. Other future modes can also use this extern
if (canUseGPSHeading) {
courseOverGround = DECIDEGREES_TO_RADIANS(gpsSol.groundCourse);
useCOG = true;
}
}
if (useCOG && shouldInitializeGPSHeading()) {
// Reset our reference and reinitialize quaternion. This will likely ideally happen more than once per flight, but for now,
// shouldInitializeGPSHeading() returns true only once.
imuComputeQuaternionFromRPY(&qP, attitude.values.roll, attitude.values.pitch, gpsSol.groundCourse);
useCOG = false; // Don't use the COG when we first reinitialize. Next time around though, yes.
}
}
#endif
@ -490,8 +527,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
UNUSED(imuIsAccelerometerHealthy);
UNUSED(useAcc);
UNUSED(useMag);
UNUSED(useYaw);
UNUSED(rawYawError);
UNUSED(useCOG);
UNUSED(canUseGPSHeading);
UNUSED(courseOverGround);
#else
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
@ -509,7 +547,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
DEGREES_TO_RADIANS(gyroAverage[X]), DEGREES_TO_RADIANS(gyroAverage[Y]), DEGREES_TO_RADIANS(gyroAverage[Z]),
useAcc, accAverage[X], accAverage[Y], accAverage[Z],
useMag, mag.magADC[X], mag.magADC[Y], mag.magADC[Z],
useYaw, rawYawError, imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
useCOG, courseOverGround, imuCalcKpGain(currentTimeUs, useAcc, gyroAverage));
imuUpdateEulerAngles();
#endif
@ -538,11 +576,32 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
}
}
bool shouldInitializeGPSHeading()
{
static bool initialized = false;
if (!initialized) {
initialized = true;
return true;
}
return false;
}
float getCosTiltAngle(void)
{
return rMat[2][2];
}
void getQuaternion(quaternion *quat)
{
quat->w = q.w;
quat->x = q.x;
quat->y = q.y;
quat->z = q.z;
}
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
{
/*
@ -559,6 +618,49 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
}
void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw)
{
if (initialRoll > 1800) {
initialRoll -= 3600;
}
if (initialPitch > 1800) {
initialPitch -= 3600;
}
if (initialYaw > 1800) {
initialYaw -= 3600;
}
const float cosRoll = cos_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
const float sinRoll = sin_approx(DECIDEGREES_TO_RADIANS(initialRoll) * 0.5f);
const float cosPitch = cos_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
const float sinPitch = sin_approx(DECIDEGREES_TO_RADIANS(initialPitch) * 0.5f);
const float cosYaw = cos_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
const float sinYaw = sin_approx(DECIDEGREES_TO_RADIANS(-initialYaw) * 0.5f);
const float q0 = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
const float q1 = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
const float q2 = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
const float q3 = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
quatProd->xx = sq(q1);
quatProd->yy = sq(q2);
quatProd->zz = sq(q3);
quatProd->xy = q1 * q2;
quatProd->xz = q1 * q3;
quatProd->yz = q2 * q3;
quatProd->wx = q0 * q1;
quatProd->wy = q0 * q2;
quatProd->wz = q0 * q3;
imuComputeRotationMatrix();
}
#ifdef SIMULATOR_BUILD
void imuSetAttitudeRPY(float roll, float pitch, float yaw)
{

4
src/main/flight/imu.h
View File

@ -29,6 +29,7 @@ extern uint32_t accTimeSum;
extern int accSumCount;
extern float accVelScale;
extern int32_t accSum[XYZ_AXIS_COUNT];
extern bool canUseGPSHeading;
typedef struct {
float w,x,y,z;
@ -79,6 +80,7 @@ typedef struct imuRuntimeConfig_s {
void imuConfigure(uint16_t throttle_correction_angle);
float getCosTiltAngle(void);
void getQuaternion(quaternion * q);
void imuUpdateAttitude(timeUs_t currentTimeUs);
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
@ -96,3 +98,5 @@ void imuSetHasNewData(uint32_t dt);
void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd);
bool imuQuaternionHeadfreeOffsetSet(void);
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def * v);
void imuComputeQuaternionFromRPY(quaternionProducts *qP, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw);
bool shouldInitializeGPSHeading(void);