From e6d86480740627ba1460c15a58ac2e64780a69e4 Mon Sep 17 00:00:00 2001 From: bsongis Date: Fri, 27 Jan 2012 18:58:59 +0000 Subject: [PATCH] NMEA and ARDUPILOT code ported from er9x DSM2(PPM) bug fixed Flash saving PCBV3 removed One delay_us(7) removed BEEPSPKR becomes AUDIO --- src/Makefile | 131 +++---- src/ardupilot.cpp | 714 ++++++++++++++++++++++++++++++++++++ src/ardupilot.h | 23 ++ src/audio.cpp | 6 +- src/beeper.cpp | 1 + src/beeper.h | 2 +- src/drivers.cpp | 169 +++++---- src/file.h | 2 +- src/general_menus.cpp | 12 +- src/main_views.cpp | 6 + src/menus.cpp | 2 +- src/nmea.cpp | 824 ++++++++++++++++++++++++++++++++++++++++++ src/nmea.h | 25 ++ src/o9xstrings.cpp | 4 +- src/o9xstrings.h | 4 +- src/open9x.cpp | 157 ++++---- src/open9x.h | 124 +++---- src/pulses.cpp | 56 ++- src/rtc.cpp | 11 - src/simu.cpp | 2 +- 20 files changed, 1895 insertions(+), 380 deletions(-) create mode 100644 src/ardupilot.cpp create mode 100644 src/ardupilot.h create mode 100644 src/nmea.cpp create mode 100644 src/nmea.h diff --git a/src/Makefile b/src/Makefile index 1b5de53e3..61399f622 100644 --- a/src/Makefile +++ b/src/Makefile @@ -28,16 +28,15 @@ #----------- BUILD OPTIONS --------------------------- #gruvin: PCB version -- OVERRIDES the following settings if not STD -# Values: STD, V3, V4 +# Values: STD, V4 PCB = STD -#NOTE!: V4 adds to V3, such that both PCBV3 and PCBV4 get defined # Following options for PCB=STD only (ignored otherwise) ... # Enable JETI-Telemetry or FrSky Telemetry reception on UART0 # For this option you need to modify your hardware! # More information at [insertURLhere] -# Values = STD, JETI, FRSKY +# Values = STD, FRSKY, JETI, NMEA, ARDUPILOT EXT = STD # Enable heli menu @@ -61,24 +60,16 @@ else NAVIGATION = POTS endif -# gruvin: BEEPER. Values = BUZZER, BUZZER_MOD or SPEAKER -# (without a mod, BUZZER can make PPM jack output switch from output to input at random) -# SPEAKER mode actually works on the stock radio, using the stock beeper. Sort of sound OK(ish). -BEEPER = BUZZER - -# gruvin: Legacy support freeing of USART1 TX/RX pins [DEPRECATED] -# OPTIONS STD or FREED -USART1 = STD - -# gruvin: PCM-in circuit mod for JR/Spektrum (and others) compatability -# Values = STD, MOD1 -PPMIN = STD +# AUDIO Mods +# Values = YES, NO +AUDIO = YES # SPLASH on START SPLASH = YES -# BATT voltage algorithm. Values = BANDGAP, UNSTABLE_BANDGAP -BATT = BANDGAP +# BATT voltage algorithm. +# Values = BANDGAP, UNSTABLE_BANDGAP (default for stock board) +BATT = UNSTABLE_BANDGAP # Decimals display in the main view (PPM calibration, # Values = YES, NO @@ -148,6 +139,14 @@ ifeq ($(EXT), JETI) CPPSRC += jeti.cpp endif +ifeq ($(EXT), ARDUPILOT) + CPPSRC += ardupilot.cpp +endif + +ifeq ($(EXT), NMEA) + CPPSRC += nmea.cpp +endif + # Disk IO support (PCB V2+ only) ifneq ($(PCB), STD) CPPSRC += gtime.cpp @@ -222,91 +221,75 @@ ifeq ($(NAVIGATION), POTS) CPPDEFS += -DNAVIGATION_POT1 -DNAVIGATION_POT2 -DNAVIGATION_POT3 endif -ifneq ($(SPLASH), NO) +ifeq ($(SPLASH), YES) CPPDEFS += -DSPLASH endif -ifeq ($(BEEPER), SPEAKER) - CPPDEFS += -DBEEPSPKR +ifeq ($(AUDIO), YES) + CPPDEFS += -DAUDIO CPPSRC += audio.cpp else CPPSRC += beeper.cpp endif -ifeq ($(PCB), STD) - # STD PCB, so ... +# If ARDUPILOT-Support is enabled +ifeq ($(EXT), ARDUPILOT) + CPPDEFS += -DARDUPILOT +endif - CPPDEFS += -DPCBSTD - - # If Hardware PPM mode ( PB0<->BP7) switch the Backlight output with the original PPM to use hardware facility to generate precise PPM (hardware mods) - # G: TODO This prevents HARDPPM being used with FRSKY. HARDPPM needs its own option XXX - ifeq ($(EXT), HARDPPM) - CPPDEFS += -DPPMPB7_HARDWARE - endif +# If NMEA-Support is enabled +ifeq ($(EXT), NMEA) + CPPDEFS += -DNMEA +endif - # If JETI-Support is enabled - ifeq ($(EXT), JETI) - MODS:=${MODS}J - CPPDEFS += -DJETI - endif +# If JETI-Support is enabled +ifeq ($(EXT), JETI) + MODS:=${MODS}J + CPPDEFS += -DJETI +endif - # If FRSKY-Support is enabled - ifeq ($(EXT), FRSKY) - MODS:=${MODS}F - CPPDEFS += -DFRSKY - CPPSRC += frsky.cpp - # If FRSKY-Hub is enabled - ifeq ($(FRSKY_HUB), YES) - CPPDEFS += -DFRSKY_HUB - endif - # If WS HowHigh is enabled - ifeq ($(WS_HOW_HIGH), YES) - MODS:=${MODS}W - CPPDEFS += -DWS_HOW_HIGH - endif +# If FRSKY-Support is enabled +ifeq ($(EXT), FRSKY) + MODS:=${MODS}F + CPPDEFS += -DFRSKY + CPPSRC += frsky.cpp + # If FRSKY-Hub is enabled + ifeq ($(FRSKY_HUB), YES) + CPPDEFS += -DFRSKY_HUB endif - - # If buzzer modified (no interference with PPM jack) - ifeq ($(BEEPER), BUZZER_MOD) - CPPDEFS += -DBUZZER_MOD + # If WS HowHigh is enabled + ifeq ($(WS_HOW_HIGH), YES) + MODS:=${MODS}W + CPPDEFS += -DWS_HOW_HIGH endif +endif - # If BandGap is not rock solid - ifeq ($(BATT), UNSTABLE_BANDGAP) - CPPDEFS += -DBATT_UNSTABLE_BANDGAP - endif - - # gruvin: Legacy support for hardware mod freeing USART1 [DEPRECATED] - ifeq ($(USART1), FREED) - CPPDEFS += -DUSART1FREED - endif - - # gruvin: PCM-in circuit mod for JR/Spektrum (and others) compatability - ifeq ($(PPMIN), MOD1) - CPPDEFS += -DPPMIN_MOD1 - endif - -else - # not PCB=STD, so ... +ifeq ($(PCB), V4) + # V4 PCB, so ... + CPPDEFS += -DPCBV4 ifeq ($(NAVIGATION), RE1) CPPDEFS += -DNAVIGATION_RE1 endif - CPPSRC += frsky.cpp - CPPDEFS += -DPCBV3 -DFRSKY -DFRSKY_HUB -DWS_HOW_HIGH ifeq ($(LOGS), YES) CPPSRC += logs.cpp CPPDEFS += -DLOGS MODS:=${MODS}L endif - ifeq ($(PCB), V4) - CPPDEFS += -DPCBV4 - endif + ifeq ($(SOMO), YES) CPPSRC += somo14d.cpp CPPDEFS += -DSOMO endif +else + # STD PCB, so ... + CPPDEFS += -DPCBSTD + + # If BandGap is not rock solid + ifeq ($(BATT), UNSTABLE_BANDGAP) + CPPDEFS += -DBATT_UNSTABLE_BANDGAP + endif endif diff --git a/src/ardupilot.cpp b/src/ardupilot.cpp new file mode 100644 index 000000000..5a382eb51 --- /dev/null +++ b/src/ardupilot.cpp @@ -0,0 +1,714 @@ +/* + * + * Author - Karl Szmutny + * Author - Uphiearl and Jean-Pierre PARISY + * + * + * 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 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 menuProcArduPilot1(uint8_t event); +void menuProcArduPilot2(uint8_t event); +void menuProcArduPilot3(uint8_t event); +void menuProcArduPilot4(uint8_t event); +void menuProcArduPilot5(uint8_t event); +void menuProcArduPilot6(uint8_t event); +void menuProcArduPilot7(uint8_t event); +void menuProcArduPilot8(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 + UBRR0H = UBRRH_VALUE; + UBRR0L = UBRRL_VALUE; + // break; + // } + + UCSR0A &= ~(1 << U2X0); // disable double speed operation + // set 8N1 + UCSR0B = 0|(0<< RXCIE0)|(0<, , ); + ------------------------------------- + + That means that "" of "" is stored in the buffer. + The first 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()", where "" 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_P (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_P (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 menuProcArduPilot2(uint8_t event) +{ + switch(event) + { + case EVT_KEY_FIRST(KEY_UP): + chainMenu(menuProcArduPilot1); + break; + case EVT_KEY_FIRST(KEY_DOWN): + chainMenu(menuProcArduPilot3); + 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_P (1*FW, 1*FH, PSTR(" Ground speed")); + lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE); + lcd_puts_P (1*FW, 4*FH, PSTR(" Climb rate") ); + lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE); +} + +void menuProcArduPilot3(uint8_t event) +{ + switch(event) + { + case EVT_KEY_FIRST(KEY_UP): + chainMenu(menuProcArduPilot2); + break; + case EVT_KEY_FIRST(KEY_DOWN): + chainMenu(menuProcArduPilot4); + 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_P (1*FW, 1*FH, PSTR(" Altitude")); + lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE); + lcd_puts_P (1*FW, 4*FH, PSTR(" Altitude Hold") ); + lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE); +} + +void menuProcArduPilot4(uint8_t event) +{ + switch(event) + { + case EVT_KEY_FIRST(KEY_UP): + chainMenu(menuProcArduPilot3); + break; + case EVT_KEY_FIRST(KEY_DOWN): + chainMenu(menuProcArduPilot5); + 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_P (1*FW, 1*FH, PSTR(" Course")); + lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE); + lcd_puts_P (1*FW, 4*FH, PSTR(" Bearing")); + lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE); +} + +void menuProcArduPilot5(uint8_t event) +{ + switch(event) + { + case EVT_KEY_FIRST(KEY_UP): + chainMenu(menuProcArduPilot4); + break; + case EVT_KEY_FIRST(KEY_DOWN): + chainMenu(menuProcArduPilot6); + 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_P (1*FW, 1*FH, PSTR(" Way Point # ")); + lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE); + lcd_puts_P (1*FW, 4*FH, PSTR(" Distance ")); + lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE); +} + +void menuProcArduPilot6(uint8_t event) +{ + switch(event) + { + case EVT_KEY_FIRST(KEY_UP): + chainMenu(menuProcArduPilot5); + break; + case EVT_KEY_FIRST(KEY_DOWN): + chainMenu(menuProcArduPilot7); + 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_P (1*FW, 1*FH, PSTR(" Air Speed ")); + lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE); + lcd_puts_P (1*FW, 4*FH, PSTR(" Climb Rate ")); + lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE); +} + +void menuProcArduPilot7(uint8_t event) +{ + switch(event) + { + case EVT_KEY_FIRST(KEY_UP): + chainMenu(menuProcArduPilot6); + break; + case EVT_KEY_FIRST(KEY_DOWN): + chainMenu(menuProcArduPilot8); + 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_P (1*FW, 1*FH, PSTR(" Roll Angle")); + lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE); + lcd_puts_P (1*FW, 4*FH, PSTR(" Pitch Angle")); + lcd_putsAtt (2*FW, 5*FH, VALSTR(1), APSIZE); +} + +void menuProcArduPilot8(uint8_t event) +{ + switch(event) + { + case EVT_KEY_FIRST(KEY_UP): + chainMenu(menuProcArduPilot7); + break; + case EVT_KEY_FIRST(KEY_DOWN): + chainMenu(menuProcArduPilot1); + 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_P (1*FW, 1*FH, PSTR(" ArduPilot Mode")); + lcd_putsAtt (2*FW, 2*FH, VALSTR(0), APSIZE); + lcd_puts_P (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 +*/ diff --git a/src/ardupilot.h b/src/ardupilot.h new file mode 100644 index 000000000..75f2c4185 --- /dev/null +++ b/src/ardupilot.h @@ -0,0 +1,23 @@ +/* + * Author - Philip Moss + * Adapted from frsky.h code by Jean-Pierre PARISY + * + * + * 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 menuProcArduPilot(uint8_t event); +#endif + diff --git a/src/audio.cpp b/src/audio.cpp index d588e46a7..43726e9a8 100644 --- a/src/audio.cpp +++ b/src/audio.cpp @@ -164,14 +164,14 @@ void audioQueue::heartbeat() case 0: //stock beeper. simply turn port on for x time! if (toneTimeLeft > 0) { -#if defined (PCBV3) +#if defined (PCBV4) TCCR0A &= ~(0b01< 0) { toneCounter += toneFreq; if ((toneCounter & 0x80) == 0x80) { -#if defined (PCBV3) +#if defined (PCBV4) TCCR0A &= ~(0b01<> 4); - - PORTD = 0xFF; -# endif - #else - in = ~PIND; - -// Legacy support for USART1 free hardware mod [DEPRECATED] -# if defined(USART1FREED) - // mask out original INP_D_TRM_LV_UP and INP_D_TRM_LV_DWN bits - in &= ~((1<7*FH) return; }subN++; -#ifdef BEEPSPKR +#ifdef AUDIO if(s_pgOfsval) // TODO check if without BEEPSPKR it's optimized! + if (newval>val) // TODO check if without AUDIO it's optimized! AUDIO_KEYPAD_UP(); else AUDIO_KEYPAD_DOWN(); diff --git a/src/nmea.cpp b/src/nmea.cpp new file mode 100644 index 000000000..b1da15e6b --- /dev/null +++ b/src/nmea.cpp @@ -0,0 +1,824 @@ +/* + * + * Author - Karl Szmutny + * Author - Uphiearl and Jean-Pierre PARISY + * Modified to accept NMEA records GGA and RMC - ReSt and Jean-Pierre PARISY + * + * 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 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 + +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 + + 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 menuProcNMEA1(uint8_t event); +void menuProcNMEA2(uint8_t event); +void menuProcNMEA3(uint8_t event); +void menuProcNMEA4(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 + UBRR0H = UBRRH_VALUE; + UBRR0L = UBRRL_VALUE; + // break; + // } + + UCSR0A &= ~(1 << U2X0); // disable double speed operation + // set 8N1 + UCSR0B = 0|(0<< RXCIE0)|(0<resets timer to 00:00 + break; + } + /* + How to use: + + You choose the values to be displayed using the function: + + initval(, , ); + ------------------------------------- + + That means that "" of "" is stored in the buffer. + The first 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()", where "" 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_P ( 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_P ( 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_P ( 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 menuProcNMEA2(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(menuProcNMEA1); + break; + case EVT_KEY_BREAK(KEY_RIGHT): + if (ignore_break==1) { + ignore_break=0; + break;} + chainMenu(menuProcNMEA3); + break; + case EVT_KEY_LONG(KEY_UP): + NMEA_DisableRXD(); + chainMenu(menuProcStatistic); + break; + case EVT_KEY_LONG(KEY_DOWN): + NMEA_DisableRXD(); + chainMenu(menuMainView); + break; + +//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_P ( 1*FW, 1*FH, PSTR("Altitude Sat Max")); + + + lcd_puts_P ( 16*FW, 3*FH, PSTR("Home")); + lcd_puts_P ( 2*FW, 4*FH, PSTR("Lift") ); + + lcd_puts_P ( 16*FW, 5*FH, PSTR("Beep") ); + if (beep_on==1) + lcd_puts_P ( 18*FW, 6*FH, PSTR("ON") ); + + else + lcd_puts_P ( 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_P ( 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 menuProcNMEA3(uint8_t event) +{ + switch(event) + { + case EVT_KEY_BREAK(KEY_LEFT): + chainMenu(menuProcNMEA2); + break; + case EVT_KEY_BREAK(KEY_RIGHT): + chainMenu(menuProcNMEA4); + break; + case EVT_KEY_LONG(KEY_UP): + NMEA_DisableRXD(); + chainMenu(menuProcStatistic); + break; + case EVT_KEY_LONG(KEY_DOWN): + NMEA_DisableRXD(); + chainMenu(menuMainView); + break; + } + 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_P ( 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_P ( 1*FW, 4*FH, PSTR("Course over ground") ); + lcd_putsAtt ( 2*FW, 5*FH, VALSTR(1), APSIZE); // course over ground +} + + + +void menuProcNMEA4(uint8_t event) +{ + switch(event) // new event received, branch accordingly + { + case EVT_KEY_BREAK(KEY_LEFT): + chainMenu(menuProcNMEA3); + break; + case EVT_KEY_BREAK(KEY_RIGHT): + chainMenu(menuProcNMEA1); + break; + case EVT_KEY_LONG(KEY_UP): + NMEA_DisableRXD(); + chainMenu(menuProcStatistic); + break; + case EVT_KEY_LONG(KEY_DOWN): + NMEA_DisableRXD(); + chainMenu(menuMainView); + break; + } + // 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_P ( 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_P ( 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) +*/ diff --git a/src/nmea.h b/src/nmea.h new file mode 100644 index 000000000..ee8183dbc --- /dev/null +++ b/src/nmea.h @@ -0,0 +1,25 @@ +/* + * Author - Philip Moss + * Adapted from frsky.h code by Jean-Pierre PARISY + * + * + * 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 menuProcNMEA(uint8_t event); + +#endif + diff --git a/src/o9xstrings.cpp b/src/o9xstrings.cpp index b7dbae136..7bc3494d2 100644 --- a/src/o9xstrings.cpp +++ b/src/o9xstrings.cpp @@ -41,7 +41,7 @@ const prog_char APM STR_OPEN9X[] = #if defined(DSM2) TR_DSM2MODE #endif -#if defined(PCBV3) +#if defined(PCBV4) TR_RE1RE2 TR_DATETIME #endif @@ -190,7 +190,7 @@ const prog_char APM STR_MENUSERROR[] = TR_MENUSERROR; const prog_char APM STR_MENURADIOSETUP[] = TR_MENURADIOSETUP; -#ifdef PCBV3 +#ifdef PCBV4 const prog_char APM STR_MENUDATEANDTIME[] = TR_MENUDATEANDTIME; #endif diff --git a/src/o9xstrings.h b/src/o9xstrings.h index 48a808717..bed8e4b98 100644 --- a/src/o9xstrings.h +++ b/src/o9xstrings.h @@ -66,7 +66,7 @@ extern const PROGMEM char STR_OPEN9X[]; #else #define OFS_ENDDSM2 (OFS_VTMRMODES + PSIZE(TR_VTMRMODES)) #endif -#if defined(PCBV3) +#if defined(PCBV4) #define OFS_RE1RE2 (OFS_ENDDSM2) #define OFS_DATETIME (OFS_RE1RE2 + PSIZE(TR_RE1RE2)) #endif @@ -119,7 +119,7 @@ extern const PROGMEM char STR_OPEN9X[]; #define STR_DSM2MODE (STR_OPEN9X + OFS_DSM2MODE) #endif -#if defined(PCBV3) +#if defined(PCBV4) #define STR_RE1RE2 (STR_OPEN9X + OFS_RE1RE2) #define STR_DATETIME (STR_OPEN9X + OFS_DATETIME) #endif diff --git a/src/open9x.cpp b/src/open9x.cpp index 89ff1f5de..037fb4fef 100644 --- a/src/open9x.cpp +++ b/src/open9x.cpp @@ -30,7 +30,7 @@ prog_uchar APM speMarker[] = { "SPE" }; #include "menus.h" // MM/SD card Disk IO Support -#if defined (PCBV3) +#if defined (PCBV4) gtime_t g_unixTime; // Global date/time register, incremented each second in per10ms() #endif @@ -51,21 +51,18 @@ uint16_t g_timeMain; uint16_t g_time_per10; #endif -#if defined (PCBSTD) && defined (BEEPSPKR) +#if defined (PCBSTD) && defined (AUDIO) uint8_t toneFreq = BEEP_DEFAULT_FREQ; uint8_t toneOn = false; #endif -bool warble = false; // TODO is it needed with BEEPSPKR? - -#ifdef BEEPSPKR +#ifdef AUDIO //new audio object audioQueue audio; #endif uint8_t heartbeat; -// TODO reduce these tabs const prog_char APM s_charTab[] = "_-.,"; //R=1 @@ -471,21 +468,6 @@ uint8_t getTrimFlightPhase(uint8_t idx, uint8_t phase) return 0; } -#if defined (PCBV3) && !defined (PCBV4) -// The ugly scanned keys thing should be gone for PCBV4+. In the meantime ... -uint8_t keyDown() -{ - uint8_t in; - PORTD = ~1; // select KEY_Y0 row (Bits 3:2 EXIT:MENU) - _delay_us(1); - in = (~PIND & 0b11000000) >> 5; - PORTD = ~2; // select Y1 row. (Bits 3:0 Left/Right/Up/Down) - _delay_us(1); - in |= (~PIND & 0xf0) >> 1; - PORTD = 0xff; - return (in); -} -#else FORCEINLINE uint8_t keyDown() { #if defined (PCBV4) @@ -494,7 +476,6 @@ FORCEINLINE uint8_t keyDown() return (~PINB) & 0x7E; #endif } -#endif void clearKeyEvents() { @@ -729,7 +710,7 @@ uint8_t checkTrim(uint8_t event) setTrimValue(phase, idx, after); -#if defined (BEEPSPKR) +#if defined (AUDIO) // toneFreq higher/lower according to trim position // limit the frequency, range -125 to 125 = toneFreq: 19 to 101 if (after > TRIM_MAX) @@ -742,18 +723,18 @@ uint8_t checkTrim(uint8_t event) if (beepTrim) { killEvents(event); - warble = false; -#if defined (BEEPSPKR) +#if defined (AUDIO) audio.event(AU_TRIM_MOVE, after); #else + warble = false; AUDIO_WARNING2(); #endif } else { - if (event & _MSK_KEY_REPT) warble = true; -#if defined (BEEPSPKR) +#if defined (AUDIO) audio.event(AU_TRIM_MOVE, after); #else + if (event & _MSK_KEY_REPT) warble = true; AUDIO_TRIM(); #endif } @@ -762,7 +743,11 @@ uint8_t checkTrim(uint8_t event) return event; } -#ifndef SIMU +#ifdef SIMU + +uint16_t BandGap = 225; + +#else // #define STARTADCONV (ADCSRA = (1< only phone jack = slave mode -#if defined(BUZZER_MOD) || defined(BEEPSPKR) +#if defined(PCBV4) return SLAVE_MODE; #else static bool lastSlaveMode = false; @@ -1753,10 +1726,10 @@ uint8_t ppmInState = 0; //0=unsync 1..8= wait for value i-1 #ifndef SIMU volatile uint8_t g_tmr16KHz; //continuous timer 16ms (16MHz/1024/256) -- 8-bit counter overflow -#if defined (PCBV3) +#if defined (PCBV4) ISR(TIMER2_OVF_vect) #else -ISR(TIMER0_OVF_vect) +ISR(TIMER0_OVF_vect) // TODO now NOBLOCK in er9x #endif { g_tmr16KHz++; // gruvin: Not 16KHz. Overflows occur at 61.035Hz (1/256th of 15.625KHz) @@ -1769,7 +1742,7 @@ uint16_t getTmr16KHz() { while(1){ uint8_t hb = g_tmr16KHz; -#if defined (PCBV3) +#if defined (PCBV4) uint8_t lb = TCNT2; #else uint8_t lb = TCNT0; @@ -1782,7 +1755,7 @@ uint16_t getTmr16KHz() extern uint16_t g_time_per10; // instantiated in menus.cpp #endif -#if defined (PCBV3) +#if defined (PCBV4) ISR(TIMER2_COMPA_vect, ISR_NOBLOCK) //10ms timer #else ISR(TIMER0_COMP_vect, ISR_NOBLOCK) //10ms timer @@ -1790,14 +1763,14 @@ ISR(TIMER0_COMP_vect, ISR_NOBLOCK) //10ms timer { cli(); -#if defined (PCBV3) +#if defined (PCBV4) static uint8_t accuracyWarble = 4; // becasue 16M / 1024 / 100 = 156.25. So bump every 4. uint8_t bump = (!(accuracyWarble++ & 0x03)) ? 157 : 156; TIMSK2 &= ~(1< #include -#if defined(PCBV3) +#if defined(PCBV4) #include "ff.h" #include "gtime.h" #endif @@ -83,6 +83,16 @@ extern uint16_t jeti_keys; #include "frsky.h" #endif +#ifdef ARDUPILOT +// ArduPilot Telemetry +#include "ardupilot.h" +#endif + +#ifdef NMEA +// NMEA Telemetry +#include "nmea.h" +#endif + extern RlcFile theFile; //used for any file operation // G: The following comments relate to the original stock PCB only @@ -198,69 +208,45 @@ extern uint16_t DEBUG2; #else // boards prior to v4 ... +#define OUT_B_LIGHT 7 +#define INP_B_KEY_LFT 6 +#define INP_B_KEY_RGT 5 +#define INP_B_KEY_UP 4 +#define INP_B_KEY_DWN 3 +#define INP_B_KEY_EXT 2 +#define INP_B_KEY_MEN 1 +#define OUT_B_PPM 0 -# define INP_B_KEY_LFT 6 -# define INP_B_KEY_RGT 5 -# define INP_B_KEY_UP 4 -# define INP_B_KEY_DWN 3 -# define INP_B_KEY_EXT 2 -# define INP_B_KEY_MEN 1 -//vinceofdrink@gmail harwared ppm -//Orginal bitbang port for PPM -# ifndef DPPMPB7_HARDWARE -# define OUT_B_PPM 0 -# else -# define OUT_B_PPM 7 // will not be used -# endif +#define INP_D_TRM_LH_UP 7 +#define INP_D_TRM_LH_DWN 6 +#define INP_D_TRM_RV_DWN 5 +#define INP_D_TRM_RV_UP 4 +#define INP_D_TRM_LV_DWN 3 +#define INP_D_TRM_LV_UP 2 +#define INP_D_TRM_RH_DWN 1 +#define INP_D_TRM_RH_UP 0 -# define INP_D_TRM_LH_UP 7 -# define INP_D_TRM_LH_DWN 6 -# define INP_D_TRM_RV_DWN 5 -# define INP_D_TRM_RV_UP 4 -# define INP_D_TRM_LV_DWN 3 -# define INP_D_TRM_LV_UP 2 -# define INP_D_TRM_RH_DWN 1 -# define INP_D_TRM_RH_UP 0 +#define INP_E_PPM_IN 7 +#define INP_E_ID2 6 +#define INP_E_Trainer 5 +#define INP_E_Gear 4 +#define OUT_E_BUZZER 3 +#define INP_E_ElevDR 2 +#define INP_E_AileDR 1 +#define INP_E_ThrCt 0 -# if defined (PCBV3) -# define OUT_C_LIGHT 0 -# else -# ifndef DPPMPB7_HARDWARE -# define OUT_B_LIGHT 7 -# else -# define OUT_B_LIGHT 0 -# endif -# endif - -# define INP_E_PPM_IN 7 -# define INP_E_ID2 6 -# define INP_E_Trainer 5 -# define INP_E_Gear 4 -# define OUT_E_BUZZER 3 -# define INP_E_ElevDR 2 -# define INP_E_AileDR 1 -# define INP_E_ThrCt 0 - -# if defined(JETI) || defined(FRSKY) -# undef INP_E_ThrCt -# undef INP_E_AileDR -# define INP_C_ThrCt 6 -# define INP_C_AileDR 7 -# endif - -# define OUT_G_SIM_CTL 4 //1 : phone-jack=ppm_in -# define INP_G_ID1 3 -# define INP_G_RF_POW 1 -# define INP_G_RuddDR 0 - -// Legacy support for USART1 hardware mod [DEPRECATED] -// G: This board will be retired before much longer. But I still need it for now. -#if defined(USART1FREED) -// do not undef the original INP_D_TRM_LV_DWN/UP as we need them later - #define INP_C_TRM_LV_UP 0 - #define INP_G_TRM_LV_DWN 2 +#if (defined(JETI) || defined(FRSKY) || defined(ARDUPILOT) || defined(NMEA)) +#undef INP_E_ThrCt +#undef INP_E_AileDR +#define INP_C_ThrCt 6 +#define INP_C_AileDR 7 #endif +#define OUT_G_SIM_CTL 4 //1 : phone-jack=ppm_in +#define INP_G_ID1 3 +#define INP_G_RF_POW 1 +#define INP_G_RuddDR 0 + #endif // defined (PCBV4) #define SLAVE_MODE (PING & (1<