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
#if defined(USE_ALT_HOLD)
#if defined(USE_BARO) || defined(USE_SONAR)
static void taskCalculateAltitude(timeUs_t currentTimeUs)
{
if (false
@ -198,7 +198,7 @@ static void taskCalculateAltitude(timeUs_t currentTimeUs)
) {
calculateEstimatedAltitude(currentTimeUs);
}}
#endif // USE_ALT_HOLD
#endif // USE_BARO || USE_SONAR
#ifdef USE_TELEMETRY
static void taskTelemetry(timeUs_t currentTimeUs)
@ -297,7 +297,7 @@ void fcTasksInit(void)
#ifdef USE_SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#ifdef USE_ALT_HOLD
#if defined(USE_BARO) || defined(USE_SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef USE_DASHBOARD
@ -503,7 +503,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
},
#endif
#if defined(USE_ALT_HOLD)
#if defined(USE_BARO) || defined(USE_SONAR)
[TASK_ALTITUDE] = {
.taskName = "ALTITUDE",
.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
#if defined(USE_ALT_HOLD)
enum {
DEBUG_ALTITUDE_ACC,
DEBUG_ALTITUDE_VEL,
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);
@ -72,8 +75,6 @@ static int32_t errorVelocityI = 0;
static int32_t altHoldThrottleAdjustment = 0;
static int16_t initialThrottleHold;
// 40hz update rate (20hz LPF on acc)
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
#define DEGREES_80_IN_DECIDEGREES 800
@ -202,7 +203,9 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
return result;
}
#endif // USE_ALT_HOLD
#if defined(USE_BARO) || defined(USE_SONAR)
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
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 = 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).
// 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
estimatedVario = applyDeadband(vel_tmp, 5);
#ifdef USE_ALT_HOLD
static float accZ_old = 0.0f;
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
#else
UNUSED(accZ_tmp);
#endif
}
#endif // USE_ALT_HOLD
#endif // USE_BARO || USE_SONAR
int32_t getEstimatedAltitude(void)
{