From a2ade445ac1d6ecb06ee757810fb9cf05231b023 Mon Sep 17 00:00:00 2001 From: bsongis Date: Fri, 27 Apr 2012 15:36:17 +0000 Subject: [PATCH] Vario. Port from unstable branch. Part 1. --- src/frsky.cpp | 44 ++++++++++++++++++++++++++++--------------- src/frsky.h | 8 ++++++-- src/model_menus.cpp | 37 +++++++++++++++++++++++++++++++++++- src/myeeprom.h | 16 +++++++++++++++- src/o9xstrings.cpp | 7 ++++++- src/o9xstrings.h | 10 ++++++++-- src/translations/en.h | 15 ++++++++++----- src/translations/fr.h | 19 ++++++++++++------- src/translations/se.h | 19 ++++++++++++------- 9 files changed, 134 insertions(+), 41 deletions(-) diff --git a/src/frsky.cpp b/src/frsky.cpp index d98f867be..12203d3ed 100644 --- a/src/frsky.cpp +++ b/src/frsky.cpp @@ -162,6 +162,21 @@ typedef enum { TS_XOR = 0x80 // decode stuffed byte } TS_STATE; +void evalVario(int16_t altitude_bp, uint8_t altitude_ap) +{ + int16_t varioAltitude_cm = altitude_bp * 100 + (altitude_bp > 0 ? altitude_ap : -altitude_ap); + + frskyHubData.queuePointer = (frskyHubData.queuePointer + 1) % VARIO_QUEUE_LENGTH; + frskyHubData.varioAltitudeQueue[frskyHubData.queuePointer] = varioAltitude_cm - frskyHubData.varioAltitude_cm; + frskyHubData.varioAltitude_cm = varioAltitude_cm; + + int16_t speed = 0; + for (uint8_t i=0; i Altitude offset if (!frskyHubData.baroAltitudeOffset) frskyHubData.baroAltitudeOffset = -frskyHubData.baroAltitude_bp; @@ -252,20 +267,8 @@ void parseTelemHubByte(uint8_t byte) if (frskyHubData.baroAltitude_bp < frskyHubData.minAltitude) frskyHubData.minAltitude = frskyHubData.baroAltitude_bp; - { - int16_t actVario = frskyHubData.baroAltitude_bp - frskyHubData.lastBaroAltitude_bp; - frskyHubData.varioAcc2 = frskyHubData.varioAcc2 - frskyHubData.varioQueue[frskyHubData.queuePointer]; - frskyHubData.varioQueue[frskyHubData.queuePointer] = actVario; - uint8_t tmp = frskyHubData.queuePointer + 5; - if (tmp >= 10) - tmp -= 10; - tmp = (uint8_t)frskyHubData.varioQueue[tmp]; - frskyHubData.varioAcc2 = frskyHubData.varioAcc2 + (int8_t)tmp; - frskyHubData.varioAcc1 = frskyHubData.varioAcc1 + actVario - (int8_t)tmp; - frskyHubData.varioSpeed = frskyHubData.varioAcc1 - frskyHubData.varioAcc2; - if (++frskyHubData.queuePointer >= 10) - frskyHubData.queuePointer = 0; - frskyHubData.lastBaroAltitude_bp = frskyHubData.baroAltitude_bp; + if (g_model.varioSource == VARIO_SOURCE_BARO) { + evalVario(frskyHubData.baroAltitude_bp, frskyHubData.baroAltitude_ap); } break; @@ -285,6 +288,9 @@ void parseTelemHubByte(uint8_t byte) else if (frskyHubData.gpsDistNeeded || g_menuStack[0] == menuProcFrsky) { getGpsDistance(); } + if (g_model.varioSource == VARIO_SOURCE_GPS) { + evalVario(frskyHubData.gpsAltitude_bp, frskyHubData.gpsAltitude_ap); + } break; case offsetof(FrskyHubData, gpsSpeed_bp): @@ -370,6 +376,14 @@ void processFrskyPacket(uint8_t *packet) frskyRSSI[0].set(packet[3]); frskyRSSI[1].set(packet[4] / 2); frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected + if (g_model.varioSource >= VARIO_SOURCE_A1) { +#warning TODO +#if 0 + frskyHubData.varioSpeed = + (frskyTelemetry[g_model.varioSource - VARIO_SOURCE_A1].value - g_model.varioAXCenter) * + ((g_model.varioAXMultiplier - 127)/10); +#endif + } break; #if defined(FRSKY_HUB) || defined (WS_HOW_HIGH) case USRPKT: // User Data packet diff --git a/src/frsky.h b/src/frsky.h index e4eb716f6..421447aff 100644 --- a/src/frsky.h +++ b/src/frsky.h @@ -68,6 +68,10 @@ class FrskyData: public FrskyRSSI { #define EARTH_RADIUSKM ((uint32_t)6371) #define EARTH_RADIUS ((uint32_t)111194) +#define VARIO_QUEUE_LENGTH 5 +#define VARIO_SPEED_LIMIT 10 //m/s +#define VARIO_SPEED_LIMIT_MUL 20 //to get 0.2m steps + #if defined(FRSKY_HUB) PACK(struct FrskyHubData { int16_t baroAltitudeOffset;// spare reused @@ -109,7 +113,7 @@ PACK(struct FrskyHubData { uint8_t cellsCount:4; // 4bits out of 16bits spare reused uint8_t minCellVolts; // 8bits out of 16bits spare reused uint16_t current; // 0x28 Current - int8_t varioQueue[10]; // circular-buffer + int16_t varioAltitudeQueue[VARIO_QUEUE_LENGTH]; //circular buffer uint8_t queuePointer; // circular-buffer pointer int8_t spare2; int16_t lastBaroAltitude_bp; @@ -131,7 +135,7 @@ PACK(struct FrskyHubData { uint16_t gpsDistance; int16_t gpsAltitudeOffset; uint8_t minCellMinVolts; - + int32_t varioAltitude_cm; }); #elif defined(WS_HOW_HIGH) diff --git a/src/model_menus.cpp b/src/model_menus.cpp index b0a3e8a79..f1c528b8b 100644 --- a/src/model_menus.cpp +++ b/src/model_menus.cpp @@ -2129,7 +2129,7 @@ void menuProcFunctionSwitches(uint8_t event) void menuProcTelemetry(uint8_t event) { #if defined(FRSKY_HUB) || defined(WS_HOW_HIGH) - MENU(STR_MENUTELEMETRY, menuTabModel, e_Telemetry, 27, {0, (uint8_t)-1, 1, 0, 2, 2, (uint8_t)-1, 1, 0, 2, 2, (uint8_t)-1, 1, 1, (uint8_t)-1, 0, 0, (uint8_t)-1, 1, 1, 1, 1, (uint8_t)-1, 2, 2, 2, 2}); + MENU(STR_MENUTELEMETRY, menuTabModel, e_Telemetry, 30, {0, (uint8_t)-1, 1, 0, 2, 2, (uint8_t)-1, 1, 0, 2, 2, (uint8_t)-1, 1, 1, (uint8_t)-1, 0, 0, (uint8_t)-1, 0, 1, (uint8_t)-1, 1, 1, 1, 1, (uint8_t)-1, 2, 2, 2, 2}); #else MENU(STR_MENUTELEMETRY, menuTabModel, e_Telemetry, 24, {0, (uint8_t)-1, 1, 0, 2, 2, (uint8_t)-1, 1, 0, 2, 2, (uint8_t)-1, 1, 1, (uint8_t)-1, 1, 1, 1, 1, (uint8_t)-1, 2, 2, 2, 2}); #endif @@ -2279,6 +2279,41 @@ void menuProcTelemetry(uint8_t event) subN++; #endif + if (s_pgOfs0 || p1valdiff)) { + switch (m_posHorz) { + case 0: + g_model.varioSpeedDownMin = checkIncDec(event, g_model.varioSpeedDownMin, 0, 255, EE_MODEL); + break; + case 1: + CHECK_INCDEC_MODELVAR(event, g_model.varioSpeedUpMin, 0, 15); + break; + } + } + } + subN++; + // Display if(s_pgOfs