Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-18 13:55:18 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'multiwii/master' into project-structure-alternative

Browse source 
Conflicts:
	src/mw.h
This commit is contained in:
Dominic Clifton 2014-04-18 19:45:30 +01:00
commit be6ce96e5d
4 changed files with 16 additions and 24 deletions
Download patch file Download diff file Expand all files Collapse all files

32
src/flight_imu.c
View file

@ -12,7 +12,7 @@ int32_t accSum[XYZ_AXIS_COUNT];
uint32_t accTimeSum = 0; // keep track for integration of acc
int accSumCount = 0;
int16_t accZ_25deg = 0;
int16_t smallAngle = 0;
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
uint32_t baroPressureSum = 0;
@ -41,7 +41,7 @@ static void getEstimatedAttitude(void);
void imuInit(void)
{
accZ_25deg = acc_1G * cosf(RAD * 25.0f);
smallAngle = lrintf(acc_1G * cosf(RAD * 25.0f));
accVelScale = 9.80665f / acc_1G / 10000.0f;
throttleAngleScale = (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle);
@ -221,17 +221,13 @@ void acc_calc(uint32_t deltaT)
accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence
accel_ned.V.Z = applyDeadband(lrintf(accz_smooth), cfg.accz_deadband);
accel_ned.V.X = applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
accel_ned.V.Y = applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);
accSum[Z] += applyDeadband(lrintf(accz_smooth), cfg.accz_deadband);
// sum up Values for later integration to get velocity and distance
accTimeSum += deltaT;
accSumCount++;
accSum[X] += lrintf(accel_ned.V.X);
accSum[Y] += lrintf(accel_ned.V.Y);
accSum[Z] += lrintf(accel_ned.V.Z);
}
void accSum_reset(void)
@ -264,10 +260,10 @@ static int16_t calculateHeading(t_fp_vector *vec)
static void getEstimatedAttitude(void)
{
uint32_t axis;
int32_t axis;
int32_t accMag = 0;
static t_fp_vector EstM;
static t_fp_vector EstN = { .A = { 1000.0f, 0.0f, 0.0f } };
static t_fp_vector EstN = { .A = { 1.0f, 0.0f, 0.0f } };
static float accLPF[3];
static uint32_t previousT;
uint32_t currentT = micros();
@ -311,10 +307,7 @@ static void getEstimatedAttitude(void)
EstM.A[axis] = (EstM.A[axis] * (float)mcfg.gyro_cmpfm_factor + magADC[axis]) * INV_GYR_CMPFM_FACTOR;
}
if (EstG.A[Z] > accZ_25deg)
f.SMALL_ANGLES_25 = 1;
else
f.SMALL_ANGLES_25 = 0;
f.SMALL_ANGLE = (EstG.A[Z] > smallAngle);
// Attitude of the estimated vector
anglerad[AI_ROLL] = atan2f(EstG.V.Y, EstG.V.Z);
@ -418,11 +411,10 @@ int getEstimatedAltitude(void)
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
vel_tmp = lrintf(vel);
// set vario
vel_tmp = lrintf(vel);
vel_tmp = applyDeadband(vel_tmp, 5);
vario = vel_tmp;
vario = applyDeadband(vel_tmp, 5);
// Altitude P-Controller
error = constrain(AltHold - EstAlt, -500, 500);
@ -431,7 +423,7 @@ int getEstimatedAltitude(void)
// Velocity PID-Controller
// P
error = setVel - lrintf(vel);
error = setVel - vel_tmp;
BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300);
// I
@ -440,8 +432,8 @@ int getEstimatedAltitude(void)
BaroPID += errorAltitudeI / 1024; // I in range +/-200
// D
accZ_old = accZ_tmp;
BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
accZ_old = accZ_tmp;
return 1;
}

2
src/main.c
View file

@ -142,7 +142,7 @@ int main(void)
calibratingA = CALIBRATING_ACC_CYCLES;
calibratingG = CALIBRATING_GYRO_CYCLES;
calibratingB = CALIBRATING_BARO_CYCLES; // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
f.SMALL_ANGLES_25 = 1;
f.SMALL_ANGLE = 1;
// loopy
while (1) {

4
src/mw.c
View file

@ -165,7 +165,7 @@ void annexCode(void)
#endif
if ((int32_t)(currentTime - calibratedAccTime) >= 0) {
if (!f.SMALL_ANGLES_25) {
if (!f.SMALL_ANGLE) {
f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated
LED0_TOGGLE;
calibratedAccTime = currentTime + 500000;
@ -742,7 +742,7 @@ void loop(void)
if (dif >= +180)
dif -= 360;
dif *= -mcfg.yaw_control_direction;
if (f.SMALL_ANGLES_25)
if (f.SMALL_ANGLE)
rcCommand[YAW] -= dif * cfg.P8[PIDMAG] / 30; // 18 deg
} else
magHold = heading;

2
src/runtime_config.h
View file

@ -41,7 +41,7 @@ typedef struct flags_t {
uint8_t PASSTHRU_MODE;
uint8_t GPS_FIX;
uint8_t GPS_FIX_HOME;
uint8_t SMALL_ANGLES_25;
uint8_t SMALL_ANGLE;
uint8_t CALIBRATE_MAG;
uint8_t VARIO_MODE;
uint8_t FIXED_WING; // set when in flying_wing or airplane mode. currently used by althold selection code