Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

Avoid compass calculation in IMU when USE_MAG not defined

Browse source
This commit is contained in:
Martin Budden 2018-02-01 10:48:05 +00:00
parent 67d33b0a52
commit d7ccd2488c
1 changed files with 39 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

69
src/main/flight/imu.c
View file

@ -180,7 +180,7 @@ void imuResetAccelerationSum(void)
#if defined(USE_ALT_HOLD) #if defined(USE_ALT_HOLD)
static void imuTransformVectorBodyToEarth(t_fp_vector * v) static void imuTransformVectorBodyToEarth(t_fp_vector * v)
{ {
/* From body frame to earth frame */ // From body frame to earth frame
const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z; const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z; const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z; const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
@ -191,7 +191,7 @@ static void imuTransformVectorBodyToEarth(t_fp_vector * v)
} }
// rotate acc into Earth frame and calculate acceleration in it // rotate acc into Earth frame and calculate acceleration in it
static void imuCalculateAcceleration(uint32_t deltaT) static void imuCalculateAcceleration(timeDelta_t deltaT)
{ {
static int32_t accZoffset = 0; static int32_t accZoffset = 0;
static float accz_smooth = 0; static float accz_smooth = 0;
@ -260,23 +260,22 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz)); const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
// Use raw heading error (from GPS or whatever else) // Use raw heading error (from GPS or whatever else)
float ez = 0; float ex = 0, ey = 0, ez = 0;
if (useYaw) { if (useYaw) {
while (yawError > M_PIf) yawError -= (2.0f * M_PIf); while (yawError > M_PIf) yawError -= (2.0f * M_PIf);
while (yawError < -M_PIf) yawError += (2.0f * M_PIf); while (yawError < -M_PIf) yawError += (2.0f * M_PIf);
ez += sin_approx(yawError / 2.0f); ez += sin_approx(yawError / 2.0f);
} }
#ifdef USE_MAG
// Use measured magnetic field vector // Use measured magnetic field vector
float ex = 0, ey = 0; float recipMagNorm = sq(mx) + sq(my) + sq(mz);
float recipNorm = sq(mx) + sq(my) + sq(mz); if (useMag && recipMagNorm > 0.01f) {
if (useMag && recipNorm > 0.01f) {
// Normalise magnetometer measurement // Normalise magnetometer measurement
recipNorm = invSqrt(recipNorm); recipMagNorm = invSqrt(recipMagNorm);
mx *= recipNorm; mx *= recipMagNorm;
my *= recipNorm; my *= recipMagNorm;
mz *= recipNorm; mz *= recipMagNorm;
// For magnetometer correction we make an assumption that magnetic field is perpendicular to gravity (ignore Z-component in 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 // This way magnetic field will only affect heading and wont mess roll/pitch angles
@ -295,15 +294,21 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
ey += rMat[2][1] * ez_ef; ey += rMat[2][1] * ez_ef;
ez += rMat[2][2] * ez_ef; ez += rMat[2][2] * ez_ef;
} }
#else
UNUSED(useMag);
UNUSED(mx);
UNUSED(my);
UNUSED(mz);
#endif
// Use measured acceleration vector // Use measured acceleration vector
recipNorm = sq(ax) + sq(ay) + sq(az); float recipAccNorm = sq(ax) + sq(ay) + sq(az);
if (useAcc && recipNorm > 0.01f) { if (useAcc && recipAccNorm > 0.01f) {
// Normalise accelerometer measurement // Normalise accelerometer measurement
recipNorm = invSqrt(recipNorm); recipAccNorm = invSqrt(recipAccNorm);
ax *= recipNorm; ax *= recipAccNorm;
ay *= recipNorm; ay *= recipAccNorm;
az *= recipNorm; az *= recipAccNorm;
// Error is sum of cross product between estimated direction and measured direction of gravity // Error is sum of cross product between estimated direction and measured direction of gravity
ex += (ay * rMat[2][2] - az * rMat[2][1]); ex += (ay * rMat[2][2] - az * rMat[2][1]);
@ -320,8 +325,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
integralFBy += dcmKiGain * ey * dt; integralFBy += dcmKiGain * ey * dt;
integralFBz += dcmKiGain * ez * dt; integralFBz += dcmKiGain * ez * dt;
} }
} } else {
else {
integralFBx = 0.0f; // prevent integral windup integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f; integralFBy = 0.0f;
integralFBz = 0.0f; integralFBz = 0.0f;
@ -352,7 +356,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
q.z += (+buffer.w * gz + buffer.x * gy - buffer.y * gx); q.z += (+buffer.w * gz + buffer.x * gy - buffer.y * gx);
// Normalise quaternion // Normalise quaternion
recipNorm = invSqrt(sq(q.w) + sq(q.x) + sq(q.y) + sq(q.z)); float recipNorm = invSqrt(sq(q.w) + sq(q.x) + sq(q.y) + sq(q.z));
q.w *= recipNorm; q.w *= recipNorm;
q.x *= recipNorm; q.x *= recipNorm;
q.y *= recipNorm; q.y *= recipNorm;
@ -362,7 +366,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz,
imuComputeRotationMatrix(); imuComputeRotationMatrix();
} }
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void){ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
{
quaternionProducts buffer; quaternionProducts buffer;
if (FLIGHT_MODE(HEADFREE_MODE)) { if (FLIGHT_MODE(HEADFREE_MODE)) {
@ -380,7 +385,7 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void){
if (attitude.values.yaw < 0) if (attitude.values.yaw < 0)
attitude.values.yaw += 3600; attitude.values.yaw += 3600;
/* Update small angle state */ // Update small angle state
if (rMat[2][2] > smallAngleCosZ) { if (rMat[2][2] > smallAngleCosZ) {
ENABLE_STATE(SMALL_ANGLE); ENABLE_STATE(SMALL_ANGLE);
} else { } else {
@ -404,13 +409,13 @@ static bool imuIsAccelerometerHealthy(void)
static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
{ {
static uint32_t previousIMUUpdateTime; static timeUs_t previousIMUUpdateTime;
float rawYawError = 0; float rawYawError = 0;
bool useAcc = false; bool useAcc = false;
bool useMag = false; bool useMag = false;
bool useYaw = false; bool useYaw = false;
uint32_t deltaT = currentTimeUs - previousIMUUpdateTime; const timeDelta_t deltaT = currentTimeUs - previousIMUUpdateTime;
previousIMUUpdateTime = currentTimeUs; previousIMUUpdateTime = currentTimeUs;
if (imuIsAccelerometerHealthy()) { if (imuIsAccelerometerHealthy()) {
@ -540,7 +545,8 @@ void imuSetHasNewData(uint32_t dt)
} }
#endif #endif
void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd) { void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd)
{
quatProd->ww = quat->w * quat->w; quatProd->ww = quat->w * quat->w;
quatProd->wx = quat->w * quat->x; quatProd->wx = quat->w * quat->x;
quatProd->wy = quat->w * quat->y; quatProd->wy = quat->w * quat->y;
@ -553,7 +559,8 @@ void imuQuaternionComputeProducts(quaternion *quat, quaternionProducts *quatProd
quatProd->zz = quat->z * quat->z; quatProd->zz = quat->z * quat->z;
} }
bool imuQuaternionHeadfreeOffsetSet(void) { bool imuQuaternionHeadfreeOffsetSet(void)
{
if ((ABS(attitude.values.roll) < 450) && (ABS(attitude.values.pitch) < 450)) { if ((ABS(attitude.values.roll) < 450) && (ABS(attitude.values.pitch) < 450)) {
const float yaw = -atan2_approx((+2.0f * (qP.wz + qP.xy)), (+1.0f - 2.0f * (qP.yy + qP.zz))); const float yaw = -atan2_approx((+2.0f * (qP.wz + qP.xy)), (+1.0f - 2.0f * (qP.yy + qP.zz)));
@ -562,13 +569,14 @@ bool imuQuaternionHeadfreeOffsetSet(void) {
offset.y = 0; offset.y = 0;
offset.z = sin_approx(yaw/2); offset.z = sin_approx(yaw/2);
return(true); return true;
} else { } else {
return(false); return false;
} }
} }
void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *result) { void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *result)
{
const float A = (q1->w + q1->x) * (q2->w + q2->x); const float A = (q1->w + q1->x) * (q2->w + q2->x);
const float B = (q1->z - q1->y) * (q2->y - q2->z); const float B = (q1->z - q1->y) * (q2->y - q2->z);
const float C = (q1->w - q1->x) * (q2->y + q2->z); const float C = (q1->w - q1->x) * (q2->y + q2->z);
@ -584,7 +592,8 @@ void imuQuaternionMultiplication(quaternion *q1, quaternion *q2, quaternion *res
result->z = D + (+ E - F - G + H) / 2.0f; result->z = D + (+ E - F - G + H) / 2.0f;
} }
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v) { void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def *v)
{
quaternionProducts buffer; quaternionProducts buffer;
imuQuaternionMultiplication(&offset, &q, &headfree); imuQuaternionMultiplication(&offset, &q, &headfree);