diff --git a/src/Makefile b/src/Makefile index 25bb1ee0d..95b2ebdd7 100644 --- a/src/Makefile +++ b/src/Makefile @@ -327,7 +327,7 @@ SRC = STD_TTS_SRC = translations/tts_$(shell sh -c "echo $(TTS) | tr '[:upper:]' '[:lower:]'").cpp TTS_SRC = $(shell sh -c "if test -f $(STD_TTS_SRC); then echo $(STD_TTS_SRC); else echo translations/tts_en.cpp; fi") -CPPSRC = opentx.cpp $(PULSESSRC) stamp.cpp menus.cpp menu_model.cpp menu_general.cpp view_main.cpp view_statistics.cpp $(EEPROMSRC) lcd.cpp keys.cpp translations.cpp $(TTS_SRC) +CPPSRC = opentx.cpp $(PULSESSRC) stamp.cpp menus.cpp menu_model.cpp menu_general.cpp view_main.cpp view_statistics.cpp $(EEPROMSRC) lcd.cpp keys.cpp maths.cpp translations.cpp $(TTS_SRC) ifeq ($(EXT), JETI) CPPSRC += telemetry_jeti.cpp diff --git a/src/maths.cpp b/src/maths.cpp new file mode 100644 index 000000000..b5d64e133 --- /dev/null +++ b/src/maths.cpp @@ -0,0 +1,190 @@ +/* + * Authors (alphabetical order) + * - Andre Bernet + * - Andreas Weitl + * - Bertrand Songis + * - Bryan J. Rentoul (Gruvin) + * - Cameron Weeks + * - Erez Raviv + * - Gabriel Birkus + * - Jean-Pierre Parisy + * - Karl Szmutny + * - Michael Blandford + * - Michal Hlavinka + * - Pat Mackenzie + * - Philip Moss + * - Rob Thomson + * - Romolo Manfredini + * - Thomas Husterer + * + * opentx is based on code named + * gruvin9x by Bryan J. Rentoul: http://code.google.com/p/gruvin9x/, + * er9x by Erez Raviv: http://code.google.com/p/er9x/, + * and the original (and ongoing) project by + * Thomas Husterer, th9x: http://code.google.com/p/th9x/ + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + */ + +#include "opentx.h" + +#if defined(FRSKY) +uint16_t getChannelRatio(uint8_t channel) +{ + return (uint16_t)g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier; +} + +lcdint_t applyChannelRatio(uint8_t channel, lcdint_t val) +{ + return ((int32_t)val+g_model.frsky.channels[channel].offset) * getChannelRatio(channel) * 2 / 51; +} +#endif + +#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH) +void checkMinMaxAltitude() +{ + if (frskyData.hub.baroAltitude_bp > frskyData.hub.maxAltitude) + frskyData.hub.maxAltitude = frskyData.hub.baroAltitude_bp; + if (frskyData.hub.baroAltitude_bp < frskyData.hub.minAltitude) + frskyData.hub.minAltitude = frskyData.hub.baroAltitude_bp; +} + +#if defined(VARIO) +void evalVario(int16_t altitude_bp, uint16_t altitude_ap) +{ + int32_t varioAltitude_cm = (int32_t)altitude_bp * 100 + (altitude_bp > 0 ? altitude_ap : -altitude_ap); + uint8_t varioAltitudeQueuePointer = frskyData.hub.varioAltitudeQueuePointer + 1; + if (varioAltitudeQueuePointer == VARIO_QUEUE_LENGTH) + varioAltitudeQueuePointer = 0; + frskyData.hub.varioAltitudeQueuePointer = varioAltitudeQueuePointer; + frskyData.hub.varioSpeed -= frskyData.hub.varioAltitudeQueue[varioAltitudeQueuePointer] ; + frskyData.hub.varioAltitudeQueue[varioAltitudeQueuePointer] = varioAltitude_cm - frskyData.hub.varioAltitude_cm; + frskyData.hub.varioAltitude_cm = varioAltitude_cm; + frskyData.hub.varioSpeed += frskyData.hub.varioAltitudeQueue[varioAltitudeQueuePointer] ; +} +#else +#define evalVario(...) +#endif +#endif + +#if defined(FRSKY_HUB) +void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude) +{ + div_t qr = div(frskyData.hub.gpsLatitude_bp, 100); + *latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLatitude_ap) * 5) / 3; + + qr = div(frskyData.hub.gpsLongitude_bp, 100); + *longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLongitude_ap) * 5) / 3; +} + +void getGpsPilotPosition() +{ + extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude); + uint32_t lat = frskyData.hub.pilotLatitude / 10000; + uint32_t angle2 = (lat*lat) / 10000; + uint32_t angle4 = angle2 * angle2; + frskyData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500); + // printf("frskyData.hub.distFromEarthAxis=%d\n", frskyData.hub.distFromEarthAxis); fflush(stdout); +} + +void getGpsDistance() +{ + uint32_t lat, lng; + + extractLatitudeLongitude(&lat, &lng); + + // printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - frskyData.hub.pilotLatitude), lng, abs(lng - frskyData.hub.pilotLongitude)); + + uint32_t angle = (lat > frskyData.hub.pilotLatitude) ? lat - frskyData.hub.pilotLatitude : frskyData.hub.pilotLatitude - lat; + uint32_t dist = EARTH_RADIUS * angle / 1000000; + uint32_t result = dist*dist; + + angle = (lng > frskyData.hub.pilotLongitude) ? lng - frskyData.hub.pilotLongitude : frskyData.hub.pilotLongitude - lng; + dist = frskyData.hub.distFromEarthAxis * angle / 1000000; + result += dist*dist; + + dist = abs(frskyData.hub.baroAltitudeOffset ? frskyData.hub.baroAltitude_bp : frskyData.hub.gpsAltitude_bp); + result += dist*dist; + + frskyData.hub.gpsDistance = isqrt32(result); + if (frskyData.hub.gpsDistance > frskyData.hub.maxGpsDistance) + frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance; +} +#endif + +#if defined(FRSKY) && defined(VARIO) +void varioPoll10ms() +{ + static uint16_t s_varioTmr = 0; + if (isFunctionActive(FUNC_VARIO)) { +#if defined(AUDIO) + // conversion in cm/s + int16_t verticalSpeed; + int16_t varioCenterMax = (int16_t)g_model.frsky.varioCenterMax * 10 + 50; + if (frskyData.hub.varioSpeed >= varioCenterMax) { + verticalSpeed = frskyData.hub.varioSpeed - varioCenterMax; + int16_t varioMax = (10+(int16_t)g_model.frsky.varioMax) * 100; + if (verticalSpeed > varioMax) verticalSpeed = varioMax; + verticalSpeed = (verticalSpeed * 10) / ((varioMax-varioCenterMax) / 100); + } + else { + int16_t varioCenterMin = (int16_t)g_model.frsky.varioCenterMin * 10 - 50; + if (frskyData.hub.varioSpeed <= varioCenterMin) { + verticalSpeed = frskyData.hub.varioSpeed - varioCenterMin; + int16_t varioMin = (-10+(int16_t)g_model.frsky.varioMin) * 100; + if (verticalSpeed < varioMin) verticalSpeed = varioMin; + verticalSpeed = (verticalSpeed * 10) / ((varioCenterMin-varioMin) / 100); + } + else { + return; + } + } + + uint16_t tmr10ms = g_tmr10ms; + if (verticalSpeed < 0 || tmr10ms > s_varioTmr) { + uint8_t SoundVarioBeepTime = (1600 - verticalSpeed) / 100; +#if defined(PCBSTD) + uint8_t SoundVarioBeepFreq = max(40, (verticalSpeed * 10 + 20000) >> 8); +#elif defined(CPUARM) + uint8_t SoundVarioBeepFreq = (verticalSpeed * 10 + 16000) >> 7; +#else + uint8_t SoundVarioBeepFreq = (verticalSpeed * 10 + 16000) >> 8; +#endif + s_varioTmr = tmr10ms + (SoundVarioBeepTime*2); + AUDIO_VARIO(SoundVarioBeepFreq, SoundVarioBeepTime); + } + +#else // defined(AUDIO) + + int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(frskyData.hub.varioSpeed/10), (int16_t)+100); + + uint16_t interval; + if (verticalSpeed == 0) { + interval = 300; + } + else { + if (verticalSpeed < 0) { + verticalSpeed = -verticalSpeed; + warble = 1; + } + interval = (uint8_t)200 / verticalSpeed; + } + if (g_tmr10ms - s_varioTmr > interval) { + s_varioTmr = g_tmr10ms; + if (warble) + AUDIO_VARIO_DOWN(); + else + AUDIO_VARIO_UP(); + } +#endif + } +} +#endif diff --git a/src/opentx.h b/src/opentx.h index 0d820374a..9e85084c2 100644 --- a/src/opentx.h +++ b/src/opentx.h @@ -1135,25 +1135,22 @@ inline bool isFunctionActive(uint8_t func) #include "jeti.h" #endif -#if defined (FRSKY) -// FrSky Telemetry -#include "telemetry_frsky.h" -#endif - -#ifdef ARDUPILOT -// ArduPilot Telemetry -#include "telemetry_ardupilot.h" -#endif - -#ifdef NMEA -// NMEA Telemetry -#include "telemetry_nmea.h" -#endif - -#ifdef MAVLINK -// Mavlink Telemetry -#include "rotarysw.h" -#include "telemetry_mavlink.h" +#if defined(FRSKY_SPORT) + // FrSky SPORT Telemetry + #include "telemetry_sport.h" +#elif defined (FRSKY) + // FrSky Telemetry + #include "telemetry_frsky.h" +#elif defined(ARDUPILOT) + // ArduPilot Telemetry + #include "telemetry_ardupilot.h" +#elif defined(NMEA) + // NMEA Telemetry + #include "telemetry_nmea.h" +#elif defined(MAVLINK) + // Mavlink Telemetry + #include "rotarysw.h" + #include "telemetry_mavlink.h" #endif // REPEAT uses 0x01 to 0x0f @@ -1317,5 +1314,52 @@ char * strcat_zchar(char * dest, char * name, uint8_t size, const char *defaultN #define STICK_TOLERANCE 64 #endif +#if defined(FRSKY_HUB) +enum BarThresholdIdx { + THLD_ALT, + THLD_RPM, + THLD_FUEL, + THLD_T1, + THLD_T2, + THLD_SPEED, + THLD_DIST, + THLD_GPSALT, + THLD_CELL, + THLD_CURRENT, + THLD_CONSUMPTION, + THLD_MAX, +}; +extern uint8_t barsThresholds[THLD_MAX]; +#endif + +#if defined(FRSKY) +uint8_t maxTelemValue(uint8_t channel); +int16_t convertTelemValue(uint8_t channel, uint8_t value); +int16_t convertCswTelemValue(CustomSwData * cs); +NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm); + +extern const pm_uint8_t bchunit_ar[]; + +#if defined(CPUARM) + #define FRSKY_MULTIPLIER_MAX 5 +#else + #define FRSKY_MULTIPLIER_MAX 3 +#endif + +lcdint_t applyChannelRatio(uint8_t channel, lcdint_t val); +void putsTelemetryChannel(xcoord_t x, uint8_t y, uint8_t channel, lcdint_t val, uint8_t att); + +#define IS_BARS_SCREEN(screenIndex) (g_model.frsky.screensType & (1<<(screenIndex))) +#endif + +#define EARTH_RADIUSKM ((uint32_t)6371) +#define EARTH_RADIUS ((uint32_t)111194) + +void checkMinMaxAltitude(); +void evalVario(int16_t altitude_bp, uint16_t altitude_ap); +void getGpsPilotPosition(); +void getGpsDistance(); +void varioPoll10ms(); + #endif diff --git a/src/telemetry_frsky.cpp b/src/telemetry_frsky.cpp index 66ed9c91a..fd4061568 100644 --- a/src/telemetry_frsky.cpp +++ b/src/telemetry_frsky.cpp @@ -70,90 +70,6 @@ uint8_t link_counter = 0; FrskyData frskyData; #if defined(FRSKY_HUB) -uint8_t barsThresholds[THLD_MAX]; -#endif - -uint16_t getChannelRatio(uint8_t channel) -{ - return (uint16_t)g_model.frsky.channels[channel].ratio << g_model.frsky.channels[channel].multiplier; -} - -lcdint_t applyChannelRatio(uint8_t channel, lcdint_t val) -{ - return ((int32_t)val+g_model.frsky.channels[channel].offset) * getChannelRatio(channel) * 2 / 51; -} - -#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH) -void checkMinMaxAltitude() -{ - if (frskyData.hub.baroAltitude_bp > frskyData.hub.maxAltitude) - frskyData.hub.maxAltitude = frskyData.hub.baroAltitude_bp; - if (frskyData.hub.baroAltitude_bp < frskyData.hub.minAltitude) - frskyData.hub.minAltitude = frskyData.hub.baroAltitude_bp; -} - -#if defined(VARIO) -void evalVario(int16_t altitude_bp, uint16_t altitude_ap) -{ - int32_t varioAltitude_cm = (int32_t)altitude_bp * 100 + (altitude_bp > 0 ? altitude_ap : -altitude_ap); - uint8_t varioAltitudeQueuePointer = frskyData.hub.varioAltitudeQueuePointer + 1; - if (varioAltitudeQueuePointer == VARIO_QUEUE_LENGTH) - varioAltitudeQueuePointer = 0; - frskyData.hub.varioAltitudeQueuePointer = varioAltitudeQueuePointer; - frskyData.hub.varioSpeed -= frskyData.hub.varioAltitudeQueue[varioAltitudeQueuePointer] ; - frskyData.hub.varioAltitudeQueue[varioAltitudeQueuePointer] = varioAltitude_cm - frskyData.hub.varioAltitude_cm; - frskyData.hub.varioAltitude_cm = varioAltitude_cm; - frskyData.hub.varioSpeed += frskyData.hub.varioAltitudeQueue[varioAltitudeQueuePointer] ; -} -#else -#define evalVario(...) -#endif -#endif - -#if defined(FRSKY_HUB) -void extractLatitudeLongitude(uint32_t * latitude, uint32_t * longitude) -{ - div_t qr = div(frskyData.hub.gpsLatitude_bp, 100); - *latitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLatitude_ap) * 5) / 3; - - qr = div(frskyData.hub.gpsLongitude_bp, 100); - *longitude = ((uint32_t)(qr.quot) * 1000000) + (((uint32_t)(qr.rem) * 10000 + frskyData.hub.gpsLongitude_ap) * 5) / 3; -} - -inline void getGpsPilotPosition() -{ - extractLatitudeLongitude(&frskyData.hub.pilotLatitude, &frskyData.hub.pilotLongitude); - uint32_t lat = frskyData.hub.pilotLatitude / 10000; - uint32_t angle2 = (lat*lat) / 10000; - uint32_t angle4 = angle2 * angle2; - frskyData.hub.distFromEarthAxis = 139*(((uint32_t)10000000-((angle2*(uint32_t)123370)/81)+(angle4/25))/12500); - // printf("frskyData.hub.distFromEarthAxis=%d\n", frskyData.hub.distFromEarthAxis); fflush(stdout); -} - -inline void getGpsDistance() -{ - uint32_t lat, lng; - - extractLatitudeLongitude(&lat, &lng); - - // printf("lat=%d (%d), long=%d (%d)\n", lat, abs(lat - frskyData.hub.pilotLatitude), lng, abs(lng - frskyData.hub.pilotLongitude)); - - uint32_t angle = (lat > frskyData.hub.pilotLatitude) ? lat - frskyData.hub.pilotLatitude : frskyData.hub.pilotLatitude - lat; - uint32_t dist = EARTH_RADIUS * angle / 1000000; - uint32_t result = dist*dist; - - angle = (lng > frskyData.hub.pilotLongitude) ? lng - frskyData.hub.pilotLongitude : frskyData.hub.pilotLongitude - lng; - dist = frskyData.hub.distFromEarthAxis * angle / 1000000; - result += dist*dist; - - dist = abs(frskyData.hub.baroAltitudeOffset ? frskyData.hub.baroAltitude_bp : frskyData.hub.gpsAltitude_bp); - result += dist*dist; - - frskyData.hub.gpsDistance = isqrt32(result); - if (frskyData.hub.gpsDistance > frskyData.hub.maxGpsDistance) - frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance; -} - typedef enum { TS_IDLE = 0, // waiting for 0x5e frame marker TS_DATA_ID, // waiting for dataID @@ -773,70 +689,7 @@ NOINLINE void telemetryPoll10ms() } #if defined(VARIO) - static uint16_t s_varioTmr = 0; - if (isFunctionActive(FUNC_VARIO)) { -#if defined(AUDIO) - // conversion in cm/s - do { - int16_t verticalSpeed; - int16_t varioCenterMax = (int16_t)g_model.frsky.varioCenterMax * 10 + 50; - if (frskyData.hub.varioSpeed >= varioCenterMax) { - verticalSpeed = frskyData.hub.varioSpeed - varioCenterMax; - int16_t varioMax = (10+(int16_t)g_model.frsky.varioMax) * 100; - if (verticalSpeed > varioMax) verticalSpeed = varioMax; - verticalSpeed = (verticalSpeed * 10) / ((varioMax-varioCenterMax) / 100); - } - else { - int16_t varioCenterMin = (int16_t)g_model.frsky.varioCenterMin * 10 - 50; - if (frskyData.hub.varioSpeed <= varioCenterMin) { - verticalSpeed = frskyData.hub.varioSpeed - varioCenterMin; - int16_t varioMin = (-10+(int16_t)g_model.frsky.varioMin) * 100; - if (verticalSpeed < varioMin) verticalSpeed = varioMin; - verticalSpeed = (verticalSpeed * 10) / ((varioCenterMin-varioMin) / 100); - } - else { - break; - } - } - - uint16_t tmr10ms = g_tmr10ms; - if (verticalSpeed < 0 || tmr10ms > s_varioTmr) { - uint8_t SoundVarioBeepTime = (1600 - verticalSpeed) / 100; -#if defined(PCBSTD) - uint8_t SoundVarioBeepFreq = max(40, (verticalSpeed * 10 + 20000) >> 8); -#elif defined(CPUARM) - uint8_t SoundVarioBeepFreq = (verticalSpeed * 10 + 16000) >> 7; -#else - uint8_t SoundVarioBeepFreq = (verticalSpeed * 10 + 16000) >> 8; -#endif - s_varioTmr = tmr10ms + (SoundVarioBeepTime*2); - AUDIO_VARIO(SoundVarioBeepFreq, SoundVarioBeepTime); - } - - } while(0); -#else - int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(frskyData.hub.varioSpeed/10), (int16_t)+100); - - uint16_t interval; - if (verticalSpeed == 0) { - interval = 300; - } - else { - if (verticalSpeed < 0) { - verticalSpeed = -verticalSpeed; - warble = 1; - } - interval = (uint8_t)200 / verticalSpeed; - } - if (g_tmr10ms - s_varioTmr > interval) { - s_varioTmr = g_tmr10ms; - if (warble) - AUDIO_VARIO_DOWN(); - else - AUDIO_VARIO_UP(); - } -#endif - } + varioPoll10ms(); #endif } diff --git a/src/telemetry_frsky.h b/src/telemetry_frsky.h index 77ec20564..81e86a62b 100644 --- a/src/telemetry_frsky.h +++ b/src/telemetry_frsky.h @@ -34,8 +34,8 @@ * */ -#ifndef FRSKY_H -#define FRSKY_H +#ifndef TELEMETRY_FRSKY_H +#define TELEMETRY_FRSKY_H #define FRSKY_RX_PACKET_SIZE 19 #define FRSKY_TX_PACKET_SIZE 12 @@ -69,9 +69,6 @@ class FrskyValueWithMinMax: public FrskyValueWithMin { void set(uint8_t value, uint8_t unit); }; -#define EARTH_RADIUSKM ((uint32_t)6371) -#define EARTH_RADIUS ((uint32_t)111194) - #define VARIO_QUEUE_LENGTH 5 #if defined(FRSKY_HUB) @@ -181,24 +178,6 @@ struct FrskyData { uint16_t power; }; -#if defined(FRSKY_HUB) -enum BarThresholdIdx { - THLD_ALT, - THLD_RPM, - THLD_FUEL, - THLD_T1, - THLD_T2, - THLD_SPEED, - THLD_DIST, - THLD_GPSALT, - THLD_CELL, - THLD_CURRENT, - THLD_CONSUMPTION, - THLD_MAX, -}; -extern uint8_t barsThresholds[THLD_MAX]; -#endif - // Global Fr-Sky telemetry data variables extern int8_t frskyStreaming; // >0 (true) == data is streaming in. 0 = nodata detected for some time #if defined(WS_HOW_HIGH) @@ -225,23 +204,6 @@ inline void frskySendAlarms(void) bool FRSKY_alarmRaised(uint8_t idx); void resetTelemetry(); -uint8_t maxTelemValue(uint8_t channel); -int16_t convertTelemValue(uint8_t channel, uint8_t value); -int16_t convertCswTelemValue(CustomSwData * cs); -NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm); - -extern const pm_uint8_t bchunit_ar[]; - -#if defined(CPUARM) -#define FRSKY_MULTIPLIER_MAX 5 -#else -#define FRSKY_MULTIPLIER_MAX 3 -#endif - -lcdint_t applyChannelRatio(uint8_t channel, lcdint_t val); -void putsTelemetryChannel(xcoord_t x, uint8_t y, uint8_t channel, lcdint_t val, uint8_t att); - -#define IS_BARS_SCREEN(screenIndex) (g_model.frsky.screensType & (1<<(screenIndex))) #endif diff --git a/src/view_telemetry.cpp b/src/view_telemetry.cpp index bf668281d..5d37af76c 100644 --- a/src/view_telemetry.cpp +++ b/src/view_telemetry.cpp @@ -37,6 +37,10 @@ #include "opentx.h" #include "menus.h" +#if defined(FRSKY_HUB) +uint8_t barsThresholds[THLD_MAX]; +#endif + uint8_t maxTelemValue(uint8_t channel) { switch (channel) { @@ -384,10 +388,10 @@ void menuTelemetryFrsky(uint8_t event) threshold = getRssiAlarmValue(source-TELEM_RSSI_TX); else if (source <= TELEM_A2) threshold = g_model.frsky.channels[source-TELEM_A1].alarms_value[0]; - #if defined(FRSKY_HUB) +#if defined(FRSKY_HUB) else threshold = convertTelemValue(source, barsThresholds[source-TELEM_ALT]); - #endif +#endif if (threshold) { thresholdX = barCoord(threshold, barMin, barMax);