piotr/edgetx-gx12
1
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Refactoring again...

Browse Source
This commit is contained in:
bsongis@gmail.com
2013-03-21 15:12:26 +00:00
parent 38ea2afa9c
commit 63461069b8
6 changed files with 263 additions and 210 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/Makefile
+1 -1
View File
@@ -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
src/maths.cpp
+190
View File
@@ -0,0 +1,190 @@
/*
* 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(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
src/opentx.h
+63 -19
View File
@@ -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
src/telemetry_frsky.cpp
+1 -148
View File
@@ -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
}
src/telemetry_frsky.h
+2 -40
View File
@@ -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
src/view_telemetry.cpp
+6 -2
View File
@@ -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);