mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-18 22:05:17 +03:00
moved source files around in preparation to adding makefile build way
added makefile (thx gke) added linker script (thx gke) moved startups into src directory as well no code changes. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@105 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop
parent
aae8ef4c3d
commit
5091f3e9ff
45 changed files with 229 additions and 14182 deletions
350
src/imu.c
Executable file
350
src/imu.c
Executable file
|
|
@ -0,0 +1,350 @@
|
|||
#include "board.h"
|
||||
#include "mw.h"
|
||||
|
||||
#define M_PI 3.14159265358979323846
|
||||
|
||||
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
|
||||
int16_t acc_25deg = 0;
|
||||
int32_t BaroAlt;
|
||||
int32_t EstAlt; // in cm
|
||||
int16_t BaroPID = 0;
|
||||
int32_t AltHold;
|
||||
int16_t errorAltitudeI = 0;
|
||||
|
||||
// **************
|
||||
// gyro+acc IMU
|
||||
// **************
|
||||
int16_t gyroData[3] = { 0, 0, 0 };
|
||||
int16_t gyroZero[3] = { 0, 0, 0 };
|
||||
int16_t angle[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
|
||||
int8_t smallAngle25 = 1;
|
||||
|
||||
static void getEstimatedAttitude(void);
|
||||
|
||||
void imuInit(void)
|
||||
{
|
||||
acc_25deg = acc_1G * 0.423;
|
||||
|
||||
// if mag sensor is enabled, use it
|
||||
if (sensors(SENSOR_MAG))
|
||||
Mag_init();
|
||||
}
|
||||
|
||||
void computeIMU(void)
|
||||
{
|
||||
uint8_t axis;
|
||||
static int16_t gyroADCprevious[3] = { 0, 0, 0 };
|
||||
int16_t gyroADCp[3];
|
||||
int16_t gyroADCinter[3];
|
||||
static uint32_t timeInterleave = 0;
|
||||
static int16_t gyroYawSmooth = 0;
|
||||
|
||||
if (sensors(SENSOR_ACC)) {
|
||||
ACC_getADC();
|
||||
getEstimatedAttitude();
|
||||
}
|
||||
|
||||
Gyro_getADC();
|
||||
|
||||
for (axis = 0; axis < 3; axis++)
|
||||
gyroADCp[axis] = gyroADC[axis];
|
||||
timeInterleave = micros();
|
||||
annexCode();
|
||||
if ((micros() - timeInterleave) > 650) {
|
||||
annex650_overrun_count++;
|
||||
} else {
|
||||
while ((micros() - timeInterleave) < 650); //empirical, interleaving delay between 2 consecutive reads
|
||||
}
|
||||
|
||||
Gyro_getADC();
|
||||
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
|
||||
// empirical, we take a weighted value of the current and the previous values
|
||||
gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis] + 1) / 3;
|
||||
gyroADCprevious[axis] = gyroADCinter[axis] / 2;
|
||||
if (!sensors(SENSOR_ACC))
|
||||
accADC[axis] = 0;
|
||||
}
|
||||
|
||||
if (feature(FEATURE_GYRO_SMOOTHING)) {
|
||||
static uint8_t Smoothing[3] = { 0, 0, 0 };
|
||||
static int16_t gyroSmooth[3] = { 0, 0, 0 };
|
||||
if (Smoothing[0] == 0) {
|
||||
// initialize
|
||||
Smoothing[ROLL] = (cfg.gyro_smoothing_factor >> 16) & 0xff;
|
||||
Smoothing[PITCH] = (cfg.gyro_smoothing_factor >> 8) & 0xff;
|
||||
Smoothing[YAW] = (cfg.gyro_smoothing_factor) & 0xff;
|
||||
}
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
gyroData[axis] = (gyroSmooth[axis] * (Smoothing[axis] - 1) + gyroData[axis] + 1) / Smoothing[axis];
|
||||
gyroSmooth[axis] = gyroData[axis];
|
||||
}
|
||||
} else if (cfg.mixerConfiguration == MULTITYPE_TRI) {
|
||||
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW] + 1) / 3;
|
||||
gyroYawSmooth = gyroData[YAW];
|
||||
}
|
||||
}
|
||||
|
||||
// **************************************************
|
||||
// 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
|
||||
// 2) Small-angle approximation: http://en.wikipedia.org/wiki/Small-angle_approximation
|
||||
// 3) C. Hastings approximation for atan2()
|
||||
// 4) Optimization tricks: http://www.hackersdelight.org/
|
||||
//
|
||||
// Currently Magnetometer uses separate CF which is used only
|
||||
// for heading approximation.
|
||||
//
|
||||
// Modified: 19/04/2011 by ziss_dm
|
||||
// Version: V1.1
|
||||
//
|
||||
// code size deduction and tmp vector intermediate step for vector rotation computation: October 2011 by Alex
|
||||
// **************************************************
|
||||
|
||||
//****** advanced users settings *******************
|
||||
/* Set the Low Pass Filter factor for ACC */
|
||||
/* Increasing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time*/
|
||||
/* Comment this if you do not want filter at all.*/
|
||||
/* Default WMC value: 8*/
|
||||
#define ACC_LPF_FACTOR 4
|
||||
|
||||
/* Set the Low Pass Filter factor for Magnetometer */
|
||||
/* Increasing this value would reduce Magnetometer noise (not visible in GUI), but would increase Magnetometer lag time*/
|
||||
/* Comment this if you do not want filter at all.*/
|
||||
/* Default WMC value: n/a*/
|
||||
//#define MG_LPF_FACTOR 4
|
||||
|
||||
/* Set the Gyro Weight for Gyro/Acc complementary filter */
|
||||
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
|
||||
/* Default WMC value: 300*/
|
||||
#define GYR_CMPF_FACTOR 310.0f
|
||||
|
||||
/* Set the Gyro Weight for Gyro/Magnetometer complementary filter */
|
||||
/* Increasing this value would reduce and delay Magnetometer influence on the output of the filter*/
|
||||
/* Default WMC value: n/a*/
|
||||
#define GYR_CMPFM_FACTOR 200.0f
|
||||
|
||||
//****** end of advanced users settings *************
|
||||
|
||||
#define INV_GYR_CMPF_FACTOR (1.0f / (GYR_CMPF_FACTOR + 1.0f))
|
||||
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
|
||||
#define GYRO_SCALE ((2380 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //should be 2279.44 but 2380 gives better result
|
||||
// +-2000/sec deg scale
|
||||
//#define GYRO_SCALE ((200.0f * PI)/((32768.0f / 5.0f / 4.0f ) * 180.0f * 1000000.0f) * 1.5f)
|
||||
// +- 200/sec deg scale
|
||||
// 1.5 is emperical, not sure what it means
|
||||
// should be in rad/sec
|
||||
|
||||
typedef struct fp_vector {
|
||||
float X;
|
||||
float Y;
|
||||
float Z;
|
||||
} t_fp_vector_def;
|
||||
|
||||
typedef union {
|
||||
float A[3];
|
||||
t_fp_vector_def V;
|
||||
} t_fp_vector;
|
||||
|
||||
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data
|
||||
void rotateV(struct fp_vector *v, float *delta)
|
||||
{
|
||||
struct fp_vector v_tmp = *v;
|
||||
v->Z -= delta[ROLL] * v_tmp.X + delta[PITCH] * v_tmp.Y;
|
||||
v->X += delta[ROLL] * v_tmp.Z - delta[YAW] * v_tmp.Y;
|
||||
v->Y += delta[PITCH] * v_tmp.Z + delta[YAW] * v_tmp.X;
|
||||
}
|
||||
|
||||
static int16_t _atan2f(float y, float x)
|
||||
{
|
||||
// no need for aidsy inaccurate shortcuts on a proper platform
|
||||
return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
|
||||
}
|
||||
|
||||
static void getEstimatedAttitude(void)
|
||||
{
|
||||
uint8_t axis;
|
||||
int32_t accMag = 0;
|
||||
static t_fp_vector EstG;
|
||||
static t_fp_vector EstM;
|
||||
#if defined(MG_LPF_FACTOR)
|
||||
static int16_t mgSmooth[3];
|
||||
#endif
|
||||
#if defined(ACC_LPF_FACTOR)
|
||||
static int16_t accTemp[3]; //projection of smoothed and normalized magnetic vector on x/y/z axis, as measured by magnetometer
|
||||
#endif
|
||||
static uint32_t previousT;
|
||||
uint32_t currentT = micros();
|
||||
float scale, deltaGyroAngle[3];
|
||||
|
||||
scale = (currentT - previousT) * GYRO_SCALE;
|
||||
previousT = currentT;
|
||||
|
||||
// Initialization
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
deltaGyroAngle[axis] = gyroADC[axis] * scale;
|
||||
#if defined(ACC_LPF_FACTOR)
|
||||
accTemp[axis] = (accTemp[axis] - (accTemp[axis] >> ACC_LPF_FACTOR)) + accADC[axis];
|
||||
accSmooth[axis] = accTemp[axis] >> ACC_LPF_FACTOR;
|
||||
#define ACC_VALUE accSmooth[axis]
|
||||
#else
|
||||
accSmooth[axis] = accADC[axis];
|
||||
#define ACC_VALUE accADC[axis]
|
||||
#endif
|
||||
accMag += (int32_t) ACC_VALUE * ACC_VALUE;
|
||||
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
#if defined(MG_LPF_FACTOR)
|
||||
mgSmooth[axis] = (mgSmooth[axis] * (MG_LPF_FACTOR - 1) + magADC[axis]) / MG_LPF_FACTOR; // LPF for Magnetometer values
|
||||
#define MAG_VALUE mgSmooth[axis]
|
||||
#else
|
||||
#define MAG_VALUE magADC[axis]
|
||||
#endif
|
||||
}
|
||||
}
|
||||
accMag = accMag * 100 / ((int32_t) acc_1G * acc_1G);
|
||||
|
||||
rotateV(&EstG.V, deltaGyroAngle);
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
rotateV(&EstM.V, deltaGyroAngle);
|
||||
}
|
||||
|
||||
if (abs(accSmooth[ROLL]) < acc_25deg && abs(accSmooth[PITCH]) < acc_25deg && accSmooth[YAW] > 0)
|
||||
smallAngle25 = 1;
|
||||
else
|
||||
smallAngle25 = 0;
|
||||
|
||||
// Apply complimentary filter (Gyro drift correction)
|
||||
// If accel magnitude >1.4G or <0.6G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
|
||||
// To do that, we just skip filter, as EstV already rotated by Gyro
|
||||
if ((36 < accMag && accMag < 196) || smallAngle25)
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
int16_t acc = ACC_VALUE;
|
||||
#if !defined(TRUSTED_ACCZ)
|
||||
if (smallAngle25 && axis == YAW)
|
||||
//We consider ACCZ = acc_1G when the acc on other axis is small.
|
||||
//It's a tweak to deal with some configs where ACC_Z tends to a value < acc_1G when high throttle is applied.
|
||||
//This tweak applies only when the multi is not in inverted position
|
||||
acc = acc_1G;
|
||||
#endif
|
||||
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + acc) * INV_GYR_CMPF_FACTOR;
|
||||
}
|
||||
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
for (axis = 0; axis < 3; axis++)
|
||||
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + MAG_VALUE) * INV_GYR_CMPFM_FACTOR;
|
||||
}
|
||||
|
||||
// Attitude of the estimated vector
|
||||
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
|
||||
angle[PITCH] = _atan2f(EstG.V.Y, EstG.V.Z);
|
||||
|
||||
if (sensors(SENSOR_MAG)) {
|
||||
#define GHETTO
|
||||
|
||||
#ifdef GHETTO
|
||||
// Attitude of the cross product vector GxM
|
||||
heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z) / 10;
|
||||
#else
|
||||
static float Cos_Roll, Sin_Roll, Cos_Pitch, Sin_Pitch;
|
||||
static float Mx1, My1, Mz1, xh, yh;
|
||||
static float rollRadians;
|
||||
static float pitchRadians;
|
||||
|
||||
// proper tilt compensation
|
||||
// Get pitch and roll in radians
|
||||
rollRadians = angle[ROLL] / 1800.0 * M_PI;
|
||||
pitchRadians = angle[PITCH] /1800.0 * M_PI;
|
||||
|
||||
//rollRadians = _atan2f(accADC[ROLL], accADC[YAW])/1800.0*M_PI;
|
||||
//pitchRadians = _atan2f(accADC[PITCH], accADC[YAW])/1800.0*M_PI;
|
||||
|
||||
// Mx2 and My2 are the corrected values
|
||||
// Mx1, My1 and Mz1 are the floating point values from the mag sensor
|
||||
//Mx1 = magADC[ROLL];
|
||||
//My1 = magADC[PITCH];
|
||||
//Mz1 = magADC[YAW];
|
||||
|
||||
Mx1 = EstM.V.X;
|
||||
My1 = EstM.V.Y;
|
||||
Mz1 = EstM.V.Z;
|
||||
|
||||
// These are used more than once, so pre-calculate for efficiency
|
||||
Cos_Roll = cosf(rollRadians);
|
||||
Cos_Pitch = cosf(pitchRadians);
|
||||
Sin_Roll = sinf(rollRadians);
|
||||
Sin_Pitch = sinf(pitchRadians);
|
||||
|
||||
// The tilt-compensation equations are as follows
|
||||
//X_h=X*cos(pitch)+Y*sin(roll)sin(pitch)-Z*cos(roll)*sin(pitch)
|
||||
//Y_h=Y*cos(roll)+Z*sin(roll)
|
||||
xh = (Mx1 * Cos_Pitch) + (My1 * Sin_Roll * Sin_Pitch) - (Mz1 * Sin_Pitch * Cos_Roll); // Correct x axis
|
||||
yh = (My1 * Cos_Roll) + (Mz1 * Sin_Roll); // Correct y axis
|
||||
|
||||
// Tilt-adjusted heading in degrees
|
||||
heading = _atan2f(yh, xh) / 10;
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
|
||||
#define INIT_DELAY 4000000 // 4 sec initialization delay
|
||||
#define BARO_TAB_SIZE 40
|
||||
|
||||
void getEstimatedAltitude(void)
|
||||
{
|
||||
uint8_t index;
|
||||
static uint32_t deadLine = INIT_DELAY;
|
||||
static int16_t BaroHistTab[BARO_TAB_SIZE];
|
||||
static int8_t BaroHistIdx;
|
||||
int32_t BaroHigh = 0;
|
||||
int32_t BaroLow = 0;
|
||||
int32_t temp32;
|
||||
int16_t last;
|
||||
|
||||
if (currentTime < deadLine)
|
||||
return;
|
||||
deadLine = currentTime + UPDATE_INTERVAL;
|
||||
|
||||
//**** Alt. Set Point stabilization PID ****
|
||||
//calculate speed for D calculation
|
||||
last = BaroHistTab[BaroHistIdx];
|
||||
BaroHistTab[BaroHistIdx] = BaroAlt / 10;
|
||||
BaroHigh += BaroHistTab[BaroHistIdx];
|
||||
index = (BaroHistIdx + (BARO_TAB_SIZE / 2)) % BARO_TAB_SIZE;
|
||||
BaroHigh -= BaroHistTab[index];
|
||||
BaroLow += BaroHistTab[index];
|
||||
BaroLow -= last;
|
||||
BaroHistIdx++;
|
||||
if (BaroHistIdx >= BARO_TAB_SIZE)
|
||||
BaroHistIdx = 0;
|
||||
|
||||
BaroPID = 0;
|
||||
//D
|
||||
temp32 = cfg.D8[PIDALT] * (BaroHigh - BaroLow) / 40;
|
||||
BaroPID -= temp32;
|
||||
|
||||
EstAlt = BaroHigh * 10 / (BARO_TAB_SIZE / 2);
|
||||
|
||||
temp32 = AltHold - EstAlt;
|
||||
if (abs(temp32) < 10 && BaroPID < 10)
|
||||
BaroPID = 0; // remove small D parametr to reduce noise near zoro position
|
||||
// P
|
||||
BaroPID += cfg.P8[PIDALT] * constrain(temp32, (-2) * cfg.P8[PIDALT], 2 * cfg.P8[PIDALT]) / 100;
|
||||
BaroPID = constrain(BaroPID, -150, +150); // sum of P and D should be in range 150
|
||||
|
||||
// I
|
||||
errorAltitudeI += temp32 * cfg.I8[PIDALT] / 50;
|
||||
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
|
||||
temp32 = errorAltitudeI / 500; // I in range +/-60
|
||||
BaroPID += temp32;
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue