From 4a099a7161c7d4a193894fa1615b26cb8a0591d7 Mon Sep 17 00:00:00 2001 From: bsongis Date: Tue, 3 Jun 2014 09:39:40 +0200 Subject: [PATCH] Vario code moved to a separate file vario.cpp --- companion/src/CMakeLists.txt | 2 +- .../opentx/opentxGruvin9xsimulator.cpp | 1 + .../firmwares/opentx/opentxM128simulator.cpp | 41 ++--- .../firmwares/opentx/opentxM128simulator.h | 8 +- ...txsimulator.cpp => opentxM64simulator.cpp} | 41 ++--- ...opentxsimulator.h => opentxM64simulator.h} | 4 +- .../firmwares/opentx/opentxSky9xsimulator.cpp | 1 + .../opentx/opentxTaranisSimulator.cpp | 1 + .../src/firmwares/opentx/opentxinterface.cpp | 6 +- radio/src/Makefile | 1 + radio/src/maths.cpp | 145 --------------- radio/src/telemetry/frsky.cpp | 10 ++ radio/src/vario.cpp | 170 ++++++++++++++++++ 13 files changed, 236 insertions(+), 195 deletions(-) rename companion/src/firmwares/opentx/{opentxsimulator.cpp => opentxM64simulator.cpp} (80%) rename companion/src/firmwares/opentx/{opentxsimulator.h => opentxM64simulator.h} (94%) create mode 100755 radio/src/vario.cpp diff --git a/companion/src/CMakeLists.txt b/companion/src/CMakeLists.txt index a0a92ac41..47e88ceea 100644 --- a/companion/src/CMakeLists.txt +++ b/companion/src/CMakeLists.txt @@ -130,7 +130,7 @@ set(common_SRCS firmwares/opentx/opentxSky9xsimulator.cpp firmwares/opentx/opentxGruvin9xsimulator.cpp firmwares/opentx/opentxM128simulator.cpp - firmwares/opentx/opentxsimulator.cpp + firmwares/opentx/opentxM64simulator.cpp file.cpp googleanalytics.cpp appdata.cpp diff --git a/companion/src/firmwares/opentx/opentxGruvin9xsimulator.cpp b/companion/src/firmwares/opentx/opentxGruvin9xsimulator.cpp index e7e037d1f..3cfce7530 100644 --- a/companion/src/firmwares/opentx/opentxGruvin9xsimulator.cpp +++ b/companion/src/firmwares/opentx/opentxGruvin9xsimulator.cpp @@ -69,6 +69,7 @@ namespace Open9xGruvin9x { #include "radio/src/pulses/pulses_avr.cpp" #include "radio/src/stamp.cpp" #include "radio/src/maths.cpp" +#include "radio/src/vario.cpp" #include "radio/src/gui/menus.cpp" #include "radio/src/gui/menu_model.cpp" #include "radio/src/gui/menu_general.cpp" diff --git a/companion/src/firmwares/opentx/opentxM128simulator.cpp b/companion/src/firmwares/opentx/opentxM128simulator.cpp index c577f5b6f..cc262d12b 100644 --- a/companion/src/firmwares/opentx/opentxM128simulator.cpp +++ b/companion/src/firmwares/opentx/opentxM128simulator.cpp @@ -55,7 +55,7 @@ #include -namespace Open9xM128 { +namespace OpenTxM128 { #include "radio/src/targets/stock/board_stock.cpp" #include "radio/src/targets/common_avr/telemetry_driver.cpp" #include "radio/src/eeprom_common.cpp" @@ -68,6 +68,7 @@ namespace Open9xM128 { #include "radio/src/pulses/pulses_avr.cpp" #include "radio/src/stamp.cpp" #include "radio/src/maths.cpp" +#include "radio/src/vario.cpp" #include "radio/src/gui/menus.cpp" #include "radio/src/gui/menu_model.cpp" #include "radio/src/gui/menu_general.cpp" @@ -109,70 +110,70 @@ uint8_t getStickMode() } -using namespace Open9xM128; +using namespace OpenTxM128; -Open9xM128Simulator::Open9xM128Simulator() +OpenTxM128Simulator::OpenTxM128Simulator() { } -bool Open9xM128Simulator::timer10ms() +bool OpenTxM128Simulator::timer10ms() { #define TIMER10MS_IMPORT #include "simulatorimport.h" } -uint8_t * Open9xM128Simulator::getLcd() +uint8_t * OpenTxM128Simulator::getLcd() { #define GETLCD_IMPORT #include "simulatorimport.h" } -bool Open9xM128Simulator::lcdChanged(bool & lightEnable) +bool OpenTxM128Simulator::lcdChanged(bool & lightEnable) { #define LCDCHANGED_IMPORT #include "simulatorimport.h" } -void Open9xM128Simulator::start(QByteArray & eeprom, bool tests) +void OpenTxM128Simulator::start(QByteArray & eeprom, bool tests) { - memcpy(Open9xM128::eeprom, eeprom.data(), std::min(sizeof(Open9xM128::eeprom), eeprom.size())); + memcpy(OpenTxM128::eeprom, eeprom.data(), std::min(sizeof(OpenTxM128::eeprom), eeprom.size())); StartEepromThread(NULL); StartMainThread(tests); } -void Open9xM128Simulator::start(const char * filename, bool tests) +void OpenTxM128Simulator::start(const char * filename, bool tests) { StartEepromThread(filename); StartMainThread(tests); } -void Open9xM128Simulator::stop() +void OpenTxM128Simulator::stop() { StopMainThread(); StopEepromThread(); } -void Open9xM128Simulator::getValues(TxOutputs &outputs) +void OpenTxM128Simulator::getValues(TxOutputs &outputs) { #define GETVALUES_IMPORT #define g_chans512 channelOutputs #include "simulatorimport.h" } -void Open9xM128Simulator::setValues(TxInputs &inputs) +void OpenTxM128Simulator::setValues(TxInputs &inputs) { #define SETVALUES_IMPORT #include "simulatorimport.h" } -void Open9xM128Simulator::setTrim(unsigned int idx, int value) +void OpenTxM128Simulator::setTrim(unsigned int idx, int value) { - idx = Open9xM128::modn12x3[4*getStickMode() + idx]; + idx = OpenTxM128::modn12x3[4*getStickMode() + idx]; uint8_t phase = getTrimFlightPhase(getFlightPhase(), idx); setTrimValue(phase, idx, value); } -void Open9xM128Simulator::getTrims(Trims & trims) +void OpenTxM128Simulator::getTrims(Trims & trims) { uint8_t phase = getFlightPhase(); trims.extended = hasExtendedTrims(); @@ -181,30 +182,30 @@ void Open9xM128Simulator::getTrims(Trims & trims) } for (int i=0; i<2; i++) { - uint8_t idx = Open9xM128::modn12x3[4*getStickMode() + i]; + uint8_t idx = OpenTxM128::modn12x3[4*getStickMode() + i]; int16_t tmp = trims.values[i]; trims.values[i] = trims.values[idx]; trims.values[idx] = tmp; } } -void Open9xM128Simulator::wheelEvent(uint8_t steps) +void OpenTxM128Simulator::wheelEvent(uint8_t steps) { } -unsigned int Open9xM128Simulator::getPhase() +unsigned int OpenTxM128Simulator::getPhase() { return getFlightPhase(); } -const char * Open9xM128Simulator::getPhaseName(unsigned int phase) +const char * OpenTxM128Simulator::getPhaseName(unsigned int phase) { static char buff[sizeof(g_model.phaseData[0].name)+1]; zchar2str(buff, g_model.phaseData[phase].name, sizeof(g_model.phaseData[0].name)); return buff; } -const char * Open9xM128Simulator::getError() +const char * OpenTxM128Simulator::getError() { #define GETERROR_IMPORT #include "simulatorimport.h" diff --git a/companion/src/firmwares/opentx/opentxM128simulator.h b/companion/src/firmwares/opentx/opentxM128simulator.h index 25941eb6b..dacdf9187 100644 --- a/companion/src/firmwares/opentx/opentxM128simulator.h +++ b/companion/src/firmwares/opentx/opentxM128simulator.h @@ -14,19 +14,19 @@ * */ -#ifndef open9xM128_simulator_h -#define open9xM128_simulator_h +#ifndef OpenTxM128_simulator_h +#define OpenTxM128_simulator_h #include "simulatorinterface.h" class RadioData; class OpenTxEepromInterface; -class Open9xM128Simulator : public SimulatorInterface { +class OpenTxM128Simulator : public SimulatorInterface { public: - Open9xM128Simulator(); + OpenTxM128Simulator(); virtual void start(QByteArray & eeprom, bool tests=true); diff --git a/companion/src/firmwares/opentx/opentxsimulator.cpp b/companion/src/firmwares/opentx/opentxM64simulator.cpp similarity index 80% rename from companion/src/firmwares/opentx/opentxsimulator.cpp rename to companion/src/firmwares/opentx/opentxM64simulator.cpp index 4dfe60bd1..edf9193fc 100644 --- a/companion/src/firmwares/opentx/opentxsimulator.cpp +++ b/companion/src/firmwares/opentx/opentxM64simulator.cpp @@ -14,7 +14,7 @@ * */ -#include "opentxsimulator.h" +#include "opentxM64simulator.h" #include "opentxinterface.h" #include "opentxeeprom.h" @@ -61,7 +61,7 @@ #include -namespace Open9x { +namespace OpenTxM64 { #include "radio/src/targets/stock/board_stock.cpp" #include "radio/src/targets/common_avr/telemetry_driver.cpp" #include "radio/src/eeprom_common.cpp" @@ -74,6 +74,7 @@ namespace Open9x { #include "radio/src/pulses/pulses_avr.cpp" #include "radio/src/stamp.cpp" #include "radio/src/maths.cpp" +#include "radio/src/vario.cpp" #include "radio/src/gui/menus.cpp" #include "radio/src/gui/menu_model.cpp" #include "radio/src/gui/menu_general.cpp" @@ -115,52 +116,52 @@ uint8_t getStickMode() } -using namespace Open9x; +using namespace OpenTxM64; -Open9xSimulator::Open9xSimulator() +OpenTxM64Simulator::OpenTxM64Simulator() { #define INIT_IMPORT #include "simulatorimport.h" } -bool Open9xSimulator::timer10ms() +bool OpenTxM64Simulator::timer10ms() { #define TIMER10MS_IMPORT #include "simulatorimport.h" } -uint8_t * Open9xSimulator::getLcd() +uint8_t * OpenTxM64Simulator::getLcd() { #define GETLCD_IMPORT #include "simulatorimport.h" } -bool Open9xSimulator::lcdChanged(bool & lightEnable) +bool OpenTxM64Simulator::lcdChanged(bool & lightEnable) { #define LCDCHANGED_IMPORT #include "simulatorimport.h" } -void Open9xSimulator::start(QByteArray & eeprom, bool tests) +void OpenTxM64Simulator::start(QByteArray & eeprom, bool tests) { - memcpy(&Open9x::eeprom[0], eeprom.data(), 2048); + memcpy(&OpenTxM64::eeprom[0], eeprom.data(), 2048); StartEepromThread(NULL); StartMainThread(tests); } -void Open9xSimulator::start(const char * filename, bool tests) +void OpenTxM64Simulator::start(const char * filename, bool tests) { StartEepromThread(filename); StartMainThread(tests); } -void Open9xSimulator::stop() +void OpenTxM64Simulator::stop() { StopMainThread(); StopEepromThread(); } -void Open9xSimulator::getValues(TxOutputs &outputs) +void OpenTxM64Simulator::getValues(TxOutputs &outputs) { #define GETVALUES_IMPORT #define g_chans512 channelOutputs @@ -169,20 +170,20 @@ void Open9xSimulator::getValues(TxOutputs &outputs) g_beepCnt = 0; } -void Open9xSimulator::setValues(TxInputs &inputs) +void OpenTxM64Simulator::setValues(TxInputs &inputs) { #define SETVALUES_IMPORT #include "simulatorimport.h" } -void Open9xSimulator::setTrim(unsigned int idx, int value) +void OpenTxM64Simulator::setTrim(unsigned int idx, int value) { - idx = Open9x::modn12x3[4*getStickMode() + idx]; + idx = OpenTxM64::modn12x3[4*getStickMode() + idx]; uint8_t phase = getTrimFlightPhase(getFlightPhase(), idx); setTrimValue(phase, idx, value); } -void Open9xSimulator::getTrims(Trims & trims) +void OpenTxM64Simulator::getTrims(Trims & trims) { uint8_t phase = getFlightPhase(); trims.extended = hasExtendedTrims(); @@ -191,26 +192,26 @@ void Open9xSimulator::getTrims(Trims & trims) } for (int i=0; i<2; i++) { - uint8_t idx = Open9x::modn12x3[4*getStickMode() + i]; + uint8_t idx = OpenTxM64::modn12x3[4*getStickMode() + i]; int16_t tmp = trims.values[i]; trims.values[i] = trims.values[idx]; trims.values[idx] = tmp; } } -unsigned int Open9xSimulator::getPhase() +unsigned int OpenTxM64Simulator::getPhase() { return getFlightPhase(); } -const char * Open9xSimulator::getPhaseName(unsigned int phase) +const char * OpenTxM64Simulator::getPhaseName(unsigned int phase) { static char buff[sizeof(g_model.phaseData[0].name)+1]; zchar2str(buff, g_model.phaseData[phase].name, sizeof(g_model.phaseData[0].name)); return buff; } -const char * Open9xSimulator::getError() +const char * OpenTxM64Simulator::getError() { #define GETERROR_IMPORT #include "simulatorimport.h" diff --git a/companion/src/firmwares/opentx/opentxsimulator.h b/companion/src/firmwares/opentx/opentxM64simulator.h similarity index 94% rename from companion/src/firmwares/opentx/opentxsimulator.h rename to companion/src/firmwares/opentx/opentxM64simulator.h index 97342872f..26827403b 100644 --- a/companion/src/firmwares/opentx/opentxsimulator.h +++ b/companion/src/firmwares/opentx/opentxM64simulator.h @@ -21,11 +21,11 @@ class OpenTxEepromInterface; -class Open9xSimulator : public SimulatorInterface { +class OpenTxM64Simulator : public SimulatorInterface { public: - Open9xSimulator(); + OpenTxM64Simulator(); virtual void start(QByteArray & eeprom, bool tests=true); diff --git a/companion/src/firmwares/opentx/opentxSky9xsimulator.cpp b/companion/src/firmwares/opentx/opentxSky9xsimulator.cpp index ca32e6289..1cd866971 100644 --- a/companion/src/firmwares/opentx/opentxSky9xsimulator.cpp +++ b/companion/src/firmwares/opentx/opentxSky9xsimulator.cpp @@ -83,6 +83,7 @@ namespace Open9xSky9x { #include "radio/src/pulses/pulses_arm.cpp" #include "radio/src/stamp.cpp" #include "radio/src/maths.cpp" +#include "radio/src/vario.cpp" #include "radio/src/gui/menus.cpp" #include "radio/src/gui/menu_model.cpp" #include "radio/src/gui/menu_general.cpp" diff --git a/companion/src/firmwares/opentx/opentxTaranisSimulator.cpp b/companion/src/firmwares/opentx/opentxTaranisSimulator.cpp index 9cf031c30..5b2e4ff4c 100644 --- a/companion/src/firmwares/opentx/opentxTaranisSimulator.cpp +++ b/companion/src/firmwares/opentx/opentxTaranisSimulator.cpp @@ -89,6 +89,7 @@ inline int geteepromsize() { #include "radio/src/pulses/pulses_arm.cpp" #include "radio/src/stamp.cpp" #include "radio/src/maths.cpp" +#include "radio/src/vario.cpp" #include "radio/src/gui/menus.cpp" #include "radio/src/gui/menu_model.cpp" #include "radio/src/gui/menu_general.cpp" diff --git a/companion/src/firmwares/opentx/opentxinterface.cpp b/companion/src/firmwares/opentx/opentxinterface.cpp index 000e3bc49..a43219db6 100644 --- a/companion/src/firmwares/opentx/opentxinterface.cpp +++ b/companion/src/firmwares/opentx/opentxinterface.cpp @@ -20,7 +20,7 @@ #include "opentxeeprom.h" #include "open9xGruvin9xeeprom.h" #include "open9xSky9xeeprom.h" -#include "opentxsimulator.h" +#include "opentxM64simulator.h" #include "opentxM128simulator.h" #include "opentxGruvin9xsimulator.h" #include "opentxSky9xsimulator.h" @@ -944,9 +944,9 @@ SimulatorInterface * OpenTxFirmware::getSimulator() { switch (board) { case BOARD_STOCK: - return new Open9xSimulator(); + return new OpenTxM64Simulator(); case BOARD_M128: - return new Open9xM128Simulator(); + return new OpenTxM128Simulator(); case BOARD_GRUVIN9X: return new Open9xGruvin9xSimulator(); case BOARD_SKY9X: diff --git a/radio/src/Makefile b/radio/src/Makefile index f96edf566..50f72cfac 100644 --- a/radio/src/Makefile +++ b/radio/src/Makefile @@ -953,6 +953,7 @@ ifeq ($(EXT), $(filter $(EXT), FRSKY FRSKY_SPORT TELEMETREZ)) endif ifeq ($(VARIO), YES) + CPPSRC += vario.cpp CPPDEFS += -DVARIO endif EEPROM_VARIANT += ${FRSKY_VARIANT} diff --git a/radio/src/maths.cpp b/radio/src/maths.cpp index 2d07eb27c..80f562a9a 100644 --- a/radio/src/maths.cpp +++ b/radio/src/maths.cpp @@ -154,19 +154,6 @@ getvalue_t div10_and_round(getvalue_t value) return value/10; } - -#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(PCBTARANIS) double gpsToDouble(bool neg, int16_t bp, int16_t ap) { @@ -232,135 +219,3 @@ void getGpsDistance() frskyData.hub.maxGpsDistance = frskyData.hub.gpsDistance; } #endif - -#if defined(CPUARM) && defined(VARIO) - -void varioWakeup() -{ - if (isFunctionActive(FUNCTION_VARIO)) { - int varioFreq, varioDuration, varioPause=0; - uint8_t varioFlags; - - int verticalSpeed = frskyData.hub.varioSpeed; - -#if 0 - if (g_model.frsky.varioSource == VARIO_SOURCE_DTE) { - #warning "Ele stick for vario tests" - verticalSpeed = getValue(MIXSRC_Ele); - } -#endif - - int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50; - int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50; - int varioMax = (10+(int)g_model.frsky.varioMax) * 100; - int varioMin = (-10+(int)g_model.frsky.varioMin) * 100; - - if (verticalSpeed > varioMax) - verticalSpeed = varioMax; - else if (verticalSpeed < varioMin) - verticalSpeed = varioMin; - - if (verticalSpeed > varioCenterMin) { - varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) + (((VARIO_FREQUENCY_RANGE+(g_eeGeneral.varioRange*10)) * (verticalSpeed-varioCenterMin)) / varioMax); - int varioPeriod = VARIO_REPEAT_MAX + ((VARIO_REPEAT_ZERO+(g_eeGeneral.varioRepeat*10)-VARIO_REPEAT_MAX) * (varioMax-verticalSpeed) * (varioMax-verticalSpeed)) / ((varioMax-varioCenterMin) * (varioMax-varioCenterMin)); - if (verticalSpeed >= varioCenterMax || varioCenterMin == varioCenterMax) - varioDuration = varioPeriod / 5; - else - varioDuration = varioPeriod * (85 - (((verticalSpeed-varioCenterMin) * 25) / (varioCenterMax-varioCenterMin))) / 100; - varioPause = varioPeriod - varioDuration; - varioFlags = PLAY_BACKGROUND; - } - else { - varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) - (((VARIO_FREQUENCY_ZERO+(g_eeGeneral.varioPitch*10)-((VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10))/2)) * (verticalSpeed-varioCenterMin)) / varioMin); - varioDuration = 80; // continuous beep: we will enter again here before the tone ends - varioFlags = PLAY_BACKGROUND|PLAY_NOW; - } - - AUDIO_VARIO(varioFreq, varioDuration, varioPause, varioFlags); - } -} - -#elif defined(FRSKY) && defined(VARIO) - -void varioWakeup() -{ - static tmr10ms_t s_varioTmr; - tmr10ms_t tmr10ms = get_tmr10ms(); - - if (isFunctionActive(FUNCTION_VARIO)) { -#if defined(AUDIO) - cli(); - int16_t verticalSpeed = frskyData.hub.varioSpeed; - sei(); - -#if defined(PCBSTD) - int16_t varioCenterMax = (int16_t)g_model.frsky.varioCenterMax * 10 + 50; - if (verticalSpeed >= varioCenterMax) { - verticalSpeed = verticalSpeed - varioCenterMax; - int16_t varioMax = (10+(int16_t)g_model.frsky.varioMax) * 100; - if (verticalSpeed > varioMax) verticalSpeed = varioMax; - verticalSpeed = (verticalSpeed * 10) / ((varioMax-varioCenterMax) / 100); - - if ((int16_t)(s_varioTmr-tmr10ms) < 0) { - uint8_t varioFreq = (verticalSpeed * 10 + 16000) >> 8; - uint8_t varioDuration = (1600 - verticalSpeed) / 100; - s_varioTmr = tmr10ms + (varioDuration*2); - AUDIO_VARIO(varioFreq, varioDuration); - } - } -#else - int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50; - int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50; - int varioMax = (10+(int)g_model.frsky.varioMax) * 100; - int varioMin = (-10+(int)g_model.frsky.varioMin) * 100; - - if (verticalSpeed < varioCenterMin || (verticalSpeed > varioCenterMax && (int16_t)(s_varioTmr - tmr10ms) < 0)) { - if (verticalSpeed > varioMax) - verticalSpeed = varioMax; - else if (verticalSpeed < varioMin) - verticalSpeed = varioMin; - - uint8_t varioFreq, varioDuration; - if (verticalSpeed > 0) { - varioFreq = (verticalSpeed * 4 + 8000) >> 7; - varioDuration = (8000 - verticalSpeed * 5) / 100; - } - else { - varioFreq = (verticalSpeed * 3 + 8000) >> 7; - varioDuration = 20; - } - s_varioTmr = tmr10ms + (varioDuration/2); - AUDIO_VARIO(varioFreq, varioDuration); - } -#endif - -#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 - } - else { - s_varioTmr = tmr10ms; - } -} - -#endif diff --git a/radio/src/telemetry/frsky.cpp b/radio/src/telemetry/frsky.cpp index fb13d2ddf..8611a7a96 100644 --- a/radio/src/telemetry/frsky.cpp +++ b/radio/src/telemetry/frsky.cpp @@ -144,6 +144,16 @@ void FrskyValueWithMinMax::set(uint8_t value, uint8_t unit) } } +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(CPUARM) inline bool rxBattAlarmRaised(uint8_t alarm) { diff --git a/radio/src/vario.cpp b/radio/src/vario.cpp new file mode 100755 index 000000000..e4ed29fff --- /dev/null +++ b/radio/src/vario.cpp @@ -0,0 +1,170 @@ +/* + * 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(CPUARM) + +void varioWakeup() +{ + if (isFunctionActive(FUNCTION_VARIO)) { + int varioFreq, varioDuration, varioPause=0; + uint8_t varioFlags; + + int verticalSpeed = frskyData.hub.varioSpeed; + +#if 0 + if (g_model.frsky.varioSource == VARIO_SOURCE_DTE) { + #warning "Ele stick for vario tests" + verticalSpeed = getValue(MIXSRC_Ele); + } +#endif + + int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50; + int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50; + int varioMax = (10+(int)g_model.frsky.varioMax) * 100; + int varioMin = (-10+(int)g_model.frsky.varioMin) * 100; + + if (verticalSpeed > varioMax) + verticalSpeed = varioMax; + else if (verticalSpeed < varioMin) + verticalSpeed = varioMin; + + if (verticalSpeed > varioCenterMin) { + varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) + (((VARIO_FREQUENCY_RANGE+(g_eeGeneral.varioRange*10)) * (verticalSpeed-varioCenterMin)) / varioMax); + int varioPeriod = VARIO_REPEAT_MAX + ((VARIO_REPEAT_ZERO+(g_eeGeneral.varioRepeat*10)-VARIO_REPEAT_MAX) * (varioMax-verticalSpeed) * (varioMax-verticalSpeed)) / ((varioMax-varioCenterMin) * (varioMax-varioCenterMin)); + if (verticalSpeed >= varioCenterMax || varioCenterMin == varioCenterMax) + varioDuration = varioPeriod / 5; + else + varioDuration = varioPeriod * (85 - (((verticalSpeed-varioCenterMin) * 25) / (varioCenterMax-varioCenterMin))) / 100; + varioPause = varioPeriod - varioDuration; + varioFlags = PLAY_BACKGROUND; + } + else { + varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) - (((VARIO_FREQUENCY_ZERO+(g_eeGeneral.varioPitch*10)-((VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10))/2)) * (verticalSpeed-varioCenterMin)) / varioMin); + varioDuration = 80; // continuous beep: we will enter again here before the tone ends + varioFlags = PLAY_BACKGROUND|PLAY_NOW; + } + + AUDIO_VARIO(varioFreq, varioDuration, varioPause, varioFlags); + } +} + +#elif defined(FRSKY) + +void varioWakeup() +{ + static tmr10ms_t s_varioTmr; + tmr10ms_t tmr10ms = get_tmr10ms(); + + if (isFunctionActive(FUNCTION_VARIO)) { +#if defined(AUDIO) + cli(); + int16_t verticalSpeed = frskyData.hub.varioSpeed; + sei(); + +#if defined(PCBSTD) + int16_t varioCenterMax = (int16_t)g_model.frsky.varioCenterMax * 10 + 50; + if (verticalSpeed >= varioCenterMax) { + verticalSpeed = verticalSpeed - varioCenterMax; + int16_t varioMax = (10+(int16_t)g_model.frsky.varioMax) * 100; + if (verticalSpeed > varioMax) verticalSpeed = varioMax; + verticalSpeed = (verticalSpeed * 10) / ((varioMax-varioCenterMax) / 100); + + if ((int16_t)(s_varioTmr-tmr10ms) < 0) { + uint8_t varioFreq = (verticalSpeed * 10 + 16000) >> 8; + uint8_t varioDuration = (1600 - verticalSpeed) / 100; + s_varioTmr = tmr10ms + (varioDuration*2); + AUDIO_VARIO(varioFreq, varioDuration); + } + } +#else + int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50; + int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50; + int varioMax = (10+(int)g_model.frsky.varioMax) * 100; + int varioMin = (-10+(int)g_model.frsky.varioMin) * 100; + + if (verticalSpeed < varioCenterMin || (verticalSpeed > varioCenterMax && (int16_t)(s_varioTmr - tmr10ms) < 0)) { + if (verticalSpeed > varioMax) + verticalSpeed = varioMax; + else if (verticalSpeed < varioMin) + verticalSpeed = varioMin; + + uint8_t varioFreq, varioDuration; + if (verticalSpeed > 0) { + varioFreq = (verticalSpeed * 4 + 8000) >> 7; + varioDuration = (8000 - verticalSpeed * 5) / 100; + } + else { + varioFreq = (verticalSpeed * 3 + 8000) >> 7; + varioDuration = 20; + } + s_varioTmr = tmr10ms + (varioDuration/2); + AUDIO_VARIO(varioFreq, varioDuration); + } +#endif + +#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 + } + else { + s_varioTmr = tmr10ms; + } +} + +#endif