rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Retain altitude calculation

This commit is contained in:
Martin Budden 2017-11-19 13:13:14 +00:00
parent d22b01b5ec
commit 417acc8f01
2 changed files with 16 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/main/fc/fc_tasks.c
View File

@ -185,7 +185,7 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
} }
#endif #endif
#if defined(USE_ALT_HOLD) #if defined(USE_BARO) || defined(USE_SONAR)
static void taskCalculateAltitude(timeUs_t currentTimeUs) static void taskCalculateAltitude(timeUs_t currentTimeUs)
{ {
if (false if (false
@ -198,7 +198,7 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs)
) { ) {
calculateEstimatedAltitude(currentTimeUs); calculateEstimatedAltitude(currentTimeUs);
}} }}
#endif // USE_ALT_HOLD #endif // USE_BARO || USE_SONAR
#ifdef USE_TELEMETRY #ifdef USE_TELEMETRY
static void taskTelemetry(timeUs_t currentTimeUs) static void taskTelemetry(timeUs_t currentTimeUs)
@ -297,7 +297,7 @@ void fcTasksInit(void)
#ifdef USE_SONAR #ifdef USE_SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR)); setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif #endif
#ifdef USE_ALT_HOLD #if defined(USE_BARO) || defined(USE_SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)); setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif #endif
#ifdef USE_DASHBOARD #ifdef USE_DASHBOARD
@ -503,7 +503,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
}, },
#endif #endif
#if defined(USE_ALT_HOLD) #if defined(USE_BARO) || defined(USE_SONAR)
[TASK_ALTITUDE] = { [TASK_ALTITUDE] = {
.taskName = "ALTITUDE", .taskName = "ALTITUDE",
.taskFunc = taskCalculateAltitude, .taskFunc = taskCalculateAltitude,

17
src/main/flight/altitude.c
View File

@ -52,13 +52,16 @@ static int32_t estimatedVario = 0; // variometer in cm/s
static int32_t estimatedAltitude = 0; // in cm static int32_t estimatedAltitude = 0; // in cm
#if defined(USE_ALT_HOLD)
enum { enum {
DEBUG_ALTITUDE_ACC, DEBUG_ALTITUDE_ACC,
DEBUG_ALTITUDE_VEL, DEBUG_ALTITUDE_VEL,
DEBUG_ALTITUDE_HEIGHT DEBUG_ALTITUDE_HEIGHT
}; };
// 40hz update rate (20hz LPF on acc)
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
#if defined(USE_ALT_HOLD)
PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0);
@ -72,8 +75,6 @@ static int32_t errorVelocityI = 0;
static int32_t altHoldThrottleAdjustment = 0; static int32_t altHoldThrottleAdjustment = 0;
static int16_t initialThrottleHold; static int16_t initialThrottleHold;
// 40hz update rate (20hz LPF on acc)
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
#define DEGREES_80_IN_DECIDEGREES 800 #define DEGREES_80_IN_DECIDEGREES 800
@ -202,7 +203,9 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
return result; return result;
} }
#endif // USE_ALT_HOLD
#if defined(USE_BARO) || defined(USE_SONAR)
void calculateEstimatedAltitude(timeUs_t currentTimeUs) void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{ {
static timeUs_t previousTimeUs = 0; static timeUs_t previousTimeUs = 0;
@ -280,7 +283,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
} }
#endif // BARO #endif // USE_BARO
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). // 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 // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
@ -290,11 +293,15 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
// set vario // set vario
estimatedVario = applyDeadband(vel_tmp, 5); estimatedVario = applyDeadband(vel_tmp, 5);
#ifdef USE_ALT_HOLD
static float accZ_old = 0.0f; static float accZ_old = 0.0f;
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old); altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp; accZ_old = accZ_tmp;
#else
UNUSED(accZ_tmp);
#endif
} }
#endif // USE_ALT_HOLD #endif // USE_BARO || USE_SONAR
int32_t getEstimatedAltitude(void) int32_t getEstimatedAltitude(void)
{ {