diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index bfdd5fc56e..0d92caebe9 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/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}, diff --git a/src/main/flight/position.c b/src/main/flight/position.c index 8b0c716ec5..99b2818d32 100644 --- a/src/main/flight/position.c +++ b/src/main/flight/position.c @@ -22,6 +22,7 @@ #include #include #include +#include #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 } diff --git a/src/main/flight/position.h b/src/main/flight/position.h index 70a7bfc1c7..bf821ca6a7 100644 --- a/src/main/flight/position.h +++ b/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); \ No newline at end of file +int16_t getEstimatedVario(void); diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index da2cb86932..144ff2bf17 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -23,6 +23,7 @@ #include #include #include +#include #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: diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 0f67195624..433b6c01ac 100644 --- a/src/main/interface/settings.c +++ b/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]) }, diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1b18fbb0d3..a20e60c779 100644 --- a/src/main/io/osd.c +++ b/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: diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 50299d6039..c881c3f9c2 100644 --- a/src/main/target/common_post.h +++ b/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 diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index b9e910fd8d..10965941d2 100644 --- a/src/main/target/common_pre.h +++ b/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 diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 1a13b0ad2f..360ada5b2a 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/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) { diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index b6bd2092ca..c13e880e9c 100644 --- a/src/main/telemetry/hott.c +++ b/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) diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c index 68d806b6ef..01ba971df0 100644 --- a/src/main/telemetry/ibus_shared.c +++ b/src/main/telemetry/ibus_shared.c @@ -31,6 +31,7 @@ #include #include #include +#include // #include #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)) { - value.int16 = (int16_t)getEstimatedVario(); - } +#ifdef USE_VARIO + 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; diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c index 2c28c4a3c9..b779ca4738 100644 --- a/src/main/telemetry/jetiexbus.c +++ b/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;