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

Enables Vario on targets that support barometers

Browse source 
OSD and telemetry output of vario data is enabled on F4 and up targets
that have
USE_BARO defined.  Settings and telem elements are removed from all
other targets.

F3 targets are not supported to free memory.
This commit is contained in:
Blaine 2018-07-30 17:16:05 -07:00
parent adc965327f
commit 2e552841f1
12 changed files with 64 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

33
src/main/flight/position.c
View file

@ -22,6 +22,7 @@
#include <stdint.h>
#include <stdlib.h>
#include <math.h>
#include <limits.h>
#include "build/debug.h"
@ -42,6 +43,28 @@ static int32_t estimatedAltitudeCm = 0; // in cm
#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
#ifdef USE_VARIO
static int16_t estimatedVario = 0; // in cm/s
int16_t calculateEstimatedVario(int32_t baroAlt, const uint32_t dTime) {
static float vel = 0;
static int32_t lastBaroAlt = 0;
int32_t baroVel = 0;
baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = baroAlt;
baroVel = constrain(baroVel, -1500.0f, 1500.0f);
baroVel = applyDeadband(baroVel, 10.0f);
vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel));
int32_t vel_tmp = lrintf(vel);
vel_tmp = applyDeadband(vel_tmp, 5.0f);
return constrain(vel_tmp, SHRT_MIN, SHRT_MAX);
}
#endif
#if defined(USE_BARO) || defined(USE_GPS)
static bool altitudeOffsetSet = false;
@ -100,10 +123,16 @@ if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
if (haveGpsAlt && haveBaroAlt) {
estimatedAltitudeCm = gpsAlt * gpsTrust + baroAlt * (1 - gpsTrust);
#ifdef USE_VARIO
estimatedVario = calculateEstimatedVario(baroAlt, dTime);
#endif
} else if (haveGpsAlt) {
estimatedAltitudeCm = gpsAlt;
} else if (haveBaroAlt) {
estimatedAltitudeCm = baroAlt;
#ifdef USE_VARIO
estimatedVario = calculateEstimatedVario(baroAlt, dTime);
#endif
}
DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
@ -125,5 +154,9 @@ int32_t getEstimatedAltitudeCm(void)
// This should be removed or fixed, but it would require changing a lot of other things to get rid of.
int16_t getEstimatedVario(void)
{
#ifdef USE_VARIO
return estimatedVario;
#else
return 0;
#endif
}