Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

begin initial merge of 2.2 features

Browse source 
mw2.2-merged stuff:
* implemented profiles 0-2 (called 'setting' in mwiigui)
* merged in MSP changes including profile switch
* cleaned up rc / aux switch stuff in mw.c based on 2.2 rewrite
* main loop switch for baro/sonar shit adjusted
todo: basically the rest of 2.2 changes (i think some minor imu/gps/baro updates)
baseflight-specific stuff:
* made boxitems transmission dynamic, based on enabled features. no more GPS / camstab trash if it's not enabled
* cleaned up gyro drivers to return scale factor to imu code
* set gyro_lpf now controls every supported gyro - not all take same values, see driver files for allowed ranges, in case of invalid lpf, defaults to something reasonable (around 30-40hz)

maybe couple other things I forgot. this is all 100% experimental, untested, and not even flown. thats why there's  no hex.
merge is still ongoing.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@294 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-03-14 14:03:30 +00:00
parent bba9bc291f
commit 491b3627f6
20 changed files with 1009 additions and 559 deletions
Download patch file Download diff file Expand all files Collapse all files

44
src/imu.c
View file

@ -4,12 +4,13 @@
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
float accLPFVel[3];
int16_t acc_25deg = 0;
int32_t BaroAlt;
int16_t sonarAlt; //to think about the unit
int32_t EstAlt; // in cm
int16_t BaroPID = 0;
int32_t AltHold;
int16_t errorAltitudeI = 0;
int32_t BaroAlt;
int16_t sonarAlt; // to think about the unit
int32_t EstAlt; // in cm
int16_t BaroPID = 0;
int32_t AltHold;
int16_t errorAltitudeI = 0;
int16_t vario = 0; // variometer in cm/s
float magneticDeclination = 0.0f; // calculated at startup from config
float accVelScale;
@ -87,15 +88,15 @@ void computeIMU(void)
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;
Smoothing[ROLL] = (mcfg.gyro_smoothing_factor >> 16) & 0xff;
Smoothing[PITCH] = (mcfg.gyro_smoothing_factor >> 8) & 0xff;
Smoothing[YAW] = (mcfg.gyro_smoothing_factor) & 0xff;
}
for (axis = 0; axis < 3; axis++) {
gyroData[axis] = (int16_t)(((int32_t)((int32_t)gyroSmooth[axis] * (Smoothing[axis] - 1)) + gyroData[axis] + 1 ) / Smoothing[axis]);
gyroSmooth[axis] = gyroData[axis];
}
} else if (cfg.mixerConfiguration == MULTITYPE_TRI) {
} else if (mcfg.mixerConfiguration == MULTITYPE_TRI) {
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW]) / 3;
gyroYawSmooth = gyroData[YAW];
}
@ -137,17 +138,10 @@ void computeIMU(void)
//****** end of advanced users settings *************
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)cfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
#define INV_GYR_CMPFM_FACTOR (1.0f / (GYR_CMPFM_FACTOR + 1.0f))
#define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000
// #define GYRO_SCALE ((2380 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) //should be 2279.44 but 2380 gives better result (ITG-3200)
// +-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
// #define GYRO_SCALE ((1998 * M_PI)/((32767.0f / 4.0f ) * 180.0f * 1000000.0f)) // 32767 / 16.4lsb/dps for MPU3000
typedef struct fp_vector {
float X;
@ -225,7 +219,7 @@ static void getEstimatedAttitude(void)
uint32_t currentT = micros();
float scale, deltaGyroAngle[3];
scale = (currentT - previousT) * GYRO_SCALE;
scale = (currentT - previousT) * gyro.scale;
previousT = currentT;
// Initialization
@ -265,7 +259,7 @@ static void getEstimatedAttitude(void)
// To do that, we just skip filter, as EstV already rotated by Gyro
if ((36 < accMag && accMag < 196) || f.SMALL_ANGLES_25) {
for (axis = 0; axis < 3; axis++)
EstG.A[axis] = (EstG.A[axis] * (float)cfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
EstG.A[axis] = (EstG.A[axis] * (float)mcfg.gyro_cmpf_factor + accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
}
if (sensors(SENSOR_MAG)) {
@ -282,7 +276,7 @@ static void getEstimatedAttitude(void)
angle[ROLL] = _atan2f(EstG.V.X, EstG.V.Z);
angle[PITCH] = -asinf(EstG.V.Y / -sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z)) * (180.0f / M_PI * 10.0f);
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
#if INACCURATE
@ -356,7 +350,7 @@ int32_t isq(int32_t x)
return x * x;
}
void getEstimatedAltitude(void)
int getEstimatedAltitude(void)
{
static uint32_t deadLine = INIT_DELAY;
static int16_t baroHistTab[BARO_TAB_SIZE_MAX];
@ -371,7 +365,7 @@ void getEstimatedAltitude(void)
float baroVel;
if ((int32_t)(currentTime - deadLine) < UPDATE_INTERVAL)
return;
return 0;
dTime = currentTime - deadLine;
deadLine = currentTime;
@ -421,5 +415,7 @@ void getEstimatedAltitude(void)
// D
BaroPID -= constrain(cfg.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
debug[3] = BaroPID;
return 1;
}
#endif /* BARO */