1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-23 00:05:17 +03:00

Directory transmitter renamed to radio

This commit is contained in:
Bertrand Songis 2013-12-02 07:13:57 +01:00
parent f4df52a033
commit 942e658c8e
707 changed files with 0 additions and 0 deletions

227
radio/src/maths.cpp Normal file
View file

@ -0,0 +1,227 @@
/*
* Authors (alphabetical order)
* - Andre Bernet <bernet.andre@gmail.com>
* - Andreas Weitl
* - Bertrand Songis <bsongis@gmail.com>
* - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
* - Cameron Weeks <th9xer@gmail.com>
* - Erez Raviv
* - Gabriel Birkus
* - Jean-Pierre Parisy
* - Karl Szmutny
* - Michael Blandford
* - Michal Hlavinka
* - Pat Mackenzie
* - Philip Moss
* - Rob Thomson
* - Romolo Manfredini <romolo.manfredini@gmail.com>
* - 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(PCBTARANIS)
double gpsToDouble(bool neg, int16_t bp, int16_t ap)
{
double result = ap;
result /= 10000;
result += (bp % 100);
result /= 60;
result += (bp / 100);
return neg?-result:result;
}
#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;
}
#if defined(PCBTARANIS)
double pilotLatitude;
double pilotLongitude;
#endif
void getGpsPilotPosition()
{
#if defined(PCBTARANIS)
pilotLatitude = gpsToDouble(frskyData.hub.gpsLatitudeNS=='S', frskyData.hub.gpsLatitude_bp, frskyData.hub.gpsLatitude_ap);
pilotLongitude = gpsToDouble(frskyData.hub.gpsLongitudeEW=='W', frskyData.hub.gpsLongitude_bp, frskyData.hub.gpsLongitude_ap);
#endif
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 ? TELEMETRY_ALT_BP : TELEMETRY_GPS_ALT_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 varioWakeup()
{
static tmr10ms_t s_varioTmr;
tmr10ms_t tmr10ms = get_tmr10ms();
if (isFunctionActive(FUNC_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 SoundVarioBeepTime = (1600 - verticalSpeed) / 100;
uint8_t SoundVarioBeepFreq = (verticalSpeed * 10 + 16000) >> 8;
s_varioTmr = tmr10ms + (SoundVarioBeepTime*2);
AUDIO_VARIO(SoundVarioBeepFreq, SoundVarioBeepTime);
}
}
#else
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);
}
else {
int16_t varioCenterMin = (int16_t)g_model.frsky.varioCenterMin * 10 - 50;
if (verticalSpeed <= varioCenterMin) {
verticalSpeed = verticalSpeed - 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;
}
}
if (verticalSpeed < 0 || (int16_t)(s_varioTmr-tmr10ms) < 0) {
#if defined(CPUARM)
uint16_t SoundVarioBeepTime;
uint16_t SoundVarioBeepFreq;
if (verticalSpeed > 0) {
if (verticalSpeed > 1276) {
SoundVarioBeepTime=5;
} else {
SoundVarioBeepTime = 320 - (verticalSpeed >> 2);
}
SoundVarioBeepFreq = 1000 + verticalSpeed;
}
else {
SoundVarioBeepTime = 80;
SoundVarioBeepFreq = (verticalSpeed * 3 + 8000) >> 3;
}
s_varioTmr = tmr10ms + (SoundVarioBeepTime/5);
#else
uint8_t SoundVarioBeepTime;
uint8_t SoundVarioBeepFreq;
if (verticalSpeed > 0) {
SoundVarioBeepTime = (8000 - verticalSpeed * 5) / 100;
SoundVarioBeepFreq = (verticalSpeed * 4 + 8000) >> 7;
}
else {
SoundVarioBeepTime = 20;
SoundVarioBeepFreq = (verticalSpeed * 3 + 8000) >> 7;
}
s_varioTmr = tmr10ms + (SoundVarioBeepTime/2);
#endif
AUDIO_VARIO(SoundVarioBeepFreq, SoundVarioBeepTime);
}
#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