radio/src/vario.cpp

   1 /*
   2  * Copyright (C) OpenTX
   3  *
   4  * Based on code named
   5  *   th9x - http://code.google.com/p/th9x
   6  *   er9x - http://code.google.com/p/er9x
   7  *   gruvin9x - http://code.google.com/p/gruvin9x
   8  *
   9  * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
  10  *
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License version 2 as
  13  * published by the Free Software Foundation.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  */
  20
  21 #include "opentx.h"
  22
  23
  24 #if defined(CPUARM)
  25
  26 void varioWakeup()
  27 {
  28   if (isFunctionActive(FUNCTION_VARIO)) {
  29     int varioFreq, varioDuration, varioPause=0;
  30     uint8_t varioFlags;
  31
  32     int verticalSpeed = 0;
  33     if (g_model.frsky.varioSource) {
  34       uint8_t item = g_model.frsky.varioSource-1;
  35       if (item < MAX_TELEMETRY_SENSORS) {
  36         verticalSpeed = telemetryItems[item].value * g_model.telemetrySensors[item].getPrecMultiplier();
  37       }
  38     }
  39
  40     int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50;
  41     int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50;
  42     int varioMax = (10+(int)g_model.frsky.varioMax) * 100;
  43     int varioMin = (-10+(int)g_model.frsky.varioMin) * 100;
  44
  45     if (verticalSpeed > varioMax)
  46       verticalSpeed = varioMax;
  47     else if (verticalSpeed < varioMin)
  48       verticalSpeed = varioMin;
  49
  50     if (verticalSpeed <= varioCenterMin) {
  51       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);
  52       varioDuration = 80; // continuous beep: we will enter again here before the tone ends
  53       varioFlags = PLAY_BACKGROUND|PLAY_NOW;
  54     }
  55     else if (verticalSpeed >= varioCenterMax || !g_model.frsky.varioCenterSilent) {
  56       varioFreq = VARIO_FREQUENCY_ZERO + (g_eeGeneral.varioPitch*10) + (((VARIO_FREQUENCY_RANGE+(g_eeGeneral.varioRange*10)) * (verticalSpeed-varioCenterMin)) / varioMax);
  57       int varioPeriod = VARIO_REPEAT_MAX + ((VARIO_REPEAT_ZERO+(g_eeGeneral.varioRepeat*10)-VARIO_REPEAT_MAX) * (varioMax-verticalSpeed) * (varioMax-verticalSpeed)) / ((varioMax-varioCenterMin) * (varioMax-varioCenterMin));
  58       if (verticalSpeed >= varioCenterMax || varioCenterMin == varioCenterMax)
  59         varioDuration = varioPeriod / 5;
  60       else
  61         varioDuration = varioPeriod * (85 - (((verticalSpeed-varioCenterMin) * 25) / (varioCenterMax-varioCenterMin))) / 100;
  62       varioPause = varioPeriod - varioDuration;
  63       varioFlags = PLAY_BACKGROUND;
  64     }
  65     else {
  66       return;
  67     }
  68
  69     AUDIO_VARIO(varioFreq, varioDuration, varioPause, varioFlags);
  70   }
  71 }
  72
  73 #elif defined(TELEMETRY_FRSKY)
  74
  75 void varioWakeup()
  76 {
  77   static tmr10ms_t s_varioTmr;
  78   tmr10ms_t tmr10ms = get_tmr10ms();
  79
  80   if (isFunctionActive(FUNCTION_VARIO)) {
  81 #if defined(AUDIO)
  82     cli();
  83     int16_t verticalSpeed = telemetryData.hub.varioSpeed;
  84     sei();
  85
  86 #if defined(PCBSTD)
  87     int16_t varioCenterMax = (int16_t)g_model.frsky.varioCenterMax * 10 + 50;
  88     if (verticalSpeed >= varioCenterMax) {
  89       verticalSpeed = verticalSpeed - varioCenterMax;
  90       int16_t varioMax = (10+(int16_t)g_model.frsky.varioMax) * 100;
  91       if (verticalSpeed > varioMax) verticalSpeed = varioMax;
  92       verticalSpeed = (verticalSpeed * 10) / ((varioMax-varioCenterMax) / 100);
  93
  94       if ((int16_t)(s_varioTmr-tmr10ms) < 0) {
  95         uint8_t varioFreq = (verticalSpeed * 10 + 16000) >> 8;
  96         uint8_t varioDuration = (1600 - verticalSpeed) / 100;
  97         s_varioTmr = tmr10ms + (varioDuration*2);
  98         AUDIO_VARIO(varioFreq, varioDuration);
  99       }
 100     }
 101 #else
 102     int varioCenterMin = (int)g_model.frsky.varioCenterMin * 10 - 50;
 103     int varioCenterMax = (int)g_model.frsky.varioCenterMax * 10 + 50;
 104     int varioMax = (10+(int)g_model.frsky.varioMax) * 100;
 105     int varioMin = (-10+(int)g_model.frsky.varioMin) * 100;
 106
 107     if (verticalSpeed < varioCenterMin || (verticalSpeed > varioCenterMax && (int16_t)(s_varioTmr - tmr10ms) < 0)) {
 108       if (verticalSpeed > varioMax)
 109         verticalSpeed = varioMax;
 110       else if (verticalSpeed < varioMin)
 111         verticalSpeed = varioMin;
 112
 113       uint8_t varioFreq, varioDuration;
 114       if (verticalSpeed > 0) {
 115         varioFreq = (verticalSpeed * 4 + 8000) >> 7;
 116         varioDuration = (8000 - verticalSpeed * 5) / 100;
 117       }
 118       else {
 119         varioFreq = (verticalSpeed * 3 + 8000) >> 7;
 120         varioDuration = 20;
 121       }
 122       s_varioTmr = tmr10ms + (varioDuration/2);
 123       AUDIO_VARIO(varioFreq, varioDuration);
 124     }
 125 #endif
 126
 127 #elif defined(BUZZER) // && !defined(AUDIO)
 128
 129     int8_t verticalSpeed = limit((int16_t)-100, (int16_t)(telemetryData.hub.varioSpeed/10), (int16_t)+100);
 130
 131     uint16_t interval;
 132     if (verticalSpeed == 0) {
 133       interval = 300;
 134     }
 135     else {
 136       if (verticalSpeed < 0) {
 137         verticalSpeed = -verticalSpeed;
 138         warble = 1;
 139       }
 140       interval = (uint8_t)200 / verticalSpeed;
 141     }
 142     if (g_tmr10ms - s_varioTmr > interval) {
 143       s_varioTmr = g_tmr10ms;
 144       if (warble)
 145         AUDIO_VARIO_DOWN();
 146       else
 147         AUDIO_VARIO_UP();
 148     }
 149 #endif
 150   }
 151   else {
 152     s_varioTmr = tmr10ms;
 153   }
 154 }
 155
 156 #endif