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

enabled mahowik althold by default. let the users figure it out.

Browse source 
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@220 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2012-09-19 11:14:12 +00:00
parent 817eb09b8a
commit 2544c290ae
2 changed files with 2830 additions and 2845 deletions
Download patch file Download diff file Expand all files Collapse all files

5603
obj/baseflight.hex
View file

File diff suppressed because it is too large Load diff

64
src/imu.c
View file

@ -2,6 +2,7 @@
#include "mw.h" #include "mw.h"
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3]; int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
float accLPFVel[3];
int16_t acc_25deg = 0; int16_t acc_25deg = 0;
int32_t BaroAlt; int32_t BaroAlt;
int16_t sonarAlt; //to think about the unit int16_t sonarAlt; //to think about the unit
@ -159,6 +160,8 @@ typedef union {
t_fp_vector_def V; t_fp_vector_def V;
} t_fp_vector; } t_fp_vector;
t_fp_vector EstG;
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data // Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateV(struct fp_vector *v, float *delta) void rotateV(struct fp_vector *v, float *delta)
{ {
@ -174,9 +177,6 @@ static int16_t _atan2f(float y, float x)
return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f)); return (int16_t)(atan2f(y, x) * (180.0f / M_PI * 10.0f));
} }
static t_fp_vector EstG;
static float accLPFVel[3];
static void getEstimatedAttitude(void) static void getEstimatedAttitude(void)
{ {
uint32_t axis; uint32_t axis;
@ -258,13 +258,9 @@ static void getEstimatedAttitude(void)
} }
#ifdef BARO #ifdef BARO
// #define NEW_ACCZ_HOLD
#define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc) #define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
#define INIT_DELAY 4000000 // 4 sec initialization delay #define INIT_DELAY 4000000 // 4 sec initialization delay
#define BARO_TAB_SIZE 40
#define ACC_Z_DEADBAND (acc_1G / 50)
#ifdef NEW_ACCZ_HOLD
int16_t applyDeadband16(int16_t value, int16_t deadband) int16_t applyDeadband16(int16_t value, int16_t deadband)
{ {
if (abs(value) < deadband) { if (abs(value) < deadband) {
@ -323,6 +319,7 @@ void getEstimatedAltitude(void)
return; return;
dTime = currentTime - deadLine; dTime = currentTime - deadLine;
deadLine = currentTime; deadLine = currentTime;
LED0_TOGGLE;
// **** Alt. Set Point stabilization PID **** // **** Alt. Set Point stabilization PID ****
baroHistTab[baroHistIdx] = BaroAlt / 10; baroHistTab[baroHistIdx] = BaroAlt / 10;
@ -333,7 +330,6 @@ void getEstimatedAltitude(void)
if (baroHistIdx == cfg.baro_tab_size) if (baroHistIdx == cfg.baro_tab_size)
baroHistIdx = 0; baroHistIdx = 0;
// EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
EstAlt = EstAlt * cfg.baro_noise_lpf + (baroHigh * 10.0f / (cfg.baro_tab_size - 1)) * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise EstAlt = EstAlt * cfg.baro_noise_lpf + (baroHigh * 10.0f / (cfg.baro_tab_size - 1)) * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
// P // P
@ -349,8 +345,6 @@ void getEstimatedAltitude(void)
// projection of ACC vector to global Z, with 1G subtructed // projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G // Math: accZ = A * G / |G| - 1G
invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z)); invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
// int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - acc_1G;
// int16_t accZ = (accADC[ROLL] * EstG.V.X + accADC[PITCH] * EstG.V.Y + accADC[YAW] * EstG.V.Z) * invG - 1/invG;
accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G; accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
accZ = applyDeadband16(accZ, acc_1G / cfg.accz_deadband); accZ = applyDeadband16(accZ, acc_1G / cfg.accz_deadband);
debug[0] = accZ; debug[0] = accZ;
@ -374,54 +368,4 @@ void getEstimatedAltitude(void)
BaroPID -= constrain(cfg.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150); BaroPID -= constrain(cfg.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
debug[3] = BaroPID; debug[3] = BaroPID;
} }
#else
void getEstimatedAltitude(void)
{
uint32_t index;
static uint32_t deadLine = INIT_DELAY;
static int16_t BaroHistTab[BARO_TAB_SIZE];
static uint32_t BaroHistIdx;
static int32_t BaroHigh = 0;
static int32_t BaroLow = 0;
int32_t temp32;
int16_t last;
if ((int32_t)(currentTime - deadLine) < 0)
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 && abs(BaroPID) < 10)
BaroPID = 0; // remove small D parameter to reduce noise near zero 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;
}
#endif
#endif /* BARO */ #endif /* BARO */