Mirror/opentx
1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-21 23:35:17 +03:00
Code Issues Wiki Activity

Small RAM saving.

Browse source 
Some code ported for the ersky9x board
Mavlink port commited
This commit is contained in:
bsongis 2012-03-21 18:31:00 +00:00
parent 45c685fab7
commit 2e5cf5a4f3
16 changed files with 16369 additions and 258 deletions
Download patch file Download diff file Expand all files Collapse all files

7010
src/AT91SAM3S2.h Normal file
View file

File diff suppressed because it is too large Load diff

7010
src/AT91SAM3S4.h Normal file
View file

File diff suppressed because it is too large Load diff

2
src/beeper.h
View file

@ -68,6 +68,7 @@ extern void beep(uint8_t val);
#define IS_AUDIO_BUSY() (g_beepCnt || beepAgain || beepOn)
#if not defined(PCBARM)
FORCEINLINE void AUDIO_HEARTBEAT()
{
if(g_beepCnt) {
@ -112,5 +113,6 @@ FORCEINLINE void AUDIO_HEARTBEAT()
HAPTIC_OFF;
}
}
#endif
#endif

100
src/board_ersky9x.cpp
View file

@ -652,3 +652,103 @@ inline void board_init()
init_spi() ;
}
inline uint8_t keyDown()
{
return ~read_keys() & 0x7E ;
}
extern uint32_t keyState(EnumKeys enuk)
{
CPU_UINT xxx = 0 ;
if(enuk < (int)DIM(keys)) return keys[enuk].state() ? 1 : 0;
switch((uint8_t)enuk)
{
#ifdef REVB
case SW_ElevDR : xxx = PIOC->PIO_PDSR & 0x80000000 ; // ELE_DR PC31
#else
case SW_ElevDR : xxx = PIOA->PIO_PDSR & 0x00000100 ; // ELE_DR PA8
#endif
break ;
case SW_AileDR : xxx = PIOA->PIO_PDSR & 0x00000004 ; // AIL-DR PA2
break ;
case SW_RuddDR : xxx = PIOA->PIO_PDSR & 0x00008000 ; // RUN_DR PA15
break ;
// INP_G_ID1 INP_E_ID2
// id0 0 1
// id1 1 1
// id2 1 0
case SW_ID0 : xxx = ~PIOC->PIO_PDSR & 0x00004000 ; // SW_IDL1 PC14
break ;
case SW_ID1 : xxx = (PIOC->PIO_PDSR & 0x00004000) ; if ( xxx ) xxx = (PIOC->PIO_PDSR & 0x00000800);
break ;
case SW_ID2 : xxx = ~PIOC->PIO_PDSR & 0x00000800 ; // SW_IDL2 PC11
break ;
case SW_Gear : xxx = PIOC->PIO_PDSR & 0x00010000 ; // SW_GEAR PC16
break ;
#ifdef REVB
case SW_ThrCt : xxx = PIOC->PIO_PDSR & 0x00100000 ; // SW_TCUT PC20
#else
case SW_ThrCt : xxx = PIOA->PIO_PDSR & 0x10000000 ; // SW_TCUT PA28
#endif
break ;
case SW_Trainer: xxx = PIOC->PIO_PDSR & 0x00000100 ; // SW-TRAIN PC8
break ;
default:;
}
if ( xxx )
{
return 1 ;
}
return 0;
}
uint16_t Analog_values[NUMBER_ANALOG] ;
// Read 8 (9 for REVB) ADC channels
// Documented bug, must do them 1 by 1
void read_9_adc()
{
register Adc *padc;
register uint32_t y;
register uint32_t x;
// PMC->PMC_PCER0 |= 0x20000000L ; // Enable peripheral clock to ADC
padc = ADC;
y = padc->ADC_ISR; // Clear EOC flags
for (y = NUMBER_ANALOG; --y > 0;) {
padc->ADC_CR = 2; // Start conversion
x = 0;
while ((padc->ADC_ISR & 0x01000000) == 0) {
// wait for DRDY flag
if (++x > 1000000) {
break; // Software timeout
}
}
x = padc->ADC_LCDR; // Clear DRSY flag
}
// Next bit may be done using the PDC
Analog_values[0] = ADC->ADC_CDR1;
Analog_values[1] = ADC->ADC_CDR2;
Analog_values[2] = ADC->ADC_CDR3;
Analog_values[3] = ADC->ADC_CDR4;
Analog_values[4] = ADC->ADC_CDR5;
Analog_values[5] = ADC->ADC_CDR9;
Analog_values[6] = ADC->ADC_CDR13;
Analog_values[7] = ADC->ADC_CDR14;
#ifdef REVB
Analog_values[8] = ADC->ADC_CDR8 ;
#endif
// Power save
// PMC->PMC_PCER0 &= ~0x20000000L ; // Disable peripheral clock to ADC
}

113
src/board_gruvin9x.cpp
View file

@ -67,3 +67,116 @@ inline void board_init()
TCCR0B = (1<<WGM02) | (0b011 << CS00);
TCCR0A = (0b01<<WGM00);
}
FORCEINLINE uint8_t keyDown()
{
return (~PINL) & 0x3F;
}
bool keyState(EnumKeys enuk)
{
uint8_t result = 0 ;
if (enuk < (int)DIM(keys))
return keys[enuk].state() ? 1 : 0;
switch(enuk){
case SW_ElevDR:
result = PINC & (1<<INP_C_ElevDR);
break;
case SW_AileDR:
result = PINC & (1<<INP_C_AileDR);
break;
case SW_RuddDR:
result = PING & (1<<INP_G_RuddDR);
break;
// INP_G_ID1 INP_B_ID2
// id0 0 1
// id1 1 1
// id2 1 0
case SW_ID0:
result = !(PING & (1<<INP_G_ID1));
break;
case SW_ID1:
result = (PING & (1<<INP_G_ID1))&& (PINB & (1<<INP_B_ID2));
break;
case SW_ID2:
result = !(PINB & (1<<INP_B_ID2));
break;
case SW_Gear:
result = PING & (1<<INP_G_Gear);
break;
case SW_ThrCt:
result = PING & (1<<INP_G_ThrCt);
break;
case SW_Trainer:
result = PINB & (1<<INP_B_Trainer);
break;
default:
break;
}
return result;
}
void readKeysAndTrims()
{
/* Original keys were connected to PORTB as follows:
Bit Key
7 other use
6 LEFT
5 RIGHT
4 UP
3 DOWN
2 EXIT
1 MENU
0 other use
*/
uint8_t enuk = KEY_MENU;
keys[BTN_RE1].input(~PIND & 0x20, BTN_RE1);
keys[BTN_RE2].input(~PIND & 0x10, BTN_RE2);
uint8_t tin = ~PINL;
uint8_t in;
in = (tin & 0x0f) << 3;
in |= (tin & 0x30) >> 3;
for(int i=1; i<7; i++)
{
//INP_B_KEY_MEN 1 .. INP_B_KEY_LFT 6
keys[enuk].input(in & (1<<i),(EnumKeys)enuk);
++enuk;
}
// Trim switches ...
static const pm_uchar crossTrim[] PROGMEM ={
1<<INP_J_TRM_LH_DWN,
1<<INP_J_TRM_LH_UP,
1<<INP_J_TRM_LV_DWN,
1<<INP_J_TRM_LV_UP,
1<<INP_J_TRM_RV_DWN,
1<<INP_J_TRM_RV_UP,
1<<INP_J_TRM_RH_DWN,
1<<INP_J_TRM_RH_UP
};
in = ~PINJ;
for (int i=0; i<8; i++) {
// INP_D_TRM_RH_UP 0 .. INP_D_TRM_LH_UP 7
keys[enuk].input(in & pgm_read_byte(crossTrim+i),(EnumKeys)enuk);
++enuk;
}
}

