mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 14:25:20 +03:00
807 lines
23 KiB
C
807 lines
23 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 <string.h>
|
|
|
|
#include "common/maths.h"
|
|
|
|
#include "platform.h"
|
|
#include "debug.h"
|
|
|
|
#include "common/axis.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 "config/runtime_config.h"
|
|
|
|
//#define DEBUG_IMU
|
|
//#define DEBUG_IMU_SPEED
|
|
|
|
int16_t accSmooth[XYZ_AXIS_COUNT];
|
|
int32_t accSum[XYZ_AXIS_COUNT];
|
|
|
|
uint32_t accTimeSum = 0; // keep track for integration of acc
|
|
int accSumCount = 0;
|
|
float accVelScale;
|
|
|
|
int16_t smallAngle = 0;
|
|
|
|
float throttleAngleScale;
|
|
float fc_acc;
|
|
|
|
float magneticDeclination = 0.0f; // calculated at startup from config
|
|
float gyroScaleRad;
|
|
|
|
|
|
rollAndPitchInclination_t inclination = { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
|
static float anglerad[2] = { 0.0f, 0.0f }; // absolute angle inclination in radians
|
|
|
|
static imuRuntimeConfig_t *imuRuntimeConfig;
|
|
static pidProfile_t *pidProfile;
|
|
static accDeadband_t *accDeadband;
|
|
static accProcessor_t accProc;
|
|
|
|
static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims);
|
|
|
|
|
|
void imuConfigure(
|
|
imuRuntimeConfig_t *initialImuRuntimeConfig,
|
|
pidProfile_t *initialPidProfile,
|
|
accDeadband_t *initialAccDeadband,
|
|
float accz_lpf_cutoff,
|
|
uint16_t throttle_correction_angle
|
|
)
|
|
{
|
|
imuRuntimeConfig = initialImuRuntimeConfig;
|
|
pidProfile = initialPidProfile;
|
|
accDeadband = initialAccDeadband;
|
|
fc_acc = calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff);
|
|
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
// **************************************************
|
|
// Simplified IMU based on "Complementary Filter"
|
|
// Inspired by http://starlino.com/imu_guide.html
|
|
//
|
|
// adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
|
|
//
|
|
// The following ideas was used in this project:
|
|
// 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
|
|
//
|
|
// Currently Magnetometer uses separate CF which is used only
|
|
// for heading approximation.
|
|
//
|
|
// **************************************************
|
|
|
|
|
|
t_fp_vector EstG;
|
|
|
|
void imuResetAccelerationSum(void)
|
|
{
|
|
accSum[0] = 0;
|
|
accSum[1] = 0;
|
|
accSum[2] = 0;
|
|
accSumCount = 0;
|
|
accTimeSum = 0;
|
|
}
|
|
|
|
/*
|
|
* Baseflight calculation by Luggi09 originates from arducopter
|
|
* ============================================================
|
|
* This function rotates magnetic vector to cancel actual yaw and
|
|
* pitch of craft. Then it computes it's direction in X/Y plane.
|
|
* This value is returned as compass heading, value is 0-360 degrees.
|
|
*
|
|
* Note that Earth's magnetic field is not parallel with ground unless
|
|
* you are near equator. Its inclination is considerable, >60 degrees
|
|
* towards ground in most of Europe.
|
|
*
|
|
* First we consider it in 2D:
|
|
*
|
|
* An example, the vector <1, 1> would be turned into the heading
|
|
* 45 degrees, representing it's angle clockwise from north.
|
|
*
|
|
* ***************** *
|
|
* * | <1,1> *
|
|
* * | / *
|
|
* * | / *
|
|
* * |/ *
|
|
* * * *
|
|
* * *
|
|
* * *
|
|
* * *
|
|
* * *
|
|
* *******************
|
|
*
|
|
* //TODO: Add explanation for how it uses the Z dimension.
|
|
*/
|
|
int16_t imuCalculateHeading(t_fp_vector *vec)
|
|
{
|
|
int16_t head;
|
|
|
|
float cosineRoll = cos_approx(anglerad[AI_ROLL]);
|
|
float sineRoll = sin_approx(anglerad[AI_ROLL]);
|
|
float cosinePitch = cos_approx(anglerad[AI_PITCH]);
|
|
float sinePitch = sin_approx(anglerad[AI_PITCH]);
|
|
float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll;
|
|
float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll;
|
|
//TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero,
|
|
// or handle the case in which they are and (atan2f(0, 0) is undefined.
|
|
float hd = (atan2_approx(Yh, Xh) * 1800.0f / M_PIf + magneticDeclination) / 10.0f;
|
|
head = lrintf(hd);
|
|
|
|
// Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive.
|
|
if (head < 0)
|
|
head += 360;
|
|
|
|
return head;
|
|
}
|
|
|
|
#if 0
|
|
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors)
|
|
{
|
|
#if defined(NAZE) || defined(DEBUG_IMU_SPEED)
|
|
uint32_t time = micros();
|
|
#endif
|
|
if (imuUpdateSensors == ONLY_GYRO || imuUpdateSensors == ACC_AND_GYRO) {
|
|
gyroUpdate();
|
|
#ifdef DEBUG_IMU_SPEED
|
|
debug[0] = micros() - time; // gyro read time
|
|
#endif
|
|
}
|
|
if (sensors(SENSOR_ACC) && (!imuUpdateSensors == ONLY_GYRO)) {
|
|
#ifdef DEBUG_IMU_SPEED
|
|
time = micros();
|
|
#endif
|
|
qAccProcessingStateMachine(accelerometerTrims);
|
|
} else {
|
|
accADC[X] = 0;
|
|
accADC[Y] = 0;
|
|
accADC[Z] = 0;
|
|
}
|
|
#ifdef DEBUG_IMU_SPEED
|
|
debug[1] = micros() - time; // acc read time
|
|
if (imuUpdateSensors == ACC_AND_GYRO) {
|
|
debug[2] = debug[0] + debug[1]; // gyro + acc read time
|
|
}
|
|
#endif
|
|
}
|
|
#else
|
|
|
|
void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t imuUpdateSensors)
|
|
{
|
|
#ifdef DEBUG_IMU_SPEED
|
|
uint32_t time = micros();
|
|
#endif
|
|
if (imuUpdateSensors == ONLY_GYRO || imuUpdateSensors == ACC_AND_GYRO) {
|
|
gyroUpdate();
|
|
#ifdef DEBUG_IMU_SPEED
|
|
debug[0] = micros() - time; // gyro read time
|
|
#endif
|
|
}
|
|
if (sensors(SENSOR_ACC) && (!imuUpdateSensors == ONLY_GYRO)) {
|
|
#ifdef DEBUG_IMU_SPEED
|
|
time = micros();
|
|
#endif
|
|
qAccProcessingStateMachine(accelerometerTrims);
|
|
} else {
|
|
accADC[X] = 0;
|
|
accADC[Y] = 0;
|
|
accADC[Z] = 0;
|
|
}
|
|
#ifdef DEBUG_IMU_SPEED
|
|
debug[1] = micros() - time; // acc read time
|
|
if (imuUpdateSensors == ACC_AND_GYRO) {
|
|
debug[2] = debug[0] + debug[1]; // gyro + acc read time
|
|
}
|
|
#endif
|
|
}
|
|
|
|
#endif
|
|
|
|
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
|
|
{
|
|
float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z);
|
|
|
|
/*
|
|
* 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 (cosZ <= 0.015f) {
|
|
return 0;
|
|
}
|
|
int angle = lrintf(acos_approx(cosZ) * throttleAngleScale);
|
|
if (angle > 900)
|
|
angle = 900;
|
|
return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f)));
|
|
}
|
|
|
|
// WITHOUT
|
|
//arm - none - eabi - size . / obj / main / cleanflight_CC3D.elf
|
|
//text data bss dec hex filename
|
|
//116324 376 12640 129340 1f93c . / obj / main / cleanflight_CC3D.elf
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
// 4D Quaternion / 3D Vector Math
|
|
//arm - none - eabi - size . / obj / main / cleanflight_CC3D.elf
|
|
//text data bss dec hex filename
|
|
//116284 364 12636 129284 1f904 . / obj / main / cleanflight_CC3D.elf
|
|
|
|
typedef struct v3_s
|
|
{
|
|
float x;
|
|
float y;
|
|
float z;
|
|
} v3_t;
|
|
|
|
const v3_t V0 = { .x = 0.0f, .y = 0.0f, .z = 0.0f };
|
|
const v3_t VX = { .x = 1.0f, .y = 0.0f, .z = 0.0f };
|
|
const v3_t VY = { .x = 0.0f, .y = 1.0f, .z = 0.0f };
|
|
const v3_t VZ = { .x = 0.0f, .y = 0.0f, .z = 1.0f };
|
|
|
|
typedef struct q4_s
|
|
{
|
|
float w;
|
|
float x;
|
|
float y;
|
|
float z;
|
|
} q4_t;
|
|
|
|
const q4_t Q0 = { .w = 1.0f, .x = 0.0f, .y = 0.0f, .z = 0.0f };
|
|
|
|
void MulQQ(const q4_t *a, const q4_t *b, q4_t *o)
|
|
{
|
|
q4_t r;
|
|
r.w = a->w * b->w - a->x * b->x - a->y * b->y - a->z * b->z;
|
|
r.x = a->w * b->x + a->z * b->y - a->y * b->z + a->x * b->w;
|
|
r.y = a->w * b->y + a->x * b->z + a->y * b->w - a->z * b->x;
|
|
r.z = a->y * b->x - a->x * b->y + a->w * b->z + a->z * b->w;
|
|
*o = r;
|
|
}
|
|
|
|
void MulQV(const q4_t *a, const v3_t *b, q4_t *o)
|
|
{
|
|
q4_t r;
|
|
r.w = -a->x * b->x - a->y * b->y - a->z * b->z;
|
|
r.x = a->w * b->x + a->z * b->y - a->y * b->z;
|
|
r.y = a->w * b->y + a->x * b->z - a->z * b->x;
|
|
r.z = a->y * b->x - a->x * b->y + a->w * b->z;
|
|
*o = r;
|
|
}
|
|
|
|
void MulQF(const q4_t *a, const float b, q4_t *o)
|
|
{
|
|
q4_t r;
|
|
r.w = a->w * b;
|
|
r.x = a->x * b;
|
|
r.y = a->y * b;
|
|
r.z = a->z * b;
|
|
*o = r;
|
|
}
|
|
|
|
void MulVF(const v3_t *a, const float b, v3_t *o)
|
|
{
|
|
v3_t r;
|
|
r.x = a->x * b;
|
|
r.y = a->y * b;
|
|
r.z = a->z * b;
|
|
*o = r;
|
|
}
|
|
|
|
void SumQQ(const q4_t *a, const q4_t *b, q4_t *o)
|
|
{
|
|
q4_t r;
|
|
r.w = a->w + b->w;
|
|
r.x = a->x + b->x;
|
|
r.y = a->y + b->y;
|
|
r.z = a->z + b->z;
|
|
*o = r;
|
|
}
|
|
|
|
|
|
void SumVV(const v3_t *a, const v3_t *b, v3_t *o)
|
|
{
|
|
v3_t r;
|
|
r.x = a->x + b->x;
|
|
r.y = a->y + b->y;
|
|
r.z = a->z + b->z;
|
|
*o = r;
|
|
}
|
|
|
|
void SubQQ(const q4_t *a, const q4_t *b, q4_t *o)
|
|
{
|
|
q4_t r;
|
|
r.w = a->w - b->w;
|
|
r.x = a->x - b->x;
|
|
r.y = a->y - b->y;
|
|
r.z = a->z - b->z;
|
|
*o = r;
|
|
}
|
|
|
|
|
|
void SubVV(const v3_t *a, const v3_t *b, v3_t *o)
|
|
{
|
|
v3_t r;
|
|
r.x = a->x - b->x;
|
|
r.y = a->y - b->y;
|
|
r.z = a->z - b->z;
|
|
*o = r;
|
|
}
|
|
|
|
void CrossQQ(const q4_t *a, const q4_t *b, q4_t *o)
|
|
{
|
|
q4_t r;
|
|
r.w = 0.0f;
|
|
r.x = a->y * b->z - a->z * b->y;
|
|
r.y = a->z * b->x - a->x * b->z;
|
|
r.z = a->x * b->y - a->y * b->x;
|
|
*o = r;
|
|
}
|
|
|
|
void CrossVV(const v3_t *a, const v3_t *b, v3_t *o)
|
|
{
|
|
v3_t r;
|
|
r.x = a->y * b->z - a->z * b->y;
|
|
r.y = a->z * b->x - a->x * b->z;
|
|
r.z = a->x * b->y - a->y * b->x;
|
|
*o = r;
|
|
}
|
|
|
|
float DotQQ(const q4_t *a, const q4_t *b)
|
|
{
|
|
return a->w * b->w + a->x * b->x + a->y * b->y + a->z * b->z;
|
|
}
|
|
|
|
float DotVV(const v3_t *a, const v3_t *b)
|
|
{
|
|
return a->x * b->x + a->y * b->y + a->z * b->z;
|
|
}
|
|
|
|
float Mag2Q(const q4_t *a) // magnitude squared
|
|
{
|
|
return a->w*a->w + a->x*a->x + a->y*a->y + a->z*a->z;
|
|
}
|
|
|
|
#define MagQ(a) sqrtf(Mag2Q(a))
|
|
|
|
float Mag2V(const v3_t *a) // magnitude squared
|
|
{
|
|
return a->x*a->x + a->y*a->y + a->z*a->z; // TODO: optimize for unit vectors (m2 nearly 1.0)
|
|
}
|
|
|
|
#define MagV(a) sqrtf(Mag2V(a))
|
|
|
|
void NormQ(const q4_t *a, q4_t *o)
|
|
{
|
|
q4_t r;
|
|
MulQF(a, 1 / MagQ(a), &r);
|
|
*o = r;
|
|
}
|
|
|
|
void NormV(const v3_t *a, v3_t *o)
|
|
{
|
|
v3_t r;
|
|
float m = MagV(a);
|
|
MulVF(a, 1 / m, &r); // TODO: m nearly 0
|
|
*o = r;
|
|
}
|
|
|
|
void ConjQ(const q4_t *a, q4_t *o)
|
|
{
|
|
q4_t r;
|
|
r.w = a->w;
|
|
r.x = -a->x;
|
|
r.y = -a->y;
|
|
r.z = -a->z;
|
|
*o = r;
|
|
}
|
|
|
|
void RotateVQ(const v3_t *v, const q4_t *q, v3_t *o) //Vector rotated by a Quaternion(matches V^ = V * Matrix)
|
|
{
|
|
// v + 2 * r X(r X v + q.w*v) --https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Performance_comparisons
|
|
// vector r is the three imaginary coefficients of quaternion q
|
|
|
|
v3_t r2_rv_vw;
|
|
{
|
|
// reverse signs to change direction of rotation
|
|
v3_t r = { .x = -q->x, .y = -q->y, .z = -q->z };
|
|
v3_t r2;
|
|
SumVV(&r, &r, &r2);
|
|
|
|
v3_t rv_vw;
|
|
{
|
|
v3_t vw;
|
|
MulVF(v, q->w, &vw);
|
|
v3_t rv;
|
|
CrossVV(&r, v, &rv);
|
|
SumVV(&rv, &vw, &rv_vw);
|
|
}
|
|
CrossVV(&r2, &rv_vw, &r2_rv_vw);
|
|
}
|
|
SumVV(v, &r2_rv_vw, o);
|
|
}
|
|
|
|
void quaternion_approx(const v3_t *w, q4_t *o) // (angle vector[rad]) --Small angle approximation
|
|
{
|
|
q4_t r;
|
|
r.x = w->x / 2;
|
|
r.y = w->y / 2;
|
|
r.z = w->z / 2;
|
|
r.w = 1.0f - (0.5f * ((r.x * r.x) + (r.y * r.y) + (r.z * r.z)));
|
|
*o = r;
|
|
}
|
|
|
|
#if 0
|
|
void quaternion(const v3_t *w, q4_t *o) // (angle vector[rad]) --Large Rotation Quaternion
|
|
{
|
|
float m = MagV(w);
|
|
if (m == 0.0f)
|
|
{
|
|
*o = Q0;
|
|
}
|
|
else
|
|
{
|
|
q4_t r;
|
|
float t2 = m * 0.5f; // # rotation angle divided by 2
|
|
float sm = sin(t2) / m; // # computation minimization
|
|
r.x = w->x * sm;
|
|
r.y = w->y * sm;
|
|
r.z = w->z * sm;
|
|
r.w = cos(t2);
|
|
*o = r;
|
|
}
|
|
}
|
|
#else
|
|
# define quaternion(w,o) quaternion_approx(w,o) // I think we can get away with the approximation
|
|
// TODO - try usining sin_approx, cos_approx
|
|
#endif
|
|
|
|
typedef struct rpy_s
|
|
{
|
|
float r;
|
|
float p;
|
|
float y;
|
|
} rpy_t;
|
|
const rpy_t RPY0 = { .r = 0, .p = 0, .y = 0 };
|
|
|
|
void quaternion_from_rpy(const rpy_t *a, q4_t *o) // (degrees) yaw->pitch->roll order
|
|
{
|
|
float cr, sr, cp, sp, cy, sy;
|
|
|
|
{ float r2 = a->r * (RAD / 2); cr = cos_approx(r2); sr = sin_approx(r2); }
|
|
{ float p2 = a->p * (RAD / 2); cp = cos_approx(p2); sp = sin_approx(p2); }
|
|
{ float y2 = a->y * (RAD / 2); cy = cos_approx(y2); sy = sin_approx(y2); }
|
|
|
|
o->w = cr*cp*cy + sr*sp*sy;
|
|
o->x = sr*cp*cy - cr*sp*sy;
|
|
o->y = cr*sp*cy + sr*cp*sy;
|
|
o->z = cr*cp*sy - sr*sp*cy;
|
|
}
|
|
|
|
void quaternion_to_rpy(const q4_t *q, rpy_t *o) // (degrees) yaw->pitch->roll order
|
|
{
|
|
// https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
|
|
// Body Z - Y - X sequence
|
|
|
|
float q0 = q->w;
|
|
float q1 = q->x;
|
|
float q2 = q->y;
|
|
float q3 = q->z;
|
|
|
|
float p0 = MAX(-1.0f, MIN(1.0f, 2 * (q0*q2 - q3*q1)));
|
|
o->p = asin(p0);
|
|
|
|
if (ABS(ABS(o->p) - (90 * RAD)) < (0.5f*RAD)) // vertical
|
|
{
|
|
o->y = 2 * atan2_approx(q3, q0);
|
|
o->r = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
float r0 = 2 * (q0*q1 + q2*q3);
|
|
float r1 = 1 - 2 * (q1*q1 + q2*q2);
|
|
if ((r0 == 0) && (r1 == 0)) { o->r = 0.0f; } // atan(0,0)!
|
|
else { o->r = atan2_approx(r0, r1); }
|
|
|
|
float y0 = 2 * (q0*q3 + q1*q2);
|
|
float y1 = 1 - 2 * (q2*q2 + q3*q3);
|
|
if ((y0 == 0) && (y1 == 0)) { o->y = 0.0f; } // atan(0,0)!
|
|
else { o->y = atan2_approx(y0, y1); }
|
|
}
|
|
|
|
o->y = -o->y; // yaw inversion hack for all boards
|
|
|
|
}
|
|
|
|
void angle_vector(const q4_t *a, v3_t *o) // convert from quaternion to angle vector[rad]
|
|
{
|
|
q4_t a1;
|
|
if (a->w < 0) { MulQF(a, -1, &a1); a = &a1; }
|
|
|
|
float t2 = acos_approx(MIN(1, a->w)); // TODO acos_approx??
|
|
|
|
if (ABS(t2) > (0.005f * RAD))
|
|
{
|
|
float s = sin_approx(t2) / (2 * t2);
|
|
o->x = a->x / s;
|
|
o->y = a->y / s;
|
|
o->z = a->z / s;
|
|
}
|
|
else
|
|
{
|
|
*o = V0;
|
|
}
|
|
}
|
|
|
|
void nlerp_step(const q4_t *a, const q4_t *b, float max_th, q4_t *o) // max_th in radians (max_rate * update time)
|
|
{
|
|
float dot = MAX(-1, MIN(1, DotQQ(a, b)));
|
|
float th = 2*acos_approx(ABS(dot)); // ABS -> change direction for shortest path
|
|
|
|
if (th <= (0.01f*RAD)) { *o = *b; } // tiny step
|
|
else
|
|
{
|
|
float tb = MIN(1, ABS(max_th / th));
|
|
float ta = 1-tb;
|
|
if (dot < 0) { tb = -tb; } // change direction for shortest path
|
|
|
|
q4_t r, a1, b1;
|
|
MulQF(a, ta, &a1);
|
|
MulQF(b, tb, &b1);
|
|
SumQQ(&a1, &b1, &r);
|
|
NormQ(&r, o);
|
|
}
|
|
}
|
|
|
|
q4_t attitude_est_e_q;
|
|
float acc_rad_scale; // adc -> G
|
|
float gyro_rads_scale; // adc -> rad/s
|
|
float cosSmallAngle;
|
|
float acc_lpf_f0, acc_lpf_f1;
|
|
v3_t gravity_lpf_b_v, acc_lpf_b_v;
|
|
|
|
void qimuInit()
|
|
{
|
|
cosSmallAngle = cos_approx(RAD*imuRuntimeConfig->small_angle);
|
|
|
|
acc_rad_scale = 1.0f / acc_1G;
|
|
gyro_rads_scale = gyro.scale * RAD;
|
|
|
|
acc_lpf_f1 = (1.0f / imuRuntimeConfig->acc_lpf_factor);
|
|
acc_lpf_f0 = 1.0f - acc_lpf_f1;
|
|
gravity_lpf_b_v = VZ;
|
|
acc_lpf_b_v = VZ;
|
|
|
|
quaternion_from_rpy(&RPY0, &attitude_est_e_q);
|
|
accProc.state = ACCPROC_READ;
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
static void qAccProcessingStateMachine(rollAndPitchTrims_t *accelerometerTrims)
|
|
{
|
|
int axis;
|
|
const float gyro_drift_factor = 0.00f;
|
|
static v3_t gyro_drift_correction_b_v = { .x = 0.0f, .y = 0.0f, .z = 0.0f }; // rad/s
|
|
|
|
const float attitude_correction_factor = 0.001f;
|
|
static v3_t attitude_correction_b_v = { .x = 0.0f, .y = 0.0f, .z = 0.0f }; // rad/s
|
|
static v3_t acc_b_v, gyro_rate_b_v;
|
|
|
|
static int16_t normalize_counter = 0;
|
|
static uint32_t previousT = 0;
|
|
static uint32_t currentT;
|
|
|
|
// get time step.. TODO: this should really be fixed to division of MPU sample rate
|
|
static float dT;
|
|
|
|
bool keepProcessing = true; // (keepProcessing == true): causes all states to execute (for slow cycle times)
|
|
|
|
do {
|
|
switch (accProc.state) {
|
|
|
|
case ACCPROC_READ:
|
|
currentT = micros();
|
|
dT = (currentT - previousT)*0.000001f;
|
|
previousT = currentT;
|
|
updateAccelerationReadings(accelerometerTrims); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
|
|
accProc.state++;
|
|
break;
|
|
|
|
case ACCPROC_CHUNK_1:
|
|
for (axis = 0; axis < 3; axis++) {
|
|
accSmooth[axis] = accADC[axis]; // TODO acc_lpf - or would this work better without it?
|
|
((float *)&acc_b_v)[axis] = accSmooth[axis] * acc_rad_scale;
|
|
((float *)&gyro_rate_b_v)[axis] = gyroADC[axis] * gyro_rads_scale;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////
|
|
// add in drift compensation
|
|
SumVV(&gyro_rate_b_v, &gyro_drift_correction_b_v, &gyro_rate_b_v);
|
|
|
|
#ifdef DEBUG_IMU
|
|
debug[0] = gyro_drift_correction_b_v.x * 10000;
|
|
debug[1] = gyro_drift_correction_b_v.y * 10000;
|
|
debug[2] = gyro_drift_correction_b_v.z * 10000;
|
|
#endif
|
|
|
|
////////////////////////////////////////////////////////////////
|
|
// add in attitude estimate correction, convert to degrees
|
|
v3_t gyro_rotation_b_v;
|
|
SumVV(&gyro_rate_b_v, &attitude_correction_b_v, &gyro_rotation_b_v);
|
|
MulVF(&gyro_rotation_b_v, dT, &gyro_rotation_b_v);
|
|
|
|
////////////////////////////////////////////////////////////////
|
|
// Update attitude estimate with gyro data
|
|
// small angle approximation should be fine, but error does creep in at high rotational rates on multiple axes - Normalize periodically
|
|
q4_t attitude_est_update_b_q;
|
|
quaternion(&gyro_rotation_b_v, &attitude_est_update_b_q); // convert angle vector to quaternion
|
|
MulQQ(&attitude_est_update_b_q, &attitude_est_e_q, &attitude_est_e_q); // and rotate estimate
|
|
|
|
v3_t gravity_b_v;
|
|
// Calculate expected gravity(allows drift to be compensated on all 3 axis when possible)
|
|
RotateVQ(&VZ, &attitude_est_e_q, &gravity_b_v);
|
|
|
|
// check small angle
|
|
if (gravity_b_v.z > cosSmallAngle) {
|
|
ENABLE_STATE(SMALL_ANGLE);
|
|
} else {
|
|
DISABLE_STATE(SMALL_ANGLE);
|
|
}
|
|
|
|
// acc_lpf
|
|
if (imuRuntimeConfig->acc_lpf_factor > 0) {
|
|
v3_t a0, a1;
|
|
MulVF(&acc_lpf_b_v, acc_lpf_f0, &a0);
|
|
MulVF(&acc_b_v, acc_lpf_f1, &a1);
|
|
SumVV(&a0, &a1, &acc_lpf_b_v);
|
|
|
|
MulVF(&gravity_lpf_b_v, acc_lpf_f0, &a0);
|
|
MulVF(&gravity_b_v, acc_lpf_f1, &a1);
|
|
SumVV(&a0, &a1, &gravity_lpf_b_v);
|
|
} else {
|
|
acc_lpf_b_v = acc_b_v;
|
|
gravity_lpf_b_v = gravity_b_v;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////////////
|
|
// Calculate Correction
|
|
float acc_m2 = Mag2V(&acc_b_v);
|
|
#ifdef DEBUG_IMU
|
|
debug[3] = acc_m2*1000;
|
|
#endif
|
|
if ((acc_m2 > 1.1025f) || (acc_m2 < 0.9025f)) {
|
|
attitude_correction_b_v = V0;
|
|
} else { // we're not accelerating
|
|
// Cross product to determine error
|
|
CrossVV(&acc_lpf_b_v, &gravity_lpf_b_v, &attitude_correction_b_v);
|
|
MulVF(&attitude_correction_b_v, attitude_correction_factor/dT, &attitude_correction_b_v); // convert to rate for drift correction
|
|
|
|
if (gyro_drift_factor != 0.0f) {
|
|
// conditionally update drift for valid axes (4.5 degree check)
|
|
if (ABS(gravity_b_v.x) < 0.997f) {
|
|
gyro_drift_correction_b_v.x = gyro_drift_correction_b_v.x + (attitude_correction_b_v.x*gyro_drift_factor);
|
|
}
|
|
if (ABS(gravity_b_v.y) < 0.997f) {
|
|
gyro_drift_correction_b_v.y = gyro_drift_correction_b_v.y + (attitude_correction_b_v.y*gyro_drift_factor);
|
|
}
|
|
if (ABS(gravity_b_v.z) < 0.997f) {
|
|
gyro_drift_correction_b_v.z = gyro_drift_correction_b_v.z + (attitude_correction_b_v.z*gyro_drift_factor);
|
|
}
|
|
}
|
|
}
|
|
|
|
// renormalize every couple of seconds
|
|
if (++normalize_counter == 1000) {
|
|
NormQ(&attitude_est_e_q, &attitude_est_e_q);
|
|
normalize_counter = 0;
|
|
}
|
|
|
|
// convert to cleanflight values
|
|
// update inclination
|
|
rpy_t rpy;
|
|
quaternion_to_rpy(&attitude_est_e_q, &rpy);
|
|
inclination.values.rollDeciDegrees = lrintf(rpy.r * (10 / RAD));
|
|
inclination.values.pitchDeciDegrees = lrintf(rpy.p * (10 / RAD));
|
|
heading = rpy.y * (1 / RAD);
|
|
if (heading < 0) heading += 360;
|
|
#ifdef DEBUG_IMU
|
|
//uint32_t endT = micros();
|
|
//debug[3] = endT - currentT;
|
|
#endif
|
|
|
|
#if 0
|
|
accProc.state++;
|
|
break;
|
|
|
|
case ACCPROC_CHUNK_2:
|
|
accProc.state++;
|
|
break;
|
|
|
|
case ACCPROC_CHUNK_3:
|
|
accProc.state++;
|
|
break;
|
|
|
|
case ACCPROC_CHUNK_4:
|
|
accProc.state++;
|
|
break;
|
|
|
|
case ACCPROC_CHUNK_5:
|
|
accProc.state++;
|
|
break;
|
|
|
|
case ACCPROC_CHUNK_6:
|
|
accProc.state++;
|
|
break;
|
|
|
|
case ACCPROC_CHUNK_7:
|
|
accProc.state = ACCPROC_COPY;
|
|
break;
|
|
|
|
case ACCPROC_COPY:
|
|
// assign deliverables (copy local to global)
|
|
/*
|
|
memcpy(&EstG, &fsmEstG, sizeof(t_fp_vector));
|
|
for (axis = 0; axis < 3; axis++) {
|
|
accSmooth[axis] = fsmAccSmooth[axis];
|
|
}
|
|
memcpy(&inclination, &fsmInclination, sizeof(rollAndPitchInclination_t));
|
|
heading = fsmHeading;
|
|
*/
|
|
#endif
|
|
keepProcessing = false;
|
|
/* no break */
|
|
|
|
default:
|
|
accProc.state = ACCPROC_READ;
|
|
break;
|
|
}
|
|
} while (keepProcessing);
|
|
}
|