Merge pull request #6447 from bforbort/vario

Enable Vario on targets that support barometers

src/main/cms/cms_menu_osd.c

@@ -101,7 +101,9 @@ OSD_Entry menuOsdActiveElemsEntries[] =
     {"PIT ANG",            OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_PITCH_ANGLE], 0},
     {"ROL ANG",            OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ROLL_ANGLE], 0},
     {"HEADING",            OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_NUMERICAL_HEADING], 0},
+#ifdef USE_VARIO
     {"VARIO",              OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_NUMERICAL_VARIO], 0},
+#endif
     {"G-FORCE",            OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_G_FORCE], 0},
     {"FLIP ARROW",         OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_FLIP_ARROW], 0},
     {"BACK",               OME_Back,    NULL, NULL, 0},

src/main/flight/position.c

@@ -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
 }

src/main/flight/position.h

@@ -25,4 +25,4 @@
 bool isAltitudeOffset(void);
 void calculateEstimatedAltitude(timeUs_t currentTimeUs);
 int32_t getEstimatedAltitudeCm(void);
-int16_t getEstimatedVario(void);
+int16_t getEstimatedVario(void);

src/main/interface/msp.c

@@ -23,6 +23,7 @@
 #include <string.h>
 #include <math.h>
 #include <stdlib.h>
+#include <limits.h>
 
 #include "platform.h"
 
@@ -889,7 +890,11 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
 #else
         sbufWriteU32(dst, 0);
 #endif
+#ifdef USE_VARIO
         sbufWriteU16(dst, getEstimatedVario());
+#else
+        sbufWriteU16(dst, 0);
+#endif
         break;
 
     case MSP_SONAR_ALTITUDE:

src/main/interface/settings.c

@@ -1012,7 +1012,9 @@ const clivalue_t valueTable[] = {
     { "osd_battery_usage_pos",      VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_MAIN_BATT_USAGE]) },
     { "osd_disarmed_pos",           VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_DISARMED]) },
     { "osd_nheading_pos",           VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_HEADING]) },
+#ifdef USE_VARIO
     { "osd_nvario_pos",             VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_NUMERICAL_VARIO]) },
+#endif
     { "osd_esc_tmp_pos",            VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ESC_TMP]) },
     { "osd_esc_rpm_pos",            VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ESC_RPM]) },
     { "osd_rtc_date_time_pos",      VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_RTC_DATETIME]) },

src/main/io/osd.c

@@ -968,7 +968,7 @@ static bool osdDrawSingleElement(uint8_t item)
             tfp_sprintf(buff, "%c%03d", osdGetDirectionSymbolFromHeading(heading), heading);
             break;
         }
-
+#ifdef USE_VARIO
     case OSD_NUMERICAL_VARIO:
         {
             const int verticalSpeed = osdGetMetersToSelectedUnit(getEstimatedVario());
@@ -976,6 +976,7 @@ static bool osdDrawSingleElement(uint8_t item)
             tfp_sprintf(buff, "%c%01d.%01d", directionSymbol, abs(verticalSpeed / 100), abs((verticalSpeed % 100) / 10));
             break;
         }
+#endif
 
 #ifdef USE_ESC_SENSOR
     case OSD_ESC_TMP:

src/main/target/common_post.h

@@ -56,6 +56,10 @@
 #endif
 #endif
 
+#ifndef USE_BARO
+#undef USE_VARIO
+#endif
+
 #if !defined(USE_SERIAL_RX)
 #undef USE_SERIALRX_CRSF
 #undef USE_SERIALRX_IBUS

src/main/target/common_pre.h

@@ -223,4 +223,5 @@
 #define USE_ABSOLUTE_CONTROL
 #define USE_HOTT_TEXTMODE
 #define USE_LED_STRIP
+#define USE_VARIO
 #endif

src/main/telemetry/frsky_hub.c

@@ -529,7 +529,9 @@ void processFrSkyHubTelemetry(timeUs_t currentTimeUs)
         // Sent every 125ms
         // Send vertical speed for opentx. ID_VERT_SPEED
         // Unit is cm/s
+#ifdef USE_VARIO
         frSkyHubWriteFrame(ID_VERT_SPEED, getEstimatedVario());
+#endif
 
         // Sent every 500ms
         if ((cycleNum % 4) == 0) {

src/main/telemetry/hott.c

@@ -309,6 +309,7 @@ static inline void hottEAMUpdateAltitude(HOTT_EAM_MSG_t *hottEAMMessage)
     hottEAMMessage->altitude_H = hottEamAltitude >> 8;
 }
 
+#ifdef USE_VARIO
 static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage)
 {
     const int32_t vario = getEstimatedVario();
@@ -316,6 +317,7 @@ static inline void hottEAMUpdateClimbrate(HOTT_EAM_MSG_t *hottEAMMessage)
     hottEAMMessage->climbrate_H = (30000 + vario) >> 8;
     hottEAMMessage->climbrate3s = 120 + (vario / 100);
 }
+#endif
 
 void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage)
 {
@@ -327,7 +329,9 @@ void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage)
     hottEAMUpdateCurrentMeter(hottEAMMessage);
     hottEAMUpdateBatteryDrawnCapacity(hottEAMMessage);
     hottEAMUpdateAltitude(hottEAMMessage);
+#ifdef USE_VARIO
     hottEAMUpdateClimbrate(hottEAMMessage);
+#endif
 }
 
 static void hottSerialWrite(uint8_t c)

src/main/telemetry/ibus_shared.c

@@ -31,6 +31,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
+#include <limits.h>
 // #include <string.h>
 
 #include "platform.h"
@@ -420,10 +421,10 @@ static void setValue(uint8_t* bufferPtr, uint8_t sensorType, uint8_t length)
             break;
         case IBUS_SENSOR_TYPE_VERTICAL_SPEED:
         case IBUS_SENSOR_TYPE_CLIMB_RATE:
-            if(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
+#ifdef USE_VARIO
-                value.int16 = (int16_t)getEstimatedVario();
+            value.int16 = (int16_t) constrain(getEstimatedVario(), SHRT_MIN, SHRT_MAX);
-            }
             break;
+#endif
         case IBUS_SENSOR_TYPE_ALT:
         case IBUS_SENSOR_TYPE_ALT_MAX:
             value.int32 = baro.BaroAlt;

src/main/telemetry/jetiexbus.c

@@ -196,7 +196,9 @@ void initJetiExBusTelemetry(void)
     }
     if (sensors(SENSOR_BARO)) {
         bitArraySet(&exSensorEnabled, EX_ALTITUDE);
+#ifdef USE_VARIO
         bitArraySet(&exSensorEnabled, EX_VARIO);
+#endif
     }
     if (sensors(SENSOR_ACC)) {
         bitArraySet(&exSensorEnabled, EX_ROLL_ANGLE);
@@ -259,9 +261,11 @@ int32_t getSensorValue(uint8_t sensor)
         return attitude.values.yaw;
         break;
 
+#ifdef USE_VARIO
     case EX_VARIO:
         return getEstimatedVario();
         break;
+#endif
 
     default:
         return -1;