109
src/board_stock.cpp
View file

@ -65,3 +65,112 @@ inline void board_init()
TIMSK |= (1<<OCIE0) | (1<<TOIE0); // Enable Output-Compare and Overflow interrrupts
/********************************/
}
FORCEINLINE uint8_t keyDown()
{
return (~PINB) & 0x7E;
}
bool keyState(EnumKeys enuk)
{
uint8_t result = 0 ;
if (enuk < (int)DIM(keys))
return keys[enuk].state() ? 1 : 0;
switch(enuk){
case SW_ElevDR:
result = PINE & (1<<INP_E_ElevDR);
break;
#if defined(JETI) || defined(FRSKY) || defined(ARDUPILOT) || defined(NMEA) || defined(MAVLINK)
case SW_AileDR:
result = PINC & (1<<INP_C_AileDR); //shad974: rerouted inputs to free up UART0
break;
#else
case SW_AileDR:
result = PINE & (1<<INP_E_AileDR);
break;
#endif
case SW_RuddDR:
result = PING & (1<<INP_G_RuddDR);
break;
// INP_G_ID1 INP_E_ID2
// id0 0 1
// id1 1 1
// id2 1 0
case SW_ID0:
result = !(PING & (1<<INP_G_ID1));
break;
case SW_ID1:
result = (PING & (1<<INP_G_ID1))&& (PINE & (1<<INP_E_ID2));
break;
case SW_ID2:
result = !(PINE & (1<<INP_E_ID2));
break;
case SW_Gear:
result = PINE & (1<<INP_E_Gear);
break;
//case SW_ThrCt : return PINE & (1<<INP_E_ThrCt);
#if defined(JETI) || defined(FRSKY) || defined(ARDUPILOT) || defined(NMEA) || defined(MAVLINK)
case SW_ThrCt:
result = PINC & (1<<INP_C_ThrCt); //shad974: rerouted inputs to free up UART0
break;
#else
case SW_ThrCt:
result = PINE & (1<<INP_E_ThrCt);
break;
#endif
case SW_Trainer:
result = PINE & (1<<INP_E_Trainer);
break;
default:
break;
}
return result;
}
inline void readKeysAndTrims()
{
uint8_t enuk = KEY_MENU;
// User buttons ...
uint8_t in = ~PINB;
for(int i=1; i<7; i++)
{
//INP_B_KEY_MEN 1 .. INP_B_KEY_LFT 6
keys[enuk].input(in & (1<<i),(EnumKeys)enuk);
++enuk;
}
// Trim switches ...
static const pm_uchar crossTrim[] PROGMEM ={
1<<INP_D_TRM_LH_DWN, // bit 7
1<<INP_D_TRM_LH_UP,
1<<INP_D_TRM_LV_DWN,
1<<INP_D_TRM_LV_UP,
1<<INP_D_TRM_RV_DWN,
1<<INP_D_TRM_RV_UP,
1<<INP_D_TRM_RH_DWN,
1<<INP_D_TRM_RH_UP // bit 0
};
in = ~PIND;
for (int i=0; i<8; i++) {
// INP_D_TRM_RH_UP 0 .. INP_D_TRM_LH_UP 7
keys[enuk].input(in & pgm_read_byte(crossTrim+i),(EnumKeys)enuk);
++enuk;
}
}

217
src/drivers.cpp
View file

