imu changes to allow for GPS rescue mode & fix home error
This commit is contained in:
s0up
parent
bede912128
commit
4e3a21d2b3
|
|
@ -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,15 @@ 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 +477,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 +491,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 +522,8 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
|
|||
UNUSED(imuIsAccelerometerHealthy);
|
||||
UNUSED(useAcc);
|
||||
UNUSED(useMag);
|
||||
UNUSED(useYaw);
|
||||
UNUSED(rawYawError);
|
||||
UNUSED(useCOG);
|
||||
UNUSED(canUseGPSHeading);
|
||||
#else
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
|
||||
|
|
@ -509,7 +541,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 +570,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 +612,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)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue