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

Merge pull request #6447 from bforbort/vario 

Enable Vario on targets that support barometers
This commit is contained in:
Michael Keller 2018-09-18 23:53:09 +12:00 committed by GitHub
parent 7e362ace48 2e552841f1
commit ea6c21f1e6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 64 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/cms/cms_menu_osd.c
View File

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

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
}

5
src/main/interface/msp.c
View File

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

2
src/main/interface/settings.c
View File

@ -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]) },

3
src/main/io/osd.c
View File

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

4
src/main/target/common_post.h
View File

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

1
src/main/target/common_pre.h
View File

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

2
src/main/telemetry/frsky_hub.c
View File

@ -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) {

4
src/main/telemetry/hott.c
View File

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

7
src/main/telemetry/ibus_shared.c
View File

@ -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)) {
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;

4
src/main/telemetry/jetiexbus.c
View File

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