@ -105,32 +105,6 @@ uint8_t getEvent()
return evt;
}
class Key
{
#define FILTERBITS 4
#ifdef SIMU
#define FFVAL 1
#else
#define FFVAL ((1<<FILTERBITS)-1)
#endif
#define KSTATE_OFF 0
#define KSTATE_RPTDELAY 95 // gruvin: delay state before key repeating starts
//#define KSTATE_SHORT 96
#define KSTATE_START 97
#define KSTATE_PAUSE 98
#define KSTATE_KILLED 99
uint8_t m_vals:FILTERBITS; // key debounce? 4 = 40ms
uint8_t m_dblcnt:2;
uint8_t m_cnt;
uint8_t m_state;
public:
void input(bool val, EnumKeys enuk);
bool state() { return m_vals==FFVAL; }
void pauseEvents() { m_state = KSTATE_PAUSE; m_cnt = 0;}
void killEvents() { m_state = KSTATE_KILLED; m_dblcnt=0; }
uint8_t getDbl() { return m_dblcnt; }
};
Key keys[NUM_KEYS];
void Key::input(bool val, EnumKeys enuk)
{
@ -199,121 +173,6 @@ void Key::input(bool val, EnumKeys enuk)
}
}
bool keyState(EnumKeys enuk)
{
uint8_t result = 0 ;
if (enuk < (int)DIM(keys))
return keys[enuk].state() ? 1 : 0;
#if defined (PCBV4)
switch(enuk){
case SW_ElevDR:
result = PINC & (1<<INP_C_ElevDR);
break;
case SW_AileDR:
result = PINC & (1<<INP_C_AileDR);
break;
case SW_RuddDR:
result = PING & (1<<INP_G_RuddDR);
break;
// INP_G_ID1 INP_B_ID2
// id0 0 1
// id1 1 1
// id2 1 0
case SW_ID0:
result = !(PING & (1<<INP_G_ID1));
break;
case SW_ID1:
result = (PING & (1<<INP_G_ID1))&& (PINB & (1<<INP_B_ID2));
break;
case SW_ID2:
result = !(PINB & (1<<INP_B_ID2));
break;
case SW_Gear:
result = PING & (1<<INP_G_Gear);
break;
case SW_ThrCt:
result = PING & (1<<INP_G_ThrCt);
break;
case SW_Trainer:
result = PINB & (1<<INP_B_Trainer);
break;
default:
break;
}
#else
switch(enuk){
case SW_ElevDR:
result = PINE & (1<<INP_E_ElevDR);
break;
#if defined(JETI) || defined(FRSKY) || defined(ARDUPILOT) || defined(NMEA) || defined(MAVLINK)
case SW_AileDR:
result = PINC & (1<<INP_C_AileDR); //shad974: rerouted inputs to free up UART0
break;
#else
case SW_AileDR:
result = PINE & (1<<INP_E_AileDR);
break;
#endif
case SW_RuddDR:
result = PING & (1<<INP_G_RuddDR);
break;
// INP_G_ID1 INP_E_ID2
// id0 0 1
// id1 1 1
// id2 1 0
case SW_ID0:
result = !(PING & (1<<INP_G_ID1));
break;
case SW_ID1:
result = (PING & (1<<INP_G_ID1))&& (PINE & (1<<INP_E_ID2));
break;
case SW_ID2:
result = !(PINE & (1<<INP_E_ID2));
break;
case SW_Gear:
result = PINE & (1<<INP_E_Gear);
break;
//case SW_ThrCt : return PINE & (1<<INP_E_ThrCt);
#if defined(JETI) || defined(FRSKY) || defined(ARDUPILOT) || defined(NMEA) || defined(MAVLINK)
case SW_ThrCt:
result = PINC & (1<<INP_C_ThrCt); //shad974: rerouted inputs to free up UART0
break;
#else
case SW_ThrCt:
result = PINE & (1<<INP_E_ThrCt);
break;
#endif
case SW_Trainer:
result = PINE & (1<<INP_E_Trainer);
break;
default:
break;
}
#endif // defined (PCBV4)
return result;
}
void pauseEvents(uint8_t event)
{
event=event & EVT_KEY_MASK;
@ -338,8 +197,6 @@ void per10ms()
g_tmr10ms++;
g_blinkTmr10ms++;
uint8_t enuk = KEY_MENU;
#if defined (PCBV4)
/* Update gloabal Date/Time every 100 per10ms cycles */
if (++g_ms100 == 100)
@ -347,81 +204,9 @@ void per10ms()
g_unixTime++; // inc global unix timestamp one second
g_ms100 = 0;
}
/* Original keys were connected to PORTB as follows:
Bit Key
7 other use
6 LEFT
5 RIGHT
4 UP
3 DOWN
2 EXIT
1 MENU
0 other use
*/
keys[BTN_RE1].input(~PIND & 0x20, BTN_RE1);
keys[BTN_RE2].input(~PIND & 0x10, BTN_RE2);
uint8_t tin = ~PINL;
uint8_t in;
in = (tin & 0x0f) << 3;
in |= (tin & 0x30) >> 3;
#else
// User buttons ...
uint8_t in = ~PINB;
#endif
for(int i=1; i<7; i++)
{
//INP_B_KEY_MEN 1 .. INP_B_KEY_LFT 6
keys[enuk].input(in & (1<<i),(EnumKeys)enuk);
++enuk;
}
// End User buttons
// Trim switches ...
#if defined (PCBV4)
static const pm_uchar crossTrim[] PROGMEM ={
1<<INP_J_TRM_LH_DWN,
1<<INP_J_TRM_LH_UP,
1<<INP_J_TRM_LV_DWN,
1<<INP_J_TRM_LV_UP,
1<<INP_J_TRM_RV_DWN,
1<<INP_J_TRM_RV_UP,
1<<INP_J_TRM_RH_DWN,
1<<INP_J_TRM_RH_UP
};
#else // stock original board ...
static const pm_uchar crossTrim[] PROGMEM ={
1<<INP_D_TRM_LH_DWN, // bit 7
1<<INP_D_TRM_LH_UP,
1<<INP_D_TRM_LV_DWN,
1<<INP_D_TRM_LV_UP,
1<<INP_D_TRM_RV_DWN,
1<<INP_D_TRM_RV_UP,
1<<INP_D_TRM_RH_DWN,
1<<INP_D_TRM_RH_UP // bit 0
};
#endif
#if defined (PCBV4)
in = ~PINJ;
#else
in = ~PIND;
#endif
for (int i=0; i<8; i++) {
// INP_D_TRM_RH_UP 0 .. INP_D_TRM_LH_UP 7
keys[enuk].input(in & pgm_read_byte(crossTrim+i),(EnumKeys)enuk);
++enuk;
}
// End Trim Switches
/**** END KEY STATE READ ****/
readKeysAndTrims();
#ifdef MAVLINK
check_mavlink() ;

83
src/lcd.cpp
View file

@ -733,6 +733,86 @@ void lcdSetRefVolt(uint8_t val)
#endif
}
void refreshDisplay()
{
register Pio *pioptr;
register uint8_t *p = displayBuf;
register uint32_t y;
register uint32_t x;
register uint32_t z;
register uint32_t ebit;
#ifdef REVB
#else
register uint8_t *lookup;
lookup = (uint8_t *) Lcd_lookup;
#endif
ebit = LCD_E;
#ifdef REVB
pioptr = PIOA;
pioptr->PIO_PER = 0x00000080; // Enable bit 7 (LCD-A0)
pioptr->PIO_OER = 0x00000080;// Set bit 7 output
#endif
pioptr = PIOC;
#ifdef REVB
pioptr->PIO_OER = 0x0C0030FFL; // Set bits 27,26,15,13,12,7-0 output
#else
pioptr->PIO_OER = 0x0C00B0FFL; // Set bits 27,26,15,13,12,7-0 output
#endif
for (y = 0; y < 8; y++) {
lcdSendCtl(/*TODO g_eeGeneral.optrexDisplay ? 0 : */0x04);
lcdSendCtl(0x10); //column addr 0
lcdSendCtl(y | 0xB0); //page addr y
pioptr->PIO_CODR = LCD_CS1; // Select LCD
PIOA->PIO_SODR = LCD_A0; // Data
pioptr->PIO_CODR = LCD_RnW; // Write
#ifdef REVB
x = *p;
#else
x = lookup[*p];
#endif
for (z = 0; z < 128; z += 1) {
// The following 7 lines replaces by a lookup table
// x = __RBIT( *p++ ) ;
// x >>= 23 ;
// if ( x & 0x00000100 )
// {
// x |= 1 ;
// }
// pioptr->PIO_ODSR = x ;
pioptr->PIO_ODSR = x;
pioptr->PIO_SODR = ebit; // Start E pulse
// Need a delay here (250nS)
p += 1;
#ifdef REVB
x = *p;
#else
x = lookup[*p];
#endif
// TC0->TC_CHANNEL[0].TC_CCR = 5 ; // Enable clock and trigger it (may only need trigger)
// while ( TC0->TC_CHANNEL[0].TC_CV < 3 ) // Value depends on MCK/2 (used 6MHz)
// {
// // Wait
// }
pioptr->PIO_CODR = ebit; // End E pulse
}
pioptr->PIO_SODR = LCD_CS1; // Deselect LCD
}
#ifdef REVB
pioptr->PIO_ODR = 0x0000003AL; // Set bits 1, 3, 4, 5 input
pioptr->PIO_PUER = 0x0000003AL;// Set bits 1, 3, 4, 5 with pullups
#else
pioptr->PIO_ODR = 0x0000003CL; // Set bits 2, 3, 4, 5 input
pioptr->PIO_PUER = 0x0000003CL; // Set bits 2, 3, 4, 5 with pullups
#endif
pioptr->PIO_ODSR = 0 ; // Drive D0 low
}
#else
#define delay_1us() _delay_us(1)
@ -789,7 +869,6 @@ void lcdSetRefVolt(uint8_t val)
lcdSendCtl(0x81);
lcdSendCtl(val);
}
#endif
void refreshDisplay()
{
@ -818,3 +897,5 @@ void refreshDisplay()
}
#endif
}
#endif

