mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 08:15:30 +03:00
452 lines
14 KiB
C
452 lines
14 KiB
C
/*
|
|
* This file is part of Cleanflight.
|
|
*
|
|
* Cleanflight is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* Cleanflight is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
// Inertial Measurement Unit (IMU)
|
|
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <math.h>
|
|
|
|
#include "common/maths.h"
|
|
|
|
#include "build_config.h"
|
|
#include "platform.h"
|
|
#include "debug.h"
|
|
|
|
#include "common/axis.h"
|
|
#include "common/filter.h"
|
|
|
|
#include "drivers/system.h"
|
|
#include "drivers/sensor.h"
|
|
#include "drivers/accgyro.h"
|
|
#include "drivers/compass.h"
|
|
|
|
#include "sensors/sensors.h"
|
|
#include "sensors/gyro.h"
|
|
#include "sensors/compass.h"
|
|
#include "sensors/acceleration.h"
|
|
#include "sensors/barometer.h"
|
|
#include "sensors/sonar.h"
|
|
|
|
#include "flight/mixer.h"
|
|
#include "flight/pid.h"
|
|
#include "flight/imu.h"
|
|
|
|
#include "io/gps.h"
|
|
|
|
#include "config/runtime_config.h"
|
|
|
|
// the limit (in degrees/second) beyond which we stop integrating
|
|
// omega_I. At larger spin rates the DCM PI controller can get 'dizzy'
|
|
// which results in false gyro drift. See
|
|
// http://gentlenav.googlecode.com/files/fastRotations.pdf
|
|
#define SPIN_RATE_LIMIT 20
|
|
|
|
int32_t accSum[XYZ_AXIS_COUNT];
|
|
|
|
uint32_t accTimeSum = 0; // keep track for integration of acc
|
|
int accSumCount = 0;
|
|
float accVelScale;
|
|
|
|
float throttleAngleScale;
|
|
float fc_acc;
|
|
float smallAngleCosZ = 0;
|
|
|
|
float magneticDeclination = 0.0f; // calculated at startup from config
|
|
static bool isAccelUpdatedAtLeastOnce = false;
|
|
|
|
static imuRuntimeConfig_t *imuRuntimeConfig;
|
|
static pidProfile_t *pidProfile;
|
|
static accDeadband_t *accDeadband;
|
|
|
|
STATIC_UNIT_TESTED float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to earth frame
|
|
static float rMat[3][3];
|
|
|
|
attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
|
|
|
static float gyroScale;
|
|
|
|
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
|
|
{
|
|
float q1q1 = sq(q1);
|
|
float q2q2 = sq(q2);
|
|
float q3q3 = sq(q3);
|
|
|
|
float q0q1 = q0 * q1;
|
|
float q0q2 = q0 * q2;
|
|
float q0q3 = q0 * q3;
|
|
float q1q2 = q1 * q2;
|
|
float q1q3 = q1 * q3;
|
|
float q2q3 = q2 * q3;
|
|
|
|
rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
|
|
rMat[0][1] = 2.0f * (q1q2 + -q0q3);
|
|
rMat[0][2] = 2.0f * (q1q3 - -q0q2);
|
|
|
|
rMat[1][0] = 2.0f * (q1q2 - -q0q3);
|
|
rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
|
|
rMat[1][2] = 2.0f * (q2q3 + -q0q1);
|
|
|
|
rMat[2][0] = 2.0f * (q1q3 + -q0q2);
|
|
rMat[2][1] = 2.0f * (q2q3 - -q0q1);
|
|
rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
|
|
}
|
|
|
|
void imuConfigure(
|
|
imuRuntimeConfig_t *initialImuRuntimeConfig,
|
|
pidProfile_t *initialPidProfile,
|
|
accDeadband_t *initialAccDeadband,
|
|
uint16_t throttle_correction_angle
|
|
)
|
|
{
|
|
imuRuntimeConfig = initialImuRuntimeConfig;
|
|
pidProfile = initialPidProfile;
|
|
accDeadband = initialAccDeadband;
|
|
fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value
|
|
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
|
|
}
|
|
|
|
void imuInit(void)
|
|
{
|
|
smallAngleCosZ = cos_approx(degreesToRadians(imuRuntimeConfig->small_angle));
|
|
gyroScale = gyro.scale * (M_PIf / 180.0f); // gyro output scaled to rad per second
|
|
accVelScale = 9.80665f / acc_1G / 10000.0f;
|
|
|
|
imuComputeRotationMatrix();
|
|
}
|
|
|
|
float calculateThrottleAngleScale(uint16_t throttle_correction_angle)
|
|
{
|
|
return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle);
|
|
}
|
|
|
|
/*
|
|
* Calculate RC time constant used in the accZ lpf.
|
|
*/
|
|
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
|
|
{
|
|
return 0.5f / (M_PIf * accz_lpf_cutoff);
|
|
}
|
|
|
|
void imuResetAccelerationSum(void)
|
|
{
|
|
accSum[0] = 0;
|
|
accSum[1] = 0;
|
|
accSum[2] = 0;
|
|
accSumCount = 0;
|
|
accTimeSum = 0;
|
|
}
|
|
|
|
void imuTransformVectorBodyToEarth(t_fp_vector * v)
|
|
{
|
|
float x,y,z;
|
|
|
|
/* From body frame to earth frame */
|
|
x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
|
|
y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
|
|
z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
|
|
|
|
v->V.X = x;
|
|
v->V.Y = -y;
|
|
v->V.Z = z;
|
|
}
|
|
|
|
// rotate acc into Earth frame and calculate acceleration in it
|
|
void imuCalculateAcceleration(uint32_t deltaT)
|
|
{
|
|
static int32_t accZoffset = 0;
|
|
static float accz_smooth = 0;
|
|
float dT;
|
|
t_fp_vector accel_ned;
|
|
|
|
// deltaT is measured in us ticks
|
|
dT = (float)deltaT * 1e-6f;
|
|
|
|
accel_ned.V.X = accSmooth[0];
|
|
accel_ned.V.Y = accSmooth[1];
|
|
accel_ned.V.Z = accSmooth[2];
|
|
|
|
imuTransformVectorBodyToEarth(&accel_ned);
|
|
|
|
if (imuRuntimeConfig->acc_unarmedcal == 1) {
|
|
if (!ARMING_FLAG(ARMED)) {
|
|
accZoffset -= accZoffset / 64;
|
|
accZoffset += accel_ned.V.Z;
|
|
}
|
|
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
|
|
} else
|
|
accel_ned.V.Z -= acc_1G;
|
|
|
|
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
|
|
|
|
// apply Deadband to reduce integration drift and vibration influence
|
|
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
|
|
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
|
|
accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
|
|
|
|
// sum up Values for later integration to get velocity and distance
|
|
accTimeSum += deltaT;
|
|
accSumCount++;
|
|
}
|
|
|
|
static float invSqrt(float x)
|
|
{
|
|
return 1.0f / sqrtf(x);
|
|
}
|
|
|
|
static bool imuUseFastGains(void)
|
|
{
|
|
return !ARMING_FLAG(ARMED) && millis() < 20000;
|
|
}
|
|
|
|
static float imuGetPGainScaleFactor(void)
|
|
{
|
|
if (imuUseFastGains()) {
|
|
return 10.0f;
|
|
}
|
|
else {
|
|
return 1.0f;
|
|
}
|
|
}
|
|
|
|
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)
|
|
{
|
|
static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki
|
|
float recipNorm;
|
|
float hx, hy, bx;
|
|
float ex = 0, ey = 0, ez = 0;
|
|
float qa, qb, qc;
|
|
|
|
// Calculate general spin rate (rad/s)
|
|
float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz));
|
|
|
|
// Use raw heading error (from GPS or whatever else)
|
|
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);
|
|
}
|
|
|
|
// Use measured magnetic field vector
|
|
recipNorm = sq(mx) + sq(my) + sq(mz);
|
|
if (useMag && recipNorm > 0.01f) {
|
|
// Normalise magnetometer measurement
|
|
recipNorm = invSqrt(recipNorm);
|
|
mx *= recipNorm;
|
|
my *= recipNorm;
|
|
mz *= recipNorm;
|
|
|
|
// 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 (assuming Z-component is zero)
|
|
// (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero)
|
|
hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz;
|
|
hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz;
|
|
bx = sqrtf(hx * hx + hy * hy);
|
|
|
|
// magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF)
|
|
float ez_ef = -(hy * bx);
|
|
|
|
// Rotate mag error vector back to BF and accumulate
|
|
ex += rMat[2][0] * ez_ef;
|
|
ey += rMat[2][1] * ez_ef;
|
|
ez += rMat[2][2] * ez_ef;
|
|
}
|
|
|
|
// Use measured acceleration vector
|
|
recipNorm = sq(ax) + sq(ay) + sq(az);
|
|
if (useAcc && recipNorm > 0.01f) {
|
|
// Normalise accelerometer measurement
|
|
recipNorm = invSqrt(recipNorm);
|
|
ax *= recipNorm;
|
|
ay *= recipNorm;
|
|
az *= recipNorm;
|
|
|
|
// Error is sum of cross product between estimated direction and measured direction of gravity
|
|
ex += (ay * rMat[2][2] - az * rMat[2][1]);
|
|
ey += (az * rMat[2][0] - ax * rMat[2][2]);
|
|
ez += (ax * rMat[2][1] - ay * rMat[2][0]);
|
|
}
|
|
|
|
// Compute and apply integral feedback if enabled
|
|
if(imuRuntimeConfig->dcm_ki > 0.0f) {
|
|
// Stop integrating if spinning beyond the certain limit
|
|
if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) {
|
|
float dcmKiGain = imuRuntimeConfig->dcm_ki;
|
|
integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki
|
|
integralFBy += dcmKiGain * ey * dt;
|
|
integralFBz += dcmKiGain * ez * dt;
|
|
}
|
|
}
|
|
else {
|
|
integralFBx = 0.0f; // prevent integral windup
|
|
integralFBy = 0.0f;
|
|
integralFBz = 0.0f;
|
|
}
|
|
|
|
// Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster
|
|
float dcmKpGain = imuRuntimeConfig->dcm_kp * imuGetPGainScaleFactor();
|
|
|
|
// Apply proportional and integral feedback
|
|
gx += dcmKpGain * ex + integralFBx;
|
|
gy += dcmKpGain * ey + integralFBy;
|
|
gz += dcmKpGain * ez + integralFBz;
|
|
|
|
// Integrate rate of change of quaternion
|
|
gx *= (0.5f * dt);
|
|
gy *= (0.5f * dt);
|
|
gz *= (0.5f * dt);
|
|
|
|
qa = q0;
|
|
qb = q1;
|
|
qc = q2;
|
|
q0 += (-qb * gx - qc * gy - q3 * gz);
|
|
q1 += (qa * gx + qc * gz - q3 * gy);
|
|
q2 += (qa * gy - qb * gz + q3 * gx);
|
|
q3 += (qa * gz + qb * gy - qc * gx);
|
|
|
|
// Normalise quaternion
|
|
recipNorm = invSqrt(sq(q0) + sq(q1) + sq(q2) + sq(q3));
|
|
q0 *= recipNorm;
|
|
q1 *= recipNorm;
|
|
q2 *= recipNorm;
|
|
q3 *= recipNorm;
|
|
|
|
// Pre-compute rotation matrix from quaternion
|
|
imuComputeRotationMatrix();
|
|
}
|
|
|
|
STATIC_UNIT_TESTED void imuUpdateEulerAngles(void)
|
|
{
|
|
/* Compute pitch/roll angles */
|
|
attitude.values.roll = lrintf(atan2f(rMat[2][1], rMat[2][2]) * (1800.0f / M_PIf));
|
|
attitude.values.pitch = lrintf(((0.5f * M_PIf) - acosf(-rMat[2][0])) * (1800.0f / M_PIf));
|
|
attitude.values.yaw = lrintf((-atan2f(rMat[1][0], rMat[0][0]) * (1800.0f / M_PIf) + magneticDeclination));
|
|
|
|
if (attitude.values.yaw < 0)
|
|
attitude.values.yaw += 3600;
|
|
|
|
/* Update small angle state */
|
|
if (rMat[2][2] > smallAngleCosZ) {
|
|
ENABLE_STATE(SMALL_ANGLE);
|
|
} else {
|
|
DISABLE_STATE(SMALL_ANGLE);
|
|
}
|
|
}
|
|
|
|
static bool imuIsAccelerometerHealthy(void)
|
|
{
|
|
int32_t axis;
|
|
int32_t accMagnitude = 0;
|
|
|
|
for (axis = 0; axis < 3; axis++) {
|
|
accMagnitude += (int32_t)accSmooth[axis] * accSmooth[axis];
|
|
}
|
|
|
|
accMagnitude = accMagnitude * 100 / (sq((int32_t)acc_1G));
|
|
|
|
// Accept accel readings only in range 0.90g - 1.10g
|
|
return (81 < accMagnitude) && (accMagnitude < 121);
|
|
}
|
|
|
|
static bool isMagnetometerHealthy(void)
|
|
{
|
|
return (magADC[X] != 0) && (magADC[Y] != 0) && (magADC[Z] != 0);
|
|
}
|
|
|
|
static void imuCalculateEstimatedAttitude(void)
|
|
{
|
|
static uint32_t previousIMUUpdateTime;
|
|
float rawYawError = 0;
|
|
bool useAcc = false;
|
|
bool useMag = false;
|
|
bool useYaw = false;
|
|
|
|
uint32_t currentTime = micros();
|
|
uint32_t deltaT = currentTime - previousIMUUpdateTime;
|
|
previousIMUUpdateTime = currentTime;
|
|
|
|
if (imuIsAccelerometerHealthy()) {
|
|
useAcc = true;
|
|
}
|
|
|
|
if (sensors(SENSOR_MAG) && isMagnetometerHealthy()) {
|
|
useMag = true;
|
|
}
|
|
#if defined(GPS)
|
|
else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 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 - GPS_ground_course);
|
|
useYaw = true;
|
|
}
|
|
#endif
|
|
|
|
imuMahonyAHRSupdate(deltaT * 1e-6f,
|
|
gyroADC[X] * gyroScale, gyroADC[Y] * gyroScale, gyroADC[Z] * gyroScale,
|
|
useAcc, accSmooth[X], accSmooth[Y], accSmooth[Z],
|
|
useMag, magADC[X], magADC[Y], magADC[Z],
|
|
useYaw, rawYawError);
|
|
|
|
imuUpdateEulerAngles();
|
|
|
|
imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
|
|
}
|
|
|
|
void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims)
|
|
{
|
|
if (sensors(SENSOR_ACC)) {
|
|
updateAccelerationReadings(accelerometerTrims);
|
|
isAccelUpdatedAtLeastOnce = true;
|
|
}
|
|
}
|
|
|
|
void imuUpdateAttitude(void)
|
|
{
|
|
if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) {
|
|
imuCalculateEstimatedAttitude();
|
|
} else {
|
|
accSmooth[X] = 0;
|
|
accSmooth[Y] = 0;
|
|
accSmooth[Z] = 0;
|
|
}
|
|
}
|
|
|
|
float getCosTiltAngle(void)
|
|
{
|
|
return rMat[2][2];
|
|
}
|
|
|
|
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
|
|
{
|
|
/*
|
|
* Use 0 as the throttle angle correction if we are inverted, vertical or with a
|
|
* small angle < 0.86 deg
|
|
* TODO: Define this small angle in config.
|
|
*/
|
|
if (rMat[2][2] <= 0.015f) {
|
|
return 0;
|
|
}
|
|
int angle = lrintf(acosf(rMat[2][2]) * throttleAngleScale);
|
|
if (angle > 900)
|
|
angle = 900;
|
|
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
|
|
}
|