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

Directory transmitter renamed to radio

Browse Source
This commit is contained in:
Bertrand Songis
2013-12-02 07:13:57 +01:00
parent f4df52a033
commit 942e658c8e
707 changed files with 0 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
radio/src/telemetry/ardupilot.cpp
+732
View File
@@ -0,0 +1,732 @@
/*
* 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"
#define NB_BUF 2
#define LG_BUF 11
#define VALSTR(val) (rbuf[val][0] ? rbuf[val] : val_unknown)
#define APSIZE (BSS | DBLSIZE)
/* Received data
Data are received as packets, each packet is identified by a prefix of three
characters and is ended by three stars. Some packets can contain more than
one value, the values are ended by a comma.
Position packet:
!!!LAT:0123456789,LON:9876543210,SPD:123,CRT:1,ALT:123456,ALH:654321,CRS:123,BER:123,WPN:12,DST:12345,BTV:12.4567 9012,RSP:123,TOW:123456,***
...----1--------------2--------------3-------4-----5----------6----------7-------8-------9------10--------11-----------12----------13-----END
1st value 2nd value
Attitude packet:
+++ASP:123,THH:123,RLL:123,PCH:123,***
...----1-------2-------3-------4---END
Mode change packet:
###STABILIZE***
...1--------END
Waypoint packet:
%%%12 45***
...1----END
Alert packet:
XXXAlert Text alert***
...1---------------END
Performance packet:
PPPThis is performant***
...1--------------END
*/
// Position packet prefix
#define PACK_POS 0x21
// value occurence number in this packet
#define LAT 1
#define LON 2
#define SPD 3
#define CRT 4
#define ALT 5
#define ALH 6
#define CRS 7
#define BER 8
#define WPN 9
#define DST 10
#define BTV 11
#define RSP 12
#define TOW 13
#define PACK_ATT 0x2b
// value occurence number in this packet
#define ASP 1
#define THH 2
#define RLL 3
#define PCH 4
#define PACK_MOD 0x23
// value occurence number in this packet
#define MOD 1
#define PACK_WPC 0x25
// value occurence number in this packet
#define WPC 1
#define PACK_ALR 0x58
// value occurence number in this packet
#define ALR 1
#define PACK_PRF 0x50
// value occurence number in this packet
#define PRF 1
// end of packet
#define PACK_END 0x2a
// end of value
#define VAL_END 0x2c
// end of title
#define TITLE_END 0x3a
// stateful machine
// Since the packets are sent continuously, we need to synchronize on the
// reception of the three chars prefixing a packet, whatever they are.
// states values
#define WAIT_PACKET 1
#define WAIT_PACK_POS1 2
#define WAIT_PACK_POS2 3
#define WAIT_PACK_POS3 4
#define WAIT_PACK_ATT1 5
#define WAIT_PACK_ATT2 6
#define WAIT_PACK_ATT3 7
#define WAIT_PACK_MOD1 8
#define WAIT_PACK_MOD2 9
#define WAIT_PACK_MOD3 10
#define WAIT_PACK_WPC1 11
#define WAIT_PACK_WPC2 12
#define WAIT_PACK_WPC3 13
#define WAIT_PACK_ALR1 14
#define WAIT_PACK_ALR2 15
#define WAIT_PACK_ALR3 16
#define WAIT_PACK_PRF1 17
#define WAIT_PACK_PRF2 18
#define WAIT_PACK_PRF3 19
#define FLUSH_TITLE1 20
#define FLUSH_TITLE2 21
#define FLUSH_TITLE3 22
#define FLUSH_TITLE4 23
#define READ_VALUE 24
uint8_t i; // working variable
uint8_t state; // currrent state
uint8_t rval, rpack; // received items
uint8_t xval[NB_BUF]; // expected value
uint8_t xpack[NB_BUF]; // expected packet
uint8_t ibuf[NB_BUF]; // subscripts on buffers values
char rbuf[NB_BUF][LG_BUF]; // receive buffers
const char val_unknown[] = "?";
void menuTelemetryArduPilot1(uint8_t event);
void menuTelemetryArduPilot2(uint8_t event);
void menuTelemetryArduPilot3(uint8_t event);
void menuTelemetryArduPilot4(uint8_t event);
void menuTelemetryArduPilot5(uint8_t event);
void menuTelemetryArduPilot6(uint8_t event);
void menuTelemetryArduPilot7(uint8_t event);
void menuTelemetryArduPilot8(uint8_t event);
void title(char x);
void initval(uint8_t num, uint8_t pack, uint8_t val);
#ifndef SIMU
ISR (USART0_RX_vect)
{
uint8_t rl;
// uint8_t rh; //USART control and Status Register 0 B
uint8_t iostat; //USART control and Status Register 0 A
rl = UDR0;
iostat = 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
MPCM0: MultiProcessor Comms Mode
*/
if (iostat & ((1 << FE0) | (1 << DOR0) | (1 << UPE0)))
{
rl = xpack[0] = xpack[1] = xval[0] = xval[1] = 0;
state = WAIT_PACKET; // restart on error
}
// 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
*/
switch (state)
{
case WAIT_PACKET:
switch (rl)
{
case PACK_POS: // found a new POS packet
state = WAIT_PACK_POS2; // wait for the 2nd char
break;
case PACK_ATT: // found a new ATT packet
state = WAIT_PACK_ATT2; // wait for the 2nd char
break;
case PACK_MOD: // found a new MOD packet
state = WAIT_PACK_MOD2; // wait for the 2nd char
break;
case PACK_WPC: // found a new WPC packet
state = WAIT_PACK_WPC2; // wait for the 2nd char
break;
case PACK_ALR: // found a new ALR packet
state = WAIT_PACK_ALR2; // wait for the 2nd char
break;
case PACK_PRF: // found a new PRF packet
state = WAIT_PACK_PRF2; // wait for the 2nd char
break;
}
break;
case WAIT_PACK_POS2: // wait for 2nd char
if (rl == PACK_POS)
state = WAIT_PACK_POS3;
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_POS3: // wait for 3rd char
if (rl == PACK_POS) // found
{
state = FLUSH_TITLE1; // flush title "LAT:"
rpack = PACK_POS;
rval = 1;
for (i = 0; i < NB_BUF; i++)
ibuf[i] = 0;
}
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_ATT2: // wait for 2nd char
if (rl == PACK_ATT)
state = WAIT_PACK_ATT3;
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_ATT3: // wait for 3rd char
if (rl == PACK_ATT)
{
state = FLUSH_TITLE1; // flush title "ASP:"
rpack = PACK_ATT;
rval = 1;
for (i = 0; i < NB_BUF; i++)
ibuf[i] = 0;
}
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_MOD2: // wait for 2nd char
if (rl == PACK_MOD)
state = WAIT_PACK_MOD3;
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_MOD3: // wait for 3rd char
if (rl == PACK_MOD)
{
state = READ_VALUE; // ready to read values from PACK_MOD packet
rpack = PACK_MOD;
rval = 1;
for (i = 0; i < NB_BUF; i++)
ibuf[i] = 0;
}
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_WPC2: // wait for 2nd char
if (rl == PACK_WPC)
state = WAIT_PACK_WPC3;
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_WPC3: // wait for 3rd char
if (rl == PACK_WPC)
{
state = READ_VALUE; // ready to read values from PACK_WPC packet
rpack = PACK_WPC;
rval = 1;
for (i = 0; i < NB_BUF; i++)
ibuf[i] = 0;
}
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_ALR2: // wait for 2nd char
if (rl == PACK_ALR)
state = WAIT_PACK_ALR3;
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_ALR3: // wait for 3rd char
if (rl == PACK_ALR)
{
state = READ_VALUE; // ready to read values from PACK_ALR packet
rpack = PACK_ALR;
rval = 1;
for (i = 0; i < NB_BUF; i++)
ibuf[i] = 0;
}
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_PRF2: // wait for 2nd char
if (rl == PACK_PRF)
state = WAIT_PACK_PRF3;
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_PRF3: // wait for 3rd char
if (rl == PACK_PRF)
{
state = READ_VALUE; // ready to read values from PACK_PRF packet
rpack = PACK_PRF;
rval = 1;
for (i = 0; i < NB_BUF; i++)
ibuf[i] = 0;
}
else
state = WAIT_PACKET; // restart if not found
break;
case FLUSH_TITLE1: // wait for 1st char of title
state = FLUSH_TITLE2;
break;
case FLUSH_TITLE2: // wait for 2nd char
state = FLUSH_TITLE3;
break;
case FLUSH_TITLE3: // wait for 3rd char
state = FLUSH_TITLE4;
break;
case FLUSH_TITLE4: // wait for 4th char
if (rl == TITLE_END)
state = READ_VALUE;
else
state = WAIT_PACKET; // was not a title, wait for the next packet
break;
case READ_VALUE:
switch (rl)
{
case PACK_END:
state = WAIT_PACKET; // packet completed, wait for the next packet
break;
case VAL_END: // comma found, value completed
state = FLUSH_TITLE1; // flush next title
rval++; // and get next value
break;
default: // store the char in the corresponding buffer
for (i = 0; i < NB_BUF; i++)
{ // is it the expected value in the expected packet ?
if (rpack == xpack[i] && rval == xval[i] && ibuf[i] < LG_BUF - 1)
{ // yes, store the char
rbuf[i] [ibuf[i]] = rl;
ibuf[i]++;
rbuf[i] [ibuf[i]] = 0;
}
}
break;
}
break;
}
}
#endif
void ARDUPILOT_Init (void)
{
DDRE &= ~(1 << DDE0); // set RXD0 pin as input
PORTE &= ~(1 << PORTE0); // disable pullup on RXD0 pin
#ifndef SIMU
// switch (Telem_baud)
// {
// case 1:
#undef BAUD
#define BAUD 38400
#include <util/setbaud.h>
UBRR0H = UBRRH_VALUE;
UBRR0L = UBRRL_VALUE;
// break;
// }
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
#endif
}
void ARDUPILOT_DisableTXD (void)
{
UCSR0B &= ~(1 << TXEN0); // disable TX
}
void ARDUPILOT_EnableTXD (void)
{
UCSR0B |= (1 << TXEN0); // enable TX
}
void ARDUPILOT_DisableRXD (void)
{
UCSR0B &= ~(1 << RXEN0); // disable RX
UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
}
void ARDUPILOT_EnableRXD (void)
{
for (i = 0; i < NB_BUF; i++)
{
ibuf[i] = 0;
rbuf[i][0] = 0;
xpack[i] = 0;
xval[i] = 0;
}
state = WAIT_PACKET; // wait for the next packet
UCSR0B |= (1 << RXEN0); // enable RX
UCSR0B |= (1 << RXCIE0); // enable Interrupt
}
void menuTelemetryArduPilot(uint8_t event)
{
menuTelemetryArduPilot1(event);
}
// Start of ArduPilot menus 1-8 <<<<<<<<<<<<<<<<<<<<<<<<<<<
void menuTelemetryArduPilot1(uint8_t event)
{
switch(event) // new event received, branch accordingly
{
case EVT_KEY_FIRST(KEY_UP):
chainMenu(menuTelemetryArduPilot8);
break;
case EVT_KEY_FIRST(KEY_DOWN):
chainMenu(menuTelemetryArduPilot2);
break;
case EVT_KEY_FIRST(KEY_MENU):
ARDUPILOT_DisableRXD();
chainMenu(menuStatisticsView);
break;
case EVT_KEY_FIRST(KEY_EXIT):
ARDUPILOT_DisableRXD();
chainMenu(menuMainView);
break;
}
/*
How to use:
You choose the values to be displayed using the function:
initval(<number>, <packet>, <value>);
-------------------------------------
That means that "<value>" of "<packet>" is stored in the <number> buffer.
The first <number> is 0.
Here are the packet names and the associated value names:
Position packet (beginning with "!!!"): "PACK_POS"
value names: "LAT", "LON", "SPD", "CRT", "ALT", "ALH", "CRS", "BER",
"WPN", "DST", "BTV", "RSP", "TOW"
Attitude packet (beginning with "+++"): "PACK_ATT"
value names: "ASP", "THH", "RLL", "PCH"
Mode change packet (beginning with "###"): "PACK_MOD"
value name: "MOD"
Waypoint packet (beginning with "%%%"): "PACK_WPC"
value name: "WPC"
Alert packet (beginning with "XXX"): "PACK_ALR"
value name: "ALR"
Performance packet (beginning with "PPP"): "PACK_PRF"
value name: "PRF"
The buffers are accessed using the macro "VALSTR(<n>)", where "<n>" is "0"
for the first buffer, and "1" for the second buffer.
When a value is missing, it is replaced by the contents of val_unknown ("?").
*/
// expecting LAT value in POS packet to be stored in the first buffer
initval (0, PACK_POS, LAT);
// and LON value in POS packet stored in the second buffer
initval (1, PACK_POS, LON);
// title of the screen
title ('1');
lcd_puts (5*FW, 1*FH, PSTR(" Latitude")); // line 1 column 5
// first buffer into line 2 column 2
lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE);
lcd_puts (5*FW, 4*FH, PSTR(" Longitude")); // line 4 column 5
// second buffer into line 5 column 2
lcd_putsAtt (1*FW, 5*FH, VALSTR(1), APSIZE);
}
void menuTelemetryArduPilot2(uint8_t event)
{
switch(event)
{
case EVT_KEY_FIRST(KEY_UP):
chainMenu(menuTelemetryArduPilot1);
break;
case EVT_KEY_FIRST(KEY_DOWN):
chainMenu(menuTelemetryArduPilot3);
break;
case EVT_KEY_FIRST(KEY_EXIT):
ARDUPILOT_DisableRXD();
chainMenu(menuMainView);
break;
}
initval (0, PACK_POS, SPD);
initval (1, PACK_POS, CRT);
title ('2');
lcd_puts (1*FW, 1*FH, PSTR(" Ground speed"));
lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE);
lcd_puts (1*FW, 4*FH, PSTR(" Climb rate") );
lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE);
}
void menuTelemetryArduPilot3(uint8_t event)
{
switch(event)
{
case EVT_KEY_FIRST(KEY_UP):
chainMenu(menuTelemetryArduPilot2);
break;
case EVT_KEY_FIRST(KEY_DOWN):
chainMenu(menuTelemetryArduPilot4);
break;
case EVT_KEY_FIRST(KEY_EXIT):
ARDUPILOT_DisableRXD();
chainMenu(menuMainView);
break;
}
initval (0, PACK_POS, ALT);
initval (1, PACK_POS, ALH);
title ('3');
lcd_puts (1*FW, 1*FH, PSTR(" Altitude"));
lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE);
lcd_puts (1*FW, 4*FH, PSTR(" Altitude Hold") );
lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE);
}
void menuTelemetryArduPilot4(uint8_t event)
{
switch(event)
{
case EVT_KEY_FIRST(KEY_UP):
chainMenu(menuTelemetryArduPilot3);
break;
case EVT_KEY_FIRST(KEY_DOWN):
chainMenu(menuTelemetryArduPilot5);
break;
case EVT_KEY_FIRST(KEY_EXIT):
ARDUPILOT_DisableRXD();
chainMenu(menuMainView);
break;
}
initval (0, PACK_POS, CRS);
initval (1, PACK_POS, BER);
title ('4');
lcd_puts (1*FW, 1*FH, PSTR(" Course"));
lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE);
lcd_puts (1*FW, 4*FH, PSTR(" Bearing"));
lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE);
}
void menuTelemetryArduPilot5(uint8_t event)
{
switch(event)
{
case EVT_KEY_FIRST(KEY_UP):
chainMenu(menuTelemetryArduPilot4);
break;
case EVT_KEY_FIRST(KEY_DOWN):
chainMenu(menuTelemetryArduPilot6);
break;
case EVT_KEY_FIRST(KEY_MENU):
break;
case EVT_KEY_FIRST(KEY_EXIT):
ARDUPILOT_DisableRXD();
chainMenu(menuMainView);
break;
}
initval (0, PACK_POS, WPN);
initval (1, PACK_POS, DST);
title ('5');
lcd_puts (1*FW, 1*FH, PSTR(" Way Point # "));
lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE);
lcd_puts (1*FW, 4*FH, PSTR(" Distance "));
lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE);
}
void menuTelemetryArduPilot6(uint8_t event)
{
switch(event)
{
case EVT_KEY_FIRST(KEY_UP):
chainMenu(menuTelemetryArduPilot5);
break;
case EVT_KEY_FIRST(KEY_DOWN):
chainMenu(menuTelemetryArduPilot7);
break;
case EVT_KEY_FIRST(KEY_MENU):
break;
case EVT_KEY_FIRST(KEY_EXIT):
ARDUPILOT_DisableRXD();
chainMenu(menuMainView);
break;
}
initval (0, PACK_ATT, ASP);
initval (1, PACK_ATT, THH);
title ('6');
lcd_puts (1*FW, 1*FH, PSTR(" Air Speed "));
lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE);
lcd_puts (1*FW, 4*FH, PSTR(" Climb Rate "));
lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE);
}
void menuTelemetryArduPilot7(uint8_t event)
{
switch(event)
{
case EVT_KEY_FIRST(KEY_UP):
chainMenu(menuTelemetryArduPilot6);
break;
case EVT_KEY_FIRST(KEY_DOWN):
chainMenu(menuTelemetryArduPilot8);
break;
case EVT_KEY_FIRST(KEY_MENU):
break;
case EVT_KEY_FIRST(KEY_EXIT):
ARDUPILOT_DisableRXD();
chainMenu(menuMainView);
break;
}
initval (0, PACK_ATT, RLL);
initval (1, PACK_ATT, PCH);
title ('7');
lcd_puts (1*FW, 1*FH, PSTR(" Roll Angle"));
lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE);
lcd_puts (1*FW, 4*FH, PSTR(" Pitch Angle"));
lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE);
}
void menuTelemetryArduPilot8(uint8_t event)
{
switch(event)
{
case EVT_KEY_FIRST(KEY_UP):
chainMenu(menuTelemetryArduPilot7);
break;
case EVT_KEY_FIRST(KEY_DOWN):
chainMenu(menuTelemetryArduPilot1);
break;
case EVT_KEY_FIRST(KEY_MENU):
break;
case EVT_KEY_FIRST(KEY_EXIT):
ARDUPILOT_DisableRXD();
chainMenu(menuMainView);
break;
}
initval (0, PACK_MOD, MOD);
initval (1, PACK_WPC, WPC);
title ('8');
lcd_puts (1*FW, 1*FH, PSTR(" ArduPilot Mode"));
lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE);
lcd_puts (1*FW, 4*FH, PSTR(" RTL Distance"));
lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE);
}
void title(char x)
{
lcd_putsAtt (0, 0, PSTR(" ARDU PILOT Mega ?/8 "), INVERS);
lcd_putcAtt(17*FW, 0*FH, x, INVERS);
}
void initval(uint8_t num, uint8_t pack, uint8_t val)
{
if (xpack[num] != pack || xval[num] != val)
{
ibuf[num] = rbuf[num][0] = 0;
xpack[num] = pack;
xval[num] = val;
state = WAIT_PACKET; // synchronize to the next packet
}
}
/*
Without ArduPilot:
Size after:
er9x.elf :
section size addr
.data 164 8388864
.text 50634 0
.bss 3485 8389028
----------------------------------
With ArduPilot:
Size after:
er9x.elf :
section size addr
.data 166 8388864
.text 53026 0
.bss 3517 8389030
*/
radio/src/telemetry/ardupilot.h
+43
View File
@@ -0,0 +1,43 @@
/*
* 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 ardupilot_h
#define ardupilot_h
void ARDUPILOT_Init(void);
void ARDUPILOT_EnableRXD (void);
void menuTelemetryArduPilot(uint8_t event);
#endif
radio/src/telemetry/frsky.cpp
+919
View File
@@ -0,0 +1,919 @@
/*
* 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
#define START_STOP 0x7e
#define BYTESTUFF 0x7d
#define STUFF_MASK 0x20
#if defined(TELEMETREZ)
#define PRIVATE 0x1B
#endif
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 frskyStreaming = 0;
#if defined(WS_HOW_HIGH)
uint8_t frskyUsrStreaming = 0;
#endif
uint8_t link_counter = 0;
FrskyData frskyData;
#if defined(FRSKY_HUB) || defined(WS_HOW_HIGH)
void checkMinMaxAltitude()
{
if (TELEMETRY_ALT_BP > frskyData.hub.maxAltitude)
frskyData.hub.maxAltitude = TELEMETRY_ALT_BP;
if (TELEMETRY_ALT_BP < frskyData.hub.minAltitude)
frskyData.hub.minAltitude = TELEMETRY_ALT_BP;
}
#endif
#if defined(VARIO) && !defined(FRSKY_SPORT) && (defined(FRSKY_HUB) || defined(WS_HOW_HIGH))
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
#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 (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;
if (g_model.frsky.varioSource == VARIO_SOURCE_ALTI) {
evalVario(frskyData.hub.baroAltitude_bp, 0);
}
frskyData.hub.baroAltitude_bp += frskyData.hub.baroAltitudeOffset;
checkMinMaxAltitude();
break;
case offsetof(FrskySerialData, baroAltitude_ap):
if (g_model.frsky.varioSource == VARIO_SOURCE_ALTI_PLUS) {
evalVario(frskyData.hub.baroAltitude_bp-frskyData.hub.baroAltitudeOffset, frskyData.hub.baroAltitude_ap);
}
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();
if (g_model.frsky.varioSource == VARIO_SOURCE_ALTI) {
evalVario(frskyData.hub.baroAltitude_bp, 0);
}
}
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 processFrskyPacket(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
}
}
// 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(TELEMETREZ)
uint8_t privateDataLen;
uint8_t privateDataPos;
#endif
#if defined(ROTARY_ENCODER_NAVIGATION) && defined(TELEMETREZ)
extern uint8_t TrotCount;
extern uint8_t TezRotary;
#endif
/*
Receive serial (RS-232) characters, detecting and storing each Fr-Sky
0x7e-framed packet as it arrives. When a complete packet has been
received, process its data into storage variables. NOTE: This is an
interrupt routine and should not get too lengthy. I originally had
the buffer being checked in the perMain function (because per10ms
isn't quite often enough for data streaming at 9600baud) but alas
that scheme lost packets also. So each packet is parsed as it arrives,
directly at the ISR function (through a call to frskyProcessPacket).
If this proves a problem in the future, then I'll just have to implement
a second buffer to receive data while one buffer is being processed (slowly).
*/
#if defined(CPUARM)
void processSerialData(uint8_t data)
#else
NOINLINE void processSerialData(uint8_t stat, uint8_t data)
#endif
{
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(CPUARM)
if (stat & ((1 << FE0) | (1 << DOR0) | (1 << UPE0))) {
// discard buffer and start fresh on any comms error
numPktBytes = 0;
}
else
#endif
{
switch (dataState)
{
case STATE_DATA_START:
if (data == START_STOP) break; // Remain in userDataStart if possible 0x7e,0x7e doublet found.
if (numPktBytes < FRSKY_RX_PACKET_SIZE)
frskyRxBuffer[numPktBytes++] = data;
dataState = STATE_DATA_IN_FRAME;
break;
case STATE_DATA_IN_FRAME:
if (data == BYTESTUFF)
{
dataState = STATE_DATA_XOR; // XOR next byte
break;
}
if (data == START_STOP) // end of frame detected
{
processFrskyPacket(frskyRxBuffer);
dataState = STATE_DATA_IDLE;
break;
}
if (numPktBytes < FRSKY_RX_PACKET_SIZE)
frskyRxBuffer[numPktBytes++] = data;
break;
case STATE_DATA_XOR:
if (numPktBytes < FRSKY_RX_PACKET_SIZE)
frskyRxBuffer[numPktBytes++] = data ^ STUFF_MASK;
dataState = STATE_DATA_IN_FRAME;
break;
case STATE_DATA_IDLE:
if (data == START_STOP) {
numPktBytes = 0;
dataState = STATE_DATA_START;
}
#if defined(TELEMETREZ)
if (data == PRIVATE) {
dataState = STATE_DATA_PRIVATE_LEN;
}
#endif
break;
#if defined(TELEMETREZ)
case STATE_DATA_PRIVATE_LEN:
dataState = STATE_DATA_PRIVATE_VALUE;
privateDataLen = data; // Count of bytes to receive
privateDataPos = 0;
break;
case STATE_DATA_PRIVATE_VALUE :
if (privateDataPos == 0) {
// Process first private data byte
// PC6, PC7
if ((data & 0x3F) == 0) {// Check byte is valid
DDRC |= 0xC0; // Set as outputs
PORTC = ( PORTC & 0x3F ) | ( data & 0xC0 ); // update outputs
}
}
#if defined(ROTARY_ENCODER_NAVIGATION)
if (privateDataPos == 1) {
TrotCount = data;
}
if (privateDataPos == 2) { // rotary encoder switch
RotEncoder = data;
}
#endif
if (++privateDataPos == privateDataLen) {
dataState = STATE_DATA_IDLE;
}
break;
#endif
} // switch
}
}
#if !defined(CPUARM) && !defined(SIMU)
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
processSerialData(stat, data);
cli() ;
UCSR0B |= (1 << RXCIE0); // enable Interrupt
}
#endif
/******************************************/
#if defined(CPUARM)
void frskyTransmitBuffer( uint32_t size )
{
#if defined(PCBSKY9X)
txPdcUsart( frskyTxBuffer, size ) ;
#endif
}
#else
void frskyTransmitBuffer()
{
UCSR0B |= (1 << UDRIE0); // enable UDRE0 interrupt
}
#endif
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;
}
}
inline void frskySendNextAlarm(void)
{
#if defined(PCBSKY9X)
if (txPdcPending())
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
frskyTransmitBuffer(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];
frskyTransmitBuffer();
}
inline void frskySendNextAlarm(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
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 /= 5;
frskyData.hub.cellsSum = voltage;
#endif
if (TELEMETRY_STREAMING()) {
if (!TELEMETRY_OPENXSENSOR()) {
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_SOURCE_A1;
if (channel <= 1) {
current = applyChannelRatio(channel, frskyData.analog[channel].value) / 10;
}
#if defined(CPUARM)
frskyData.hub.power = (current * voltage) / 100;
#else
frskyData.hub.power = ((current>>1) * (voltage>>1)) / 25;
#endif
frskyData.hub.currentPrescale += current;
if (frskyData.hub.currentPrescale >= 3600) {
frskyData.hub.currentConsumption += 1;
frskyData.hub.currentPrescale -= 3600;
}
}
if (frskyData.hub.power > frskyData.hub.maxPower)
frskyData.hub.maxPower = frskyData.hub.power;
}
}
void telemetryWakeup()
{
#if defined(PCBSKY9X)
rxPdcUsart(processSerialData); // Receive serial data here
#endif
// 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();
}
#ifndef SIMU
#if defined(WS_HOW_HIGH)
if (frskyUsrStreaming > 0) {
frskyUsrStreaming--;
}
#endif
if (frskyStreaming > 0) {
frskyStreaming--;
}
else {
frskyData.rssi[0].set(0);
frskyData.rssi[1].set(0);
}
#endif
#if defined(VARIO)
if (TELEMETRY_STREAMING() && !IS_FAI_ENABLED())
varioWakeup();
#endif
}
#if 0
// not used any more
bool FRSKY_alarmRaised(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
#if !defined(CPUARM)
inline void FRSKY_EnableTXD(void)
{
frskyTxBufferCount = 0;
UCSR0B |= (1 << TXEN0); // enable TX
}
inline void FRSKY_EnableRXD(void)
{
UCSR0B |= (1 << RXEN0); // enable RX
UCSR0B |= (1 << RXCIE0); // enable Interrupt
}
#endif
void FRSKY_Init(void)
{
// clear frsky variables
resetTelemetry();
#if defined(PCBTARANIS)
// TODO
#elif defined(PCBSKY9X)
startPdcUsartReceive() ;
#elif !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.
FRSKY_EnableTXD(); // enable FrSky-Telemetry reception
FRSKY_EnableRXD(); // enable FrSky-Telemetry reception
#endif
}
void FrskyValueWithMin::set(uint8_t value)
{
if (this->value == 0) {
this->value = value;
}
else {
sum += value;
if (link_counter == 0) {
this->value = sum / 8;
sum = 0;
}
}
if (value && (!min || value < min))
min = value;
}
void FrskyValueWithMinMax::set(uint8_t value, uint8_t unit)
{
FrskyValueWithMin::set(value);
if (unit != UNIT_VOLTS) {
this->value = value;
}
if (!max || value > max)
max = value;
}
void resetTelemetry()
{
memclear(&frskyData, sizeof(frskyData));
#if defined(FRSKY_HUB)
frskyData.hub.gpsLatitude_bp = 2;
frskyData.hub.gpsLongitude_bp = 2;
frskyData.hub.gpsFix = -1;
#endif
#ifdef SIMU
frskyData.analog[0].set(120, UNIT_VOLTS);
frskyData.analog[1].set(240, UNIT_VOLTS);
frskyData.rssi[0].value = 75;
frskyData.rssi[1].value = 75;
frskyData.hub.fuelLevel = 75;
frskyData.hub.rpm = 12000;
#if defined(GPS)
frskyData.hub.gpsFix = 1;
frskyData.hub.gpsLatitude_bp = 4401;
frskyData.hub.gpsLatitude_ap = 7710;
frskyData.hub.gpsLongitude_bp = 1006;
frskyData.hub.gpsLongitude_ap = 8872;
frskyData.hub.gpsSpeed_bp = (100 * 250) / 463;
getGpsPilotPosition();
frskyData.hub.gpsLatitude_bp = 4401;
frskyData.hub.gpsLatitude_ap = 7455;
frskyData.hub.gpsLongitude_bp = 1006;
frskyData.hub.gpsLongitude_ap = 9533;
getGpsDistance();
#endif
frskyData.hub.cellsCount = 6;
frskyData.hub.gpsAltitude_bp = 50;
frskyData.hub.baroAltitude_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
}
radio/src/telemetry/frsky.h
+246
View File
@@ -0,0 +1,246 @@
/*
* 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 spare2;
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 maxCurrent;
uint16_t maxPower;
/* end */
});
#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];
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_ALT_BP frskyData.hub.baroAltitude_bp
#define TELEMETRY_ALT_AP frskyData.hub.baroAltitude_ap
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_GPS_ALT_AP frskyData.hub.gpsAltitude_ap
#define TELEMETRY_GPS_ALT_BP frskyData.hub.gpsAltitude_bp
#define TELEMETRY_ALT frskyData.hub.baroAltitude_bp, frskyData.hub.baroAltitude_ap
#define TELEMETRY_ALT_FORMAT "%d.%02d,"
#define TELEMETRY_CELLS 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_CURRENT frskyData.hub.current / 100, frskyData.hub.current % 100
#define TELEMETRY_CURRENT_FORMAT "%d.%02d,"
#define TELEMETRY_VFAS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10
#define TELEMETRY_VFAS_FORMAT "%d.%d,"
#define TELEMETRY_VSPEED frskyData.hub.varioSpeed < 0 ? '-' : ' ', frskyData.hub.varioSpeed / 100, frskyData.hub.varioSpeed % 100
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
#define TELEMETRY_STREAMING() (frskyStreaming > 0)
#if defined(FRSKY_HUB)
#define TELEMETRY_OPENXSENSOR() (frskyData.hub.openXsensor)
#else
#define TELEMETRY_OPENXSENSOR() (0)
#endif
#endif
radio/src/telemetry/frsky_sport.cpp
+686
View File
@@ -0,0 +1,686 @@
/*
* 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"
#define START_STOP 0x7e
#define BYTESTUFF 0x7d
#define STUFF_MASK 0x20
// FrSky PRIM IDs (1 byte)
#define DATA_FRAME 0x10
// FrSky old DATA IDs (1 byte)
#define GPS_ALT_BP_ID 0x01
#define TEMP1_ID 0x02
#define RPM_ID 0x03
#define FUEL_ID 0x04
#define TEMP2_ID 0x05
#define VOLTS_ID 0x06
#define GPS_ALT_AP_ID 0x09
#define BARO_ALT_BP_ID 0x10
#define GPS_SPEED_BP_ID 0x11
#define GPS_LONG_BP_ID 0x12
#define GPS_LAT_BP_ID 0x13
#define GPS_COURS_BP_ID 0x14
#define GPS_DAY_MONTH_ID 0x15
#define GPS_YEAR_ID 0x16
#define GPS_HOUR_MIN_ID 0x17
#define GPS_SEC_ID 0x18
#define GPS_SPEED_AP_ID 0x19
#define GPS_LONG_AP_ID 0x1A
#define GPS_LAT_AP_ID 0x1B
#define GPS_COURS_AP_ID 0x1C
#define BARO_ALT_AP_ID 0x21
#define GPS_LONG_EW_ID 0x22
#define GPS_LAT_NS_ID 0x23
#define ACCEL_X_ID 0x24
#define ACCEL_Y_ID 0x25
#define ACCEL_Z_ID 0x26
#define CURRENT_ID 0x28
#define VARIO_ID 0x30
#define VFAS_ID 0x39
#define VOLTS_BP_ID 0x3A
#define VOLTS_AP_ID 0x3B
#define FRSKY_LAST_ID 0x3F
// FrSky new DATA IDs (2 bytes)
#define RSSI_ID 0xf101
#define ADC1_ID 0xf102
#define ADC2_ID 0xf103
#define BATT_ID 0xf104
#define SWR_ID 0xf105
#define T1_FIRST_ID 0x0400
#define T1_LAST_ID 0x040f
#define T2_FIRST_ID 0x0410
#define T2_LAST_ID 0x041f
#define RPM_FIRST_ID 0x0500
#define RPM_LAST_ID 0x050f
#define FUEL_FIRST_ID 0x0600
#define FUEL_LAST_ID 0x060f
#define ALT_FIRST_ID 0x0100
#define ALT_LAST_ID 0x010f
#define VARIO_FIRST_ID 0x0110
#define VARIO_LAST_ID 0x011f
#define ACCX_FIRST_ID 0x0700
#define ACCX_LAST_ID 0x070f
#define ACCY_FIRST_ID 0x0710
#define ACCY_LAST_ID 0x071f
#define ACCZ_FIRST_ID 0x0720
#define ACCZ_LAST_ID 0x072f
#define CURR_FIRST_ID 0x0200
#define CURR_LAST_ID 0x020f
#define VFAS_FIRST_ID 0x0210
#define VFAS_LAST_ID 0x021f
#define GPS_SPEED_FIRST_ID 0x0830
#define GPS_SPEED_LAST_ID 0x083f
#define CELLS_FIRST_ID 0x0300
#define CELLS_LAST_ID 0x030f
// FrSky wrong IDs ?
#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
if (!frskyData.hub.baroAltitudeOffset)
frskyData.hub.baroAltitudeOffset = -baroAltitude;
baroAltitude += frskyData.hub.baroAltitudeOffset;
frskyData.hub.baroAltitude = baroAltitude;
baroAltitude /= 100;
if (baroAltitude > frskyData.hub.maxAltitude)
frskyData.hub.maxAltitude = baroAltitude;
if (baroAltitude < frskyData.hub.minAltitude)
frskyData.hub.minAltitude = baroAltitude;
}
void processHubPacket(uint8_t id, uint16_t value)
{
if (id > FRSKY_LAST_ID)
return;
if (id == GPS_LAT_BP_ID) {
if (value)
frskyData.hub.gpsFix = 1;
else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLatitude_bp > 1)
frskyData.hub.gpsFix = 0;
}
else if (id == GPS_LONG_BP_ID) {
if (value)
frskyData.hub.gpsFix = 1;
else if (frskyData.hub.gpsFix > 0 && frskyData.hub.gpsLongitude_bp > 1)
frskyData.hub.gpsFix = 0;
}
if (id == GPS_ALT_BP_ID || (id >= GPS_ALT_AP_ID && id <= GPS_LAT_NS_ID && id != BARO_ALT_BP_ID && id != BARO_ALT_AP_ID)) {
// if we don't have a fix, we may discard the value
if (frskyData.hub.gpsFix <= 0)
return;
}
((uint16_t*)&frskyData.hub)[id] = value;
switch (id) {
case RPM_ID:
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 TEMP1_ID:
if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1)
frskyData.hub.maxTemperature1 = frskyData.hub.temperature1;
break;
case TEMP2_ID:
if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2)
frskyData.hub.maxTemperature2 = frskyData.hub.temperature2;
break;
case CURRENT_ID:
if (frskyData.hub.current > frskyData.hub.maxCurrent)
frskyData.hub.maxCurrent = frskyData.hub.current;
break;
case VOLTS_AP_ID:
#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 BARO_ALT_AP_ID:
if (frskyData.hub.baroAltitude_ap > 9)
frskyData.hub.varioHighPrecision = true;
if (!frskyData.hub.varioHighPrecision)
frskyData.hub.baroAltitude_ap *= 10;
setBaroAltitude((int32_t)100 * frskyData.hub.baroAltitude_bp + (frskyData.hub.baroAltitude_bp >= 0 ? frskyData.hub.baroAltitude_ap : -frskyData.hub.baroAltitude_ap));
break;
case GPS_ALT_AP_ID:
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 GPS_SPEED_BP_ID:
// Speed => Max speed
if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
break;
case VOLTS_ID:
// 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;
}
// TODO precision could be 0.01V instead of 0.02V
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;
}
case GPS_HOUR_MIN_ID:
frskyData.hub.hour = ((uint8_t)(frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
break;
case ACCEL_X_ID:
case ACCEL_Y_ID:
case ACCEL_Z_ID:
((int16_t*)(&frskyData.hub))[id] /= 10;
break;
}
}
bool checkSportPacket(uint8_t *packet)
{
short crc = 0;
for (int i=1; i<FRSKY_RX_PACKET_SIZE; i++) {
crc += packet[i]; //0-1FF
crc += crc >> 8; //0-100
crc &= 0x00ff;
crc += crc >> 8; //0-0FF
crc &= 0x00ff;
}
return (crc == 0x00ff);
}
#define SPORT_DATA_U8(packet) (packet[4])
#define SPORT_DATA_S32(packet) (*((int32_t *)(packet+4)))
#define SPORT_DATA_U32(packet) (*((uint32_t *)(packet+4)))
#define HUB_DATA_U16(packet) (*((uint16_t *)(packet+4)))
void processSportPacket(uint8_t *packet)
{
/* uint8_t dataId = packet[0]; */
uint8_t prim = packet[1];
uint16_t appId = *((uint16_t *)(packet+2));
if (!checkSportPacket(packet))
return;
switch (prim)
{
case DATA_FRAME:
if (appId == RSSI_ID) {
frskyData.rssi[0].set(SPORT_DATA_U8(packet));
}
if (appId == SWR_ID) {
frskyData.rssi[1].set(SPORT_DATA_U8(packet));
}
else if (appId == ADC1_ID || appId == ADC2_ID) {
// A1/A2 of DxR receivers
frskyData.analog[appId-ADC1_ID].set(SPORT_DATA_U8(packet));
#if defined(VARIO)
uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1;
if (varioSource == appId-ADC1_ID) {
frskyData.hub.varioSpeed = applyChannelRatio(varioSource, frskyData.analog[varioSource].value);
}
#endif
}
else if (appId == BATT_ID) {
frskyData.analog[0].set(SPORT_DATA_U8(packet));
}
else if ((appId >> 8) == 0) {
// The old FrSky IDs
uint8_t id = (uint8_t)appId;
uint16_t value = HUB_DATA_U16(packet);
processHubPacket(id, value);
}
else if (appId == BETA_BARO_ALT_ID) {
int32_t baroAltitude = SPORT_DATA_S32(packet);
setBaroAltitude(10 * (baroAltitude >> 8));
}
else if (appId == BETA_VARIO_ID) {
int32_t varioSpeed = SPORT_DATA_S32(packet);
frskyData.hub.varioSpeed = 10 * (varioSpeed >> 8);
}
else if (appId >= T1_FIRST_ID && appId <= T1_LAST_ID) {
frskyData.hub.temperature1 = SPORT_DATA_S32(packet);
if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1)
frskyData.hub.maxTemperature1 = frskyData.hub.temperature1;
}
else if (appId >= T2_FIRST_ID && appId <= T2_LAST_ID) {
frskyData.hub.temperature2 = SPORT_DATA_S32(packet);
if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2)
frskyData.hub.maxTemperature2 = frskyData.hub.temperature2;
}
else if (appId >= RPM_FIRST_ID && appId <= RPM_LAST_ID) {
frskyData.hub.rpm = SPORT_DATA_U32(packet) / (g_model.frsky.blades+2);
if (frskyData.hub.rpm > frskyData.hub.maxRpm)
frskyData.hub.maxRpm = frskyData.hub.rpm;
}
else if (appId >= FUEL_FIRST_ID && appId <= FUEL_LAST_ID) {
frskyData.hub.fuelLevel = SPORT_DATA_U32(packet);
}
else if (appId >= ALT_FIRST_ID && appId <= ALT_LAST_ID) {
setBaroAltitude(SPORT_DATA_S32(packet));
}
else if (appId >= VARIO_FIRST_ID && appId <= VARIO_LAST_ID) {
frskyData.hub.varioSpeed = SPORT_DATA_S32(packet);
}
else if (appId >= ACCX_FIRST_ID && appId <= ACCX_LAST_ID) {
frskyData.hub.accelX = SPORT_DATA_S32(packet);
}
else if (appId >= ACCY_FIRST_ID && appId <= ACCY_LAST_ID) {
frskyData.hub.accelY = SPORT_DATA_S32(packet);
}
else if (appId >= ACCZ_FIRST_ID && appId <= ACCZ_LAST_ID) {
frskyData.hub.accelZ = SPORT_DATA_S32(packet);
}
else if (appId >= CURR_FIRST_ID && appId <= CURR_LAST_ID) {
frskyData.hub.current = SPORT_DATA_U32(packet);
if (frskyData.hub.current > frskyData.hub.maxCurrent)
frskyData.hub.maxCurrent = frskyData.hub.current;
}
else if (appId >= VFAS_FIRST_ID && appId <= VFAS_LAST_ID) {
frskyData.hub.vfas = SPORT_DATA_U32(packet)/10; //TODO: remove /10 and display with PREC2 when using SPORT
}
else if (appId >= GPS_SPEED_FIRST_ID && appId <= GPS_SPEED_LAST_ID) {
frskyData.hub.gpsSpeed_bp = SPORT_DATA_U32(packet);
if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
}
else if (appId >= CELLS_FIRST_ID && appId <= CELLS_LAST_ID) {
uint32_t cells = SPORT_DATA_U32(packet);
uint8_t battnumber = cells & 0xF;
uint32_t minCell, minCellNum;
//TODO: Use reported total voltages (bits 4-7)?
frskyData.hub.cellVolts[battnumber] = ((cells & 0x000FFF00) >> 8) / 10;
frskyData.hub.cellVolts[battnumber+1] = ((cells & 0xFFF00000) >> 20) / 10;
if (frskyData.hub.cellsCount < battnumber+2)
frskyData.hub.cellsCount = battnumber+2;
if (frskyData.hub.cellVolts[battnumber+1] == 0)
frskyData.hub.cellsCount--;
if ((frskyData.hub.cellVolts[battnumber] < frskyData.hub.cellVolts[battnumber+1]) || (frskyData.hub.cellVolts[battnumber+1] == 0)) {
minCell = frskyData.hub.cellVolts[battnumber];
minCellNum = battnumber;
}
else {
minCell = frskyData.hub.cellVolts[battnumber+1];
minCellNum = battnumber+1;
}
if (!frskyData.hub.minCellVolts || minCell < frskyData.hub.minCellVolts || minCellNum==frskyData.hub.minCellIdx) {
frskyData.hub.minCellIdx = minCellNum;
frskyData.hub.minCellVolts = minCell;
}
}
break;
}
}
// 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 (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 /= 5;
frskyData.hub.cellsSum = voltage;
#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_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 (frskyData.rssi[1].value > 0x33) {
AUDIO_SWR_RED();
s_global_warning = STR_ANTENNAPROBLEM;
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
}
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) && g_model.moduleData[INTERNAL_MODULE].rfProtocol == RF_PROTO_D8) {
AUDIO_A2_RED();
alarmsCheckTime = get_tmr10ms() + 300; /* next check in 3seconds */
}
else if (alarmRaised(1, 0) && g_model.moduleData[INTERNAL_MODULE].rfProtocol == RF_PROTO_D8) {
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));
#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.rssi[1].value = 30;
frskyData.hub.fuelLevel = 75;
frskyData.hub.rpm = 12000;
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.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
}
radio/src/telemetry/frsky_sport.h
+209
View File
@@ -0,0 +1,209 @@
/*
* 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 maxCurrent;
uint16_t maxPower;
/* end */
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;
});
struct FrskyData {
FrskyValueWithMinMax analog[2];
FrskyValueWithMinMax rssi[2];
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_ALT_BP (frskyData.hub.baroAltitude / 100)
#define TELEMETRY_ALT_AP (frskyData.hub.baroAltitude % 100)
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#define TELEMETRY_GPS_ALT_AP frskyData.hub.gpsAltitude_ap
#define TELEMETRY_GPS_ALT_BP frskyData.hub.gpsAltitude_bp
#define TELEMETRY_ALT frskyData.hub.baroAltitude < 0 ? '-' : ' ', abs(frskyData.hub.baroAltitude / 100), abs(frskyData.hub.baroAltitude % 100)
#define TELEMETRY_ALT_FORMAT "%c%d.%02d,"
#define TELEMETRY_CELLS 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_CURRENT frskyData.hub.current / 10, frskyData.hub.current % 10
#define TELEMETRY_CURRENT_FORMAT "%d.%d,"
#define TELEMETRY_VFAS frskyData.hub.vfas / 10, frskyData.hub.vfas % 10
#define TELEMETRY_VFAS_FORMAT "%d.%d,"
#define TELEMETRY_VSPEED frskyData.hub.varioSpeed < 0 ? '-' : ' ', abs(frskyData.hub.varioSpeed / 100), abs(frskyData.hub.varioSpeed % 100)
#define TELEMETRY_VSPEED_FORMAT "%c%d.%02d,"
/* Would be great, but f_printf() doesn't take floats...
#define TELEMETRY_ALT float(frskyData.hub.baroAltitude)/100
#define TELEMETRY_ALT_FORMAT "%.2f,"
#define TELEMETRY_CELLS float(frskyData.hub.cellsSum)/10, float(frskyData.hub.cellVolts[0])/100, float(frskyData.hub.cellVolts[1])/100, float(frskyData.hub.cellVolts[2])/100, float(frskyData.hub.cellVolts[3])/100, float(frskyData.hub.cellVolts[4])/100, float(frskyData.hub.cellVolts[5])/100
#define TELEMETRY_CELLS_FORMAT "%.1f,%.2f,%.2f,%.2f,%.2f,%.2f,%.2f,"
#define TELEMETRY_CURRENT float(frskyData.hub.current)/100
#define TELEMETRY_CURRENT_FORMAT "%.2f,"
#define TELEMETRY_VFAS float(frskyData.hub.vfas)/10
#define TELEMETRY_VFAS_FORMAT "%.1f,"
#define TELEMETRY_VSPEED float(frskyData.hub.varioSpeed)/100
#define TELEMETRY_VSPEED_FORMAT "%.2f,"
*/
#define TELEMETRY_STREAMING() (frskyData.rssi[0].value > 0)
#endif
radio/src/telemetry/jeti.cpp
+223
View File
@@ -0,0 +1,223 @@
/*
* 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"
uint8_t jetiRxBuffer[32];
uint8_t jetiReady;
uint16_t jetiKeys;
void telemetryInterrupt10ms()
{
if (jetiReady) {
jetiKeys = JETI_KEY_NOCHANGE;
if (switchState((EnumKeys)(KEY_UP))) jetiKeys &= JETI_KEY_UP;
if (switchState((EnumKeys)(KEY_DOWN))) jetiKeys &= JETI_KEY_DOWN;
if (switchState((EnumKeys)(KEY_LEFT))) jetiKeys &= JETI_KEY_LEFT;
if (switchState((EnumKeys)(KEY_RIGHT))) jetiKeys &= JETI_KEY_RIGHT;
jetiReady = 0; // invalidate buffer
JETI_EnableTXD();
}
}
#ifndef SIMU
ISR(USART0_UDRE_vect)
{
if (jetiKeys != 0xff) {
UCSR0B &= ~(1 << TXB80);
if (jetiKeys & 0x0100)
UCSR0B |= (1 << TXB80);
UDR0 = jetiKeys;
jetiKeys = 0xff;
}
else {
JETI_DisableTXD();
}
}
ISR (USART0_RX_vect)
{
uint8_t stat;
uint8_t rh;
uint8_t rl;
static uint8_t jbp;
stat = UCSR0A;
rh = UCSR0B;
rl = UDR0;
if (stat & ((1 << FE0) | (1 << DOR0) | (1 << UPE0))) {
// discard buffer and start new on any error
jetiReady = 0;
jbp = 0;
}
else if ((rh & (1 << RXB80)) == 0) {
// control
if (rl == 0xfe) {
// start condition
jetiReady = 0;
jbp = 0;
}
else if (rl == 0xff) {
// stop condition
jetiReady = 1;
}
}
else {
// data
if (jbp < 32) {
if (rl==0xDF)
jetiRxBuffer[jbp++] = '@'; //@ => ° Issue 163
else
jetiRxBuffer[jbp++] = rl;
}
}
}
#endif
void JETI_Init (void)
{
DDRE &= ~(1 << DDE0); // set RXD0 pin as input
PORTE &= ~(1 << PORTE0); // disable pullup on RXD0 pin
#ifndef SIMU
#undef BAUD
#define BAUD 9600
#include <util/setbaud.h>
UBRR0H = UBRRH_VALUE;
UBRR0L = UBRRL_VALUE;
UCSR0A &= ~(1 << U2X0); // disable double speed operation
// set 9O1
UCSR0C = (1 << UPM01) | (1 << UPM00) | (1 << UCSZ01) | (1 << UCSZ00);
UCSR0B = (1 << UCSZ02);
// flush receive buffer
while ( UCSR0A & (1 << RXC0) ) UDR0;
#endif
}
void JETI_EnableTXD (void)
{
UCSR0B |= (1 << TXEN0); // enable TX
UCSR0B |= (1 << UDRIE0); // enable UDRE0 interrupt
}
void JETI_DisableTXD (void)
{
UCSR0B &= ~(1 << TXEN0); // disable TX
UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
}
void JETI_EnableRXD (void)
{
UCSR0B |= (1 << RXEN0); // enable RX
UCSR0B |= (1 << RXCIE0); // enable Interrupt
}
void JETI_DisableRXD (void)
{
UCSR0B &= ~(1 << RXEN0); // disable RX
UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
}
#if 0
void JETI_putw (uint16_t c)
{
loop_until_bit_is_set(UCSR0A, UDRE0);
UCSR0B &= ~(1 << TXB80);
if (c & 0x0100)
{
UCSR0B |= (1 << TXB80);
}
UDR0 = c;
}
void JETI_putc (uint8_t c)
{
loop_until_bit_is_set(UCSR0A, UDRE0);
// UCSRB &= ~(1 << TXB8);
UCSR0B |= (1 << TXB80);
UDR0 = c;
}
void JETI_puts (char *s)
{
while (*s)
{
JETI_putc (*s);
s++;
}
}
void JETI_put_start (void)
{
loop_until_bit_is_set(UCSR0A, UDRE0);
UCSR0B &= ~(1 << TXB80);
UDR0 = 0xFE;
}
void JETI_put_stop (void)
{
loop_until_bit_is_set(UCSR0A, UDRE0);
UCSR0B &= ~(1 << TXB80);
UDR0 = 0xFF;
}
#endif
void menuTelemetryJeti(uint8_t event)
{
lcdDrawTelemetryTopBar();
for (uint8_t i=0; i<16; i++) {
lcd_putcAtt((i+2)*FW, 3*FH, jetiRxBuffer[i], BSS);
lcd_putcAtt((i+2)*FW, 4*FH, jetiRxBuffer[i+16], BSS);
}
if (event == EVT_KEY_FIRST(KEY_EXIT)) {
JETI_DisableRXD();
jetiReady = 0;
chainMenu(menuMainView);
}
}
radio/src/telemetry/jeti.h
+56
View File
@@ -0,0 +1,56 @@
/*
* 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 jeti_h
#define jeti_h
#define JETI_KEY_LEFT 0x70
#define JETI_KEY_RIGHT 0xe0
#define JETI_KEY_UP 0xd0
#define JETI_KEY_DOWN 0xb0
#define JETI_KEY_NOCHANGE 0xf0
void JETI_Init(void);
void JETI_EnableRXD (void);
void JETI_DisableRXD (void);
void JETI_EnableTXD (void);
void JETI_DisableTXD (void);
void telemetryInterrupt10ms();
void menuTelemetryJeti(uint8_t event);
#endif
radio/src/telemetry/mavlink.cpp
+739
View File
@@ -0,0 +1,739 @@
/*
* 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
* - Gerard Valade <gerard.valade@gmail.com>
* - Jean-Pierre Parisy
* - Karl Szmutny
* - Michael Blandford
* - Michal Hlavinka
* - Pat Mackenzie
* - Philip Moss
* - Rienk de Jong
* - 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.
*
*/
/*! \file mavlink.cpp
* Mavlink telemetry decoder for Arducopter/Arduplane telemetry information.
*/
#include "telemetry/mavlink.h"
//#define DUMP_RX_TX
// this might need to move to the flight software
//static
mavlink_system_t mavlink_system = { 7, MAV_COMP_ID_MISSIONPLANNER, 0, 0, 0, 0 };
//static uint8_t system_id = 7;
//static uint8_t component_id = 1;
//static uint8_t target_system = 7;
//static uint8_t target_component = 1;
// Mavlink message decoded Status Text
#define PARAM_NB_REPEAT 10
#define LEN_STATUSTEXT 20
static char mav_statustext[LEN_STATUSTEXT];
static int8_t mav_heartbeat = 0;
static int8_t mav_heartbeat_recv = 0;
static uint8_t data_stream_start_stop = 0;
// Telemetry data hold
Telemetry_Data_t telemetry_data;
// *****************************************************
static void MAVLINK_parse_char(uint8_t c);
#ifdef DUMP_RX_TX
#define MAX_RX_BUFFER 16
uint8_t mavlinkRxBufferCount = 0;
uint8_t mavlinkRxBuffer[MAX_RX_BUFFER];
uint8_t mav_dump_rx = 0;
void MAVLINK_rxhandler(uint8_t byte) {
if (mav_dump_rx) {
if (byte == MAVLINK_STX) {
mavlinkRxBufferCount = 0;
}
if (mavlinkRxBufferCount < MAX_RX_BUFFER) {
mavlinkRxBuffer[mavlinkRxBufferCount++] = byte;
}
}
MAVLINK_parse_char(byte);
}
#else
void MAVLINK_rxhandler(uint8_t byte) {
MAVLINK_parse_char(byte);
}
#endif
SerialFuncP RXHandler = MAVLINK_rxhandler;
/*! \brief Reset basic Mavlink varables
* \todo Figure out exact function
*
*/
void MAVLINK_reset(uint8_t warm_reset) {
if (warm_reset && telemetry_data.status) {
mav_statustext[0] = 0;
}
#ifdef DUMP_RX_TX
mavlinkRxBufferCount = 0;
mav_dump_rx = 0;
#endif
mavlink_status_t* p_status = mavlink_get_channel_status(MAVLINK_COMM_0);
p_status->current_rx_seq = 0;
p_status->current_tx_seq = 0;
memset(&telemetry_data, 0, sizeof(telemetry_data));
telemetry_data.rcv_control_mode = ERROR_NUM_MODES;
telemetry_data.req_mode = ERROR_NUM_MODES;
telemetry_data.type = MAV_TYPE_ENUM_END;
telemetry_data.autopilot = MAV_AUTOPILOT_ENUM_END;
telemetry_data.type_autopilot = MAVLINK_INVALID_TYPE;
mav_heartbeat = 0;
mav_heartbeat_recv = 0;
data_stream_start_stop = 0;
}
//! \brief initalize mavlink extension
void MAVLINK_Init(void) {
mav_statustext[0] = 0;
MAVLINK_reset(0);
SERIAL_Init();
}
/*! \brief Status log message
* \details Processes the mavlink status messages. This message contains a
* severity and a message. The severity is an enum difined by MAV_SEVERITY also
* see RFC-5424 for the severity levels.
* The original message is maximum 50 characters and is without termination
* character. For readablity on the 9x the only the first 15 (LEN_STATUSTEXT)
* characters are used. To get the full message you can use the commented
* funtion below.
*/
static inline void REC_MAVLINK_MSG_ID_STATUSTEXT(const mavlink_message_t* msg) {
_MAV_RETURN_char_array(msg, mav_statustext, LEN_STATUSTEXT, 1);
}
/*! \brief System status including cpu load, battery status and communication status.
* \details From this message we use use only the batery infomation. The rest
* is not realy of use while flying. The complete message can be decoded in to
* a struct with the first two commented lines.
* The batery votage is in mV. We devide by 100 to display tenths of volts.'
* \todo Add battery remaining variable in telemetry_data struct for estimated
* remaining battery. Decoding funtion allready in place.
*/
static inline void REC_MAVLINK_MSG_ID_SYS_STATUS(const mavlink_message_t* msg) {
telemetry_data.vbat = mavlink_msg_sys_status_get_voltage_battery(msg) / 100; // Voltage * 10
telemetry_data.ibat = mavlink_msg_sys_status_get_current_battery(msg) / 10;
telemetry_data.rem_bat = mavlink_msg_sys_status_get_battery_remaining(msg);
#ifdef MAVLINK_PARAMS
telemetry_data.vbat_low = (getMavlinParamsValue(BATT_MONITOR) > 0)
&& (((float) telemetry_data.vbat / 10.0) < getMavlinParamsValue(LOW_VOLT)) && (telemetry_data.vbat > 50);
#else
telemetry_data.vbat_low = (telemetry_data.rem_bat < 10);
#endif
}
/*! \brief Recive rc channels
*
*/
static inline void REC_MAVLINK_MSG_ID_RC_CHANNELS_RAW(const mavlink_message_t* msg) {
uint8_t temp_rssi =(mavlink_msg_rc_channels_raw_get_rssi(msg) * 100) / 255;
uint8_t temp_scale = 25 + g_model.mavlink.rc_rssi_scale * 5;
telemetry_data.rc_rssi = (temp_rssi * 100) / temp_scale;
}
/*! \brief Arducopter specific radio message
*
*/
static inline void REC_MAVLINK_MSG_ID_RADIO(const mavlink_message_t* msg) {
if (msg->sysid != 51)
return;
telemetry_data.pc_rssi = (mavlink_msg_radio_get_rssi(msg) * 100) / 255;
telemetry_data.packet_drop = mavlink_msg_radio_get_rxerrors(msg);
telemetry_data.packet_fixed = mavlink_msg_radio_get_fixed(msg);
telemetry_data.radio_sysid = msg->sysid;
telemetry_data.radio_compid = msg->compid;
}
static inline void REC_MAVLINK_MSG_ID_RADIO_STATUS(const mavlink_message_t* msg) {
REC_MAVLINK_MSG_ID_RADIO(msg);
}
//! \brief Navigaion output message
static inline void REC_MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT(const mavlink_message_t* msg) {
telemetry_data.bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg);
}
//! \brief Hud navigation message
static inline void REC_MAVLINK_MSG_ID_VFR_HUD(const mavlink_message_t* msg) {
telemetry_data.heading = mavlink_msg_vfr_hud_get_heading(msg);
telemetry_data.loc_current.rel_alt = mavlink_msg_vfr_hud_get_alt(msg);
}
/*! \brief Heartbeat message
* \details Heartbeat message is used for the following information:
* type and autopilot is used to determain if the UAV is a ArduPlane or Arducopter
*/
static inline void REC_MAVLINK_MSG_ID_HEARTBEAT(const mavlink_message_t* msg) {
telemetry_data.mode = mavlink_msg_heartbeat_get_base_mode(msg);
telemetry_data.custom_mode = mavlink_msg_heartbeat_get_custom_mode(msg);
telemetry_data.status = mavlink_msg_heartbeat_get_system_status(msg);
telemetry_data.mav_sysid = msg->sysid;
telemetry_data.mav_compid = msg->compid;
uint8_t type = mavlink_msg_heartbeat_get_type(msg);
uint8_t autopilot = mavlink_msg_heartbeat_get_autopilot(msg);
if ((type != telemetry_data.type) || (autopilot != telemetry_data.autopilot))
{
telemetry_data.type = mavlink_msg_heartbeat_get_type(msg);
telemetry_data.autopilot = mavlink_msg_heartbeat_get_autopilot(msg);
if (autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA)
{
if ((type == MAV_TYPE_QUADROTOR) ||
(type == MAV_TYPE_COAXIAL) ||
(type == MAV_TYPE_HELICOPTER) ||
(type == MAV_TYPE_HEXAROTOR) ||
(type == MAV_TYPE_OCTOROTOR) ||
(type == MAV_TYPE_TRICOPTER))
{
telemetry_data.type_autopilot = MAVLINK_ARDUCOPTER;
}
else if (type == MAV_TYPE_FIXED_WING)
{
telemetry_data.type_autopilot = MAVLINK_ARDUPLANE;
}
else
telemetry_data.type_autopilot = MAVLINK_INVALID_TYPE;
}
else
telemetry_data.type_autopilot = MAVLINK_INVALID_TYPE;
}
telemetry_data.active = (telemetry_data.mode & MAV_MODE_FLAG_SAFETY_ARMED) ? true : false;
mav_heartbeat = 3; // 450ms display '*'
mav_heartbeat_recv = 1;
}
static inline void REC_MAVLINK_MSG_ID_HIL_CONTROLS(const mavlink_message_t* msg) {
telemetry_data.nav_mode = mavlink_msg_hil_controls_get_mode(msg);
}
/*! \brief Process GPS raw intger message
* \details This message contains the following data:
* - fix type: 0-1: no fix, 2: 2D fix, 3: 3D fix.
* - Latitude, longitude in 1E7 * degrees
* - Altutude 1E3 * meters above MSL.
* - Course over ground in 100 * degrees
* - GPS HDOP horizontal dilution of precision in cm (m*100).
* - Ground speed in m/s * 100
*/
static inline void REC_MAVLINK_MSG_ID_GPS_RAW_INT(const mavlink_message_t* msg) {
telemetry_data.fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg);
telemetry_data.loc_current.lat = mavlink_msg_gps_raw_int_get_lat(msg) / 1E7;
telemetry_data.loc_current.lon = mavlink_msg_gps_raw_int_get_lon(msg) / 1E7;
telemetry_data.loc_current.gps_alt = mavlink_msg_gps_raw_int_get_alt(msg) / 1E3;
telemetry_data.eph = mavlink_msg_gps_raw_int_get_eph(msg) / 100.0;
telemetry_data.course = mavlink_msg_gps_raw_int_get_cog(msg) / 100.0;
telemetry_data.v = mavlink_msg_gps_raw_int_get_vel(msg) / 100.0 ;
telemetry_data.satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(msg);
}
#ifdef MAVLINK_PARAMS
const pm_char * getParamId(uint8_t idx) {
const pm_char *mav_params_id [((NB_PID_PARAMS / 2) + 4)] = { //
//
PSTR("RATE_YAW"), // Rate Yaw
PSTR("STB_YAW"), // Stabilize Yaw
PSTR("RATE_PIT"), // Rate Pitch
PSTR("STB_PIT"), // Stabilize Pitch
PSTR("RATE_RLL"), // Rate Roll
PSTR("STB_RLL"), // Stabilize Roll
PSTR("THR_ALT"), // PSTR("THR_BAR"), // Altitude Hold
PSTR("HLD_LON"), // Loiter
PSTR("HLD_LAT"), // Loiter
PSTR("NAV_LON"), // Nav WP
PSTR("NAV_LAT"), // Nav WP
PSTR("LOW_VOLT"), // Battery low voltage
PSTR("VOLT_DIVIDER"), //
PSTR("BATT_MONITOR"), //
PSTR("BATT_CAPACITY") };
uint8_t i;
if (idx < NB_PID_PARAMS) {
i = idx / 2;
} else {
i = idx - (NB_PID_PARAMS / 2);
}
return mav_params_id[i];
}
void setMavlinParamsValue(uint8_t idx, float val) {
MavlinkParam_t *param = getParam(idx);
if (idx < NB_PARAMS && val != param->value) {
param->value = val;
param->repeat = PARAM_NB_REPEAT;
uint8_t link_idx = NB_PID_PARAMS;
switch (idx) {
case RATE_PIT_P:
case RATE_PIT_I:
case STB_PIT_P:
case STB_PIT_I:
link_idx = idx + 4;
break;
case RATE_RLL_P:
case RATE_RLL_I:
case STB_RLL_P:
case STB_RLL_I:
link_idx = idx - 4;
break;
case HLD_LON_P:
case HLD_LON_I:
case NAV_LON_P:
case NAV_LON_I:
link_idx = idx + 2;
break;
case HLD_LAT_P:
case HLD_LAT_I:
case NAV_LAT_P:
case NAV_LAT_I:
link_idx = idx - 2;
break;
default:
break;
}
if (link_idx < NB_PID_PARAMS) {
MavlinkParam_t *p = getParam(link_idx);
p->value = val;
p->repeat = PARAM_NB_REPEAT;
}
}
}
void putsMavlinParams(uint8_t x, uint8_t y, uint8_t idx, uint8_t att) {
if (idx < NB_PARAMS) {
const pm_char * s = getParamId(idx);
char c;
while ((c = pgm_read_byte(s++))) {
lcd_putcAtt(x, y, (c == '_' ? ' ' : c), 0);
x += FW;
}
if (idx < NB_PID_PARAMS) {
x = 11 * FW;
lcd_putcAtt(x, y, "PI"[idx & 0x01], att);
}
}
}
static inline void setParamValue(int8_t *id, float value) {
int8_t *p_id;
for (int8_t idx = 0; idx < NB_PARAMS; idx++) {
const pm_char * s = getParamId(idx);
p_id = id;
while (1) {
char c1 = pgm_read_byte(s++);
if (!c1) {
// Founded !
uint8_t founded = !*p_id;
if (idx < NB_PID_PARAMS) {
p_id++;
switch (*p_id++) {
case 'P':
founded = !*p_id;
break;
case 'I':
founded = !*p_id;
idx++;
break;
default:
founded = 0;
break;
}
}
// test end of string char == 0 and a valid PI
if (founded) {
MavlinkParam_t *param = getParam(idx);
param->repeat = 0;
param->valid = 1;
param->value = value;
mav_req_params_nb_recv++;
}
return;
} else if (c1 != *p_id++) {
break;
}
}
if (idx < NB_PID_PARAMS) {
// Skip I Parameter from PID
idx++;
}
}
}
static inline void REC_MAVLINK_MSG_ID_PARAM_VALUE(const mavlink_message_t* msg) {
mavlink_param_value_t param_value;
mavlink_msg_param_value_decode(msg, &param_value);
char *id = param_value.param_id;
setParamValue((int8_t*)id, param_value.param_value);
data_stream_start_stop = 0; // stop data stream while getting params list
watch_mav_req_params_list = mav_req_params_nb_recv < (NB_PARAMS - 5) ? 20 : 0; // stop timeout
}
#endif
static inline void handleMessage(mavlink_message_t* p_rxmsg) {
switch (p_rxmsg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT:
REC_MAVLINK_MSG_ID_HEARTBEAT(p_rxmsg);
break;
case MAVLINK_MSG_ID_STATUSTEXT:
REC_MAVLINK_MSG_ID_STATUSTEXT(p_rxmsg);
AUDIO_WARNING1();
break;
case MAVLINK_MSG_ID_SYS_STATUS:
REC_MAVLINK_MSG_ID_SYS_STATUS(p_rxmsg);
break;
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
REC_MAVLINK_MSG_ID_RC_CHANNELS_RAW(p_rxmsg);
break;
case MAVLINK_MSG_ID_RADIO:
REC_MAVLINK_MSG_ID_RADIO(p_rxmsg);
break;
case MAVLINK_MSG_ID_RADIO_STATUS:
REC_MAVLINK_MSG_ID_RADIO_STATUS(p_rxmsg);
break;
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
REC_MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT(p_rxmsg);
break;
case MAVLINK_MSG_ID_VFR_HUD:
REC_MAVLINK_MSG_ID_VFR_HUD(p_rxmsg);
break;
case MAVLINK_MSG_ID_HIL_CONTROLS:
REC_MAVLINK_MSG_ID_HIL_CONTROLS(p_rxmsg);
break;
case MAVLINK_MSG_ID_GPS_RAW_INT:
REC_MAVLINK_MSG_ID_GPS_RAW_INT(p_rxmsg);
break;
#ifdef MAVLINK_PARAMS
case MAVLINK_MSG_ID_PARAM_VALUE:
REC_MAVLINK_MSG_ID_PARAM_VALUE(p_rxmsg);
break;
#endif
}
}
/*! \brief Mavlink message parser
* \details Parses the characters in to a mavlink message.
* Case statement parses each character as it is recieved.
* \attention One big change form the 0.9 to 1.0 version is the
* MAVLINK_CRC_EXTRA. This requires the mavlink_message_crcs array of 256 bytes.
* \todo create dot for the statemachine
*/
static void MAVLINK_parse_char(uint8_t c) {
static mavlink_message_t m_mavlink_message;
//! The currently decoded message
static mavlink_message_t* p_rxmsg = &m_mavlink_message;
//! The current decode status
mavlink_status_t* p_status = mavlink_get_channel_status(MAVLINK_COMM_0);
#if MAVLINK_CRC_EXTRA
static const uint8_t mavlink_message_crcs[256] PROGMEM = MAVLINK_MESSAGE_CRCS;
#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
#endif
// Initializes only once, values keep unchanged after first initialization
//mavlink_parse_state_initialize(p_status);
//p_status->msg_received = 0;
switch (p_status->parse_state) {
case MAVLINK_PARSE_STATE_UNINIT:
case MAVLINK_PARSE_STATE_IDLE:
if (c == MAVLINK_STX) {
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
mavlink_start_checksum(p_rxmsg);
}
break;
case MAVLINK_PARSE_STATE_GOT_STX:
// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
p_rxmsg->len = c;
p_status->packet_idx = 0;
mavlink_update_checksum(p_rxmsg, c);
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
break;
case MAVLINK_PARSE_STATE_GOT_LENGTH:
p_rxmsg->seq = c;
mavlink_update_checksum(p_rxmsg, c);
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
break;
case MAVLINK_PARSE_STATE_GOT_SEQ:
p_rxmsg->sysid = c;
mavlink_update_checksum(p_rxmsg, c);
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
break;
case MAVLINK_PARSE_STATE_GOT_SYSID:
p_rxmsg->compid = c;
mavlink_update_checksum(p_rxmsg, c);
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
break;
case MAVLINK_PARSE_STATE_GOT_COMPID:
p_rxmsg->msgid = c;
mavlink_update_checksum(p_rxmsg, c);
if (p_rxmsg->len == 0) {
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
} else {
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
}
break;
case MAVLINK_PARSE_STATE_GOT_MSGID:
_MAV_PAYLOAD_NON_CONST(p_rxmsg)[p_status->packet_idx++] = (char) c;
mavlink_update_checksum(p_rxmsg, c);
if (p_status->packet_idx == p_rxmsg->len) {
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
}
break;
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
#if MAVLINK_CRC_EXTRA
mavlink_update_checksum(p_rxmsg, pgm_read_byte(&(mavlink_message_crcs[p_rxmsg->msgid])));
#endif
if (c != (p_rxmsg->checksum & 0xFF)) {
// Check first checksum byte
p_status->parse_error = 3;
} else {
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
}
break;
case MAVLINK_PARSE_STATE_GOT_CRC1:
if (c != (p_rxmsg->checksum >> 8)) {
// Check second checksum byte
p_status->parse_error = 4;
} else {
// Successfully got message
if (mav_heartbeat < 0)
mav_heartbeat = 0;
p_status->current_rx_seq = p_rxmsg->seq;
//p_status->msg_received = 1;
p_status->parse_state = MAVLINK_PARSE_STATE_IDLE;
//memcpy(r_message, p_rxmsg, sizeof(mavlink_message_t));
handleMessage(p_rxmsg);
}
break;
}
// Error occur
if (p_status->parse_error) {
p_status->parse_state = MAVLINK_PARSE_STATE_IDLE;
if (c == MAVLINK_STX) {
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
mavlink_start_checksum(p_rxmsg);
}
p_status->parse_error = 0;
}
// If a message has been sucessfully decoded, check index
/*
if (p_status->msg_received == 1) {
p_status->current_rx_seq = p_rxmsg->seq;
p_status->packet_rx_success_count++;
}
*/
//r_mavlink_status->current_rx_seq = p_status->current_rx_seq + 1;
//r_mavlink_status->packet_rx_success_count = p_status->packet_rx_success_count;
//r_mavlink_status->packet_rx_drop_count = p_status->parse_error;
//p_status->parse_error = 0;
//return p_status->msg_received;
}
#ifdef MAVLINK_PARAMS
static inline void MAVLINK_msg_param_request_list_send() {
mavlink_channel_t chan = MAVLINK_COMM_0;
mavlink_msg_param_request_list_send(chan, mavlink_system.sysid, mavlink_system.compid);
}
static inline void MAVLINK_msg_param_set(uint8_t idx) {
const pm_char* s = getParamId(idx);
int8_t buf[15];
int8_t *p = buf;
while (1) {
char c = pgm_read_byte(s++);
if (!c) {
if (idx < NB_PID_PARAMS) {
*p++ = '_';
uint8_t colIdx = idx & 0x01;
*p++ = "PI"[colIdx];
}
*p++ = 0;
break;
}
*p++ = c;
}
//float param_value = ((float) telemetry_data.params[idx].pi_param[subIdx].pi_value / 100.00 + 0.005);
float param_value = getParam(idx)->value;
mavlink_channel_t chan = MAVLINK_COMM_0;
uint8_t param_type = (uint8_t)MAV_PARAM_TYPE_REAL32;
const char* param_id = (const char*)buf;
mavlink_msg_param_set_send(chan, mavlink_system.sysid, mavlink_system.compid, param_id, param_value, param_type);
}
#endif
static inline void MAVLINK_msg_request_data_stream_pack_send(uint8_t req_stream_id, uint16_t req_message_rate,
uint8_t start_stop) {
mavlink_channel_t chan = MAVLINK_COMM_0;
mavlink_msg_request_data_stream_send(chan, mavlink_system.sysid, mavlink_system.compid, req_stream_id, req_message_rate,
start_stop);
}
//! \brief old mode switch funtion
static inline void MAVLINK_msg_action_pack_send(uint8_t action) {
// mavlink_channel_t chan = MAVLINK_COMM_0;
// mavlink_msg_action_send(chan, mavlink_system.sysid, mavlink_system.compid, action);
}
//! \brief old mode switch funtion
static inline void MAVLINK_msg_set_mode_send(uint8_t mode) {
mavlink_channel_t chan = MAVLINK_COMM_0;
mavlink_msg_set_mode_send(chan, mavlink_system.sysid, mode, 0);
}
/*! \brief Looks like som kind of task switcher on a timer
* \todo Figure out where this was used for and intergrate in current
* implemnetation. Funcion dissabled without any side affects.
*/
#if 0
void MAVLINK10mspoll(uint8_t count) {
switch (count) {
case 2: // MAVLINK_MSG_ID_ACTION
if (watch_mav_req_id_action > 0) {
watch_mav_req_id_action--;
// Repeat is not ack : 150ms*0x07
if ((watch_mav_req_id_action & 0x07) == 0) {
uint8_t action = MAVLINK_CtrlMode2Action(telemetry_data.req_mode);
MAVLINK_msg_action_pack_send(action);
char *ptr = mav_statustext;
*ptr++ = 'R';
*ptr++ = 'Q';
*ptr++ = ' ';
*ptr++ = action / 10 + '0';
*ptr++ = action % 10 + '0';
*ptr++ = 0;
}
}
if (telemetry_data.ack_result < 5) {
if (telemetry_data.ack_result > 0) {
telemetry_data.ack_result++;
}
}
break;
#ifdef MAVLINK_PARAMS
case 4: // MAVLINK_MSG_ID_PARAM_REQUEST_LIST
if (watch_mav_req_params_list > 0) {
watch_mav_req_params_list--;
if (watch_mav_req_params_list == 0) {
mav_req_params_nb_recv = 0;
MAVLINK_msg_param_request_list_send();
watch_mav_req_params_list = 20;
}
}
break;
#endif
case 6: // MAVLINK_MSG_ID_REQUEST_DATA_STREAM
if (watch_mav_req_start_data_stream > 0) {
watch_mav_req_start_data_stream--;
if (watch_mav_req_start_data_stream == 0) {
uint8_t req_stream_id = 2;
uint16_t req_message_rate = 1;
MAVLINK_msg_request_data_stream_pack_send(req_stream_id, req_message_rate, data_stream_start_stop);
watch_mav_req_start_data_stream = 20;
data_stream_start_stop = 1; // maybe start next time
}
}
break;
#ifdef MAVLINK_PARAMS
case 8: // MAVLINK_MSG_ID_PARAM_SET
if (watch_mav_req_params_set > 0) {
watch_mav_req_params_set--;
if (watch_mav_req_params_set == 0) {
for (uint8_t idx = 0; idx < NB_PARAMS; idx++) {
if (getParam(idx)->repeat) {
getParam(idx)->repeat--;
MAVLINK_msg_param_set(idx);
watch_mav_req_params_set = 3; // 300ms
return;
}
}
}
}
break;
#endif
default:
return;
}
}
#endif
/*! \brief Telemetry monitoring, calls \link MAVLINK10mspoll.
* \todo Reimplemnt \link MAVLINK10mspoll
*
*/
void telemetryWakeup() {
uint16_t tmr10ms = get_tmr10ms();
uint8_t count = tmr10ms & 0x0f; // 15*10ms
if (!count) {
if (mav_heartbeat > -30) {
// TODO mavlink_system.sysid = g_eeGeneral.mavTargetSystem;
mav_heartbeat--;
if (mav_heartbeat == -30) {
MAVLINK_reset(1);
SERIAL_Init();
}
// SERIAL_startTX();
}
}
if (mav_heartbeat_recv && !IS_TX_BUSY) {
// MAVLINK10mspoll(count);
}
}
radio/src/telemetry/mavlink.h
+338
View File
@@ -0,0 +1,338 @@
/*
* 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
* - Gerard Valade <gerard.valade@gmail.com>
* - Jean-Pierre Parisy
* - Karl Szmutny
* - Michael Blandford
* - Michal Hlavinka
* - Pat Mackenzie
* - Philip Moss
* - Rienk de Jong
* - Rob Thomson
* - Romolo Manfredini <romolo.manfredini@gmail.com>
* - Thomas Husterer
*
* Adapted from mavlink for ardupilot
*
* 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.
*
*/
/*! \file mavlink.h
* Mavlink include file
*/
#ifndef _MAVLINK_H_
#define _MAVLINK_H_
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
#define MAVLINK_COMM_NUM_BUFFERS 1
#include "GCS_MAVLink/include_v1.0/mavlink_types.h"
#include "serial.h"
#include "opentx.h"
#include "serial.h"
//#include "include/mavlink_helpers.h"
extern mavlink_system_t mavlink_system;
extern void SERIAL_start_uart_send();
extern void SERIAL_end_uart_send();
extern void SERIAL_send_uart_bytes(const uint8_t * buf, uint16_t len);
# define MAV_SYSTEM_ID 1
//mavlink_system.type = 2; //MAV_QUADROTOR;
#define MAVLINK_START_UART_SEND(chan,len) SERIAL_start_uart_send()
#define MAVLINK_END_UART_SEND(chan,len) SERIAL_end_uart_send()
#define MAVLINK_SEND_UART_BYTES(chan,buf,len) SERIAL_send_uart_bytes(buf,len)
#include "../GCS_MAVLink/include_v1.0/ardupilotmega/mavlink.h"
//#define MAVLINK_PARAMS
//#define DUMP_RX_TX
#define ERROR_NUM_MODES 99
#define ERROR_MAV_ACTION_NB 99
// Auto Pilot modes
// ----------------
#define AC_STABILIZE 0 // hold level position
#define AC_ACRO 1 // rate control
#define AC_ALT_HOLD 2 // AUTO control
#define AC_AUTO 3 // AUTO control
#define AC_GUIDED 4 // AUTO control
#define AC_LOITER 5 // Hold a single location
#define AC_RTL 6 // AUTO control
#define AC_CIRCLE 7 // AUTO control
#define AC_POSITION 8 // AUTO control
#define AC_LAND 9 // AUTO control
#define AC_OF_LOITER 10 // Hold a single location using optical flow
// sensor
#define AC_TOY_A 11 // THOR Enum for Toy mode
#define AC_TOY_M 12 // THOR Enum for Toy mode
#define AC_NUM_MODES 13
// Mavlink airframe types
#define MAVLINK_ARDUCOPTER 0
#define MAVLINK_ARDUPLANE 1
#define MAVLINK_INVALID_TYPE 2
#define AP_MANUAL 0
#define AP_CIRCLE 1
#define AP_STABILIZE 2
#define AP_TRAINING 3
#define AP_FLY_BY_WIRE_A 5
#define AP_FLY_BY_WIRE_B 6
#define AP_AUTO 10
#define AP_RTL 11
#define AP_LOITER 12
#define AP_GUIDED 15
#define AP_INITIALISING 16
#define AP_NUM_MODES 17
static const uint8_t ap_modes_lut[18] PROGMEM = {0,1,2,3,12,4,5,12,12,12,6,7,8,9,12,12,10,11};
/*
* Type definitions
*/
#ifdef MAVLINK_PARAMS
enum ACM_PARAMS {
RATE_YAW_P, // Rate Yaw
RATE_YAW_I, // Rate Yaw
STB_YAW_P, // Stabilize Yaw
STB_YAW_I, // Stabilize Yaw
RATE_PIT_P, // Rate Pitch
RATE_PIT_I, // Rate Pitch
STB_PIT_P, // Stabilize Pitch
STB_PIT_I, // Stabilize Pitch
RATE_RLL_P, // Rate Roll
RATE_RLL_I, // Rate Roll
STB_RLL_P, // Stabilize Roll
STB_RLL_I, // Stabilize Roll
THR_ALT_P, // THR_BAR, // Altitude Hold
THR_ALT_I, // THR_BAR, // Altitude Hold
HLD_LON_P, // Loiter
HLD_LON_I, // Loiter
HLD_LAT_P, // Loiter
HLD_LAT_I, // Loiter
NAV_LON_P, // Nav WP
NAV_LON_I, // Nav WP
NAV_LAT_P, // Nav WPs
NAV_LAT_I, // Nav WP
NB_PID_PARAMS, // Number of PI Parameters
LOW_VOLT = NB_PID_PARAMS,
IN_VOLT, //
BATT_MONITOR, //
BATT_CAPACITY, //
NB_PARAMS
};
//#define NB_PID_PARAMS 24
#define NB_COL_PARAMS 2
#define NB_ROW_PARAMS ((NB_PARAMS+1)/NB_COL_PARAMS)
typedef struct MavlinkParam_ {
uint8_t repeat :4;
uint8_t valid :4;
float value;
} MavlinkParam_t;
#endif
typedef struct Location_ {
float lat; ///< Latitude in degrees
float lon; ///< Longitude in degrees
float gps_alt; ///< Altitude in meters
float rel_alt;
} Location_t;
typedef struct Telemetry_Data_ {
// INFOS
uint8_t status; ///< System status flag, see MAV_STATUS ENUM
uint8_t type;
uint8_t autopilot;
uint8_t type_autopilot;
uint16_t packet_drop;
uint16_t packet_fixed;
uint8_t radio_sysid;
uint8_t radio_compid;
uint8_t mav_sysid;
uint8_t mav_compid;
uint8_t mode;
uint32_t custom_mode;
bool active;
uint8_t nav_mode;
uint8_t rcv_control_mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h
uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
uint8_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt)
uint8_t ibat; ///< Battery voltage, in millivolts (1 = 1 millivolt)
uint8_t rem_bat; ///< Battery voltage, in millivolts (1 = 1 millivolt)
bool vbat_low;
uint8_t rc_rssi;
uint8_t pc_rssi;
uint8_t debug;
// MSG ACTION / ACK
uint8_t req_mode;
int8_t ack_result;
// GPS
uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
uint8_t satellites_visible; ///< Number of satellites visible
Location_t loc_current;
float eph;
uint16_t course;
float v; // Ground speed
// Navigation
uint16_t heading;
uint16_t bearing;
#ifdef MAVLINK_PARAMS
// Params
MavlinkParam_t params[NB_PARAMS];
#endif
} Telemetry_Data_t;
// Telemetry data hold
extern Telemetry_Data_t telemetry_data;
/*
* Funtion definitions
*/
extern inline uint8_t MAVLINK_CtrlMode2Action(uint8_t mode) {
uint8_t action;
return action;
}
#if 0
extern inline uint8_t MAVLINK_Action2CtrlMode(uint8_t action) {
uint8_t mode = ERROR_NUM_MODES;
switch (action) {
return action;
}
}
#endif
void telemetryWakeup();
void MAVLINK_Init(void);
void menuTelemetryMavlink(uint8_t event);
void MAVLINK10mspoll(uint16_t time);
#ifdef MAVLINK_PARAMS
void putsMavlinParams(uint8_t x, uint8_t y, uint8_t idx, uint8_t att);
void setMavlinParamsValue(uint8_t idx, float val);
inline uint8_t getIdxParam(uint8_t rowIdx, uint8_t colIdx) {
return (rowIdx * NB_COL_PARAMS) + colIdx;
}
inline MavlinkParam_t * getParam(uint8_t idx) {
return &telemetry_data.params[idx];
}
inline float getMavlinParamsValue(uint8_t idx) {
return telemetry_data.params[idx].value;
}
inline uint8_t isDirtyParamsValue(uint8_t idx) {
return telemetry_data.params[idx].repeat;
}
inline uint8_t isValidParamsValue(uint8_t idx) {
return telemetry_data.params[idx].valid;
}
inline float getCoefPrecis(uint8_t precis) {
switch (precis) {
case 1:
return 10.0;
case 2:
return 100.0;
case 3:
return 1000.0;
}
return 1.0;
}
inline int16_t getMaxMavlinParamsValue(uint8_t idx) {
int16_t max = 0;
switch (idx) {
case LOW_VOLT:
max = 2500; // 25.0 Volt max
break;
case IN_VOLT:
max = 900; // 7.00 Volt max
break;
case BATT_MONITOR:
max = 3;
break;
case BATT_CAPACITY:
max = 7000; // 7000 mAh max
break;
default:
if (idx < NB_PID_PARAMS) {
max = (idx & 0x01) ? 1000 : 750;
}
break;
}
return max;
}
inline uint8_t getPrecisMavlinParamsValue(uint8_t idx) {
uint8_t precis = 2;
switch (idx) {
case LOW_VOLT:
precis = 2;
break;
case IN_VOLT:
precis = 2;
break;
case BATT_MONITOR:
precis = 0;
break;
case BATT_CAPACITY:
precis = 0;
break;
default:
if (idx < NB_PID_PARAMS) {
if (idx & 0x01)
precis = 3;
}
break;
}
return precis;
}
void lcd_outdezFloat(uint8_t x, uint8_t y, float val, uint8_t precis, uint8_t mode);
#endif
#endif
radio/src/telemetry/nmea.cpp
+842
View File
@@ -0,0 +1,842 @@
/*
* 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"
#define LG_BUF 14
#define NB_LONG_BUF 3
#define NB_SHORT_BUF 3
#define LONG_BUF(val) (val)
#define SHORT_BUF(val) (val+NB_LONG_BUF)
#define VALSTR(val) (rbuf[val][0] ? rbuf[val] : val_unknown)
#define APSIZE (BSS | DBLSIZE)
uint8_t i; // working variable
uint8_t state; // currrent state
uint8_t rval, rpack; // received items
uint8_t xval[NB_LONG_BUF+NB_SHORT_BUF]; // expected value
uint8_t xpack[NB_LONG_BUF+NB_SHORT_BUF]; // expected packet
uint8_t ibuf[NB_LONG_BUF]; // subscripts on long buffers values
char rbuf[NB_LONG_BUF][LG_BUF]; // long receive buffers
char sbuf[NB_SHORT_BUF]; // short receive buffers
const char val_unknown[] = "?";
int32_t home_alt, save_alt, rel_alt, prev_alt, lift_alt, max_alt, abs_alt; // integer values for altitude computations
int32_t gpstimer=0;
int32_t gpstime;
uint8_t ggareceived;
uint8_t beep_on;
uint8_t show_timer;
/* Received data
Data are received as packets, each packet is identified by a prefix of seven
characters ('$GPGGA,' or '$GPRMC,')and is ended by one star plus two bytes checksum.
The values are terminated by a comma.
$GPGGA - Global Positioning System Fix Data, Time, Position and fix related data fora GPS receiver.
11
1 2 3 4 5 6 7 8 9 10 | 12 13 14 15
| | | | | | | | | | | | | | |
GGA,hhmmss.ss,llll.ll,a,yyyyy.yy,a,x,xx,x.x,x.x,M,x.x,M,x.x,xxxx*hh<CR><LF>
Field Number:
1) Universal Time Coordinated (UTC)
2) Latitude
3) N or S (North or South)
4) Longitude
5) E or W (East or West)
6) GPS Quality Indicator,
0 - fix not available,
1 - GPS fix,
2 - Differential GPS fix
7) Number of satellites in view, 00 - 12
8) Horizontal Dilution of precision
9) Antenna Altitude above/below mean-sea-level (geoid)
10) Units of antenna altitude, meters
11) Geoidal separation, the difference between the WGS-84 earth
ellipsoid and mean-sea-level (geoid), "-" means mean-sea-level
below ellipsoid
12) Units of geoidal separation, meters
13) Age of differential GPS data, time in seconds since last SC104
type 1 or 9 update, null field when DGPS is not used
14) Differential reference station ID, 0000-1023
*
15) Checksum
CrLf
$GPRMC - Recommended Minimum Navigation Information
12
1 2 3 4 5 6 7 8 9 10 11|
| | | | | | | | | | | |
RMC,hhmmss.ss,A,llll.ll,a,yyyyy.yy,a,x.x,x.x,xxxx,x.x,a*hh<CR><LF>
Field Number:
1) UTC Time
2) Status, V = Navigation receiver warning
3) Latitude
4) N or S
5) Longitude
6) E or W
7) Speed over ground, knots
8) Track made good, degrees true. = = Course over ground (COG)
9) Date, ddmmyy
10) Magnetic Variation, degrees
11) E or W
12) Checksum
*/
// GGA record prefix
#define PACK_GGA 0x47 // "G"
#define PACK_GGA3 0x41 // "A"
// value occurence number in the GGA packet
#define TIM 1
#define LAT 2
#define NOS 3
#define LON 4
#define EOW 5
#define FIX 6
#define SAT 7
#define DIL 8
#define ALT 9
#define MTR 10
#define GEO 11
#define MET 12
#define AGE 13
#define DIF 14
// RMC record prefix
#define PACK_RMC 0x52 // "R"
#define PACK_RMC2 0x4D // "M"
#define PACK_RMC3 0x43 // "C"
// value occurence number in the RMC packet
#define TIM 1
#define NRW 2
#define LT1 3
#define NSO 4
#define LN1 5
#define EWE 6
#define SOG 7
#define COG 8
#define DAT 9
#define MAG 10
#define EAW 11
// end of packet
#define PACK_END 0x2a // *
// end of value
#define VAL_END 0x2c // ,
// stateful machine
// Since the packets are sent continuously, we need to synchronize on the
// reception of the three chars prefixing a packet, whatever they are.
// states values
#define WAIT_PACKET 1
#define WAIT_PACK_GGA1 2
#define WAIT_PACK_GGA2 3
#define WAIT_PACK_GGA3 4
#define WAIT_PACK_RMC2 5
#define WAIT_PACK_RMC3 6
#define WAIT_VAL_END 7
#define READ_VALUE 8
void menuTelemetryNMEA1(uint8_t event);
void menuTelemetryNMEA2(uint8_t event);
void menuTelemetryNMEA3(uint8_t event);
void menuTelemetryNMEA4(uint8_t event);
void title(char x);
void initval(uint8_t num, uint8_t pack, uint8_t val);
int32_t binary (char *str);
int32_t bintime (char *str);
#ifndef SIMU
ISR (USART0_RX_vect)
{
uint8_t rl;
// uint8_t rh; //USART control and Status Register 0 B
uint8_t iostat; //USART control and Status Register 0 A
rl = UDR0;
iostat = 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
MPCM0: MultiProcessor Comms Mode
*/
if (iostat & ((1 << FE0) | (1 << DOR0) | (1 << UPE0)))
{
rl = xpack[0] = xpack[1] = xval[0] = xval[1] = 0;
initval (LONG_BUF(2), PACK_GGA, TIM); // always get UTC time for timer
state = WAIT_PACKET; // restart on error
}
// 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
*/
switch (state)
{
case WAIT_PACKET:
switch (rl)
{
case PACK_GGA: // found a new GGA packet "G"
state = WAIT_PACK_GGA2; // wait for the 2nd char
break;
case PACK_RMC: // found a new RMS packet "R"
state = WAIT_PACK_RMC2; // wait for the 2nd char
break;
}
break;
case WAIT_PACK_GGA2: // received 2nd char "G"
if (rl == PACK_GGA)
state = WAIT_PACK_GGA3; // wait for 3rd character "A"
else
state = WAIT_PACKET; // restart if not "G"
break;
case WAIT_PACK_GGA3: // received 3rd char "A"
if (rl == PACK_GGA3) // found
{
state = WAIT_VAL_END; // wait for ","
rpack = PACK_GGA;
rval = 1; //clear the buffer
for (i = 0; i < NB_LONG_BUF; i++)
ibuf[i] = 0;
}
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_RMC2: // wait for 2nd char "M"
if (rl == PACK_RMC2)
state = WAIT_PACK_RMC3;
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_PACK_RMC3: // wait for 3rd char "C"
if (rl == PACK_RMC3)
{
state = WAIT_VAL_END; // wait for ","
rpack = PACK_RMC;
rval = 1;
for (i = 0; i < NB_LONG_BUF; i++) // clear buffer
ibuf[i] = 0;
}
else
state = WAIT_PACKET; // restart if not found
break;
case WAIT_VAL_END:
if (rl == VAL_END) // "," nach "GGA" oder "RMC"
{
state = READ_VALUE;
rval = 1;
for (i = 0; i < NB_LONG_BUF; i++) // clear buffer
ibuf[i] = 0;
}
else
state = WAIT_PACKET; // restart if not found
break;
case READ_VALUE:
switch (rl)
{
case PACK_END:
if (rpack == PACK_GGA)
ggareceived = 1;
state = WAIT_PACKET; // packet completed, wait for the next packet
break;
case VAL_END: // comma found, value completed
rval++; // and get next value
break;
default: // store the char in the corresponding buffer
for (i = 0; i < NB_LONG_BUF; i++)
{ // is it the expected value in the expected packet ?
if (rpack == xpack[i] && rval == xval[i] && ibuf[i] < LG_BUF - 1)
{ // yes, store the char
rbuf[i] [ibuf[i]] = rl;
ibuf[i]++;
rbuf[i] [ibuf[i]] = 0;
}
}
for (i = NB_LONG_BUF; i < NB_LONG_BUF+NB_SHORT_BUF; i++) {
if (rpack == xpack[i] // is this the expected short value in the expected packet ?
&& rval == xval[i])
sbuf[i-NB_LONG_BUF] = rl; // yes, store the char
}
}
break;
}
}
#endif
void NMEA_Init (void)
{
#ifndef SIMU
DDRE &= ~(1 << DDE0); // set RXD0 pin as input
PORTE &= ~(1 << PORTE0); // disable pullup on RXD0 pin
// switch (Telem_baud)
// {
// case 1:
#undef BAUD
#define BAUD 4800
#include <util/setbaud.h>
UBRR0H = UBRRH_VALUE;
UBRR0L = UBRRL_VALUE;
// break;
// }
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
#endif
home_alt = rel_alt = ggareceived =0;
gpstimer = -1;
beep_on=1;
}
// TX Capabilities are not required for NMEA
// void NMEA_DisableTXD (void)
// {
// UCSR0B &= ~(1 << TXEN0); // disable TX
// }
// void NMEA_EnableTXD (void)
// {
// UCSR0B |= (1 << TXEN0); // enable TX
// }
void NMEA_DisableRXD (void)
{
UCSR0B &= ~(1 << RXEN0); // disable RX
UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
}
void NMEA_EnableRXD (void)
{
for (i = 0; i < NB_LONG_BUF; i++)
{
ibuf[i] = 0;
rbuf[i][0] = 0;
xpack[i] = 0;
xval[i] = 0;
}
initval (LONG_BUF(2), PACK_GGA, TIM); // always get UTC time for timer
state = WAIT_PACKET; // wait for the next packet
UCSR0B |= (1 << RXEN0); // enable RX
UCSR0B |= (1 << RXCIE0); // enable Interrupt
}
void menuTelemetryNMEA(uint8_t event)
{
menuTelemetryNMEA1(event);
}
// Start of NMEA menus 1-4 <<<<<<<<<<<<<<<<<<<<<<<<<<<
void menuTelemetryNMEA1(uint8_t event)
{
switch(event) // new event received, branch accordingly
{
case EVT_KEY_BREAK(KEY_LEFT):
chainMenu(menuTelemetryNMEA4);
return;
case EVT_KEY_BREAK(KEY_RIGHT):
chainMenu(menuTelemetryNMEA2);
return;
case EVT_KEY_LONG(KEY_UP):
NMEA_DisableRXD();
chainMenu(menuStatisticsView);
return;
case EVT_KEY_LONG(KEY_DOWN):
NMEA_DisableRXD();
chainMenu(menuMainView);
return;
case EVT_KEY_FIRST(KEY_MENU):
if (show_timer == 0) {
show_timer = 1;
if (gpstimer <= 0)
gpstimer = bintime(rbuf[2]);
}
else
show_timer = 0;
break;
case EVT_KEY_FIRST(KEY_EXIT):
if ((show_timer == 1) &&(rbuf[2][0]))
gpstimer = bintime(rbuf[2]); // get actual GPS time ->resets timer to 00:00
break;
}
/*
How to use:
You choose the values to be displayed using the function:
initval(<number>, <packet>, <value>);
-------------------------------------
That means that "<value>" of "<packet>" is stored in the <number> buffer.
The first <number> is 0.
Here are the packet names and the associated value names:
Position packet (beginning with "GGA"): "PACK_GGA"
value names: "TIM", "LAT", "NOS", "LON", "EOW", "FIX", "SAT", "DIL", "ALT", "MTR", "GEO", "MET", "AGE", "DIF",
Required minimum packet (beginning with "RMC"): "PACK_RMC"
value names: "TIM", "NRW", "LT1", "NSO", "LN1", "EWE", "SOG", "COG", "DAT", "MAG", "EAW"
The buffers are accessed using the macro "VALSTR(<n>)", where "<n>" is "0"
for the first buffer, and "1" for the second buffer.
When a value is missing, it is replaced by the contents of val_unknown ("?").
*/
if (ggareceived)
{
gpstime=bintime(rbuf[2]);
ggareceived=0;
}
initval (LONG_BUF(0), PACK_RMC, TIM); // sets rbuf[0][.]
initval (LONG_BUF(1), PACK_RMC, DAT); // sets rbuf[1][.]
initval (SHORT_BUF(0), PACK_RMC, NRW); // sets sbuf[0]
initval (SHORT_BUF(2), PACK_GGA, SAT); // -> sbuf[2]
title ('1');
lcd_puts ( 2*FW, 1*FH, PSTR("UTC-Time Sat"));
if (rbuf[0][0]) { // show always if data have been received
lcd_putcAtt ( 19*FW, 1*FH, sbuf[2], 0); // satellites in view
lcd_putsnAtt ( 2*FW, 2*FH, &rbuf[0][0], 2, APSIZE); // hours
lcd_putcAtt ( 6*FW, 2*FH, ':', DBLSIZE); // ":"
lcd_putsnAtt ( 8*FW, 2*FH, &rbuf[0][2], 2, APSIZE); // minutes
lcd_putcAtt ( 12*FW, 2*FH, ':', DBLSIZE); // ":"
lcd_putsnAtt ( 14*FW, 2*FH, &rbuf[0][4], 2, APSIZE); // seconds
}
else
lcd_putsAtt ( 2*FW, 2*FH, val_unknown, APSIZE); // "?"
if ((show_timer == 1) && rbuf[0][0]) { // show the Timer when data have been received
lcd_puts ( 2*FW, 4*FH, PSTR("Timer")); // display "Timer"
putsTime ( 5*FW, 5*FH, (gpstime-gpstimer), DBLSIZE, DBLSIZE); // display difference as mm:ss
}
else
{
lcd_puts ( 2*FW, 4*FH, PSTR("Date")); // show the UTC Date
if (rbuf[1][0]) {
lcd_putsnAtt( 2*FW, 5*FH, &rbuf[1][0], 2, APSIZE); // year
lcd_putcAtt ( 6*FW, 5*FH, '/', DBLSIZE); // "/"
lcd_putsnAtt( 8*FW, 5*FH, &rbuf[1][2], 2, APSIZE); // month
lcd_putcAtt (12*FW, 5*FH, '/', DBLSIZE); // "/"
lcd_putsnAtt(14*FW, 5*FH, &rbuf[1][4], 2, APSIZE); // day
}
else
lcd_putsAtt ( 2*FW, 5*FH, val_unknown, APSIZE); // "?"
}
}
void menuTelemetryNMEA2(uint8_t event)
{
static uint8_t ignore_break;
switch(event)
{
// Menu navigation
case EVT_KEY_BREAK(KEY_LEFT):
if (ignore_break==1) {
ignore_break=0;
break;}
chainMenu(menuTelemetryNMEA1);
return;
case EVT_KEY_BREAK(KEY_RIGHT):
if (ignore_break==1) {
ignore_break=0;
break;}
chainMenu(menuTelemetryNMEA3);
return;
case EVT_KEY_LONG(KEY_UP):
NMEA_DisableRXD();
chainMenu(menuStatisticsView);
return;
case EVT_KEY_LONG(KEY_DOWN):
NMEA_DisableRXD();
chainMenu(menuMainView);
return;
//Beep setting
case EVT_KEY_LONG(KEY_LEFT):
ignore_break = 1;
beep_on=0;
AUDIO_MENUS(); // short blip
break;
case EVT_KEY_LONG(KEY_RIGHT):
ignore_break = 1;
beep_on=1;
AUDIO_MENUS(); // short blip
break;
//Altitude setting
/* Set a home position for altitude. Normally used before starting
the model when GPS has got a fix.
MENU[short] --> alternating relative and absolute altitudes
MENU[long] --> set home altitude to current
EXIT[long] --> reset max altitude to 0
Switch ON / OFF short beep with positive lift
LEFT[long] --> Positive lift Beep off
RIGHT[long] --> Positive lift Beep on */
case EVT_KEY_BREAK(KEY_MENU):
if (ignore_break==1) {
ignore_break=0;
break;}
if (!home_alt) // umschalten zwischen absoluter und relativer Höhe
home_alt = save_alt;
else
home_alt=0;
if (save_alt==0) // wenn noch keine Home Höhe gesetzt war, wird sie es jetzt, weil sonst
// das Umschalten keine Wirkung zeigt
save_alt = home_alt = abs_alt; // absolute altitude
AUDIO_MENUS(); // short blip for non negative lift
break;
case EVT_KEY_LONG(KEY_MENU):
ignore_break = 1;
save_alt = home_alt = abs_alt; // Home altitude auf aktuelle absolute Höhe setzen
AUDIO_MENUS(); // short blip for non negative lift
break;
case EVT_KEY_LONG(KEY_EXIT): // Max Altitude auf 0 zurücksetzen
max_alt=0;
AUDIO_MENUS(); // short blip for non negative lift
break;
}
title ('2');
lcd_puts ( 1*FW, 1*FH, PSTR("Altitude Sat Max"));
lcd_puts ( 16*FW, 3*FH, PSTR("Home"));
lcd_puts ( 2*FW, 4*FH, PSTR("Lift") );
lcd_puts ( 16*FW, 5*FH, PSTR("Beep") );
if (beep_on==1)
lcd_puts ( 18*FW, 6*FH, PSTR("ON") );
else
lcd_puts ( 17*FW, 6*FH, PSTR("OFF") );
lcd_outdezNAtt( 20*FW, 4*FH, home_alt, PREC1, 6); // display home_alt, small characters
if (xpack[0] != PACK_GGA)
ggareceived = 0;
initval (LONG_BUF(0), PACK_GGA, ALT); // -> rbuf[0]
initval (LONG_BUF(1), PACK_GGA, GEO); // -> rbuf[1]
initval (SHORT_BUF(0), PACK_GGA, MTR); // -> sbuf[0]
initval (SHORT_BUF(1), PACK_GGA, FIX); // -> sbuf[1]
initval (SHORT_BUF(2), PACK_GGA, SAT); // -> sbuf[2]
if (ggareceived) // at least one second has elapsed
{
ggareceived = 0;
/* ALT and GEO have one single digit following the decimal point
e.g. ALT=359.7 GEO=47.7
The altitude over mean sea level is to be calculated as:
altitude minus geoidal separation
*/
abs_alt = binary(rbuf[0]) - binary(rbuf[1]); // alt - geo that is absolute altitude
if (abs_alt> max_alt) max_alt=abs_alt; // hold max altitude relative to 0 m
rel_alt=abs_alt - home_alt; // alt - geo - home altitude relative to home
lift_alt = rel_alt - prev_alt;
prev_alt = rel_alt;
if ((lift_alt >= 0) && (sbuf[1]>0x30) && beep_on) // GGA record must have Fix> 0
AUDIO_MENUS(); // short blip for non negative lift
}
if (rbuf[0][0]) {
lcd_putcAtt ( 13*FW, 1*FH, sbuf[2], 0); // satellites in view
if (sbuf[1]>0x30) { // & GGA has FIX > 0
lcd_outdezNAtt( 10*FW, 2*FH, rel_alt, DBLSIZE|PREC1, 7); // altitude
if (home_alt >= 0)
lcd_outdezNAtt( 20*FW, 2*FH, (max_alt-home_alt), PREC1, 6); // display small characters
else
lcd_outdezNAtt( 20*FW, 2*FH, max_alt, PREC1, 6); // display small characters
lcd_putcAtt ( 11*FW, 3*FH, sbuf[0], 0); // dimension [m]
lcd_outdezNAtt( 10*FW, 5*FH, lift_alt, DBLSIZE|PREC1, 6); // lift
lcd_putcAtt ( 11*FW, 6*FH, sbuf[0], 0); // dimension [m/S]
lcd_puts ( 12*FW, 6*FH, PSTR("/S") );
}
}
else {
lcd_putsAtt ( 2*FW, 2*FH, val_unknown, APSIZE);
lcd_putsAtt ( 2*FW, 5*FH, val_unknown, APSIZE);
}
}
void menuTelemetryNMEA3(uint8_t event)
{
switch(event)
{
case EVT_KEY_BREAK(KEY_LEFT):
chainMenu(menuTelemetryNMEA2);
return;
case EVT_KEY_BREAK(KEY_RIGHT):
chainMenu(menuTelemetryNMEA4);
return;
case EVT_KEY_LONG(KEY_UP):
NMEA_DisableRXD();
chainMenu(menuStatisticsView);
return;
case EVT_KEY_LONG(KEY_DOWN):
NMEA_DisableRXD();
chainMenu(menuMainView);
return;
}
initval (LONG_BUF(0), PACK_RMC, SOG);
initval (LONG_BUF(1), PACK_RMC, COG);
initval (SHORT_BUF(2), PACK_GGA, SAT); // -> sbuf[2]
title ('3');
lcd_puts ( 0*FW, 1*FH, PSTR("GrndSpeed[knt] Sat"));
if (rbuf[0][0]) // if first position is 00, buffer is empty, taken as false
{ // any other value is true
uint8_t i = 0;
while (rbuf[0][i])
{
if (rbuf[0][i] == '.') // find decimal point and insert End of String 3 positions higher
{
rbuf[0][i+3] = 0;
break;
}
i++;
}
lcd_putsAtt ( 2*FW, 2*FH, VALSTR(0), APSIZE); // speed over ground
}
else
lcd_putsAtt ( 2*FW, 2*FH, val_unknown, APSIZE);
lcd_putcAtt ( 19*FW, 1*FH, sbuf[2], 0); // satellites in view
lcd_puts ( 1*FW, 4*FH, PSTR("Course over ground") );
lcd_putsAtt ( 2*FW, 5*FH, VALSTR(1), APSIZE); // course over ground
}
void menuTelemetryNMEA4(uint8_t event)
{
switch(event) // new event received, branch accordingly
{
case EVT_KEY_BREAK(KEY_LEFT):
chainMenu(menuTelemetryNMEA3);
return;
case EVT_KEY_BREAK(KEY_RIGHT):
chainMenu(menuTelemetryNMEA1);
return;
case EVT_KEY_LONG(KEY_UP):
NMEA_DisableRXD();
chainMenu(menuStatisticsView);
return;
case EVT_KEY_LONG(KEY_DOWN):
NMEA_DisableRXD();
chainMenu(menuMainView);
return;
}
// expecting LAT value in POS packet to be stored in the first buffer
initval (LONG_BUF(0), PACK_GGA, LAT);
initval (SHORT_BUF(0), PACK_GGA, NOS);
// and LON value in POS packet stored in the second buffer
initval (LONG_BUF(1), PACK_GGA, LON);
initval (SHORT_BUF(1), PACK_GGA, EOW);
initval (SHORT_BUF(2), PACK_GGA, SAT); // -> sbuf[2]
// title of the screen
title ('4');
lcd_puts ( 3*FW, 1*FH, PSTR("Latitude Sat")); // line 1 column 3
// first buffer into line 2 column 2
if (rbuf[0][0])
{
lcd_putcAtt ( 13*FW, 1*FH, sbuf[0], 0); // N or S
lcd_putcAtt ( 19*FW, 1*FH, sbuf[2], 0); // satellites in view
lcd_putsnAtt ( 1*FW, 2*FH, rbuf[0], 2, APSIZE);
lcd_putcAtt ( 5*FW, 2*FH, '@',0);
lcd_putsAtt ( 6*FW, 2*FH, &rbuf[0][2], APSIZE); // minutes with small decimal point
}
else
lcd_putsAtt ( 2*FW, 2*FH, val_unknown, APSIZE);
lcd_puts ( 3*FW, 4*FH, PSTR("Longitude")); // line 4 column 5
// second buffer into line 5 column 2
if (rbuf[0][0])
{
lcd_putcAtt ( 13*FW, 4*FH, sbuf[1], 0); // E or W
lcd_putsnAtt ( 0*FW, 5*FH, rbuf[1], 3, APSIZE);
lcd_putcAtt ( 6*FW, 5*FH, '@',0);
lcd_putsAtt ( 7*FW, 5*FH, &rbuf[1][3], APSIZE); // minutes with small decimal point
}
else
lcd_putsAtt ( 2*FW, 5*FH, val_unknown, APSIZE);
}
void title(char x)
{
lcd_putsAtt (0*FW, 0*FH, PSTR(" GPS NMEA data ?/4 "), INVERS);
lcd_putcAtt(16*FW, 0*FH, x, INVERS);
}
void initval(uint8_t num, uint8_t pack, uint8_t val)
{
if (xpack[num] != pack || xval[num] != val)
{
if (num < NB_LONG_BUF) {
ibuf[num] = rbuf[num][0] = 0;
}
else
sbuf[num-NB_LONG_BUF] = '?';
xpack[num] = pack;
xval[num] = val;
state = WAIT_PACKET; // synchronize to the next packet
}
}
int32_t binary (char *str)
{
int32_t numval = 0;
uint8_t sign = 0;
while (*str) {
if (*str == '-')
sign = 1;
else if (*str >= '0' && *str <= '9')
numval = numval * 10 + (*str - '0');
str++;
}
if (sign)
numval = -numval;
return numval;
}
int32_t bintime (char *str)
{
int32_t numval=0;
if (*str) {
numval = ((str[0] - '0') * 10l) + (str[1] - '0'); // hours
numval = numval * 3600l;
numval = numval + ((( (str[2] - '0') * 10l) + (str[3] - '0')) * 60l); // minutes
numval = numval + ((str[4] - '0') * 10l) + (str[5] - '0'); // seconds
}
return numval;
}
/*
Without NMEA:
Size after:
AVR Memory Usage
----------------
Device: atmega64
Program: 54226 bytes (82.7% Full)
(.text + .data + .bootloader)
Data: 3440 bytes (84.0% Full)
(.data + .bss + .noinit)
----------------------------------
With NMEA:
Size after:
AVR Memory Usage
----------------
Device: atmega64
Program: 57098 bytes (87.1% Full)
(.text + .data + .bootloader)
Data: 3524 bytes (86.0% Full)
(.data + .bss + .noinit)
*/
radio/src/telemetry/nmea.h
+45
View File
@@ -0,0 +1,45 @@
/*
* 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 nmea_h
#define nmea_h
void NMEA_Init(void);
void NMEA_EnableRXD (void);
void menuTelemetryNMEA(uint8_t event);
#endif