928
src/mavlink.cpp Normal file
View file

@ -0,0 +1,928 @@
/*
* Authors (alphabetical order)
* - Bertrand Songis <bsongis@gmail.com>
* - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
* - Cameron Weeks <th9xer@gmail.com>
* - Erez Raviv
* - Gerard Valade <gerard.valade@gmail.com>
* - Jean-Pierre Parisy
* - Karl Szmutny <shadow@privy.de>
* - 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"
#include "serial.h"
#include "mavlink.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)] PROGMEM = { //
//
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 check_mavlink() {
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
} MAVLINK_menu = MENU_INFO;
inline mavlink_menu_ operator++(mavlink_menu_ &eDOW, int) {
int i = static_cast<int>(eDOW);
i++;
if (i < MAX_MAVLINK_MENU) {
eDOW = static_cast<mavlink_menu_>(i);
}
return eDOW;
}
inline mavlink_menu_ operator--(mavlink_menu_ &eDOW, int) {
int i = static_cast<int>(eDOW);
if (i > 0) {
eDOW = static_cast<mavlink_menu_>(--i);
}
return eDOW;
}
void mav_title(const pm_char * s, uint8_t index) {
lcd_putsAtt(0, 0, PSTR("MAVLINK"), INVERS);
lcd_putsAtt(10 * FW, 0, s, 0);
DisplayScreenIndex(index, MAX_MAVLINK_MENU, INVERS);
lcd_putcAtt(8 * FW, 0, (mav_heartbeat > 0) ? '*' : ' ', 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 menuProcMavlinkInfos(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_putcAtt(x1, y, ' ', 0);
} else {
lcd_putcAtt(x1, y, *ptr++, 0);
}
x1 += FW;
}
x1 = FW;
y += FH;
if (telemetry_data.status) {
if (!isValidReqControlMode()) {
lcd_putsnAtt(x1, y, PSTR("REQ"), 3, 0);
putsControlMode(x2, y, telemetry_data.req_mode, 0, 6);
y += FH;
}
lcd_putsnAtt(x1, y, PSTR("MODE"), 4, 0);
putsMavlinkControlMode(x2, y, 6);
y += FH;
lcd_putsnAtt(x1, y, PSTR("BATT"), 4, 0);
lcd_outdezNAtt(xnum, y, telemetry_data.vbat, PREC1, 5);
y += FH;
lcd_putsnAtt(x1, y, PSTR("DROP"), 4, 0);
lcd_outdezAtt(xnum, y, telemetry_data.packet_drop, 0);
}
}
void menuProcMavlinkGPS(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_putsnAtt(x2 + 5 * FW, y, PSTR("SAT"), 3, 0);
lcd_outdezNAtt(x2 + 8 * FW + 3 * FWNUM, y, telemetry_data.satellites_visible, 0, 2);
// if (telemetry_data.fix_type > 0) {
y += FH;
lcd_putsnAtt(0, y, PSTR("HDOP"), 4, 0);
lcd_outdezFloat(xnum, y, telemetry_data.eph, 2);
y += FH;
lcd_putsnAtt(0, y, PSTR("COOR"), 4, 0);
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 menuProcMavlinkDump(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 menuProcMavlink(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);
break;
}
switch (MAVLINK_menu) {
case MENU_INFO:
menuProcMavlinkInfos();
break;
case MENU_GPS:
menuProcMavlinkGPS();
break;
#ifdef DUMP_RX_TX
case MENU_DUMP_TX:
case MENU_DUMP_RX:
menuProcMavlinkDump(event);
break;
#endif
default:
break;
}
}

358
src/mavlink.h Normal file
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 check_mavlink();
void MAVLINK_Init(void);
void menuProcMavlink(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

119
src/open9x.cpp
View file

@ -507,15 +507,6 @@ uint8_t getTrimFlightPhase(uint8_t idx, uint8_t phase)
return 0;
}
FORCEINLINE uint8_t keyDown()
{
#if defined (PCBV4)
return (~PINL) & 0x3F;
#else
return (~PINB) & 0x7E;
#endif
}
void clearKeyEvents()
{
#ifdef SIMU
@ -796,17 +787,42 @@ uint16_t BandGap = 2040 ;
#else
uint16_t BandGap ;
#endif
static uint16_t s_anaFilt[8];
#if defined(PCBARM) and defined(REVB)
#define NUMBER_ANALOG 9
#else
#define NUMBER_ANALOG 8
#endif
static uint16_t s_anaFilt[NUMBER_ANALOG];
uint16_t anaIn(uint8_t chan)
{
// ana-in: 3 1 2 0 4 5 6 7
//static pm_char crossAna[] PROGMEM ={4,2,3,1,5,6,7,0}; // wenn schon Tabelle, dann muss sich auch lohnen
// Google Translate (German): // if table already, then it must also be worthwhile
#if defined(PCBARM)
static const uint8_t crossAna[]={1,5,7,0,4,6,2,3,8};
#else
static const pm_char crossAna[] PROGMEM ={3,1,2,0,4,5,6,7};
#endif
volatile uint16_t *p = &s_anaFilt[pgm_read_byte(crossAna+chan)];
return *p;
}
#if defined(PCBARM)
void getADC_filt()
{
register uint32_t x ;
static uint16_t t_ana[2][NUMBER_ANALOG] ;
read_9_adc() ;
for( x = 0 ; x < NUMBER_ANALOG ; x += 1 )
{
s_anaFilt[x] = s_anaFilt[x]/2 + (t_ana[1][x] >> 2 ) ;
t_ana[1][x] = ( t_ana[1][x] + t_ana[0][x] ) >> 1 ;
t_ana[0][x] = ( t_ana[0][x] + Analog_values[x] ) >> 1 ;
}
}
#else
void getADC_filt()
{
static uint16_t t_ana[2][8];
@ -826,7 +842,33 @@ void getADC_filt()
t_ana[0][adc_input] = (t_ana[0][adc_input] + ADCW) >> 1;
}
}
#endif
#if defined(PCBARM)
void getADC_osmp()
{
register uint32_t x;
register uint32_t y;
uint16_t temp[NUMBER_ANALOG];
for( x = 0; x < NUMBER_ANALOG; x += 1 )
{
temp[x] = 0;
}
for( y = 0; y < 4; y += 1 )
{
read_9_adc();
for( x = 0; x < NUMBER_ANALOG; x += 1 )
{
temp[x] += Analog_values[x];
}
}
for( x = 0; x < NUMBER_ANALOG; x += 1 )
{
s_anaFilt[x] = temp[x] >> 3;
}
}
#else
void getADC_osmp()
{
uint16_t temp_ana;
@ -845,7 +887,21 @@ void getADC_osmp()
s_anaFilt[adc_input] = temp_ana / 2; // divide by 2^n to normalize result.
}
}
#endif
#if defined(PCBARM)
void getADC_single()
{
register uint32_t x ;
read_9_adc() ;
for( x = 0 ; x < NUMBER_ANALOG ; x += 1 )
{
s_anaFilt[x] = Analog_values[x] >> 1 ;
}
}
#else
void getADC_single()
{
for (uint8_t adc_input=0; adc_input<8; adc_input++) {
@ -858,13 +914,9 @@ void getADC_single()
s_anaFilt[adc_input]= ADCW * 2; // use 11 bit numbers
}
}
#endif
getADCp getADC[3] = {
getADC_single,
getADC_osmp,
getADC_filt
};
#if not defined(PCBARM)
void getADC_bandgap()
{
#if defined (PCBV4)
@ -888,6 +940,7 @@ void getADC_bandgap()
BandGap=ADCW;
#endif
}
#endif
#endif // SIMU
@ -896,6 +949,7 @@ uint16_t g_vbat100mV = 0;
volatile uint8_t tick10ms = 0;
uint16_t g_LightOffCounter;
#if not defined(PCBARM)
FORCEINLINE bool checkSlaveMode()
{
// no power -> only phone jack = slave mode
@ -917,6 +971,7 @@ FORCEINLINE bool checkSlaveMode()
return lastSlaveMode;
#endif
}
#endif
uint16_t s_timeCumTot;
uint16_t s_timeCumThr; // THR in 1/16 sec
@ -1725,7 +1780,7 @@ void perMain()
g_menuStack[g_menuStackPtr](evt);
refreshDisplay();
#if defined (PCBV4)
#if defined(PCBV4)
// PPM signal on phono-jack. In or out? ...
if(checkSlaveMode()) {
PORTG |= (1<<OUT_G_SIM_CTL); // 1=ppm out
@ -1733,7 +1788,7 @@ void perMain()
else{
PORTG &= ~(1<<OUT_G_SIM_CTL); // 0=ppm in
}
#else
#elif defined(PCBSTD)
// PPM signal on phono-jack. In or out? ...
if(checkSlaveMode()) {
PORTG &= ~(1<<OUT_G_SIM_CTL); // 0=ppm out
@ -1769,7 +1824,7 @@ void perMain()
int16_t g_ppmIns[8];
uint8_t ppmInState = 0; //0=unsync 1..8= wait for value i-1
#ifndef SIMU
#if not defined(SIMU) and not defined(PCBARM)
volatile uint8_t g_tmr16KHz; //continuous timer 16ms (16MHz/1024/256) -- 8-bit counter overflow
#if defined (PCBV4)
@ -2100,6 +2155,7 @@ int main(void)
lcd_init();
#if not defined(PCBARM)
// Init Stack while interrupts are disabled
#define STACKPTR _SFR_IO16(0x3D)
{
@ -2114,6 +2170,7 @@ int main(void)
*p-- = 0x55 ;
}
}
#endif
g_menuStack[0] = menuMainView;
g_menuStack[1] = menuProcModelSelect;
@ -2154,12 +2211,12 @@ int main(void)
uint8_t cModel = g_eeGeneral.currModel;
#if defined (PCBV4)
if (MCUSR != (1 << PORF)) {
#else
if (MCUCSR != (1 << PORF)) {
#if defined(PCBV4)
if (MCUSR != (1 << PORF))
#elif defined(PCBSTD)
if (MCUCSR != (1 << PORF))
#endif
{
#ifdef SPLASH
if (g_model.protocol != PROTO_FAAST && g_model.protocol != PROTO_DSM2)
doSplash();
@ -2229,20 +2286,30 @@ int main(void)
while(1) {
uint16_t t0 = getTmr16KHz();
getADC[g_eeGeneral.filterInput]();
if (g_eeGeneral.filterInput == 1) {
getADC_filt() ;
}
else if ( g_eeGeneral.filterInput == 2) {
getADC_osmp() ;
}
else {
getADC_single() ;
}
#if defined(PCBV4)
// For PCB V4, use our own 1.2V, external reference (connected to ADC3)
ADCSRB &= ~(1<<MUX5);
ADMUX = 0x03|ADC_VREF_TYPE; // Switch MUX to internal reference
#else
#elif defined(PCBSTD)
ADMUX = 0x1E|ADC_VREF_TYPE; // Switch MUX to internal reference
#endif
perMain();
// Bandgap has had plenty of time to settle...
#if not defined(PCBARM)
getADC_bandgap();
#endif
if(heartbeat == 0x3)
{

73
src/open9x.h
View file

@ -56,6 +56,14 @@ typedef const uint8_t pm_uint8_t;
typedef const int16_t pm_int16_t;
typedef const int8_t pm_int8_t;
#define wdt_reset()
#define pgm_read_byte(address_short) (*(uint8_t*)(address_short))
#define PSTR(adr) adr
#define PROGMEM
#define pgm_read_adr(x) *(x)
#define cli()
#define sei()
#define wdt_enable(x)
extern void board_init();
#else
#include <avr/io.h>
#include <avr/pgmspace.h>
@ -233,7 +241,11 @@ extern RlcFile theFile; //used for any file operation
#endif // defined (PCBV4)
#if defined(PCBARM)
#define SLAVE_MODE 1
#else
#define SLAVE_MODE (PING & (1<<INP_G_RF_POW))
#endif
extern const pm_uint8_t bchout_ar[];
extern const pm_uint8_t modn12x3[];
@ -285,6 +297,34 @@ enum EnumKeys {
};
class Key
{
#define FILTERBITS 4
#ifdef SIMU
#define FFVAL 1
#else
#define FFVAL ((1<<FILTERBITS)-1)
#endif
#define KSTATE_OFF 0
#define KSTATE_RPTDELAY 95 // gruvin: delay state before key repeating starts
//#define KSTATE_SHORT 96
#define KSTATE_START 97
#define KSTATE_PAUSE 98
#define KSTATE_KILLED 99
uint8_t m_vals:FILTERBITS; // key debounce? 4 = 40ms
uint8_t m_dblcnt:2;
uint8_t m_cnt;
uint8_t m_state;
public:
void input(bool val, EnumKeys enuk);
bool state() { return m_vals==FFVAL; }
void pauseEvents() { m_state = KSTATE_PAUSE; m_cnt = 0;}
void killEvents() { m_state = KSTATE_KILLED; m_dblcnt=0; }
uint8_t getDbl() { return m_dblcnt; }
};
extern Key keys[NUM_KEYS];
#define CURVE_BASE 7
#define SWASH_TYPE_120 1
@ -407,20 +447,14 @@ extern uint8_t pxxFlag;
extern char idx2char(int8_t idx);
/// stoppt alle events von dieser taste bis eine kurze Zeit abgelaufen ist
void pauseEvents(uint8_t enuk);
/// stoppt alle events von dieser taste bis diese wieder losgelassen wird
void killEvents(uint8_t enuk);
/// liefert den Wert einer beliebigen Taste KEY_MENU..SW_Trainer
bool keyState(EnumKeys enuk);
/// Liefert das naechste Tasten-Event, auch trim-Tasten.
/// Das Ergebnis hat die Form:
/// EVT_KEY_BREAK(key), EVT_KEY_FIRST(key), EVT_KEY_REPT(key) oder EVT_KEY_LONG(key)
void killEvents(uint8_t enuk);
uint8_t getEvent();
void putEvent(uint8_t evt);
#if defined (PCBV4)
extern uint8_t keyDown();
#endif
uint8_t keyDown();
bool keyState(EnumKeys enuk);
void readKeysAndTrims();
/// Gibt Alarm Maske auf lcd aus.
/// Die Maske wird so lange angezeigt bis eine beliebige Taste gedrueckt wird.
@ -493,8 +527,6 @@ void getADC_single();
void getADC_osmp();
void getADC_filt();
typedef void (*getADCp)();
// checkIncDec flags
#define EE_GENERAL 0x01
#define EE_MODEL 0x02
@ -504,7 +536,18 @@ extern uint8_t s_eeDirtyMsk;
#define STORE_MODELVARS eeDirty(EE_MODEL)
#define STORE_GENERALVARS eeDirty(EE_GENERAL)
#if defined (PCBV4)
#if defined (PCBARM)
#include "AT91SAM3S2.h"
#define BACKLIGHT_ON (PWM->PWM_CH_NUM[0].PWM_CDTY = 0/*TODO g_eeGeneral.bright*/)
#define BACKLIGHT_OFF (PWM->PWM_CH_NUM[0].PWM_CDTY = 100)
#ifdef REVB
#define NUMBER_ANALOG 9
#else
#define NUMBER_ANALOG 8
extern uint16_t Analog_values[NUMBER_ANALOG] ;
void read_9_adc(void ) ;
#endif
#elif defined (PCBV4)
#define SPEAKER_ON TCCR0A |= (1 << COM0A0)
#define SPEAKER_OFF TCCR0A &= ~(1 << COM0A0)
#define BACKLIGHT_ON PORTC |= (1 << OUT_C_LIGHT)
@ -713,7 +756,7 @@ extern uint16_t jeti_keys;
//audio settungs are external to keep out clutter!
// TODO english learning for me... what does mean "keep out clutter"?
#include "audio.h"
#elif not defined(PCBARM)
#else
#include "beeper.h"
#endif

