You are on page 1of 13

/* * 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 * * open9x 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 "open9x.h" #include "menus.h" #define #define #define #define /* NB_BUF 2 LG_BUF 11 VALSTR(val) (rbuf[val][0] ? rbuf[val] : val_unknown) 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. 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 // working varia // currrent state // received items // expected value // expected packet // subscripts on buffers values // receive buffers

// states values #define WAIT_PACKET #define WAIT_PACK_POS1 #define WAIT_PACK_POS2 #define WAIT_PACK_POS3 #define WAIT_PACK_ATT1 #define WAIT_PACK_ATT2 #define WAIT_PACK_ATT3 #define WAIT_PACK_MOD1 #define WAIT_PACK_MOD2 #define WAIT_PACK_MOD3 #define WAIT_PACK_WPC1 #define WAIT_PACK_WPC2 #define WAIT_PACK_WPC3 #define WAIT_PACK_ALR1 #define WAIT_PACK_ALR2 #define WAIT_PACK_ALR3 #define WAIT_PACK_PRF1 #define WAIT_PACK_PRF2 #define WAIT_PACK_PRF3 #define FLUSH_TITLE1 #define FLUSH_TITLE2 #define FLUSH_TITLE3 #define FLUSH_TITLE4 #define READ_VALUE

uint8_t i; ble uint8_t state; uint8_t rval, rpack; uint8_t xval[NB_BUF]; uint8_t xpack[NB_BUF]; uint8_t ibuf[NB_BUF]; char rbuf[NB_BUF][LG_BUF]; const char val_unknown[] = "?"; void void void void void void void void void void

menuTelemetryArduPilot1(uint8_t event); menuTelemetryArduPilot2(uint8_t event); menuTelemetryArduPilot3(uint8_t event); menuTelemetryArduPilot4(uint8_t event); menuTelemetryArduPilot5(uint8_t event); menuTelemetryArduPilot6(uint8_t event); menuTelemetryArduPilot7(uint8_t event); menuTelemetryArduPilot8(uint8_t event); title(char x); initval(uint8_t num, uint8_t pack, uint8_t val);

#ifndef SIMU ISR (USART0_RX_vect)

{ // B uint8_t rl; uint8_t rh; uint8_t iostat; rl = UDR0; iostat = UCSR0A; /* bit 7 2 RxC0 RxC0: TXC0: UDRE0: FE0: DOR0: UPE0: U2X0: MPCM0: */ 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 2 1 0 RxCIE0 TxCIE0 UDRIE0 RXEN0 RxCIE0: TXCIE0: UDRIE0: RXEN0: TXEN0: UCSZ02: RXB80: TXB80: */ switch (state) { case WAIT_PACKET: switch (rl) { case PACK_POS: t state = WAIT_PACK_POS2; break; case PACK_ATT: t state = WAIT_PACK_ATT2; break; case PACK_MOD: t state = WAIT_PACK_MOD2; break; case PACK_WPC: // wait for the 2nd char // found a new WPC packe // wait for the 2nd char // found a new MOD packe // wait for the 2nd char // found a new ATT packe 5 TXEN0 4 UCSZ02 RXB80 TXB80 3 1 TxC0 6 UDRE0 0 FE0 5 DOR0 UPE0 4 U2X0 3 MPCM0 //USART control and Status Register 0 //USART control and Status Register 0 A //USART control and Status Register 0 A

Receive complete Transmit Complete USART Data Register Empty Frame Error Data OverRun USART Parity Error Double Tx Speed MultiProcessor Comms Mode

Receive complete int enable Transmit Complete int enable USART Data Register Empty int enable Rx enable Tx Enable Character Size bit 2 Rx data bit 8 Tx data bit 8

// found a new POS packe

t state = WAIT_PACK_WPC2; break; case PACK_ALR: t state = WAIT_PACK_ALR2; break; case PACK_PRF: t state = WAIT_PACK_PRF2; break; } break; case WAIT_PACK_POS2: if (rl == PACK_POS) state = WAIT_PACK_POS3; else state = WAIT_PACKET; break; case WAIT_PACK_POS3: if (rl == PACK_POS) { state = FLUSH_TITLE1; rpack = PACK_POS; rval = 1; for (i = 0; i < NB_BUF; i++) ibuf[i] = 0; } else state = WAIT_PACKET; break; case WAIT_PACK_ATT2: if (rl == PACK_ATT) state = WAIT_PACK_ATT3; else state = WAIT_PACKET; break; case WAIT_PACK_ATT3: if (rl == PACK_ATT) { state = FLUSH_TITLE1; rpack = PACK_ATT; rval = 1; for (i = 0; i < NB_BUF; i++) ibuf[i] = 0; } else state = WAIT_PACKET; break; case WAIT_PACK_MOD2: if (rl == PACK_MOD) state = WAIT_PACK_MOD3; else state = WAIT_PACKET; break; case WAIT_PACK_MOD3: if (rl == PACK_MOD) { state = READ_VALUE; D packet // wait for the 2nd char // wait for the 2nd char // found a new PRF packe // wait for the 2nd char // found a new ALR packe

// wait for 2nd char

// restart if not found // wait for 3rd char // found // flush title "LAT:"

// restart if not found // wait for 2nd char

// restart if not found // wait for 3rd char // flush title "ASP:"

// restart if not found // wait for 2nd char

// restart if not found // wait for 3rd char // ready to read values from PACK_MO

rpack = PACK_MOD; rval = 1; for (i = 0; i < NB_BUF; i++) ibuf[i] = 0; } else state = WAIT_PACKET; break; case WAIT_PACK_WPC2: if (rl == PACK_WPC) state = WAIT_PACK_WPC3; else state = WAIT_PACKET; break; case WAIT_PACK_WPC3: if (rl == PACK_WPC) { state = READ_VALUE; C packet rpack = PACK_WPC; rval = 1; for (i = 0; i < NB_BUF; i++) ibuf[i] = 0; } else state = WAIT_PACKET; break; case WAIT_PACK_ALR2: if (rl == PACK_ALR) state = WAIT_PACK_ALR3; else state = WAIT_PACKET; break; case WAIT_PACK_ALR3: if (rl == PACK_ALR) { state = READ_VALUE; R packet rpack = PACK_ALR; rval = 1; for (i = 0; i < NB_BUF; i++) ibuf[i] = 0; } else state = WAIT_PACKET; break; case WAIT_PACK_PRF2: if (rl == PACK_PRF) state = WAIT_PACK_PRF3; else state = WAIT_PACKET; break; case WAIT_PACK_PRF3: if (rl == PACK_PRF) { state = READ_VALUE; F packet rpack = PACK_PRF; rval = 1; for (i = 0; i < NB_BUF; i++) // restart if not found // wait for 2nd char

// restart if not found // wait for 3rd char // ready to read values from PACK_WP

// restart if not found // wait for 2nd char

// restart if not found // wait for 3rd char // ready to read values from PACK_AL

// restart if not found // wait for 2nd char

// restart if not found // wait for 3rd char // ready to read values from PACK_PR

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 th e next packet break; case VAL_END: // comma found, value co mpleted state = FLUSH_TITLE1; // flush next title rval++; // and get next value break; default: // store the cha r in the corresponding buffer for (i = 0; i < NB_BUF; i++) { // is it the exp ected value in the expected packet ? if (rpack == xpack[i] && rval == xval[i] && ibuf[i] < LG_BUF - 1 ) { // yes, store th e char rbuf[i] [ibuf[i]] = rl; ibuf[i]++; rbuf[i] [ibuf[i]] = 0; } } break; } break; } } #endif void ARDUPILOT_Init (void) { DDRE &= ~(1 << DDE0); PORTE &= ~(1 << PORTE0);

// set RXD0 pin as input // 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); n // set 8N1 UCSR0B = 0|(0<< RXCIE0)|(0<<TXCIE0)|(0<<UDRIE0)|(0<<RXEN0)|(0<<TXEN0)|(0<<UC SZ02); UCSR0C = 0|(1 << UCSZ01) | (1 << UCSZ00); while ( UCSR0A & (1 << RXC0) ) UDR0; // flush receive buf fer #endif } void ARDUPILOT_DisableTXD (void) { UCSR0B &= ~(1 << TXEN0); } void ARDUPILOT_EnableTXD (void) { UCSR0B |= (1 << TXEN0); } void ARDUPILOT_DisableRXD (void) { UCSR0B &= ~(1 << RXEN0); UCSR0B &= ~(1 << RXCIE0); } 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; UCSR0B |= (1 << RXEN0); UCSR0B |= (1 << RXCIE0); } void menuTelemetryArduPilot(uint8_t event) { menuTelemetryArduPilot1(event); } // disable double speed operatio

// disable TX

// enable TX

// disable RX // disable Interrupt

// wait for the next packet // enable RX // enable Interrupt

// Start of ArduPilot menus 1-8 <<<<<<<<<<<<<<<<<<<<<<<<<<< void menuTelemetryArduPilot1(uint8_t event) { switch(event) eived, 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 ("?") . */

// new event rec

// 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 packe t } } /* 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 */

You might also like