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

Files movement.

Browse Source 
Issue 26 fixed (JETI)
This commit is contained in:
bsongis@gmail.com
2013-05-07 05:42:43 +00:00
parent 37880ee9ec
commit c7c907e7ce
306 changed files with 937 additions and 892 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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
*/
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
src/telemetry/frsky.cpp
+864
View File
@@ -0,0 +1,864 @@
/*
* 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
int8_t frskyStreaming = -1;
#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 ((uint8_t)structPos == offsetof(FrskyHubData, 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(FrskyHubData, 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(FrskyHubData, gpsAltitude_bp) ||
((uint8_t)structPos >= offsetof(FrskyHubData, gpsAltitude_ap) && (uint8_t)structPos <= offsetof(FrskyHubData, gpsLatitudeNS) && (uint8_t)structPos != offsetof(FrskyHubData, baroAltitude_bp) && (uint8_t)structPos != offsetof(FrskyHubData, baroAltitude_ap))) {
// If we don't have a fix, we may discard the value
if (frskyData.hub.gpsFix <= 0)
return;
}
((uint8_t*)&frskyData.hub)[structPos] = lowByte;
((uint8_t*)&frskyData.hub)[structPos+1] = byte;
switch ((uint8_t)structPos) {
case offsetof(FrskyHubData, 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(FrskyHubData, temperature1):
if (frskyData.hub.temperature1 > frskyData.hub.maxTemperature1)
frskyData.hub.maxTemperature1 = frskyData.hub.temperature1;
break;
case offsetof(FrskyHubData, temperature2):
if (frskyData.hub.temperature2 > frskyData.hub.maxTemperature2)
frskyData.hub.maxTemperature2 = frskyData.hub.temperature2;
break;
case offsetof(FrskyHubData, current):
if (frskyData.hub.current > frskyData.hub.maxCurrent)
frskyData.hub.maxCurrent = frskyData.hub.current;
break;
case offsetof(FrskyHubData, 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(FrskyHubData, 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(FrskyHubData, baroAltitude_ap):
if (g_model.frsky.varioSource == VARIO_SOURCE_ALTI_PLUS) {
evalVario(frskyData.hub.baroAltitude_bp-frskyData.hub.baroAltitudeOffset, frskyData.hub.baroAltitude_ap);
}
break;
case offsetof(FrskyHubData, 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(FrskyHubData, gpsSpeed_bp):
// Speed => Max speed
if (frskyData.hub.gpsSpeed_bp > frskyData.hub.maxGpsSpeed)
frskyData.hub.maxGpsSpeed = frskyData.hub.gpsSpeed_bp;
break;
case offsetof(FrskyHubData, 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;
}
case offsetof(FrskyHubData, hour):
frskyData.hub.hour = ((uint8_t)(frskyData.hub.hour + g_eeGeneral.timezone + 24)) % 24;
break;
case offsetof(FrskyHubData, accelX):
case offsetof(FrskyHubData, accelY):
case offsetof(FrskyHubData, accelZ):
*(int16_t*)(&((uint8_t*)&frskyData.hub)[structPos]) /= 10;
break;
}
}
#endif
#if defined(WS_HOW_HIGH)
void parseTelemWSHowHighByte(uint8_t byte)
{
if (frskyUsrStreaming < (FRSKY_TIMEOUT10ms*3 - 10)) {
((uint8_t*)&frskyData.hub)[offsetof(FrskyHubData, 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(FrskyHubData, 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
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.power = (current * voltage) / 100;
frskyData.currentPrescale += current;
if (frskyData.currentPrescale >= 3600) {
frskyData.currentConsumption += 1;
frskyData.currentPrescale -= 3600;
}
}
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)
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;
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;
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();
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
}
src/telemetry/frsky.h
+217
View File
@@ -0,0 +1,217 @@
/*
* 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 FrskyHubData {
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 spare1:1; //
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
/* 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;
/* end */
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
uint16_t gpsDistance;
int16_t gpsAltitudeOffset;
uint8_t varioAltitudeQueuePointer; // circular-buffer pointer
uint8_t minCellIdx;
int16_t cellsSum;
// TODO later uint16_t minVfas;
});
#define FRSKY_HUB_DATA FrskyHubData hub;
#elif defined(WS_HOW_HIGH)
PACK(struct WSHowHighData {
int16_t baroAltitude_bp; // 0..9,999 meters
int16_t baroAltitudeOffset;
int16_t minAltitude;
int16_t maxAltitude;
#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
});
#define FRSKY_HUB_DATA WSHowHighData hub;
#elif defined(VARIO)
PACK(struct VarioData {
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
});
#define FRSKY_HUB_DATA VarioData hub;
#else
#define FRSKY_HUB_DATA
#endif
struct FrskyData {
FrskyValueWithMinMax analog[2];
FrskyValueWithMin rssi[2];
FRSKY_HUB_DATA
uint16_t currentConsumption;
uint16_t currentPrescale;
uint16_t power;
};
// Global Fr-Sky telemetry data variables
extern int8_t frskyStreaming; // >0 (true) == data is streaming in. 0 = nodata detected for some time
#if defined(WS_HOW_HIGH)
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);
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_ALT_AP frskyData.hub.gpsAltitude_ap
#define TELEMETRY_GPS_ALT_BP frskyData.hub.gpsAltitude_bp
#define TELEMETRY_GPS_SPEED_BP frskyData.hub.gpsSpeed_bp
#define TELEMETRY_GPS_SPEED_AP frskyData.hub.gpsSpeed_ap
#endif
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);
}
}
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
src/telemetry/mavlink.cpp
+915
View File
@@ -0,0 +1,915 @@
/*
* 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
* - 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"
#include "serial.h"
#define DUMP_RX_TX
// this might need to move to the flight software
//static
mavlink_system_t mavlink_system = { 7, 1, 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;
//static mavlink_message_t mavlink_message_in;
// Mavlink message decoded Status Text
#define PARAM_NB_REPEAT 10
#define LEN_STATUSTEXT 15
static uint8_t mav_statustext[LEN_STATUSTEXT];
static int8_t mav_heartbeat = 0;
static int8_t mav_heartbeat_recv = 0;
static int8_t watch_mav_req_id_action = 0;
static int8_t watch_mav_req_start_data_stream = 15;
static uint8_t data_stream_start_stop = 0;
int8_t watch_mav_req_params_list = 5;
static uint8_t mav_req_params_nb_recv = 0;
static int8_t watch_mav_req_params_set = 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;
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;
mav_heartbeat = 0;
mav_heartbeat_recv = 0;
watch_mav_req_id_action = 0;
watch_mav_req_start_data_stream = 15;
watch_mav_req_params_list = 5;
watch_mav_req_params_set = 0;
data_stream_start_stop = 0;
}
void MAVLINK_Init(void) {
mav_statustext[0] = 0;
MAVLINK_reset(0);
SERIAL_Init();
}
static inline void REC_MAVLINK_MSG_ID_STATUSTEXT(const mavlink_message_t* msg) {
//memcpy(mav_statustext, msg->payload + sizeof(uint8_t), sizeof(int8_t) * LEN_STATUSTEXT);
// _MAV_RETURN_int8_t_array(msg, mav_statustext, LEN_STATUSTEXT, 1);
memcpy(mav_statustext, &_MAV_PAYLOAD(msg)[1], LEN_STATUSTEXT);
}
static inline void REC_MAVLINK_MSG_ID_SYS_STATUS(const mavlink_message_t* msg) {
mavlink_sys_status_t sys_status;
mavlink_msg_sys_status_decode(msg, &sys_status);
telemetry_data.packet_drop = mavlink_msg_sys_status_get_packet_drop(msg);
uint8_t mode = mavlink_msg_sys_status_get_mode(msg);
uint8_t nav_mode = mavlink_msg_sys_status_get_nav_mode(msg);
telemetry_data.rcv_control_mode = MAVLINK_NavMode2CtrlMode(mode, nav_mode);
//telemetry_data.mode = mode;
//telemetry_data.nav_mode = nav_mode;
telemetry_data.status = mavlink_msg_sys_status_get_status(msg);
//telemetry_data.load = mavlink_msg_sys_status_get_load(msg);
telemetry_data.vbat = mavlink_msg_sys_status_get_vbat(msg) / 100; // Voltage * 10
telemetry_data.vbat_low = (getMavlinParamsValue(BATT_MONITOR) > 0)
&& (((float) telemetry_data.vbat / 10.0) < getMavlinParamsValue(LOW_VOLT)) && (telemetry_data.vbat > 50);
}
static inline void REC_MAVLINK_MSG_ID_GPS_RAW(const mavlink_message_t* msg) {
telemetry_data.fix_type = mavlink_msg_gps_raw_get_fix_type(msg);
telemetry_data.loc_current.lat = mavlink_msg_gps_raw_get_lat(msg);
telemetry_data.loc_current.lon = mavlink_msg_gps_raw_get_lon(msg);
telemetry_data.loc_current.alt = mavlink_msg_gps_raw_get_alt(msg);
telemetry_data.eph = mavlink_msg_gps_raw_get_eph(msg);
telemetry_data.hdg = mavlink_msg_gps_raw_get_hdg(msg);
telemetry_data.v = mavlink_msg_gps_raw_get_v(msg);
}
static inline void REC_MAVLINK_MSG_ID_GPS_STATUS(const mavlink_message_t* msg) {
telemetry_data.satellites_visible = mavlink_msg_gps_status_get_satellites_visible(msg);
}
static inline void REC_MAVLINK_MSG_ID_ACTION_ACK(const mavlink_message_t* msg) {
uint8_t ack_action = mavlink_msg_action_ack_get_action(msg);
telemetry_data.ack_result = mavlink_msg_action_ack_get_result(msg);
uint8_t *ptr = mav_statustext;
if (!telemetry_data.ack_result)
*ptr++ = 'N';
*ptr++ = 'A';
*ptr++ = 'C';
*ptr++ = 'K';
*ptr++ = ' ';
*ptr++ = ack_action / 10 + '0';
*ptr++ = ack_action % 10 + '0';
*ptr++ = 0;
watch_mav_req_id_action = 0;
AUDIO_WARNING1();
}
#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;
}
watch_mav_req_params_set = 4; // 1;
}
}
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);
int8_t *id = param_value.param_id;
setParamValue(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:
mav_heartbeat = 3; // 450ms display '*'
mav_heartbeat_recv = 1;
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);
watch_mav_req_start_data_stream = 20;
break;
case MAVLINK_MSG_ID_GPS_RAW:
REC_MAVLINK_MSG_ID_GPS_RAW(p_rxmsg);
break;
case MAVLINK_MSG_ID_GPS_STATUS:
REC_MAVLINK_MSG_ID_GPS_STATUS(p_rxmsg);
break;
case MAVLINK_MSG_ID_ACTION_ACK:
REC_MAVLINK_MSG_ID_ACTION_ACK(p_rxmsg);
break;
#ifdef MAVLINK_PARAMS
case MAVLINK_MSG_ID_PARAM_VALUE:
REC_MAVLINK_MSG_ID_PARAM_VALUE(p_rxmsg);
break;
#endif
}
}
#if 0
static void MAVLINK_parse_char(uint8_t c) {
// mavlink_message_t msg;
// mavlink_status_t status;
static mavlink_message_t m_mavlink_message;
static mavlink_status_t m_mavlink_status;
mavlink_message_t *p_rxmsg = &m_mavlink_message;///< The currently decoded message
mavlink_status_t *p_status = &m_mavlink_status;///< The current decode status
if (mavlink_parse_char(MAVLINK_COMM_0, c, p_rxmsg, p_status)) {
handleMessage(p_rxmsg);
}
}
#endif
static void MAVLINK_parse_char(uint8_t c) {
static mavlink_message_t m_mavlink_message;
static mavlink_message_t* p_rxmsg = &m_mavlink_message; ///< The currently decoded message
mavlink_status_t* p_status = mavlink_get_channel_status(MAVLINK_COMM_0); ///< The current decode status
// 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(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 (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;
mavlink_msg_param_set_send(chan, mavlink_system.sysid, mavlink_system.compid, buf, param_value);
}
#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);
}
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);
}
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);
}
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 && telemetry_data.req_mode < NUM_MODES) {
uint8_t action = MAVLINK_CtrlMode2Action(telemetry_data.req_mode);
MAVLINK_msg_action_pack_send(action);
uint8_t *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;
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;
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;
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;
default:
return;
}
}
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_startTX();
}
}
if (mav_heartbeat_recv && !IS_TX_BUSY) {
MAVLINK10mspoll(count);
}
}
// Start of Mavlink menus <<<<<<<<<<<<<<<<<<<<<<<<<<<
void displayScreenIndex(uint8_t index, uint8_t count, uint8_t attr);
enum mavlink_menu_ {
MENU_INFO = 0, //
MENU_GPS, //
#ifdef DUMP_RX_TX
MENU_DUMP_RX, //
MENU_DUMP_TX, //
#endif
MAX_MAVLINK_MENU
};
uint8_t MAVLINK_menu = MENU_INFO;
void mav_title(const pm_char * s, uint8_t index)
{
lcd_putsAtt(0, 0, PSTR("MAVLINK"), INVERS);
lcd_puts(10 * FW, 0, s);
displayScreenIndex(index, MAX_MAVLINK_MENU, INVERS);
lcd_putc(8 * FW, 0, (mav_heartbeat > 0) ? '*' : ' ');
}
void lcd_outdezFloat(uint8_t x, uint8_t y, float val, uint8_t precis, uint8_t mode = 0) {
char c;
int16_t lnum = val;
uint8_t x1 = x;
val -= lnum;
int8_t i = 0;
lnum = abs(lnum);
for (; i < 4; i++) {
c = (lnum % 10) + '0';
x1 -= FWNUM;
lcd_putcAtt(x1, y, c, mode);
lnum /= 10;
if (lnum == 0) {
break;
}
}
if (lnum != 0) {
// Error number too big
x1 = x;
for (i = 0; i < 4; i++) {
x1 -= FW;
lcd_putcAtt(x1, y, '?', mode);
}
} else {
if (val < 0) {
val = -val;
x1 -= FWNUM;
lcd_putcAtt(x1, y, '-', mode);
}
if (precis)
lcd_putcAtt(x, y, '.', mode);
for (i = 0; i < precis; i++) {
val *= 10;
int a = val;
c = a + '0';
x += FWNUM;
lcd_putcAtt(x, y, c, mode);
val -= a;
}
}
}
bool isValidReqControlMode()
{
if (telemetry_data.req_mode < NUM_MODES) {
if (telemetry_data.req_mode != telemetry_data.rcv_control_mode) {
return false;
}
}
return true;
}
void putsMavlinkControlMode(uint8_t x, uint8_t y, uint8_t len) {
if (telemetry_data.status) {
uint8_t attr = 0;
uint8_t mode = telemetry_data.rcv_control_mode;
if (telemetry_data.req_mode < NUM_MODES) {
if (telemetry_data.req_mode != telemetry_data.rcv_control_mode) {
attr = INVERS;
switch (telemetry_data.ack_result) {
case 5:
AUDIO_ERROR();
telemetry_data.req_mode = NUM_MODES;
break;
default:
//mode = telemetry_data.req_mode;
break;
}
}
}
putsControlMode(x, y, mode, attr, len);
}
}
void MAVLINK_ReqMode(uint8_t mode, uint8_t send) {
telemetry_data.req_mode = mode;
telemetry_data.ack_result = 0;
if (send) {
watch_mav_req_id_action = 0x43;
}
}
void menuTelemetryMavlinkInfos(void) {
mav_title(PSTR("INFOS"), MAVLINK_menu);
uint8_t x1, x2, xnum, y;
x1 = FW;
x2 = 7 * FW;
xnum = x2 + 5 * FWNUM;
y = FH;
uint8_t * ptr = mav_statustext;
for (uint8_t j = 0; j < LEN_STATUSTEXT; j++) {
if (*ptr == 0) {
lcd_putc(x1, y, ' ');
} else {
lcd_putc(x1, y, *ptr++);
}
x1 += FW;
}
x1 = FW;
y += FH;
if (telemetry_data.status) {
if (!isValidReqControlMode()) {
lcd_puts(x1, y, PSTR("REQ"));
putsControlMode(x2, y, telemetry_data.req_mode, 0, 6);
y += FH;
}
lcd_puts(x1, y, PSTR("MODE"));
putsMavlinkControlMode(x2, y, 6);
y += FH;
lcd_puts(x1, y, PSTR("BATT"));
lcd_outdezNAtt(xnum, y, telemetry_data.vbat, PREC1, 5);
y += FH;
lcd_puts(x1, y, PSTR("DROP"));
lcd_outdezAtt(xnum, y, telemetry_data.packet_drop, 0);
}
}
void menuTelemetryMavlinkGPS(void) {
mav_title(PSTR("GPS"), MAVLINK_menu);
uint8_t x1, x2, xnum, y;
x1 = FW;
x2 = 5 * FW + FWNUM;
xnum = 5 * FW + 3 * FWNUM;
y = FH;
lcd_putsnAtt(x1, y, PSTR("GPS"), 3, 0);
uint8_t fix_type = telemetry_data.fix_type;
if (fix_type <= 2) {
lcd_putsnAtt(x2, y, PSTR("__NOOK") + 2 * fix_type, 2, 0);
} else {
lcd_outdezNAtt(xnum, y, fix_type, 0, 3);
}
lcd_puts(x2 + 5 * FW, y, PSTR("SAT"));
lcd_outdezNAtt(x2 + 8 * FW + 3 * FWNUM, y, telemetry_data.satellites_visible, 0, 2);
// if (telemetry_data.fix_type > 0) {
y += FH;
lcd_puts(0, y, PSTR("HDOP"));
lcd_outdezFloat(xnum, y, telemetry_data.eph, 2);
y += FH;
lcd_puts(0, y, PSTR("COOR"));
lcd_outdezFloat(xnum, y, telemetry_data.loc_current.lat, 5);
// y += FH;
// lcd_putsnAtt(x1, y, PSTR("LON"), 3, 0);
lcd_outdezFloat(xnum + 10 * FWNUM, y, telemetry_data.loc_current.lon, 5);
y += FH;
lcd_putsnAtt(x1, y, PSTR("ALT"), 3, 0);
lcd_outdezFloat(xnum, y, telemetry_data.loc_current.alt, 2);
y += FH;
lcd_putsnAtt(x1, y, PSTR("HDG"), 3, 0);
lcd_outdezFloat(xnum, y, telemetry_data.hdg, 2);
y += FH;
lcd_putsnAtt(x1, y, PSTR("V"), 1, 0);
lcd_outdezFloat(xnum, y, telemetry_data.v, 2);
//}
}
#ifdef DUMP_RX_TX
void lcd_outhex2(uint8_t x, uint8_t y, uint8_t val) {
x += FWNUM * 2;
for (int i = 0; i < 2; i++) {
x -= FWNUM;
char c = val & 0xf;
c = c > 9 ? c + 'A' - 10 : c + '0';
lcd_putcAtt(x, y, c, c >= 'A' ? CONDENSED : 0);
val >>= 4;
}
}
void menuTelemetryMavlinkDump(uint8_t event) {
uint8_t x = 0;
uint8_t y = FH;
uint16_t count = 0;
uint16_t bufferLen = 0;
uint8_t *ptr = NULL;
switch (MAVLINK_menu) {
case MENU_DUMP_RX:
mav_dump_rx = 1;
mav_title(PSTR("RX"), MAVLINK_menu);
bufferLen = mavlinkRxBufferCount;
ptr = mavlinkRxBuffer;
break;
case MENU_DUMP_TX:
mav_title(PSTR("TX"), MAVLINK_menu);
bufferLen = serialTxBufferCount;
ptr = ptrTxISR;
break;
default:
break;
}
for (uint16_t var = 0; var < bufferLen; var++) {
uint8_t byte = *ptr++;
lcd_outhex2(x, y, byte);
x += 2 * FW;
count++;
if (count > 8) {
count = 0;
x = 0;
y += FH;
if (y == (6 * FH))
break;
}
}
}
#endif
void menuTelemetryMavlink(uint8_t event) {
switch (event) // new event received, branch accordingly
{
case EVT_ENTRY:
MAVLINK_menu = MENU_INFO;
break;
case EVT_KEY_FIRST(KEY_UP):
MAVLINK_menu--;
break;
case EVT_KEY_FIRST(KEY_DOWN):
MAVLINK_menu++;
break;
case EVT_KEY_FIRST(KEY_MENU):
case EVT_KEY_FIRST(KEY_EXIT):
//MAVLINK_Quit();
chainMenu(menuMainView);
return;
}
switch (MAVLINK_menu) {
case MENU_INFO:
menuTelemetryMavlinkInfos();
break;
case MENU_GPS:
menuTelemetryMavlinkGPS();
break;
#ifdef DUMP_RX_TX
case MENU_DUMP_TX:
case MENU_DUMP_RX:
menuTelemetryMavlinkDump(event);
break;
#endif
default:
break;
}
}
src/telemetry/mavlink.h
+358
View File
@@ -0,0 +1,358 @@
/*
* Authors - Gerard Valade <gerard.valade@gmail.com>
*
* 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.
*
*/
#ifndef _MAVLINK_H_
#define _MAVLINK_H_
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
#define MAVLINK_COMM_NUM_BUFFERS 1
//#define MAVLINK10
#ifdef MAVLINK10
#include "GCS_MAVLink/include_v1.0/mavlink_types.h"
#else
#include "GCS_MAVLink/include/mavlink_types.h"
#endif
#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(uint8_t * buf, uint16_t len);
#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)
#ifdef MAVLINK10
#include "GCS_MAVLink/include_v1.0/ardupilotmega/mavlink.h"
#else
#include "GCS_MAVLink/include/ardupilotmega/mavlink.h"
#endif
#define MAVLINK_PARAMS
#define ERROR_NUM_MODES 99
#define ERROR_MAV_ACTION_NB 99
#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 WP
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 alt; ///< Altitude in meters
} Location_t;
typedef struct Telemetry_Data_ {
// INFOS
uint8_t status; ///< System status flag, see MAV_STATUS ENUM
uint16_t packet_drop;
//uint8_t mode;
//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 vbat_low;
// 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;
float hdg;
float v; // Ground speed
#ifdef MAVLINK_PARAMS
// Params
MavlinkParam_t params[NB_PARAMS];
#endif
} Telemetry_Data_t;
// Telemetry data hold
extern Telemetry_Data_t telemetry_data;
#ifndef MAVLINK10
extern inline uint8_t MAVLINK_NavMode2CtrlMode(uint8_t mode, uint8_t nav_mode) {
uint8_t control_mode = ERROR_NUM_MODES;
switch (mode) {
case MAV_MODE_UNINIT:
control_mode = INITIALISING;
break;
case MAV_MODE_AUTO:
switch (nav_mode) {
case MAV_NAV_HOLD: // ACM
case MAV_NAV_LOITER: // APM
control_mode = LOITER;
break;
case MAV_NAV_WAYPOINT:
control_mode = AUTO;
break;
case MAV_NAV_RETURNING:
control_mode = RTL;
break;
}
break;
case MAV_MODE_GUIDED:
control_mode = GUIDED;
break;
/* from ardupilot */
case MAV_MODE_MANUAL:
control_mode = MANUAL;
break;
case MAV_MODE_TEST1:
control_mode = STABILIZE;
break;
case MAV_MODE_TEST2:
switch (nav_mode) {
case 1:
control_mode = FLY_BY_WIRE_A;
break;
case 2:
control_mode = FLY_BY_WIRE_B;
break;
}
break;
case MAV_MODE_TEST3:
control_mode = CIRCLE;
break;
default:
if (mode >= 100) {
control_mode = mode - 100;
}
break;
}
return control_mode;
}
#endif
extern inline uint8_t MAVLINK_CtrlMode2Action(uint8_t mode) {
uint8_t action;
switch (mode) {
case STABILIZE:
action = MAV_ACTION_SET_MANUAL;
break;
case RTL:
action = MAV_ACTION_RETURN;
break;
case LAND:
action = MAV_ACTION_LAND;
break;
case LOITER:
action = MAV_ACTION_LOITER;
break;
case AUTO:
action = MAV_ACTION_SET_AUTO;
break;
case MANUAL:
action = MAV_ACTION_SET_MANUAL;
break;
default:
action = ERROR_MAV_ACTION_NB;
break;
}
return action;
}
#if 0
extern inline uint8_t MAVLINK_Action2CtrlMode(uint8_t action) {
uint8_t mode = ERROR_NUM_MODES;
switch (action) {
case MAV_ACTION_SET_MANUAL:
mode = STABILIZE;
break;
/*
case ACRO:
action = 0;
break;
case SIMPLE:
action = 0;
break;
case ALT_HOLD:
action = 0;
break;*/
case MAV_ACTION_SET_AUTO:
mode = AUTO;
break;
/*case GUIDED:
action = 0;
break;*/
case MAV_ACTION_LOITER:
mode = LOITER;
break;
case MAV_ACTION_RETURN:
mode = RTL;
break;
/*case CIRCLE:
action = 0;
break;*/
default:
break;
}
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
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)
*/
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