122
src/rotarysw.cpp Normal file
View file

@ -0,0 +1,122 @@
/*
* Author - Gerard Valade
*
*
* 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 CTIME_ROTARY_ANIM 500
uint16_t rotarySwChanged = 0;
int8_t rotarySwIdx = -1;
int8_t rotarySwLastPPMVal = 0;
void putsRotarySwPos(uint8_t x, uint8_t y, uint8_t idx, uint8_t att) {
lcd_putcAtt(x, y, 'P', att);
lcd_putcAtt(x + FW, y, '1' + idx, att);
}
#ifdef MAVLINK
extern bool isValidReqControlMode();
extern void putsMavlinkControlMode(uint8_t x, uint8_t y, uint8_t len);
extern void MAVLINK_ReqMode(uint8_t mode, uint8_t send);
#endif
void putsControlMode(uint8_t x, uint8_t y, uint8_t idx, uint8_t attr, uint8_t len) {
if (idx < ACM_NUM_MODE) {
lcd_putsnAtt(x, y, PSTR(CONROL_MODE_STR) + 6 * idx, len, attr);
} else if (idx < NUM_MODES) {
idx -= ACM_NUM_MODE;
lcd_putsnAtt(x, y, PSTR(CONROL_MODE_STR_APM) + 6 * idx, len, attr);
} else if (idx < NUM_MODES_ALL) {
idx -= NUM_MODES;
lcd_putsnAtt(x, y, PSTR(DISPLAY_ONLY_STR) + 6 * idx, len, attr);
} else {
for (uint8_t i = 0; i < len; i++) {
lcd_putcAtt(x, y, '-', attr);
}
}
}
void setRotarySwIdx(int8_t idx) {
if (rotarySwIdx != idx) {
rotarySwIdx = idx;
if (rotarySwIdx >= 0) {
rotarySwChanged = (get_tmr10ms() >> 3) + 3;
#ifdef MAVLINK
uint8_t send = g_model.mavlink.rotarySw[rotarySwIdx].typeRotary == ROTARY_TYPE_MAVLINK ? 1 : 0;
MAVLINK_ReqMode(g_model.mavlink.rotarySw[rotarySwIdx].numMode, send);
#endif
}
}
}
void animRotarySw(uint8_t x) {
uint16_t s_time = get_tmr10ms() >> 3; // 80ms time unit
uint8_t swToggle = (s_time < rotarySwChanged);
uint8_t att1 = 0;
if (s_timerState[0] != TMR_OFF) {
att1 = DBLSIZE | (s_timerState[0] == TMR_BEEPING ? BLINK : 0);
putsTime(x + 14 * FW - 2, FH * 2, s_timerVal[0], att1, att1);
}
s_time = s_time >> 4; // 1280ms time unit
uint8_t num4Display = s_time & 0x01; // 1/4 display time
#ifdef MAVLINK
s_time = telemetry_data.status ? (s_time >> 1) : 0; // 2560ms time unit
#else
s_time = 0;
#endif
uint8_t num2Display = s_time & 0x01; // 1/2 time display time
if (rotarySwIdx >= 0) {
att1 = swToggle ? INVERS : 0;
putsControlMode(x + 4 * FW, 2 * FH, g_model.mavlink.rotarySw[rotarySwIdx].numMode, att1 | NO_UNIT, 6);
}
switch (num2Display) {
case 0:
att1 = (g_vbat100mV < g_eeGeneral.vBatWarn ? BLINK : 0);
putsVBat(x + 4 * FW, 2 * FH, att1 | NO_UNIT | DBLSIZE);
switch (num4Display) {
case 0:
lcd_putsnAtt(x + 4 * FW, 3 * FH, PSTR("ExpExFFneMedCrs") + 3 * g_model.trimInc, 3, 0);
lcd_putsnAtt(x + 8 * FW - FW / 2, 3 * FH, PSTR(" TTm") + 3 * g_model.thrTrim, 3, 0);
break;
default:
lcd_putcAtt(x + 4 * FW, 3 * FH, 'V', 0);
if (s_timerState != TMR_OFF) {
putsTmrMode(x + 7 * FW - FW / 2, 3 * FH, g_model.timers[0].mode, 0);
}
break;
}
break;
default:
#ifdef MAVLINK
att1 = (telemetry_data.vbat_low ? BLINK : 0);
lcd_outdezAtt(x + 4 * FW, 2 * FH, telemetry_data.vbat, att1 | PREC1 | DBLSIZE);
if (isValidReqControlMode())
{
lcd_putsnAtt(x + 4 * FW, 3 * FH, PSTR("MAVLNK"), 6, 0);
} else {
putsMavlinkControlMode(x + 4 * FW, 3 * FH, 6);
}
#endif
break;
}
}

109
src/rotarysw.h Normal file
View file

@ -0,0 +1,109 @@
/*
* Author - Gerard Valade
*
*
* 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 rotarysw_h
#define rotarysw_h
#include "myeeprom.h"
extern int8_t rotarySwIdx;
extern int8_t rotarySwLastPPMVal;
extern void putsControlMode(uint8_t x, uint8_t y, uint8_t idx, uint8_t attr, uint8_t len);
extern void putsRotarySwPos(uint8_t x, uint8_t y, uint8_t idx1, uint8_t att);
extern void setRotarySwIdx(int8_t idx);
extern void setRotarySwDisplay(int8_t idx);
extern void animRotarySw(uint8_t x);
extern void menuProcRotarySwitches(uint8_t event);
// Control mode define from arducoper
enum CONTROL_MODE {
STABILIZE = 0, // hold level position
ACRO, // rate control
ALT_HOLD, // AUTO control
AUTO, // AUTO control
GUIDED, // AUTO control
LOITER, // AUTO control
RTL, // AUTO control
CIRCLE, // AUTO control
POSITION, // AUTO control
LAND, // AUTO control
OF_LOITER, // Hold a single location using optical flow sensor
//
// Adding control mode define from ardupilot
MANUAL,
FLY_BY_WIRE_A, // Fly By Wire A has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical = manual throttle
FLY_BY_WIRE_B, // Fly By Wire B has left stick horizontal => desired roll angle, left stick vertical => desired pitch angle, right stick vertical => desired airspeed
FLY_BY_WIRE_C, // Fly By Wire C has left stick horizontal => desired roll angle, left stick vertical => desired climb rate, right stick vertical => desired airspeed
// Fly By Wire B and Fly By Wire C require airspeed sensor
// This only for display
INITIALISING,
NUM_MODES_ALL,
};
#define ACM_NUM_MODE (OF_LOITER+1)
#define NUM_MODES (FLY_BY_WIRE_C+1)
// 0123456789012345678901234567890123456789012345678901234567890123456789
// 0 1 2 3 4 5 6 7 8 9 0 1
#define CONROL_MODE_STR "STAB ACRO ALT_H AUTO GUIDEDLOITERRTL CIRCLEPOSITILAND OF_LOI"
#define CONROL_MODE_STR_APM "MANUALWIRE_AWIRE_BWIRE_C"
#define DISPLAY_ONLY_STR "INIT"
inline void init_rotary_sw() {
setRotarySwIdx(-1); // Reinit sw roll idx
for (uint8_t i = 0; i < NUM_ROTARY_SW; i++) {
if (g_model.mavlink.rotarySw[i].numMode >= NUM_MODES || g_model.mavlink.rotarySw[i].typeRotary > ROTARY_TYPE_MAVLINK)
{
g_model.mavlink.rotarySw[i].typeRotary = 0;
g_model.mavlink.rotarySw[i].numMode = 0;
}
#ifdef OLD_ROTARY
for (uint8_t j = 0; j < sizeof(g_model.mavlink.rotarySw[i].name); j++) // makes sure name is valid
{
uint8_t idx = char2idx(g_model.mavlink.rotarySw[i].name[j]);
g_model.mavlink.rotarySw[i].name[j] = idx2char(idx);
}
#endif
}
}
inline int8_t find_rotary_sw_pos(uint8_t srcRaw, uint8_t swTog, uint8_t swOn) {
if ((swOn && swTog) || rotarySwIdx == -1) { // toggle switch on
int8_t idx = rotarySwIdx;
int8_t inc = srcRaw == MIX_INC_ROTARY_SW ? 1 : -1;
for (uint8_t k = 0; k < NUM_ROTARY_SW; k++) { // find first enable slot
idx = (idx + inc) & 0x7; // do modulo 8
if (g_model.mavlink.rotarySw[idx].typeRotary) {
setRotarySwIdx(idx);
return idx;
}
}
setRotarySwIdx(-1);
}
return rotarySwIdx;
}
inline int8_t get_rotary_value() {
if (g_model.mavlink.rotarySw[rotarySwIdx].typeRotary == ROTARY_TYPE_PPM) {
uint8_t num = g_model.mavlink.rotarySw[rotarySwIdx].numMode;
rotarySwLastPPMVal = g_model.mavlink.modesVal[num];
}
return rotarySwLastPPMVal;
}
#endif

226
src/serial.cpp Normal file
View file

@ -0,0 +1,226 @@
/*
* Authors - Gerard Valade <gerard.valade@gmail.com>
*
* Adapted from frsky
*
* 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 "serial.h"
/*
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).
*/
#ifndef SIMU
ISR (USART0_RX_vect)
{
uint8_t iostat; //USART control and Status Register 0 A
// uint8_t rh; //USART control and Status Register 0 B
UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
sei();
iostat = UCSR0A; //USART control and Status Register 0 A
uint8_t byte = UDR0;
/*
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))) {
byte = 0;
}
//rh = UCSR0B; //USART control and Status Register 0 B
#ifdef MAVLINK
(RXHandler)(byte);
#endif
cli();
UCSR0B |= (1 << RXCIE0); // enable Interrupt
}
#endif
inline void SERIAL_EnableRXD(void) {
UCSR0B |= (1 << RXEN0); // enable RX
UCSR0B |= (1 << RXCIE0); // enable Interrupt
}
#if 0
void SERIAL_DisableRXD(void) {
UCSR0B &= ~(1 << RXEN0); // disable RX
UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
}
#endif
/*
USART0 (transmit) Data Register Emtpy ISR
Usef to transmit FrSky data packets, which are buffered in frskyTXBuffer.
*/
uint8_t serialTxBuffer[MAX_TX_BUFFER]; // 32 characters
uint8_t serialTxBufferCount = 0;
uint8_t * ptrTxISR = 0;
serial_tx_state_t serialTxState = TX_STATE_EMPTY;
#ifndef SIMU
ISR(USART0_UDRE_vect)
{
if (serialTxBufferCount > 0) {
UDR0 = *ptrTxISR++;
serialTxBufferCount--;
} else {
UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
serialTxState = TX_STATE_EMPTY;
}
}
#endif
void SERIAL_start_uart_send() {
ptrTxISR = serialTxBuffer;
serialTxBufferCount = 0;
}
void SERIAL_end_uart_send() {
ptrTxISR = serialTxBuffer;
//UCSR0B |= (1 << UDRIE0); // enable UDRE0 interrupt
serialTxState = TX_STATE_READY;
}
void SERIAL_send_uart_bytes(uint8_t * buf, uint16_t len) {
while (len--) {
*ptrTxISR++ = *buf++;
serialTxBufferCount++;
}
}
#if 0
void SERIAL_transmitBuffer(uint8_t len) {
serialTxBufferCount = len;
ptrTxISR = serialTxBuffer;
//UCSR0B |= (1 << UDRIE0); // enable UDRE0 interrupt
serialTxState = TX_STATE_READY;
}
#endif
void SERIAL_startTX(void) {
if (serialTxState == TX_STATE_READY) {
serialTxState = TX_STATE_BUSY;
UCSR0B |= (1 << UDRIE0); // enable UDRE0 interrupt
}
}
static void uart_19200(void) {
#ifndef SIMU
#define BAUD 19200
#include <util/setbaud.h>
UBRR0H = UBRRH_VALUE;
UBRR0L = UBRRL_VALUE;
#if USE_2X
UCSR0A |= (1 << U2X0);
#else
UCSR0A &= ~(1 << U2X0);
#endif
#endif
}
static void uart_38400(void) {
#ifndef SIMU
#undef BAUD // avoid compiler warning
#define BAUD 38400
#include <util/setbaud.h>
UBRR0H = UBRRH_VALUE;
UBRR0L = UBRRL_VALUE;
#if USE_2X
UCSR0A |= (1 << U2X0);
#else
UCSR0A &= ~(1 << U2X0);
#endif
#endif
}
static void uart_57600(void) {
#ifndef SIMU
#undef BAUD // avoid compiler warning
#define BAUD 57600
#include <util/setbaud.h>
UBRR0H = UBRRH_VALUE;
UBRR0L = UBRRL_VALUE;
#if USE_2X
UCSR0A |= (1 << U2X0);
#else
UCSR0A &= ~(1 << U2X0);
#endif
#endif
}
inline void SERIAL_EnableTXD(void) {
//UCSR0B |= (1 << TXEN0); // enable TX
UCSR0B |= (1 << TXEN0) | (1 << UDRIE0); // enable TX and TX interrupt
}
#if 0
void SERIAL_DisableTXD(void) {
UCSR0B &= ~(1 << TXEN0); // disable TX
UCSR0B &= ~(1 << UDRIE0); // disable Interrupt
}
#endif
void SERIAL_Init(void) {
DDRE &= ~(1 << DDE0); // set RXD0 pin as input
PORTE &= ~(1 << PORTE0); // disable pullup on RXD0 pin
uint8_t b = 1; // TODO g_eeGeneral.baudRate;
if (b == 0) {
uart_19200();
}
else if (b == 1) {
uart_38400();
}
else if (b == 2) {
uart_57600();
}
#if 0
uart_57600();
#endif
// 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
SERIAL_EnableTXD();
SERIAL_EnableRXD();
}

48
src/serial.h Normal file
View file

@ -0,0 +1,48 @@
/*
* Authors - Gerard Valade <gerard.valade@gmail.com>
*
* Adapted from frsky
*
* 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 _SERIAL_H_
#define _SERIAL_H_
typedef enum serial_tx_state_ {
TX_STATE_EMPTY = 0, //
TX_STATE_READY, //
TX_STATE_BUSY
} serial_tx_state_t;
extern serial_tx_state_t serialTxState;
typedef void (*SerialFuncP)(uint8_t event);
extern SerialFuncP RXHandler;
#define MAX_TX_BUFFER 32
//#if 0
extern uint8_t serialTxBuffer[MAX_TX_BUFFER]; // 32 characters
extern uint8_t serialTxBufferCount;
extern uint8_t * ptrTxISR;
//#endif
void SERIAL_Init(void);
//void SERIAL_transmitBuffer(uint8_t len);
extern void SERIAL_start_uart_send();
extern void SERIAL_end_uart_send();
extern void SERIAL_send_uart_bytes(uint8_t * buf, uint16_t len);
void SERIAL_startTX(void);
#define IS_TX_BUSY (serialTxState!=TX_STATE_EMPTY)
#endif