Telemetry refactoring

This commit is contained in:
bsongis
2014-05-15 18:07:52 +02:00
parent 7b1f11398d
commit b376192b61
32 changed files with 1526 additions and 1635 deletions
@@ -58,6 +58,7 @@
namespace Open9xGruvin9x {
#include "radio/src/targets/gruvin9x/board_gruvin9x.cpp"
#include "radio/src/targets/common_avr/telemetry_driver.cpp"
#include "radio/src/eeprom_common.cpp"
#include "radio/src/eeprom_rlc.cpp"
#include "radio/src/opentx.cpp"
@@ -84,6 +85,7 @@ namespace Open9xGruvin9x {
#include "radio/src/buzzer.cpp"
#include "radio/src/targets/gruvin9x/somo14d.cpp"
#include "radio/src/telemetry/frsky.cpp"
#include "radio/src/telemetry/frsky_d.cpp"
#include "radio/src/translations/tts_en.cpp"
#include "radio/src/haptic.cpp"
@@ -57,6 +57,7 @@
namespace Open9xM128 {
#include "radio/src/targets/stock/board_stock.cpp"
#include "radio/src/targets/common_avr/telemetry_driver.cpp"
#include "radio/src/eeprom_common.cpp"
#include "radio/src/eeprom_rlc.cpp"
#include "radio/src/opentx.cpp"
@@ -82,6 +83,7 @@ namespace Open9xM128 {
#include "radio/src/audio_avr.cpp"
#include "radio/src/targets/stock/voice.cpp"
#include "radio/src/telemetry/frsky.cpp"
#include "radio/src/telemetry/frsky_d.cpp"
#include "radio/src/translations/tts_en.cpp"
#include "radio/src/haptic.cpp"
@@ -99,6 +99,8 @@ namespace Open9xSky9x {
#include "radio/src/translations.cpp"
#include "radio/src/fonts.cpp"
#include "radio/src/telemetry/frsky.cpp"
#include "radio/src/telemetry/frsky_sport.cpp"
#include "radio/src/telemetry/frsky_d.cpp"
#include "radio/src/targets/sky9x/audio_driver.cpp"
#include "radio/src/audio_arm.cpp"
#include "radio/src/buzzer.cpp"
@@ -112,8 +112,11 @@ inline int geteepromsize() {
#define SDCARD
#include "radio/src/translations.cpp"
#include "radio/src/fonts.cpp"
#include "radio/src/telemetry/frsky.cpp"
#include "radio/src/telemetry/frsky_sport.cpp"
#include "radio/src/telemetry/frsky_d.cpp"
#include "radio/src/targets/taranis/audio_driver.cpp"
#include "radio/src/targets/taranis/telemetry_driver.cpp"
#include "radio/src/audio_arm.cpp"
#include "radio/src/translations/tts_cz.cpp"
#include "radio/src/translations/tts_de.cpp"
@@ -63,6 +63,7 @@
namespace Open9x {
#include "radio/src/targets/stock/board_stock.cpp"
#include "radio/src/targets/common_avr/telemetry_driver.cpp"
#include "radio/src/eeprom_common.cpp"
#include "radio/src/eeprom_rlc.cpp"
#include "radio/src/opentx.cpp"
@@ -83,6 +84,7 @@ namespace Open9x {
#include "radio/src/keys.cpp"
#include "radio/src/simpgmspace.cpp"
#include "radio/src/telemetry/frsky.cpp"
#include "radio/src/telemetry/frsky_d.cpp"
#include "radio/src/templates.cpp"
#include "radio/src/translations.cpp"
#include "radio/src/fonts.cpp"
+9 -9
View File
@@ -310,7 +310,7 @@ ifeq ($(PCB), $(filter $(PCB), STD 9X 9XR))
INCDIRS += targets/stock
CPPDEFS = -DF_CPU=$(F_CPU)UL -DPCBSTD -DCPUM64 -DEEPROM_VARIANT=$(shell echo ${EEPROM_VARIANT} | bc)
BOARDSRC = targets/stock/board_stock.cpp
EXTRABOARDSRC = targets/stock/lcd_driver.cpp
EXTRABOARDSRC = targets/stock/lcd_driver.cpp targets/common_avr/telemetry_driver.cpp
EEPROMSRC = eeprom_common.cpp eeprom_rlc.cpp
PULSESSRC = protocols/pulses_avr.cpp
@@ -373,7 +373,7 @@ ifeq ($(PCB), $(filter $(PCB), STD128 9X128 9XR128))
CPPDEFS = -DF_CPU=$(F_CPU)UL -DPCBSTD -DCPUM128 -DEEPROM_VARIANT=$(shell echo ${EEPROM_VARIANT} | bc)
INCDIRS += targets/stock
BOARDSRC = targets/stock/board_stock.cpp
EXTRABOARDSRC = targets/stock/lcd_driver.cpp
EXTRABOARDSRC = targets/stock/lcd_driver.cpp targets/common_avr/telemetry_driver.cpp
EEPROMSRC = eeprom_common.cpp eeprom_rlc.cpp
PULSESSRC = protocols/pulses_avr.cpp
@@ -432,7 +432,7 @@ ifeq ($(PCB), $(filter $(PCB), 9X2561))
CPPDEFS = -DF_CPU=$(F_CPU)UL -DPCBSTD -DCPUM2561 -DEEPROM_VARIANT=$(shell echo ${EEPROM_VARIANT} | bc)
INCDIRS += targets/stock
BOARDSRC = targets/stock/board_stock.cpp
EXTRABOARDSRC = targets/stock/lcd_driver.cpp
EXTRABOARDSRC = targets/stock/lcd_driver.cpp targets/common_avr/telemetry_driver.cpp
EEPROMSRC = eeprom_common.cpp eeprom_rlc.cpp
PULSESSRC = protocols/pulses_avr.cpp
@@ -492,7 +492,7 @@ ifeq ($(PCB), GRUVIN9X)
THR_TRACE = YES
INCDIRS += targets/gruvin9x targets/stock FatFs FatFs/option
BOARDSRC = targets/gruvin9x/board_gruvin9x.cpp
EXTRABOARDSRC = targets/stock/lcd_driver.cpp
EXTRABOARDSRC = targets/stock/lcd_driver.cpp targets/common_avr/telemetry_driver.cpp
EEPROMSRC = eeprom_common.cpp eeprom_rlc.cpp
PULSESSRC = protocols/pulses_avr.cpp
CPPSRC += audio_avr.cpp haptic.cpp
@@ -530,7 +530,7 @@ ifeq ($(PCB), MEGA2560)
THR_TRACE = YES
INCDIRS += targets/mega2560 targets/stock FatFs FatFs/option
BOARDSRC = targets/mega2560/board_mega2560.cpp
EXTRABOARDSRC = targets/mega2560/lcd_driver_ST7565R.cpp
EXTRABOARDSRC = targets/mega2560/lcd_driver_ST7565R.cpp targets/common_avr/telemetry_driver.cpp
EEPROMSRC = eeprom_common.cpp eeprom_rlc.cpp
PULSESSRC = protocols/pulses_avr.cpp
CPPSRC += audio_avr.cpp
@@ -660,7 +660,7 @@ ifeq ($(PCB), TARANIS)
EEPROMSRC = eeprom_common.cpp eeprom_rlc.cpp eeprom_conversions.cpp
PULSESSRC = protocols/pulses_arm.cpp protocols/ppm_arm.cpp protocols/pxx_arm.cpp
CPPSRC += audio_arm.cpp
CPPSRC += targets/taranis/pulses_driver.cpp targets/taranis/keys_driver.cpp targets/taranis/adc_driver.cpp targets/taranis/trainer_driver.cpp targets/taranis/audio_driver.cpp targets/taranis/uart3_driver.cpp targets/taranis/sport_driver.cpp
CPPSRC += targets/taranis/pulses_driver.cpp targets/taranis/keys_driver.cpp targets/taranis/adc_driver.cpp targets/taranis/trainer_driver.cpp targets/taranis/audio_driver.cpp targets/taranis/uart3_driver.cpp targets/taranis/telemetry_driver.cpp
CPPSRC += bmp.cpp gui/view_channels.cpp gui/view_about.cpp gui/view_text.cpp loadboot.cpp
SRC += targets/taranis/STM32F2xx_StdPeriph_Lib_V1.1.0/Libraries/CMSIS/Device/ST/STM32F2xx/Source/Templates/system_stm32f2xx.c
SRC += targets/taranis/STM32F2xx_StdPeriph_Lib_V1.1.0/Libraries/STM32F2xx_StdPeriph_Driver/src/misc.c
@@ -928,11 +928,11 @@ endif
ifeq ($(EXT), $(filter $(EXT), FRSKY TELEMETREZ))
CPPDEFS += -DFRSKY
ifeq ($(PCB), TARANIS)
CPPSRC += telemetry/frsky_sport.cpp
ifeq ($(PCB), $(filter $(PCB), SKY9X 9XRPRO TARANIS))
CPPSRC += telemetry/frsky.cpp telemetry/frsky_sport.cpp telemetry/frsky_d.cpp
CPPDEFS += -DFRSKY_SPORT
else
CPPSRC += telemetry/frsky.cpp
CPPSRC += telemetry/frsky.cpp telemetry/frsky_d.cpp
endif
CPPSRC += gui/view_telemetry.cpp
+1 -1
View File
@@ -385,7 +385,7 @@ void eeLoadModel(uint8_t id)
read32_eeprom_data((File_system[id+1].block_no << 12) + sizeof(struct t_eeprom_header), (uint8_t *)&g_model, size) ;
}
resetAll();
flightReset();
if (pulsesStarted()) {
checkAll();
+1 -1
View File
@@ -964,7 +964,7 @@ void eeLoadModel(uint8_t id)
eeCheck(true);
}
resetAll();
flightReset();
if (pulsesStarted()) {
checkAll();
+11 -11
View File
@@ -237,7 +237,7 @@ TEST(EEPROM, rm)
}
#if defined(FRSKY) && !defined(FRSKY_SPORT)
extern void processFrskyPacket(uint8_t *packet);
extern void frskyDProcessPacket(uint8_t *packet);
TEST(FrSky, gpsNfuel)
{
g_model.frsky.usrProto = 1;
@@ -250,13 +250,13 @@ TEST(FrSky, gpsNfuel)
uint8_t pkt5[] = { 0xfd, 0x07, 0x00, 0x5e, 0x22, 0x45, 0x00, 0x5e, 0x11, 0x02 };
uint8_t pkt6[] = { 0xfd, 0x07, 0x00, 0x00, 0x5e, 0x19, 0x93, 0x00, 0x5e, 0x04 };
uint8_t pkt7[] = { 0xfd, 0x03, 0x00, 0x64, 0x00, 0x5e };
processFrskyPacket(pkt1);
processFrskyPacket(pkt2);
processFrskyPacket(pkt3);
processFrskyPacket(pkt4);
processFrskyPacket(pkt5);
processFrskyPacket(pkt6);
processFrskyPacket(pkt7);
frskyDProcessPacket(pkt1);
frskyDProcessPacket(pkt2);
frskyDProcessPacket(pkt3);
frskyDProcessPacket(pkt4);
frskyDProcessPacket(pkt5);
frskyDProcessPacket(pkt6);
frskyDProcessPacket(pkt7);
EXPECT_EQ(frskyData.hub.gpsCourse_bp, 44);
EXPECT_EQ(frskyData.hub.gpsCourse_ap, 03);
EXPECT_EQ(frskyData.hub.gpsLongitude_bp / 100, 120);
@@ -272,9 +272,9 @@ TEST(FrSky, dateNtime)
uint8_t pkt1[] = { 0xfd, 0x07, 0x00, 0x5e, 0x15, 0x0f, 0x07, 0x5e, 0x16, 0x0b };
uint8_t pkt2[] = { 0xfd, 0x07, 0x00, 0x00, 0x5e, 0x17, 0x06, 0x12, 0x5e, 0x18 };
uint8_t pkt3[] = { 0xfd, 0x03, 0x00, 0x32, 0x00, 0x5e };
processFrskyPacket(pkt1);
processFrskyPacket(pkt2);
processFrskyPacket(pkt3);
frskyDProcessPacket(pkt1);
frskyDProcessPacket(pkt2);
frskyDProcessPacket(pkt3);
EXPECT_EQ(frskyData.hub.day, 15);
EXPECT_EQ(frskyData.hub.month, 07);
EXPECT_EQ(frskyData.hub.year, 11);
+6 -6
View File
@@ -446,10 +446,10 @@ void menuMainViewChannelsMonitor(uint8_t event)
void onMainViewMenu(const char *result)
{
if (result == STR_RESET_TIMER1) {
resetTimer(0);
timerReset(0);
}
else if (result == STR_RESET_TIMER2) {
resetTimer(1);
timerReset(1);
}
#if defined(PCBTARANIS)
else if (result == STR_VIEW_NOTES) {
@@ -464,11 +464,11 @@ void onMainViewMenu(const char *result)
#endif
#if defined(FRSKY)
else if (result == STR_RESET_TELEMETRY) {
resetTelemetry();
telemetryReset();
}
#endif
else if (result == STR_RESET_FLIGHT) {
resetAll();
flightReset();
}
else if (result == STR_STATISTICS) {
chainMenu(menuStatisticsView);
@@ -630,7 +630,7 @@ void menuMainView(uint8_t event)
#endif
#if !defined(PCBTARANIS)
if (view == VIEW_TIMER2) {
resetTimer(1);
timerReset(1);
}
#endif
AUDIO_KEYPAD_UP();
@@ -638,7 +638,7 @@ void menuMainView(uint8_t event)
#if !defined(NAVIGATION_MENUS)
case EVT_KEY_LONG(KEY_EXIT):
resetAll();
flightReset();
AUDIO_KEYPAD_UP();
break;
#endif
+1 -1
View File
@@ -199,7 +199,7 @@ void menuTelemetryFrsky(uint8_t event)
break;
#else
case EVT_KEY_FIRST(KEY_ENTER):
resetTelemetry();
telemetryReset();
break;
#endif
}
+1 -1
View File
@@ -856,7 +856,7 @@ enum Functions {
enum ResetFunctionParam {
FUNC_RESET_TIMER1,
FUNC_RESET_TIMER2,
FUNC_RESET_ALL,
FUNC_RESET_FLIGHT,
#if defined(FRSKY)
FUNC_RESET_TELEMETRY,
#endif
+25 -32
View File
@@ -1557,7 +1557,7 @@ uint16_t s_timeCum16ThrP; // THR% in 1/16 sec
uint8_t trimsCheckTimer = 0;
void resetTimer(uint8_t idx)
void timerReset(uint8_t idx)
{
TimerState & timerState = timersStates[idx];
timerState.state = TMR_OFF; // is changed to RUNNING dep from mode
@@ -1565,7 +1565,7 @@ void resetTimer(uint8_t idx)
timerState.val_10ms = 0 ;
}
void resetAll()
void flightReset()
{
static bool firstReset = true;
if (firstReset)
@@ -1573,10 +1573,10 @@ void resetAll()
else
AUDIO_RESET();
resetTimer(0);
resetTimer(1);
timerReset(0);
timerReset(1);
#if defined(FRSKY)
resetTelemetry();
telemetryReset();
#endif
for (uint8_t i=0; i<NUM_LOGICAL_SWITCH; i++) {
csLastValue[i] = CS_LAST_VALUE_INIT;
@@ -1873,14 +1873,14 @@ void evalFunctions()
switch (CFN_PARAM(sd)) {
case FUNC_RESET_TIMER1:
case FUNC_RESET_TIMER2:
resetTimer(CFN_PARAM(sd));
timerReset(CFN_PARAM(sd));
break;
case FUNC_RESET_ALL:
resetAll();
case FUNC_RESET_FLIGHT:
flightReset();
break;
#if defined(FRSKY)
case FUNC_RESET_TELEMETRY:
resetTelemetry();
telemetryReset();
break;
#endif
#if ROTARY_ENCODERS > 0
@@ -2483,7 +2483,7 @@ void opentxStart()
void opentxClose()
{
#if defined(FRSKY)
FRSKY_End();
// TODO needed? telemetryEnd();
#endif
#if defined(SDCARD)
@@ -3014,26 +3014,6 @@ ISR(TIMER3_CAPT_vect) // G: High frequency noise can cause stack overflo with IS
}
#endif
/*
USART0 Transmit Data Register Emtpy ISR
Used to transmit FrSky data packets and DSM2 protocol
*/
// TODO serial_arm and serial_avr
#if defined(FRSKY) && !defined(CPUARM)
// TODO in frsky.cpp?
FORCEINLINE void FRSKY_USART0_vect()
{
if (frskyTxBufferCount > 0) {
UDR0 = frskyTxBuffer[--frskyTxBufferCount];
}
else {
UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
}
}
#endif
#if defined(DSM2_SERIAL) && !defined(CPUARM)
FORCEINLINE void DSM2_USART0_vect()
{
@@ -3048,7 +3028,20 @@ FORCEINLINE void DSM2_USART0_vect()
#endif
#if !defined(SIMU) && !defined(CPUARM)
#if defined (FRSKY) || defined(DSM2_SERIAL)
// USART0 Transmit Data Register Emtpy ISR
FORCEINLINE void FRSKY_USART0_vect()
{
if (frskyTxBufferCount > 0) {
UDR0 = frskyTxBuffer[--frskyTxBufferCount];
}
else {
UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
}
}
ISR(USART0_UDRE_vect)
{
#if defined(FRSKY) && defined(DSM2_SERIAL)
@@ -3444,10 +3437,10 @@ int main(void)
lcdSetRefVolt(25);
sei(); // interrupts needed for FRSKY_Init and eeReadAll.
sei(); // interrupts needed for telemetryInit and eeReadAll.
#if defined(FRSKY) && !defined(DSM2_SERIAL)
FRSKY_Init();
telemetryInit();
#endif
#if defined(DSM2_SERIAL) && !defined(FRSKY)
+4 -6
View File
@@ -490,6 +490,7 @@ enum PotType {
#define MAX_TRAINER_CHANNELS() (8)
#define MAX_CHANNELS(idx) (idx==0 ? MAX_PORT1_CHANNELS() : (idx==1 ? MAX_PORT2_CHANNELS() : MAX_TRAINER_CHANNELS()))
#elif defined(PCBSKY9X)
#define IS_MODULE_XJT() (IS_PXX_PROTOCOL(g_model.protocol))
#define NUM_PORT1_CHANNELS() (IS_PXX_PROTOCOL(g_model.protocol) ? 8 : (IS_DSM2_PROTOCOL(g_model.protocol) ? 6 : (8+g_model.moduleData[0].channelsCount)))
#define NUM_PORT2_CHANNELS() (8+g_model.moduleData[1].channelsCount)
#define NUM_CHANNELS(idx) (idx==0 ? NUM_PORT1_CHANNELS() : (8+g_model.moduleData[idx].channelsCount))
@@ -813,8 +814,8 @@ extern int16_t csLastValue[NUM_LOGICAL_SWITCH];
#define TMR_RUNNING 1
#define TMR_NEGATIVE 2
#define TMR_STOPPED 3
void resetTimer(uint8_t idx);
void resetAll();
void timerReset(uint8_t idx);
void flightReset();
extern uint8_t unexpectedShutdown;
extern uint8_t g_tmr1Latency_max;
@@ -1231,10 +1232,7 @@ inline bool isFunctionActive(uint8_t func)
extern volatile rotenc_t g_rotenc[1];
#endif
#if defined(FRSKY_SPORT)
// FrSky SPORT Telemetry
#include "telemetry/frsky_sport.h"
#elif defined (FRSKY)
#if defined (FRSKY)
// FrSky Telemetry
#include "telemetry/frsky.h"
#elif defined(JETI)
+1 -1
View File
@@ -743,7 +743,7 @@ void setupPulses()
#if defined(DSM2_SERIAL) && defined(FRSKY)
if (s_current_protocol[0] == 255 || IS_DSM2_PROTOCOL(s_current_protocol[0])) {
FRSKY_Init();
telemetryInit();
}
#endif
+39
View File
@@ -0,0 +1,39 @@
/*
* 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.
*
*/
// Telemetry driver
void telemetryPortInit();
void telemetryTransmitBuffer();
@@ -0,0 +1,142 @@
/*
* 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)
void telemetryEnableTx(void)
{
UCSR0B |= (1 << TXEN0); // enable TX
}
void telemetryEnableRx(void)
{
UCSR0B |= (1 << RXEN0); // enable RX
UCSR0B |= (1 << RXCIE0); // enable Interrupt
}
void processSerialData(uint8_t data);
extern uint8_t numPktBytes; // TODO not driver, change name
ISR(USART0_RX_vect)
{
uint8_t stat;
uint8_t data;
UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
sei();
stat = UCSR0A; // USART control and Status Register 0 A
/*
bit 7 6 5 4 3 2 1 0
RxC0 TxC0 UDRE0 FE0 DOR0 UPE0 U2X0 MPCM0
RxC0: Receive complete
TXC0: Transmit Complete
UDRE0: USART Data Register Empty
FE0: Frame Error
DOR0: Data OverRun
UPE0: USART Parity Error
U2X0: Double Tx Speed
PCM0: MultiProcessor Comms Mode
*/
// rh = UCSR0B; //USART control and Status Register 0 B
/*
bit 7 6 5 4 3 2 1 0
RXCIE0 TxCIE0 UDRIE0 RXEN0 TXEN0 UCSZ02 RXB80 TXB80
RxCIE0: Receive Complete int enable
TXCIE0: Transmit Complete int enable
UDRIE0: USART Data Register Empty int enable
RXEN0: Rx Enable
TXEN0: Tx Enable
UCSZ02: Character Size bit 2
RXB80: Rx data bit 8
TXB80: Tx data bit 8
*/
data = UDR0; // USART data register 0
if (stat & ((1 << FE0) | (1 << DOR0) | (1 << UPE0))) {
// discard buffer and start fresh on any comms error
numPktBytes = 0;
}
else {
processSerialData(data);
}
cli() ;
UCSR0B |= (1 << RXCIE0); // enable Interrupt
}
void telemetryPortInit()
{
#if !defined(SIMU)
DDRE &= ~(1 << DDE0); // set RXD0 pin as input
PORTE &= ~(1 << PORTE0); // disable pullup on RXD0 pin
#undef BAUD
#define BAUD 9600
#include <util/setbaud.h>
UBRR0H = UBRRH_VALUE;
UBRR0L = UBRRL_VALUE;
UCSR0A &= ~(1 << U2X0); // disable double speed operation.
// set 8N1
UCSR0B = 0 | (0 << RXCIE0) | (0 << TXCIE0) | (0 << UDRIE0) | (0 << RXEN0) | (0 << TXEN0) | (0 << UCSZ02);
UCSR0C = 0 | (1 << UCSZ01) | (1 << UCSZ00);
while (UCSR0A & (1 << RXC0)) UDR0; // flush receive buffer
// These should be running right from power up on a FrSky enabled '9X.
telemetryEnableTx(); // enable FrSky-Telemetry emission
frskyTxBufferCount = 0; // TODO not driver code
telemetryEnableRx(); // enable FrSky-Telemetry reception
#endif
}
void telemetryTransmitBuffer()
{
UCSR0B |= (1 << UDRIE0); // enable UDRE0 interrupt
}
#endif
@@ -37,6 +37,8 @@
#ifndef board_gruvin9x_h
#define board_gruvin9x_h
#include "../common_avr/board_avr.h"
#define GPIO_BUTTON_MENU pinl
#define GPIO_BUTTON_EXIT pinl
#define GPIO_BUTTON_RIGHT pinl
@@ -37,6 +37,8 @@
#ifndef board_mega2560_h
#define board_mega2560_h
#include "../common_avr/board_avr.h"
#define GPIO_BUTTON_MENU pinl
#define GPIO_BUTTON_EXIT pinl
#define GPIO_BUTTON_RIGHT pinl
-12
View File
@@ -59,15 +59,6 @@ volatile uint32_t lcdInputs;
/** Pins description corresponding to Rxd,Txd, (UART pins) */
#define CONSOLE_PINS {PINS_UART}
/** Second serial baudrate 9600. */
#define SECOND_BAUDRATE 9600
/** Usart Hw interface used by the console (UART0). */
#define SECOND_USART USART0
/** Usart Hw ID used by the console (UART0). */
#define SECOND_ID ID_USART0
/** Pins description corresponding to Rxd,Txd, (UART pins) */
#define SECOND_PINS {PINS_USART0}
extern "C" void sam_boot( void ) ;
// Prototype
@@ -619,9 +610,6 @@ void boardInit()
#endif
init_SDcard();
UART2_Configure(9600, Master_frequency);
startPdcUsartReceive();
}
#endif
+8 -2
View File
@@ -147,6 +147,11 @@ void usbMassStorage();
#define PIN_LOW 0x000
#define PIN_HIGH 0x100
// Telemetry port
#define SECOND_USART USART0
#define SECOND_ID ID_USART0
#define SECOND_PINS {PINS_USART0}
void configure_pins( uint32_t pins, uint16_t config );
uint16_t getCurrent();
@@ -285,7 +290,8 @@ void rotencEnd();
void debugPutc(const char c);
// Telemetry driver
void UART2_Configure(uint32_t baudrate, uint32_t masterClock);
void startPdcUsartReceive();
void telemetryPortInit(uint32_t baudrate);
uint32_t telemetryTransmitPending();
void telemetryTransmitBuffer(uint8_t * buffer, uint32_t size);
#endif
+14 -3
View File
@@ -117,7 +117,7 @@ void startPdcUsartReceive()
pUsart->US_RCR = RX_UART_BUFFER_SIZE ;
pUsart->US_RNCR = RX_UART_BUFFER_SIZE ;
pUsart->US_PTCR = US_PTCR_RXTEN ;
TelemetryActiveBuffer = 0 ;
TelemetryActiveBuffer = 0;
}
void rxPdcUsart( void (*pChProcess)(uint8_t x) )
@@ -158,7 +158,7 @@ void rxPdcUsart( void (*pChProcess)(uint8_t x) )
#endif
}
uint32_t txPdcUsart( uint8_t *buffer, uint32_t size )
uint32_t txPdcUsart(uint8_t *buffer, uint32_t size)
{
register Usart *pUsart = SECOND_USART;
@@ -174,7 +174,7 @@ uint32_t txPdcUsart( uint8_t *buffer, uint32_t size )
return 0 ;
}
uint32_t txPdcPending()
uint32_t telemetryTransmitPending()
{
register Usart *pUsart = SECOND_USART;
uint32_t x ;
@@ -188,3 +188,14 @@ uint32_t txPdcPending()
return x ;
}
void telemetryPortInit(uint32_t baudrate)
{
UART2_Configure(baudrate, Master_frequency);
startPdcUsartReceive();
}
void telemetryTransmitBuffer(uint8_t * buffer, uint32_t size)
{
txPdcUsart(buffer, size);
}
+2
View File
@@ -37,6 +37,8 @@
#ifndef board_stock_h
#define board_stock_h
#include "../common_avr/board_avr.h"
#define GPIO_BUTTON_MENU pinb
#define GPIO_BUTTON_EXIT pinb
#define GPIO_BUTTON_RIGHT pinb
@@ -157,7 +157,6 @@ void boardInit()
init5msTimer();
__enable_irq();
eepromInit();
sportInit();
usbInit();
#if defined(HAPTIC)
+3 -3
View File
@@ -114,13 +114,10 @@ extern uint16_t sessionTimer;
void delaysInit(void);
#define DEBUG_BAUDRATE 115200
#define SPORT_BAUDRATE 57600
void uart3Init(unsigned int mode);
void uart3Putc(const char c);
void sportInit(void);
void delaysInit(void);
void delay_01us(uint16_t nb);
@@ -246,6 +243,9 @@ void eeWriteBlockCmp(const void *pointer_ram, uint16_t pointer_eeprom, size_t si
// Debug driver
void debugPutc(const char c);
// Telemetry driver
void telemetryPortInit(uint32_t baudrate);
// Haptic driver
void hapticInit(void);
void hapticOff(void);
@@ -38,7 +38,7 @@
extern Fifo<512> telemetryFifo;
void sportInit(void)
void telemetryPortInit(uint32_t baudrate)
{
USART_InitTypeDef USART_InitStructure;
GPIO_InitTypeDef GPIO_InitStructure;
@@ -66,7 +66,7 @@ void sportInit(void)
RCC_APB1PeriphClockCmd(RCC_APB1Periph_SPORT, ENABLE);
USART_InitStructure.USART_BaudRate = SPORT_BAUDRATE;
USART_InitStructure.USART_BaudRate = baudrate;
USART_InitStructure.USART_WordLength = USART_WordLength_8b;
USART_InitStructure.USART_StopBits = USART_StopBits_1;
USART_InitStructure.USART_Parity = USART_Parity_No;
@@ -81,7 +81,7 @@ void sportInit(void)
NVIC_SetPriority(USART2_IRQn, 6);
}
void sportPutc(const char c)
void telemetryPutc(const char c)
{
GPIO_SetBits(GPIO_PIN_SPORT_ON, PIN_SPORT_ON);
USART_SendData(SPORT, c);
+1 -1
View File
@@ -78,7 +78,7 @@ void uart3Init(unsigned int mode)
switch(mode) {
case UART_MODE_SPORT:
uart3Setup(SPORT_BAUDRATE);
uart3Setup(FRSKY_SPORT_BAUDRATE);
break;
#if defined(DEBUG)
case UART_MODE_DEBUG:
File diff suppressed because it is too large Load Diff
+435 -274
View File
@@ -1,274 +1,435 @@
/*
* 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.
*
*/
#ifndef TELEMETRY_FRSKY_H
#define TELEMETRY_FRSKY_H
#define FRSKY_RX_PACKET_SIZE 19
#define FRSKY_TX_PACKET_SIZE 12
#include <inttypes.h>
// .20 seconds
#define FRSKY_TIMEOUT10ms 20
enum AlarmLevel {
alarm_off = 0,
alarm_yellow = 1,
alarm_orange = 2,
alarm_red = 3
};
#define ALARM_GREATER(channel, alarm) ((g_model.frsky.channels[channel].alarms_greater >> alarm) & 1)
#define ALARM_LEVEL(channel, alarm) ((g_model.frsky.channels[channel].alarms_level >> (2*alarm)) & 3)
class FrskyValueWithMin {
public:
uint8_t value;
uint8_t min;
uint16_t sum;
void set(uint8_t value);
};
class FrskyValueWithMinMax: public FrskyValueWithMin {
public:
uint8_t max;
void set(uint8_t value, uint8_t unit);
};
#define VARIO_QUEUE_LENGTH 5
#if defined(FRSKY_HUB)
PACK(struct FrskySerialData {
int16_t baroAltitudeOffset;// spare reused
int16_t gpsAltitude_bp; // 0x01 before punct
int16_t temperature1; // 0x02 -20 .. 250 deg. celcius
uint16_t rpm; // 0x03 0..60,000 revs. per minute
uint16_t fuelLevel; // 0x04 0, 25, 50, 75, 100 percent
int16_t temperature2; // 0x05 -20 .. 250 deg. celcius
uint16_t volts; // 0x06 1/500V increments (0..4.2V)
uint32_t distFromEarthAxis;// 2 spares reused
int16_t gpsAltitude_ap; // 0x01+8 after punct
uint8_t cellVolts[12]; // 6 spares reused
int16_t baroAltitude_bp; // 0x10 0..9,999 meters
uint16_t gpsSpeed_bp; // 0x11 before punct
uint16_t gpsLongitude_bp; // 0x12 before punct
uint16_t gpsLatitude_bp; // 0x13 before punct
uint16_t gpsCourse_bp; // 0x14 before punct (0..359.99 deg. -- seemingly 2-decimal precision)
uint8_t day; // 0x15
uint8_t month; // 0x15
uint16_t year; // 0x16
uint8_t hour; // 0x17
uint8_t min; // 0x17
uint16_t sec; // 0x18
uint16_t gpsSpeed_ap; // 0x11+8
uint16_t gpsLongitude_ap; // 0x12+8
uint16_t gpsLatitude_ap; // 0x13+8
uint16_t gpsCourse_ap; // 0x14+8
uint32_t pilotLatitude; // 2 spares reused
uint32_t pilotLongitude; // 2 spares reused
uint16_t baroAltitude_ap; // 0x21 after punct
uint16_t gpsLongitudeEW; // 0x1A+8 East/West
uint16_t gpsLatitudeNS; // 0x1B+8 North/South
int16_t accelX; // 0x24 1/256th gram (-8g ~ +8g)
int16_t accelY; // 0x25 1/256th gram (-8g ~ +8g)
int16_t accelZ; // 0x26 1/256th gram (-8g ~ +8g)
uint8_t gpsDistNeeded:1; // 1bits out of 16bits spare reused
int8_t gpsFix:2; // 2bits out of 16bits spare reused: -1=never fixed, 0=not fixed now, 1=fixed
uint8_t openXsensor:1; // 1bits out of 16bits spare reused: we receive data from the openXsensor
uint8_t cellsCount:4; // 4bits out of 16bits spare reused
uint8_t minCellVolts; // 8bits out of 16bits spare reused
uint16_t current; // 0x28 Current
int16_t varioAltitudeQueue[VARIO_QUEUE_LENGTH]; //circular buffer
int32_t varioAltitude_cm;
int16_t varioSpeed; // 0x30 Vertical speed in cm/s
uint16_t gpsDistance;
int16_t gpsAltitudeOffset;
uint8_t varioAltitudeQueuePointer; // circular-buffer pointer
uint8_t minCellIdx;
int16_t cellsSum;
uint16_t currentConsumption; // 0x35 openXsensor only! Otherwise calculated by the Tx from current
uint16_t currentPrescale;
uint16_t power; // 0x37 openXsensor only! Otherwise calculated by the Tx from current and voltage
int16_t airSpeed;
uint16_t vfas; // 0x39 Added to FrSky protocol for home made sensors with a better precision
uint16_t volts_bp; // 0x3A
uint16_t volts_ap; // 0x3B
// end of FrSky Hub data
/* next fields must keep this order! */
int16_t minAltitude;
int16_t maxAltitude;
uint16_t maxRpm;
int16_t maxTemperature1;
int16_t maxTemperature2;
uint16_t maxGpsSpeed;
uint16_t maxGpsDistance;
uint16_t maxAirSpeed;
int16_t minCell;
int16_t minCells;
int16_t minVfas;
uint16_t maxCurrent;
uint16_t maxPower;
/* end */
int16_t dTE;
});
#elif defined(WS_HOW_HIGH)
PACK(struct FrskySerialData {
int16_t baroAltitude_bp; // 0..9,999 meters
int16_t baroAltitudeOffset;
int16_t minAltitude;
int16_t maxAltitude;
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
uint16_t maxPower;
#if defined(VARIO)
int16_t varioAltitudeQueue[VARIO_QUEUE_LENGTH]; //circular buffer
uint8_t varioAltitudeQueuePointer; // circular-buffer pointer
int32_t varioAltitude_cm;
int16_t varioSpeed; // Vertical speed in cm/s
#endif
});
#elif defined(VARIO)
PACK(struct FrskySerialData {
int16_t varioAltitudeQueue[VARIO_QUEUE_LENGTH]; //circular buffer
uint8_t varioAltitudeQueuePointer; // circular-buffer pointer
int32_t varioAltitude_cm;
int16_t varioSpeed; // Vertical speed in cm/s
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
uint16_t maxPower;
});
#else
PACK(struct FrskySerialData {
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
uint16_t maxPower;
});
#endif
struct FrskyData {
FrskyValueWithMinMax analog[2];
FrskyValueWithMin rssi[2];
#if defined(CPUARM)
FrskyValueWithMinMax swr;
#endif
FrskySerialData hub;
};
// Global Fr-Sky telemetry data variables
extern uint8_t frskyStreaming; // >0 (true) == data is streaming in. 0 = nodata detected for some time
#if defined(WS_HOW_HIGH)
extern uint8_t frskyUsrStreaming;
#endif
#define SEND_RSSI_ALARMS 6
#define SEND_MODEL_ALARMS 4
extern uint8_t frskyAlarmsSendState;
extern FrskyData frskyData;
extern uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
extern uint8_t frskyTxBufferCount;
void FRSKY_Init(void);
#define FRSKY_End()
void telemetryWakeup(void);
void telemetryInterrupt10ms(void);
inline void frskySendAlarms(void)
{
frskyAlarmsSendState = SEND_RSSI_ALARMS;
}
bool FRSKY_alarmRaised(uint8_t idx);
void resetTelemetry();
#define TELEMETRY_STREAMING() (frskyStreaming > 0)
#define TELEMETRY_CELL_VOLTAGE(k) (frskyData.hub.cellVolts[k] * 2)
#define TELEMETRY_MIN_CELL_VOLTAGE (frskyData.hub.minCellVolts * 2)
#define TELEMETRY_BARO_ALT_AVAILABLE() (frskyData.hub.baroAltitudeOffset)
#define TELEMETRY_BARO_ALT_UNIT (IS_IMPERIAL_ENABLE() ? LENGTH_UNIT_IMP : LENGTH_UNIT_METR)
#define TELEMETRY_RELATIVE_BARO_ALT_BP frskyData.hub.baroAltitude_bp
#define TELEMETRY_RELATIVE_BARO_ALT_AP frskyData.hub.baroAltitude_ap
#define TELEMETRY_RELATIVE_GPS_ALT_BP frskyData.hub.gpsAltitude_bp
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_BARO_ALT_PREPARE()
#define TELEMETRY_BARO_ALT_FORMAT "%d,"
#define TELEMETRY_BARO_ALT_ARGS frskyData.hub.baroAltitude_bp,
#define TELEMETRY_GPS_ALT_FORMAT "%d,"
#define TELEMETRY_GPS_ALT_ARGS frskyData.hub.gpsAltitude_bp,
#define TELEMETRY_GPS_SPEED_UNIT (IS_IMPERIAL_ENABLE() ? SPEED_UNIT_IMP : SPEED_UNIT_METR)
#define TELEMETRY_GPS_SPEED_FORMAT "%d,"
#define TELEMETRY_GPS_SPEED_ARGS getConvertedTelemetryValue(frskyData.hub.gpsSpeed_bp, UNIT_KTS),
#if defined(CPUARM)
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, frskyData.hub.cellVolts[0]*2/100, frskyData.hub.cellVolts[0]*2%100, frskyData.hub.cellVolts[1]*2/100, frskyData.hub.cellVolts[1]*2%100, frskyData.hub.cellVolts[2]*2/100, frskyData.hub.cellVolts[2]*2%100, frskyData.hub.cellVolts[3]*2/100, frskyData.hub.cellVolts[3]*2%100, frskyData.hub.cellVolts[4]*2/100, frskyData.hub.cellVolts[4]*2%100, frskyData.hub.cellVolts[5]*2/100, frskyData.hub.cellVolts[5]*2%100, frskyData.hub.cellVolts[6]*2/100, frskyData.hub.cellVolts[6]*2%100, frskyData.hub.cellVolts[7]*2/100, frskyData.hub.cellVolts[7]*2%100, frskyData.hub.cellVolts[8]*2/100, frskyData.hub.cellVolts[8]*2%100, frskyData.hub.cellVolts[9]*2/100, frskyData.hub.cellVolts[9]*2%100, frskyData.hub.cellVolts[10]*2/100, frskyData.hub.cellVolts[10]*2%100, frskyData.hub.cellVolts[11]*2/100, frskyData.hub.cellVolts[11]*2%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,Cell 7,Cell 8,Cell 9,Cell 10,Cell 11,Cell 12,"
#else
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, frskyData.hub.cellVolts[0]*2/100, frskyData.hub.cellVolts[0]*2%100, frskyData.hub.cellVolts[1]*2/100, frskyData.hub.cellVolts[1]*2%100, frskyData.hub.cellVolts[2]*2/100, frskyData.hub.cellVolts[2]*2%100, frskyData.hub.cellVolts[3]*2/100, frskyData.hub.cellVolts[3]*2%100, frskyData.hub.cellVolts[4]*2/100, frskyData.hub.cellVolts[4]*2%100, frskyData.hub.cellVolts[5]*2/100, frskyData.hub.cellVolts[5]*2%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,"
#endif
#define TELEMETRY_CURRENT_FORMAT "%d.%02d,"
#define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 100, frskyData.hub.current % 100,
#define TELEMETRY_VFAS_FORMAT "%d.%d,"
#define TELEMETRY_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10,
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
#define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', frskyData.hub.varioSpeed / 100, frskyData.hub.varioSpeed % 100,
#if defined(FRSKY_HUB)
#define TELEMETRY_OPENXSENSOR() (frskyData.hub.openXsensor)
#else
#define TELEMETRY_OPENXSENSOR() (0)
#endif
#endif
/*
* 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.
*
*/
#ifndef FRSKY_H
#define FRSKY_H
#include <inttypes.h>
#define FRSKY_SPORT_BAUDRATE 57600
#define FRSKY_D_BAUDRATE 9600
#define FRSKY_SPORT_PACKET_SIZE 9
// 20 seconds
#define FRSKY_TIMEOUT10ms 20
// Global Fr-Sky telemetry data variables
extern uint8_t frskyStreaming; // >0 (true) == data is streaming in. 0 = nodata detected for some time
#if defined(WS_HOW_HIGH)
extern uint8_t frskyUsrStreaming;
#endif
extern uint8_t link_counter;
#if defined(CPUARM)
enum TelemetryStates {
TELEMETRY_INIT,
TELEMETRY_OK,
TELEMETRY_KO
};
extern uint8_t telemetryState;
#endif
class FrskyValueWithMin {
public:
uint8_t value;
uint8_t min;
uint16_t sum;
void set(uint8_t value);
};
class FrskyValueWithMinMax: public FrskyValueWithMin {
public:
uint8_t max;
void set(uint8_t value, uint8_t unit);
};
#if defined(CPUARM)
PACK(struct FrskySerialData {
int16_t spare1;
int16_t gpsAltitude_bp; // 0x01 before punct
int16_t temperature1; // 0x02 -20 .. 250 deg. celcius
uint16_t rpm; // 0x03 0..60,000 revs. per minute
uint16_t fuelLevel; // 0x04 0, 25, 50, 75, 100 percent
int16_t temperature2; // 0x05 -20 .. 250 deg. celcius
uint16_t volts; // 0x06 1/500V increments (0..4.2V)
uint32_t distFromEarthAxis;// 2 spares reused
int16_t gpsAltitude_ap; // 0x01+8 after punct
uint16_t spare2[6];
int16_t baroAltitude_bp; // 0x10 0..9,999 meters
uint16_t gpsSpeed_bp; // 0x11 before punct
uint16_t gpsLongitude_bp; // 0x12 before punct
uint16_t gpsLatitude_bp; // 0x13 before punct
uint16_t gpsCourse_bp; // 0x14 before punct (0..359.99 deg. -- seemingly 2-decimal precision)
uint8_t day; // 0x15
uint8_t month; // 0x15
uint16_t year; // 0x16
uint8_t hour; // 0x17
uint8_t min; // 0x17
uint16_t sec; // 0x18
uint16_t gpsSpeed_ap; // 0x11+8
uint16_t gpsLongitude_ap; // 0x12+8
uint16_t gpsLatitude_ap; // 0x13+8
uint16_t gpsCourse_ap; // 0x14+8
uint32_t pilotLatitude; // 2 spares reused
uint32_t pilotLongitude; // 2 spares reused
uint16_t baroAltitude_ap; // 0x21 after punct
uint16_t gpsLongitudeEW; // 0x1A+8 East/West
uint16_t gpsLatitudeNS; // 0x1B+8 North/South
int16_t accelX; // 0x24 1/256th gram (-8g ~ +8g)
int16_t accelY; // 0x25 1/256th gram (-8g ~ +8g)
int16_t accelZ; // 0x26 1/256th gram (-8g ~ +8g)
int16_t spare3;
uint16_t current; // 0x28 Current
int16_t spare4[7];
int16_t varioSpeed; // 0x30 Vertical speed in cm/s
int32_t baroAltitudeOffset;
int32_t baroAltitude;
int32_t gpsAltitudeOffset;
uint32_t gpsDistance;
uint16_t vfas; // 0x39 Added to FrSky protocol for home made sensors with a better precision
uint16_t volts_bp; // 0x3A
uint16_t volts_ap; // 0x3B
uint16_t spare5[4];
// end of FrSky Hub data
/* these fields must keep this order! */
int16_t minAltitude;
int16_t maxAltitude;
uint16_t maxRpm;
int16_t maxTemperature1;
int16_t maxTemperature2;
uint16_t maxGpsSpeed;
uint16_t maxGpsDistance;
uint16_t maxAirSpeed;
int16_t minCell;
int16_t minCells;
int16_t minVfas;
uint16_t maxCurrent;
uint16_t maxPower;
/* end */
uint8_t gpsDistNeeded:1; // 1bits out of 16bits spare reused
int8_t gpsFix:2; // 2bits out of 16bits spare reused: -1=never fixed, 0=not fixed now, 1=fixed
uint8_t openXsensor:1; // 1bits out of 16bits spare reused: we receive data from the openXsensor
uint8_t cellsCount:4; // 4bits out of 16bits spare reused
int32_t gpsAltitude;
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
uint16_t cellVolts[12];
int16_t cellsSum;
uint8_t minCellIdx;
uint16_t minCellVolts;
bool varioHighPrecision;
uint16_t airSpeed;
uint16_t dTE;
});
#elif defined(FRSKY_HUB)
PACK(struct FrskySerialData {
int16_t baroAltitudeOffset;// spare reused
int16_t gpsAltitude_bp; // 0x01 before punct
int16_t temperature1; // 0x02 -20 .. 250 deg. celcius
uint16_t rpm; // 0x03 0..60,000 revs. per minute
uint16_t fuelLevel; // 0x04 0, 25, 50, 75, 100 percent
int16_t temperature2; // 0x05 -20 .. 250 deg. celcius
uint16_t volts; // 0x06 1/500V increments (0..4.2V)
uint32_t distFromEarthAxis;// 2 spares reused
int16_t gpsAltitude_ap; // 0x01+8 after punct
uint8_t cellVolts[12]; // 6 spares reused
int16_t baroAltitude_bp; // 0x10 0..9,999 meters
uint16_t gpsSpeed_bp; // 0x11 before punct
uint16_t gpsLongitude_bp; // 0x12 before punct
uint16_t gpsLatitude_bp; // 0x13 before punct
uint16_t gpsCourse_bp; // 0x14 before punct (0..359.99 deg. -- seemingly 2-decimal precision)
uint8_t day; // 0x15
uint8_t month; // 0x15
uint16_t year; // 0x16
uint8_t hour; // 0x17
uint8_t min; // 0x17
uint16_t sec; // 0x18
uint16_t gpsSpeed_ap; // 0x11+8
uint16_t gpsLongitude_ap; // 0x12+8
uint16_t gpsLatitude_ap; // 0x13+8
uint16_t gpsCourse_ap; // 0x14+8
uint32_t pilotLatitude; // 2 spares reused
uint32_t pilotLongitude; // 2 spares reused
uint16_t baroAltitude_ap; // 0x21 after punct
uint16_t gpsLongitudeEW; // 0x1A+8 East/West
uint16_t gpsLatitudeNS; // 0x1B+8 North/South
int16_t accelX; // 0x24 1/256th gram (-8g ~ +8g)
int16_t accelY; // 0x25 1/256th gram (-8g ~ +8g)
int16_t accelZ; // 0x26 1/256th gram (-8g ~ +8g)
uint8_t gpsDistNeeded:1; // 1bits out of 16bits spare reused
int8_t gpsFix:2; // 2bits out of 16bits spare reused: -1=never fixed, 0=not fixed now, 1=fixed
uint8_t openXsensor:1; // 1bits out of 16bits spare reused: we receive data from the openXsensor
uint8_t cellsCount:4; // 4bits out of 16bits spare reused
uint8_t minCellVolts; // 8bits out of 16bits spare reused
uint16_t current; // 0x28 Current
int16_t spare[5];
int32_t varioAltitude_cm;
int16_t varioSpeed; // 0x30 Vertical speed in cm/s
uint16_t gpsDistance;
int16_t gpsAltitudeOffset;
uint8_t varioAltitudeQueuePointer; // circular-buffer pointer
uint8_t minCellIdx;
int16_t cellsSum;
uint16_t currentConsumption; // 0x35 openXsensor only! Otherwise calculated by the Tx from current
uint16_t currentPrescale;
uint16_t power; // 0x37 openXsensor only! Otherwise calculated by the Tx from current and voltage
int16_t airSpeed;
uint16_t vfas; // 0x39 Added to FrSky protocol for home made sensors with a better precision
uint16_t volts_bp; // 0x3A
uint16_t volts_ap; // 0x3B
// end of FrSky Hub data
/* next fields must keep this order! */
int16_t minAltitude;
int16_t maxAltitude;
uint16_t maxRpm;
int16_t maxTemperature1;
int16_t maxTemperature2;
uint16_t maxGpsSpeed;
uint16_t maxGpsDistance;
uint16_t maxAirSpeed;
int16_t minCell;
int16_t minCells;
int16_t minVfas;
uint16_t maxCurrent;
uint16_t maxPower;
/* end */
int16_t dTE;
});
#elif defined(WS_HOW_HIGH)
PACK(struct FrskySerialData {
int16_t baroAltitude_bp; // 0..9,999 meters
int16_t baroAltitudeOffset;
int16_t minAltitude;
int16_t maxAltitude;
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
uint16_t maxPower;
#if defined(VARIO)
int16_t varioSpeed; // Vertical speed in cm/s
#endif
});
#elif defined(VARIO)
PACK(struct FrskySerialData {
int16_t varioSpeed; // Vertical speed in cm/s
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
uint16_t maxPower;
});
#else
PACK(struct FrskySerialData {
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
uint16_t maxPower;
});
#endif
struct FrskyData {
FrskyValueWithMinMax analog[2];
FrskyValueWithMin rssi[2];
#if defined(CPUARM)
FrskyValueWithMin swr;
#endif
FrskySerialData hub;
};
enum AlarmLevel {
alarm_off = 0,
alarm_yellow = 1,
alarm_orange = 2,
alarm_red = 3
};
#define ALARM_GREATER(channel, alarm) ((g_model.frsky.channels[channel].alarms_greater >> alarm) & 1)
#define ALARM_LEVEL(channel, alarm) ((g_model.frsky.channels[channel].alarms_level >> (2*alarm)) & 3)
#if defined(CPUARM)
#define TELEMETRY_STREAMING() (frskyData.rssi[0].value > 0)
#define TELEMETRY_CELL_VOLTAGE_MUTLIPLIER 1
#define TELEMETRY_BARO_ALT_AVAILABLE() (frskyData.hub.baroAltitudeOffset)
#define TELEMETRY_BARO_ALT_UNIT (IS_IMPERIAL_ENABLE() ? LENGTH_UNIT_IMP : LENGTH_UNIT_METR)
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_ABSOLUTE_GPS_ALT (frskyData.hub.gpsAltitude)
#define TELEMETRY_RELATIVE_GPS_ALT (frskyData.hub.gpsAltitude + frskyData.hub.gpsAltitudeOffset)
#define TELEMETRY_RELATIVE_GPS_ALT_BP (TELEMETRY_RELATIVE_GPS_ALT / 100)
#define TELEMETRY_RELATIVE_BARO_ALT_BP (frskyData.hub.baroAltitude / 100)
#define TELEMETRY_RELATIVE_BARO_ALT_AP (frskyData.hub.baroAltitude % 100)
#define TELEMETRY_BARO_ALT_PREPARE() div_t baroAltitudeDivision = div(getConvertedTelemetryValue(frskyData.hub.baroAltitude, UNIT_DIST), 100)
#define TELEMETRY_BARO_ALT_FORMAT "%c%d.%02d,"
#define TELEMETRY_BARO_ALT_ARGS frskyData.hub.baroAltitude < 0 ? '-' : ' ', abs(baroAltitudeDivision.quot), abs(baroAltitudeDivision.rem),
#define TELEMETRY_GPS_ALT_FORMAT "%c%d.%02d,"
#define TELEMETRY_GPS_ALT_ARGS frskyData.hub.gpsAltitude < 0 ? '-' : ' ', abs(frskyData.hub.gpsAltitude / 100), abs(frskyData.hub.gpsAltitude % 100),
#define TELEMETRY_GPS_SPEED_UNIT (IS_IMPERIAL_ENABLE() ? SPEED_UNIT_IMP : SPEED_UNIT_METR)
#define TELEMETRY_GPS_SPEED_FORMAT "%d,"
#define TELEMETRY_GPS_SPEED_ARGS getConvertedTelemetryValue(frskyData.hub.gpsSpeed_bp, UNIT_KTS),
#if defined(CPUARM)
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100, TELEMETRY_CELL_VOLTAGE(6)/100, TELEMETRY_CELL_VOLTAGE(6)%100, TELEMETRY_CELL_VOLTAGE(7)/100, TELEMETRY_CELL_VOLTAGE(7)%100, TELEMETRY_CELL_VOLTAGE(8)/100, TELEMETRY_CELL_VOLTAGE(8)%100, TELEMETRY_CELL_VOLTAGE(9)/100, TELEMETRY_CELL_VOLTAGE(9)%100, TELEMETRY_CELL_VOLTAGE(10)/100, TELEMETRY_CELL_VOLTAGE(10)%100, TELEMETRY_CELL_VOLTAGE(11)/100, TELEMETRY_CELL_VOLTAGE(11)%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,Cell 7,Cell 8,Cell 9,Cell 10,Cell 11,Cell 12,"
#else
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,"
#endif
#define TELEMETRY_CURRENT_FORMAT "%d.%d,"
#define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 10, frskyData.hub.current % 10,
#define TELEMETRY_VFAS_FORMAT "%d.%d,"
#define TELEMETRY_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10,
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
#define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', abs(frskyData.hub.varioSpeed / 100), abs(frskyData.hub.varioSpeed % 100),
#define TELEMETRY_OPENXSENSOR() (0)
#else
#define TELEMETRY_STREAMING() (frskyStreaming > 0)
#define TELEMETRY_CELL_VOLTAGE_MUTLIPLIER 2
#define TELEMETRY_BARO_ALT_AVAILABLE() (frskyData.hub.baroAltitudeOffset)
#define TELEMETRY_BARO_ALT_UNIT (IS_IMPERIAL_ENABLE() ? LENGTH_UNIT_IMP : LENGTH_UNIT_METR)
#define TELEMETRY_RELATIVE_BARO_ALT_BP frskyData.hub.baroAltitude_bp
#define TELEMETRY_RELATIVE_BARO_ALT_AP frskyData.hub.baroAltitude_ap
#define TELEMETRY_RELATIVE_GPS_ALT_BP frskyData.hub.gpsAltitude_bp
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_BARO_ALT_PREPARE()
#define TELEMETRY_BARO_ALT_FORMAT "%d,"
#define TELEMETRY_BARO_ALT_ARGS frskyData.hub.baroAltitude_bp,
#define TELEMETRY_GPS_ALT_FORMAT "%d,"
#define TELEMETRY_GPS_ALT_ARGS frskyData.hub.gpsAltitude_bp,
#define TELEMETRY_GPS_SPEED_UNIT (IS_IMPERIAL_ENABLE() ? SPEED_UNIT_IMP : SPEED_UNIT_METR)
#define TELEMETRY_GPS_SPEED_FORMAT "%d,"
#define TELEMETRY_GPS_SPEED_ARGS getConvertedTelemetryValue(frskyData.hub.gpsSpeed_bp, UNIT_KTS),
#if defined(CPUARM)
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, frskyData.hub.cellVolts[0]*2/100, frskyData.hub.cellVolts[0]*2%100, frskyData.hub.cellVolts[1]*2/100, frskyData.hub.cellVolts[1]*2%100, frskyData.hub.cellVolts[2]*2/100, frskyData.hub.cellVolts[2]*2%100, frskyData.hub.cellVolts[3]*2/100, frskyData.hub.cellVolts[3]*2%100, frskyData.hub.cellVolts[4]*2/100, frskyData.hub.cellVolts[4]*2%100, frskyData.hub.cellVolts[5]*2/100, frskyData.hub.cellVolts[5]*2%100, frskyData.hub.cellVolts[6]*2/100, frskyData.hub.cellVolts[6]*2%100, frskyData.hub.cellVolts[7]*2/100, frskyData.hub.cellVolts[7]*2%100, frskyData.hub.cellVolts[8]*2/100, frskyData.hub.cellVolts[8]*2%100, frskyData.hub.cellVolts[9]*2/100, frskyData.hub.cellVolts[9]*2%100, frskyData.hub.cellVolts[10]*2/100, frskyData.hub.cellVolts[10]*2%100, frskyData.hub.cellVolts[11]*2/100, frskyData.hub.cellVolts[11]*2%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,Cell 7,Cell 8,Cell 9,Cell 10,Cell 11,Cell 12,"
#else
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, frskyData.hub.cellVolts[0]*2/100, frskyData.hub.cellVolts[0]*2%100, frskyData.hub.cellVolts[1]*2/100, frskyData.hub.cellVolts[1]*2%100, frskyData.hub.cellVolts[2]*2/100, frskyData.hub.cellVolts[2]*2%100, frskyData.hub.cellVolts[3]*2/100, frskyData.hub.cellVolts[3]*2%100, frskyData.hub.cellVolts[4]*2/100, frskyData.hub.cellVolts[4]*2%100, frskyData.hub.cellVolts[5]*2/100, frskyData.hub.cellVolts[5]*2%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,"
#endif
#define TELEMETRY_CURRENT_FORMAT "%d.%02d,"
#define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 100, frskyData.hub.current % 100,
#define TELEMETRY_VFAS_FORMAT "%d.%d,"
#define TELEMETRY_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10,
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
#define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', frskyData.hub.varioSpeed / 100, frskyData.hub.varioSpeed % 100,
#if defined(FRSKY_HUB)
#define TELEMETRY_OPENXSENSOR() (frskyData.hub.openXsensor)
#else
#define TELEMETRY_OPENXSENSOR() (0)
#endif
#endif
#define TELEMETRY_CELL_VOLTAGE(k) (frskyData.hub.cellVolts[k] * TELEMETRY_CELL_VOLTAGE_MUTLIPLIER)
#define TELEMETRY_MIN_CELL_VOLTAGE (frskyData.hub.minCellVolts * TELEMETRY_CELL_VOLTAGE_MUTLIPLIER)
extern FrskyData frskyData;
#define START_STOP 0x7e
#define BYTESTUFF 0x7d
#define STUFF_MASK 0x20
// Receive buffer state machine state enum
enum FrSkyDataState {
STATE_DATA_IDLE,
STATE_DATA_START,
STATE_DATA_IN_FRAME,
STATE_DATA_XOR,
#if defined(TELEMETREZ)
STATE_DATA_PRIVATE_LEN,
STATE_DATA_PRIVATE_VALUE
#endif
};
#if defined(PCBTARANIS)
#define frskySendAlarms()
#else
#define SEND_RSSI_ALARMS 6
#define SEND_MODEL_ALARMS 4
extern uint8_t frskyAlarmsSendState;
#define FRSKY_TX_PACKET_SIZE 12
extern uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
extern uint8_t frskyTxBufferCount;
inline void frskySendAlarms(void)
{
frskyAlarmsSendState = SEND_RSSI_ALARMS;
}
#endif
// FrSky D Protocol
void frskyDSendNextAlarm(void);
void frskyDProcessPacket(uint8_t *packet);
// FrSky S.PORT Protocol
void frskySportProcessPacket(uint8_t *packet);
void telemetryWakeup(void);
void telemetryReset();
void telemetryInit(void);
void telemetryInterrupt10ms(void);
#endif
+472
View File
@@ -0,0 +1,472 @@
/*
* 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"
// Enumerate FrSky packet codes
#define LINKPKT 0xfe
#define USRPKT 0xfd
#define A11PKT 0xfc
#define A12PKT 0xfb
#define A21PKT 0xfa
#define A22PKT 0xf9
#define ALRM_REQUEST 0xf8
#define RSSI1PKT 0xf7
#define RSSI2PKT 0xf6
#define RSSI_REQUEST 0xf1
#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH)
void checkMinMaxAltitude()
{
if (TELEMETRY_RELATIVE_BARO_ALT_BP > frskyData.hub.maxAltitude)
frskyData.hub.maxAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP;
if (TELEMETRY_RELATIVE_BARO_ALT_BP < frskyData.hub.minAltitude)
frskyData.hub.minAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP;
}
#endif
#if defined(FRSKY_HUB)
typedef enum {
TS_IDLE = 0, // waiting for 0x5e frame marker
TS_DATA_ID, // waiting for dataID
TS_DATA_LOW, // waiting for data low byte
TS_DATA_HIGH, // waiting for data high byte
TS_XOR = 0x80 // decode stuffed byte
} TS_STATE;
void parseTelemHubByte(uint8_t byte)
{
static int8_t structPos;
static uint8_t lowByte;
static TS_STATE state = TS_IDLE;
if (byte == 0x5e) {
state = TS_DATA_ID;
return;
}
if (state == TS_IDLE) {
return;
}
if (state & TS_XOR) {
byte = byte ^ 0x60;
state = (TS_STATE)(state - TS_XOR);
}
if (byte == 0x5d) {
state = (TS_STATE)(state | TS_XOR);
return;
}
if (state == TS_DATA_ID) {
if (byte > 0x3f) {
state = TS_IDLE;
}
else {
structPos = byte*2;
state = TS_DATA_LOW;
}
return;
}
if (state == TS_DATA_LOW) {
lowByte = byte;
state = TS_DATA_HIGH;
return;
}
state = TS_IDLE;
#if defined(GPS)
if ((uint8_t)structPos == offsetof(FrskySerialData, gpsLatitude_bp)) {
if (lowByte || byte)
frskyData.hub.gpsFix = 1;
else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLatitude_bp > 1)
frskyData.hub.gpsFix = 0;
}
else if ((uint8_t)structPos == offsetof(FrskySerialData, gpsLongitude_bp)) {
if (lowByte || byte)
frskyData.hub.gpsFix = 1;
else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLongitude_bp > 1)
frskyData.hub.gpsFix = 0;
}
if ((uint8_t)structPos == offsetof(FrskySerialData, gpsAltitude_bp) ||
((uint8_t)structPos >= offsetof(FrskySerialData, gpsAltitude_ap) && (uint8_t)structPos <= offsetof(FrskySerialData, gpsLatitudeNS) && (uint8_t)structPos != offsetof(FrskySerialData, baroAltitude_bp) && (uint8_t)structPos != offsetof(FrskySerialData, baroAltitude_ap))) {
// If we don't have a fix, we may discard the value
if (frskyData.hub.gpsFix <= 0)
return;
}
#endif
#if 0
uint16_t previousValue = *((uint16_t *)(((uint8_t*)&frskyData.hub) + structPos));
#endif
((uint8_t*)&frskyData.hub)[structPos] = lowByte;
((uint8_t*)&frskyData.hub)[structPos+1] = byte;
switch ((uint8_t)structPos) {
case offsetof(FrskySerialData, rpm):
frskyData.hub.rpm *= (uint8_t)60/(g_model.frsky.blades+2);
if (frskyData.hub.rpm > frskyData.hub.maxRpm)
frskyData.hub.maxRpm = frskyData.hub.rpm;
break;
case offsetof(FrskySerialData, temperature1):
if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1)
frskyData.hub.maxTemperature1 = frskyData.hub.temperature1;
break;
case offsetof(FrskySerialData, temperature2):
if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2)
frskyData.hub.maxTemperature2 = frskyData.hub.temperature2;
break;
case offsetof(FrskySerialData, current):
#if defined(FAS_OFFSET) || !defined(CPUM64)
if ((int16_t)frskyData.hub.current > 0 && ((int16_t)frskyData.hub.current + g_model.frsky.fasOffset) > 0)
frskyData.hub.current += g_model.frsky.fasOffset;
else
frskyData.hub.current = 0;
#endif
if (frskyData.hub.current > frskyData.hub.maxCurrent)
frskyData.hub.maxCurrent = frskyData.hub.current;
break;
case offsetof(FrskySerialData, currentConsumption):
// we receive data from openXsensor. stops the calculation of consumption and power
frskyData.hub.openXsensor = 1;
break;
case offsetof(FrskySerialData, volts_ap):
#if defined(FAS_BSS)
frskyData.hub.vfas = (frskyData.hub.volts_bp * 10 + frskyData.hub.volts_ap);
#else
frskyData.hub.vfas = ((frskyData.hub.volts_bp * 100 + frskyData.hub.volts_ap * 10) * 21) / 110;
#endif
/* TODO later if (!frskyData.hub.minVfas || frskyData.hub.minVfas > frskyData.hub.vfas)
frskyData.hub.minVfas = frskyData.hub.vfas; */
break;
case offsetof(FrskySerialData, baroAltitude_bp):
// First received barometer altitude => Altitude offset
if (!frskyData.hub.baroAltitudeOffset)
frskyData.hub.baroAltitudeOffset = -frskyData.hub.baroAltitude_bp;
frskyData.hub.baroAltitude_bp += frskyData.hub.baroAltitudeOffset;
checkMinMaxAltitude();
break;
#if defined(GPS)
case offsetof(FrskySerialData, gpsAltitude_ap):
if (!frskyData.hub.gpsAltitudeOffset)
frskyData.hub.gpsAltitudeOffset = -frskyData.hub.gpsAltitude_bp;
frskyData.hub.gpsAltitude_bp += frskyData.hub.gpsAltitudeOffset;
if (!frskyData.hub.baroAltitudeOffset) {
if (frskyData.hub.gpsAltitude_bp > frskyData.hub.maxAltitude)
frskyData.hub.maxAltitude = frskyData.hub.gpsAltitude_bp;
if (frskyData.hub.gpsAltitude_bp < frskyData.hub.minAltitude)
frskyData.hub.minAltitude = frskyData.hub.gpsAltitude_bp;
}
if (!frskyData.hub.pilotLatitude && !frskyData.hub.pilotLongitude) {
// First received GPS position => Pilot GPS position
getGpsPilotPosition();
}
else if (frskyData.hub.gpsDistNeeded || g_menuStack[g_menuStackPtr] == menuTelemetryFrsky) {
getGpsDistance();
}
break;
case offsetof(FrskySerialData, gpsSpeed_bp):
// Speed => Max speed
if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
break;
#endif
case offsetof(FrskySerialData, volts):
// Voltage => Cell number + Cell voltage
{
uint8_t battnumber = ((frskyData.hub.volts & 0x00F0) >> 4);
if (battnumber < 12) {
if (frskyData.hub.cellsCount < battnumber+1) {
frskyData.hub.cellsCount = battnumber+1;
}
uint8_t cellVolts = (uint8_t)(((((frskyData.hub.volts & 0xFF00) >> 8) + ((frskyData.hub.volts & 0x000F) << 8)))/10);
frskyData.hub.cellVolts[battnumber] = cellVolts;
if (!frskyData.hub.minCellVolts || cellVolts < frskyData.hub.minCellVolts || battnumber==frskyData.hub.minCellIdx) {
frskyData.hub.minCellIdx = battnumber;
frskyData.hub.minCellVolts = cellVolts;
}
}
break;
}
#if defined(GPS)
case offsetof(FrskySerialData, hour):
frskyData.hub.hour = ((uint8_t)(frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
break;
#endif
case offsetof(FrskySerialData, accelX):
case offsetof(FrskySerialData, accelY):
case offsetof(FrskySerialData, accelZ):
*(int16_t*)(&((uint8_t*)&frskyData.hub)[structPos]) /= 10;
break;
#if 0
case offsetof(FrskySerialData, gpsAltitude_bp):
case offsetof(FrskySerialData, fuelLevel):
case offsetof(FrskySerialData, gpsLongitude_bp):
case offsetof(FrskySerialData, gpsLatitude_bp):
case offsetof(FrskySerialData, gpsCourse_bp):
case offsetof(FrskySerialData, day):
case offsetof(FrskySerialData, year):
case offsetof(FrskySerialData, sec):
case offsetof(FrskySerialData, gpsSpeed_ap):
case offsetof(FrskySerialData, gpsLongitude_ap):
case offsetof(FrskySerialData, gpsLatitude_ap):
case offsetof(FrskySerialData, gpsCourse_ap):
case offsetof(FrskySerialData, gpsLongitudeEW):
case offsetof(FrskySerialData, gpsLatitudeNS):
case offsetof(FrskySerialData, varioSpeed):
case offsetof(FrskySerialData, power): /* because sent by openXsensor */
case offsetof(FrskySerialData, vfas):
case offsetof(FrskySerialData, volts_bp):
break;
default:
*((uint16_t *)(((uint8_t*)&frskyData.hub) + structPos)) = previousValue;
break;
#endif
}
}
#endif
#if defined(WS_HOW_HIGH)
void parseTelemWSHowHighByte(uint8_t byte)
{
if (frskyUsrStreaming < (FRSKY_TIMEOUT10ms*3 - 10)) {
((uint8_t*)&frskyData.hub)[offsetof(FrskySerialData, baroAltitude_bp)] = byte;
checkMinMaxAltitude();
}
else {
// At least 100mS passed since last data received
((uint8_t*)&frskyData.hub)[offsetof(FrskySerialData, baroAltitude_bp)+1] = byte;
}
// baroAltitude_bp unit here is feet!
frskyUsrStreaming = FRSKY_TIMEOUT10ms*3; // reset counter
}
#endif
void frskyDProcessPacket(uint8_t *packet)
{
// What type of packet?
switch (packet[0])
{
case LINKPKT: // A1/A2/RSSI values
{
link_counter += 32;
frskyData.analog[0].set(packet[1], g_model.frsky.channels[0].type);
frskyData.analog[1].set(packet[2], g_model.frsky.channels[1].type);
frskyData.rssi[0].set(packet[3]);
frskyData.rssi[1].set(packet[4] / 2);
frskyStreaming = FRSKY_TIMEOUT10ms; // reset counter only if valid frsky packets are being detected
#if defined(VARIO)
uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1;
if (varioSource < 2)
frskyData.hub.varioSpeed = applyChannelRatio(varioSource, frskyData.analog[varioSource].value);
#endif
break;
}
#if defined(FRSKY_HUB) || defined (WS_HOW_HIGH)
case USRPKT: // User Data packet
uint8_t numBytes = 3 + (packet[1] & 0x07); // sanitize in case of data corruption leading to buffer overflow
for (uint8_t i=3; i<numBytes; i++) {
#if defined(FRSKY_HUB)
if (IS_USR_PROTO_FRSKY_HUB())
parseTelemHubByte(packet[i]);
#endif
#if defined(WS_HOW_HIGH)
if (IS_USR_PROTO_WS_HOW_HIGH())
parseTelemWSHowHighByte(packet[i]);
#endif
}
break;
#endif
}
}
#if !defined(PCBTARANIS)
// Alarms level sent to the FrSky module
uint8_t frskyAlarmsSendState = 0 ;
#if defined(CPUARM)
void frskyPushValue(uint8_t *&ptr, uint8_t value)
{
// byte stuff the only byte than might need it
if (value == START_STOP) {
*ptr++ = BYTESTUFF;
*ptr++ = 0x5e;
}
else if (value == BYTESTUFF) {
*ptr++ = BYTESTUFF;
*ptr++ = 0x5d;
}
else {
*ptr++ = value;
}
}
void frskyDSendNextAlarm(void)
{
#if defined(PCBSKY9X)
if (telemetryTransmitPending()) {
return; // we only have one buffer. If it's in use, then we can't send yet.
}
#endif
uint8_t *ptr = &frskyTxBuffer[0];
*ptr++ = START_STOP; // Start of packet
// Now send a packet
frskyAlarmsSendState -= 1;
uint8_t alarm = 1 - (frskyAlarmsSendState % 2);
if (frskyAlarmsSendState < SEND_MODEL_ALARMS) {
uint8_t channel = 1 - (frskyAlarmsSendState / 2);
*ptr++ = (A22PKT + frskyAlarmsSendState); // fc - fb - fa - f9
frskyPushValue(ptr, g_model.frsky.channels[channel].alarms_value[alarm]);
*ptr++ = ALARM_GREATER(channel, alarm);
*ptr++ = (IS_SOUND_OFF() ? alarm_off : ALARM_LEVEL(channel, alarm));
}
else {
*ptr++ = (RSSI1PKT-alarm); // f7 - f6
frskyPushValue(ptr, getRssiAlarmValue(alarm));
*ptr++ = 0x00;
*ptr++ = (IS_SOUND_OFF() ? alarm_off : ((2+alarm+g_model.frsky.rssiAlarms[alarm].level) % 4));
}
*ptr++ = 0x00;
*ptr++ = 0x00;
*ptr++ = 0x00;
*ptr++ = 0x00;
*ptr++ = 0x00;
*ptr++ = START_STOP; // End of packet
telemetryTransmitBuffer(frskyTxBuffer, ptr - &frskyTxBuffer[0]);
}
#else
void frskyPushValue(uint8_t *&ptr, uint8_t value)
{
// byte stuff the only byte than might need it
bool bytestuff = false;
if (value == START_STOP) {
bytestuff = true;
value = 0x5e;
}
else if (value == BYTESTUFF) {
bytestuff = true;
value = 0x5d;
}
*ptr++ = value;
if (bytestuff)
*ptr = BYTESTUFF;
}
void frskySendPacket(uint8_t type, uint8_t value, uint8_t p1, uint8_t p2)
{
uint8_t *ptr = &frskyTxBuffer[0];
*ptr++ = START_STOP; // End of packet
*ptr++ = 0x00;
*ptr++ = 0x00;
*ptr++ = 0x00;
*ptr++ = 0x00;
*ptr++ = 0x00;
*ptr++ = (IS_SOUND_OFF() ? alarm_off : p2);
*ptr++ = p1;
frskyPushValue(ptr, value);
*ptr++ = type;
*ptr++ = START_STOP; // Start of packet
frskyTxBufferCount = ptr - &frskyTxBuffer[0];
telemetryTransmitBuffer();
}
inline void frskyDSendNextAlarm(void)
{
if (frskyTxBufferCount)
return; // we only have one buffer. If it's in use, then we can't send yet.
// Now send a packet
frskyAlarmsSendState -= 1;
uint8_t alarm = 1 - (frskyAlarmsSendState % 2);
if (frskyAlarmsSendState < SEND_MODEL_ALARMS) {
uint8_t channel = 1 - (frskyAlarmsSendState / 2);
frskySendPacket(A22PKT + frskyAlarmsSendState, g_model.frsky.channels[channel].alarms_value[alarm], ALARM_GREATER(channel, alarm), ALARM_LEVEL(channel, alarm));
}
else {
frskySendPacket(RSSI1PKT-alarm, getRssiAlarmValue(alarm), 0, (2+alarm+g_model.frsky.rssiAlarms[alarm].level) % 4);
}
}
#endif
#endif
// checks if an alarm is raised, not used any more
#if 0
bool isFrskyAlarmRaised(uint8_t idx)
{
for (int i=0; i<2; i++) {
if (ALARM_LEVEL(idx, i) != alarm_off) {
if (ALARM_GREATER(idx, i)) {
if (frskyData.analog[idx].value > g_model.frsky.channels[idx].alarms_value[i])
return true;
}
else {
if (frskyData.analog[idx].value < g_model.frsky.channels[idx].alarms_value[i])
return true;
}
}
}
return false;
}
#endif
+6 -316
View File
@@ -36,10 +36,6 @@
#include "../opentx.h"
#define START_STOP 0x7e
#define BYTESTUFF 0x7d
#define STUFF_MASK 0x20
// FrSky PRIM IDs (1 byte)
#define DATA_FRAME 0x10
@@ -126,22 +122,6 @@
#define BETA_VARIO_ID 0x8030
#define BETA_BARO_ALT_ID 0x8010
uint8_t telemetryState = TELEMETRY_INIT;
uint8_t frskyRxBuffer[FRSKY_RX_PACKET_SIZE]; // Receive buffer. 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE]; // Ditto for transmit buffer
#if !defined(CPUARM)
uint8_t frskyTxBufferCount = 0;
#endif
uint8_t link_counter = 0;
FrskyData frskyData;
#if defined(DEBUG) && !defined(SIMU)
extern FIL g_telemetryFile;
#endif
Fifo<512> telemetryFifo;
void setBaroAltitude(int32_t baroAltitude)
{
// First received barometer altitude => Altitude offset
@@ -294,7 +274,7 @@ void processHubPacket(uint8_t id, uint16_t value)
bool checkSportPacket(uint8_t *packet)
{
short crc = 0;
for (int i=1; i<FRSKY_RX_PACKET_SIZE; i++) {
for (int i=1; i<FRSKY_SPORT_PACKET_SIZE; i++) {
crc += packet[i]; //0-1FF
crc += crc >> 8; //0-100
crc &= 0x00ff;
@@ -309,7 +289,7 @@ bool checkSportPacket(uint8_t *packet)
#define SPORT_DATA_U32(packet) (*((uint32_t *)(packet+4)))
#define HUB_DATA_U16(packet) (*((uint16_t *)(packet+4)))
void processSportPacket(uint8_t *packet)
void frskySportProcessPacket(uint8_t *packet)
{
/* uint8_t dataId = packet[0]; */
uint8_t prim = packet[1];
@@ -324,6 +304,7 @@ void processSportPacket(uint8_t *packet)
case DATA_FRAME:
if (appId == RSSI_ID) {
link_counter += 32;
frskyData.rssi[0].set(SPORT_DATA_U8(packet));
}
if (appId == SWR_ID) {
@@ -331,7 +312,8 @@ void processSportPacket(uint8_t *packet)
}
else if (appId == ADC1_ID || appId == ADC2_ID) {
// A1/A2 of DxR receivers
frskyData.analog[appId-ADC1_ID].set(SPORT_DATA_U8(packet));
uint8_t idx = appId - ADC1_ID;
frskyData.analog[idx].set(SPORT_DATA_U8(packet), g_model.frsky.channels[idx].type);
#if defined(VARIO)
uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1;
if (varioSource == appId-ADC1_ID) {
@@ -340,7 +322,7 @@ void processSportPacket(uint8_t *packet)
#endif
}
else if (appId == BATT_ID) {
frskyData.analog[0].set(SPORT_DATA_U8(packet));
frskyData.analog[0].set(SPORT_DATA_U8(packet), g_model.frsky.channels[0].type);
}
else if ((appId >> 8) == 0) {
// The old FrSky IDs
@@ -523,295 +505,3 @@ void processSportPacket(uint8_t *packet)
}
}
// Receive buffer state machine state enum
enum FrSkyDataState {
STATE_DATA_IDLE,
STATE_DATA_IN_FRAME,
STATE_DATA_XOR,
};
void processSerialData(uint8_t data)
{
static uint8_t numPktBytes = 0;
static uint8_t dataState = STATE_DATA_IDLE;
#if defined(BLUETOOTH)
// TODO if (g_model.bt_telemetry)
btPushByte(data);
#endif
#if defined(PCBTARANIS)
if (g_eeGeneral.uart3Mode == UART_MODE_SPORT) {
uart3Putc(data);
}
#endif
if (data == START_STOP) {
dataState = STATE_DATA_IN_FRAME;
numPktBytes = 0;
}
else {
switch (dataState) {
case STATE_DATA_XOR:
frskyRxBuffer[numPktBytes++] = data ^ STUFF_MASK;
dataState = STATE_DATA_IN_FRAME;
break;
case STATE_DATA_IN_FRAME:
if (data == BYTESTUFF)
dataState = STATE_DATA_XOR; // XOR next byte
else
frskyRxBuffer[numPktBytes++] = data;
break;
}
}
if (numPktBytes == FRSKY_RX_PACKET_SIZE) {
processSportPacket(frskyRxBuffer);
dataState = STATE_DATA_IDLE;
}
}
uint8_t frskyAlarmsSendState = 0 ;
void telemetryInterrupt10ms()
{
uint16_t voltage = 0; /* unit: 1/10 volts */
#if defined(FRSKY_HUB)
for (uint8_t i=0; i<frskyData.hub.cellsCount; i++)
voltage += frskyData.hub.cellVolts[i];
voltage /= 10;
frskyData.hub.cellsSum = voltage;
if (frskyData.hub.cellsSum < frskyData.hub.minCells) {
frskyData.hub.minCells = frskyData.hub.cellsSum;
}
#endif
if (TELEMETRY_STREAMING()) {
uint8_t channel = g_model.frsky.voltsSource;
if (channel <= 1) {
voltage = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
}
#if defined(FRSKY_HUB)
else if (channel == 2) {
voltage = frskyData.hub.vfas;
}
#endif
#if defined(FRSKY_HUB)
uint16_t current = frskyData.hub.current; /* unit: 1/10 amps */
#else
uint16_t current = 0;
#endif
channel = g_model.frsky.currentSource - FRSKY_CURRENT_SOURCE_A1;
if (channel <= 1) {
current = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
}
frskyData.hub.power = (current * voltage) / 100;
if (frskyData.hub.power > frskyData.hub.maxPower)
frskyData.hub.maxPower = frskyData.hub.power;
frskyData.hub.currentPrescale += current;
if (frskyData.hub.currentPrescale >= 3600) {
frskyData.hub.currentConsumption += 1;
frskyData.hub.currentPrescale -= 3600;
}
}
}
inline bool alarmRaised(uint8_t channel, uint8_t idx)
{
return g_model.frsky.channels[channel].ratio > 0 && g_model.frsky.channels[channel].alarms_value[idx] > 0 && frskyData.analog[channel].value < g_model.frsky.channels[channel].alarms_value[idx];
}
void telemetryWakeup()
{
#if defined(PCBTARANIS)
uint8_t data;
#if defined(DEBUG) && !defined(SIMU)
static tmr10ms_t lastTime = 0;
tmr10ms_t newTime = get_tmr10ms();
struct gtm utm;
gettime(&utm);
#endif
while (telemetryFifo.pop(data)) {
processSerialData(data);
#if defined(DEBUG) && !defined(SIMU)
if (lastTime != newTime) {
f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0: %02X", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100, data);
lastTime = newTime;
}
else {
f_printf(&g_telemetryFile, " %02X", data);
}
#endif
}
#elif defined(PCBSKY9X)
rxPdcUsart(processSerialData); // Receive serial data here
#endif
#if 0
// Attempt to transmit any waiting Fr-Sky alarm set packets every 50ms (subject to packet buffer availability)
static uint8_t frskyTxDelay = 5;
if (frskyAlarmsSendState && (--frskyTxDelay == 0)) {
frskyTxDelay = 5; // 50ms
// frskySendNextAlarm();
}
#endif
#if defined(VARIO)
if (TELEMETRY_STREAMING() && !IS_FAI_ENABLED()) {
varioWakeup();
}
#endif
static tmr10ms_t alarmsCheckTime = 0;
static uint8_t alarmsCheckStep = 0;
if (int32_t(get_tmr10ms() - alarmsCheckTime) > 0) {
alarmsCheckTime = get_tmr10ms() + 100; /* next check in 1second */
if (alarmsCheckStep == 0) {
#if defined(PCBTARANIS)
// TODO not only Taranis!
if ((IS_MODULE_XJT(0) || IS_MODULE_XJT(1)) && frskyData.swr.value > 0x33) {
AUDIO_SWR_RED();
POPUP_WARNING(STR_ANTENNAPROBLEM);
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
#endif
}
else if (TELEMETRY_STREAMING()) {
if (alarmsCheckStep == 1) {
if (getRssiAlarmValue(1) && frskyData.rssi[0].value < getRssiAlarmValue(1)) {
AUDIO_RSSI_RED();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
else if (getRssiAlarmValue(0) && frskyData.rssi[0].value < getRssiAlarmValue(0)) {
AUDIO_RSSI_ORANGE();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
}
else if (alarmsCheckStep == 2) {
if (alarmRaised(1, 1)) {
AUDIO_A2_RED();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
else if (alarmRaised(1, 0)) {
AUDIO_A2_ORANGE();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
}
else if (alarmsCheckStep == 3) {
if (alarmRaised(0, 1)) {
AUDIO_A1_RED();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
else if (alarmRaised(0, 0)) {
AUDIO_A1_ORANGE();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
}
}
if (++alarmsCheckStep == 4) {
alarmsCheckStep = 0;
}
}
if (TELEMETRY_STREAMING()) {
if (telemetryState == TELEMETRY_KO) {
AUDIO_TELEMETRY_BACK();
}
telemetryState = TELEMETRY_OK;
}
else if (telemetryState == TELEMETRY_OK) {
telemetryState = TELEMETRY_KO;
AUDIO_TELEMETRY_LOST();
}
}
void FRSKY_Init(void)
{
// clear frsky variables
resetTelemetry();
}
void FRSKY_End(void)
{
}
void FrskyValueWithMinMax::set(uint8_t value)
{
this->value = value;
if (value) {
if (!max || value > max)
max = value;
if (!min || value < min)
min = value;
}
}
void resetTelemetry()
{
memclear(&frskyData, sizeof(frskyData));
telemetryState = TELEMETRY_INIT;
#if defined(FRSKY_HUB)
frskyData.hub.gpsLatitude_bp = 2;
frskyData.hub.gpsLongitude_bp = 2;
frskyData.hub.gpsFix = -1;
#endif
#if defined(SIMU)
frskyData.analog[0].set(120);
frskyData.analog[1].set(240);
frskyData.rssi[0].value = 75;
frskyData.swr.value = 30;
frskyData.hub.fuelLevel = 75;
frskyData.hub.rpm = 12000;
frskyData.hub.vfas = 100;
frskyData.hub.minVfas = 90;
frskyData.hub.gpsFix = 1;
frskyData.hub.gpsLatitude_bp = 4401;
frskyData.hub.gpsLatitude_ap = 7710;
frskyData.hub.gpsLongitude_bp = 1006;
frskyData.hub.gpsLongitude_ap = 8872;
getGpsPilotPosition();
frskyData.hub.gpsLatitude_bp = 4401;
frskyData.hub.gpsLatitude_ap = 7455;
frskyData.hub.gpsLongitude_bp = 1006;
frskyData.hub.gpsLongitude_ap = 9533;
getGpsDistance();
frskyData.hub.gpsSpeed_bp = 100;
frskyData.hub.gpsSpeed_ap = 50;
frskyData.hub.airSpeed = 100;
frskyData.hub.cellsCount = 6;
frskyData.hub.baroAltitudeOffset = 500 * 100;
frskyData.hub.baroAltitude = 50 * 100;
frskyData.hub.gpsAltitude_bp = 50;
frskyData.hub.minAltitude = 10;
frskyData.hub.maxAltitude = 500;
frskyData.hub.accelY = 100;
frskyData.hub.temperature1 = -30;
frskyData.hub.maxTemperature1 = 100;
frskyData.hub.current = 5;
frskyData.hub.maxCurrent = 56;
#endif
}
-227
View File
@@ -1,227 +0,0 @@
/*
* 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.
*
*/
#ifndef TELEMETRY_SPORT_H
#define TELEMETRY_SPORT_H
#include <inttypes.h>
#define FRSKY_RX_PACKET_SIZE 9
#define FRSKY_TX_PACKET_SIZE 12
#define TELEMETRY_INIT 0
#define TELEMETRY_OK 1
#define TELEMETRY_KO 2
extern uint8_t telemetryState;
enum AlarmLevel {
alarm_off = 0,
alarm_yellow = 1,
alarm_orange = 2,
alarm_red = 3
};
#define ALARM_GREATER(channel, alarm) ((g_model.frsky.channels[channel].alarms_greater >> alarm) & 1)
#define ALARM_LEVEL(channel, alarm) ((g_model.frsky.channels[channel].alarms_level >> (2*alarm)) & 3)
class FrskyValueWithMinMax {
public:
uint8_t value;
uint8_t min;
uint8_t max;
void set(uint8_t value);
};
PACK(struct FrskySerialData {
int16_t spare1;
int16_t gpsAltitude_bp; // 0x01 before punct
int16_t temperature1; // 0x02 -20 .. 250 deg. celcius
uint16_t rpm; // 0x03 0..60,000 revs. per minute
uint16_t fuelLevel; // 0x04 0, 25, 50, 75, 100 percent
int16_t temperature2; // 0x05 -20 .. 250 deg. celcius
uint16_t volts; // 0x06 1/500V increments (0..4.2V)
uint32_t distFromEarthAxis;// 2 spares reused
int16_t gpsAltitude_ap; // 0x01+8 after punct
uint16_t spare2[6];
int16_t baroAltitude_bp; // 0x10 0..9,999 meters
uint16_t gpsSpeed_bp; // 0x11 before punct
uint16_t gpsLongitude_bp; // 0x12 before punct
uint16_t gpsLatitude_bp; // 0x13 before punct
uint16_t gpsCourse_bp; // 0x14 before punct (0..359.99 deg. -- seemingly 2-decimal precision)
uint8_t day; // 0x15
uint8_t month; // 0x15
uint16_t year; // 0x16
uint8_t hour; // 0x17
uint8_t min; // 0x17
uint16_t sec; // 0x18
uint16_t gpsSpeed_ap; // 0x11+8
uint16_t gpsLongitude_ap; // 0x12+8
uint16_t gpsLatitude_ap; // 0x13+8
uint16_t gpsCourse_ap; // 0x14+8
uint32_t pilotLatitude; // 2 spares reused
uint32_t pilotLongitude; // 2 spares reused
uint16_t baroAltitude_ap; // 0x21 after punct
uint16_t gpsLongitudeEW; // 0x1A+8 East/West
uint16_t gpsLatitudeNS; // 0x1B+8 North/South
int16_t accelX; // 0x24 1/256th gram (-8g ~ +8g)
int16_t accelY; // 0x25 1/256th gram (-8g ~ +8g)
int16_t accelZ; // 0x26 1/256th gram (-8g ~ +8g)
int16_t spare3;
uint16_t current; // 0x28 Current
int16_t spare4[7];
int16_t varioSpeed; // 0x30 Vertical speed in cm/s
int32_t baroAltitudeOffset;
int32_t baroAltitude;
int32_t gpsAltitudeOffset;
uint32_t gpsDistance;
uint16_t vfas; // 0x39 Added to FrSky protocol for home made sensors with a better precision
uint16_t volts_bp; // 0x3A
uint16_t volts_ap; // 0x3B
uint16_t spare5[4];
// end of FrSky Hub data
/* these fields must keep this order! */
int16_t minAltitude;
int16_t maxAltitude;
uint16_t maxRpm;
int16_t maxTemperature1;
int16_t maxTemperature2;
uint16_t maxGpsSpeed;
uint16_t maxGpsDistance;
uint16_t maxAirSpeed;
int16_t minCell;
int16_t minCells;
int16_t minVfas;
uint16_t maxCurrent;
uint16_t maxPower;
/* end */
int32_t gpsAltitude;
uint8_t gpsDistNeeded;
int8_t gpsFix; // -1=never fixed, 0=fix lost, 1=fixed
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
uint8_t cellsCount;
uint16_t cellVolts[12];
int16_t cellsSum;
uint8_t minCellIdx;
uint16_t minCellVolts;
bool varioHighPrecision;
uint16_t airSpeed;
uint16_t dTE;
});
struct FrskyData {
FrskyValueWithMinMax analog[2];
FrskyValueWithMinMax rssi[2];
FrskyValueWithMinMax swr;
FrskySerialData hub;
};
#define SEND_RSSI_ALARMS 6
#define SEND_MODEL_ALARMS 4
extern uint8_t frskyAlarmsSendState;
extern FrskyData frskyData;
extern uint8_t frskyTxBuffer[FRSKY_TX_PACKET_SIZE];
extern uint8_t frskyTxBufferCount;
void FRSKY_Init(void);
void FRSKY_End(void);
void telemetryWakeup();
void telemetryInterrupt10ms(void);
inline void frskySendAlarms(void)
{
frskyAlarmsSendState = SEND_RSSI_ALARMS;
}
bool FRSKY_alarmRaised(uint8_t idx);
void resetTelemetry();
#define TELEMETRY_STREAMING() (frskyData.rssi[0].value > 0)
#define TELEMETRY_CELL_VOLTAGE(k) frskyData.hub.cellVolts[k]
#define TELEMETRY_MIN_CELL_VOLTAGE frskyData.hub.minCellVolts
#define TELEMETRY_BARO_ALT_AVAILABLE() (frskyData.hub.baroAltitudeOffset)
#define TELEMETRY_BARO_ALT_UNIT (IS_IMPERIAL_ENABLE() ? LENGTH_UNIT_IMP : LENGTH_UNIT_METR)
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_ABSOLUTE_GPS_ALT (frskyData.hub.gpsAltitude)
#define TELEMETRY_RELATIVE_GPS_ALT (frskyData.hub.gpsAltitude + frskyData.hub.gpsAltitudeOffset)
#define TELEMETRY_RELATIVE_GPS_ALT_BP (TELEMETRY_RELATIVE_GPS_ALT / 100)
#define TELEMETRY_RELATIVE_BARO_ALT_BP (frskyData.hub.baroAltitude / 100)
#define TELEMETRY_RELATIVE_BARO_ALT_AP (frskyData.hub.baroAltitude % 100)
#define TELEMETRY_BARO_ALT_PREPARE() div_t baroAltitudeDivision = div(getConvertedTelemetryValue(frskyData.hub.baroAltitude, UNIT_DIST), 100)
#define TELEMETRY_BARO_ALT_FORMAT "%c%d.%02d,"
#define TELEMETRY_BARO_ALT_ARGS frskyData.hub.baroAltitude < 0 ? '-' : ' ', abs(baroAltitudeDivision.quot), abs(baroAltitudeDivision.rem),
#define TELEMETRY_GPS_ALT_FORMAT "%c%d.%02d,"
#define TELEMETRY_GPS_ALT_ARGS frskyData.hub.gpsAltitude < 0 ? '-' : ' ', abs(frskyData.hub.gpsAltitude / 100), abs(frskyData.hub.gpsAltitude % 100),
#define TELEMETRY_GPS_SPEED_UNIT (IS_IMPERIAL_ENABLE() ? SPEED_UNIT_IMP : SPEED_UNIT_METR)
#define TELEMETRY_GPS_SPEED_FORMAT "%d,"
#define TELEMETRY_GPS_SPEED_ARGS getConvertedTelemetryValue(frskyData.hub.gpsSpeed_bp, UNIT_KTS),
#if defined(CPUARM)
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100, TELEMETRY_CELL_VOLTAGE(6)/100, TELEMETRY_CELL_VOLTAGE(6)%100, TELEMETRY_CELL_VOLTAGE(7)/100, TELEMETRY_CELL_VOLTAGE(7)%100, TELEMETRY_CELL_VOLTAGE(8)/100, TELEMETRY_CELL_VOLTAGE(8)%100, TELEMETRY_CELL_VOLTAGE(9)/100, TELEMETRY_CELL_VOLTAGE(9)%100, TELEMETRY_CELL_VOLTAGE(10)/100, TELEMETRY_CELL_VOLTAGE(10)%100, TELEMETRY_CELL_VOLTAGE(11)/100, TELEMETRY_CELL_VOLTAGE(11)%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,Cell 7,Cell 8,Cell 9,Cell 10,Cell 11,Cell 12,"
#else
#define TELEMETRY_CELLS_ARGS frskyData.hub.cellsSum / 10, frskyData.hub.cellsSum % 10, TELEMETRY_CELL_VOLTAGE(0)/100, TELEMETRY_CELL_VOLTAGE(0)%100, TELEMETRY_CELL_VOLTAGE(1)/100, TELEMETRY_CELL_VOLTAGE(1)%100, TELEMETRY_CELL_VOLTAGE(2)/100, TELEMETRY_CELL_VOLTAGE(2)%100, TELEMETRY_CELL_VOLTAGE(3)/100, TELEMETRY_CELL_VOLTAGE(3)%100, TELEMETRY_CELL_VOLTAGE(4)/100, TELEMETRY_CELL_VOLTAGE(4)%100, TELEMETRY_CELL_VOLTAGE(5)/100, TELEMETRY_CELL_VOLTAGE(5)%100,
#define TELEMETRY_CELLS_FORMAT "%d.%d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,%d.%02d,"
#define TELEMETRY_CELLS_LABEL "Cell volts,Cell 1,Cell 2,Cell 3,Cell 4,Cell 5,Cell 6,"
#endif
#define TELEMETRY_CURRENT_FORMAT "%d.%d,"
#define TELEMETRY_CURRENT_ARGS frskyData.hub.current / 10, frskyData.hub.current % 10,
#define TELEMETRY_VFAS_FORMAT "%d.%d,"
#define TELEMETRY_VFAS_ARGS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10,
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
#define TELEMETRY_VSPEED_ARGS frskyData.hub.varioSpeed < 0 ? '-' : ' ', abs(frskyData.hub.varioSpeed / 100), abs(frskyData.hub.varioSpeed % 100),
#endif