mirror of
https://github.com/opentx/opentx.git
synced 2025-07-26 01:35:21 +03:00
More AVR cleanup
This commit is contained in:
Arne Schwabe
parent
4300e4b022
commit
d831b0f55e
8 changed files with 2 additions and 1517 deletions
|
|
@ -48,8 +48,6 @@
|
|||
#define __NOINIT
|
||||
#endif
|
||||
|
||||
typedef int32_t int24_t;
|
||||
|
||||
#if __GNUC__
|
||||
#define PACK( __Declaration__ ) __Declaration__ __attribute__((__packed__))
|
||||
#else
|
||||
|
|
|
|||
|
|
@ -32,7 +32,7 @@ int16_t trims[NUM_TRIMS] = {0};
|
|||
int32_t chans[MAX_OUTPUT_CHANNELS] = {0};
|
||||
BeepANACenter bpanaCenter = 0;
|
||||
|
||||
int24_t act [MAX_MIXERS] = {0};
|
||||
int32_t act [MAX_MIXERS] = {0};
|
||||
SwOn swOn [MAX_MIXERS]; // TODO better name later...
|
||||
|
||||
uint8_t mixWarning;
|
||||
|
|
|
|||
|
|
@ -1053,7 +1053,7 @@ PACK(typedef struct {
|
|||
}) SwOn;
|
||||
|
||||
extern SwOn swOn[MAX_MIXERS];
|
||||
extern int24_t act[MAX_MIXERS];
|
||||
extern int32_t act[MAX_MIXERS];
|
||||
|
||||
#if defined(BOLD_FONT)
|
||||
inline bool isExpoActive(uint8_t expo)
|
||||
|
|
|
|||
|
|
@ -1,331 +0,0 @@
|
|||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
#if (defined(FRSKY_HUB) || defined(WS_HOW_HIGH))
|
||||
void checkMinMaxAltitude()
|
||||
{
|
||||
if (TELEMETRY_RELATIVE_BARO_ALT_BP > telemetryData.hub.maxAltitude)
|
||||
telemetryData.hub.maxAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP;
|
||||
if (TELEMETRY_RELATIVE_BARO_ALT_BP < telemetryData.hub.minAltitude)
|
||||
telemetryData.hub.minAltitude = TELEMETRY_RELATIVE_BARO_ALT_BP;
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(FRSKY_HUB)
|
||||
void parseTelemHubByte(uint8_t byte)
|
||||
{
|
||||
static int8_t structPos;
|
||||
static uint8_t lowByte;
|
||||
static TS_STATE state = TS_IDLE;
|
||||
|
||||
if (byte == 0x5e) {
|
||||
state = TS_DATA_ID;
|
||||
return;
|
||||
}
|
||||
if (state == TS_IDLE) {
|
||||
return;
|
||||
}
|
||||
if (state & TS_XOR) {
|
||||
byte = byte ^ 0x60;
|
||||
state = (TS_STATE)(state - TS_XOR);
|
||||
}
|
||||
else if (byte == 0x5d) {
|
||||
state = (TS_STATE)(state | TS_XOR);
|
||||
return;
|
||||
}
|
||||
if (state == TS_DATA_ID) {
|
||||
if (byte > 0x3f) {
|
||||
state = TS_IDLE;
|
||||
}
|
||||
else {
|
||||
structPos = byte * 2;
|
||||
state = TS_DATA_LOW;
|
||||
}
|
||||
return;
|
||||
}
|
||||
if (state == TS_DATA_LOW) {
|
||||
lowByte = byte;
|
||||
state = TS_DATA_HIGH;
|
||||
return;
|
||||
}
|
||||
|
||||
state = TS_IDLE;
|
||||
|
||||
#if defined(GPS)
|
||||
if ((uint8_t)structPos == offsetof(FrskyTelemetryData, gpsLatitude_bp)) {
|
||||
if (lowByte || byte)
|
||||
telemetryData.hub.gpsFix = 1;
|
||||
else if (telemetryData.hub.gpsFix > 0 && telemetryData.hub.gpsLatitude_bp > 1)
|
||||
telemetryData.hub.gpsFix = 0;
|
||||
}
|
||||
else if ((uint8_t)structPos == offsetof(FrskyTelemetryData, gpsLongitude_bp)) {
|
||||
if (lowByte || byte)
|
||||
telemetryData.hub.gpsFix = 1;
|
||||
else if (telemetryData.hub.gpsFix > 0 && telemetryData.hub.gpsLongitude_bp > 1)
|
||||
telemetryData.hub.gpsFix = 0;
|
||||
}
|
||||
|
||||
if ((uint8_t)structPos == offsetof(FrskyTelemetryData, gpsAltitude_bp) ||
|
||||
((uint8_t)structPos >= offsetof(FrskyTelemetryData, gpsAltitude_ap) && (uint8_t)structPos <= offsetof(FrskyTelemetryData, gpsLatitudeNS) && (uint8_t)structPos != offsetof(FrskyTelemetryData, baroAltitude_bp) && (uint8_t)structPos != offsetof(FrskyTelemetryData, baroAltitude_ap))) {
|
||||
// If we don't have a fix, we may discard the value
|
||||
if (telemetryData.hub.gpsFix <= 0)
|
||||
return;
|
||||
}
|
||||
#endif // #if defined(GPS)
|
||||
|
||||
((uint8_t*)&telemetryData.hub)[structPos] = lowByte;
|
||||
((uint8_t*)&telemetryData.hub)[structPos+1] = byte;
|
||||
|
||||
switch ((uint8_t)structPos) {
|
||||
|
||||
case offsetof(FrskyTelemetryData, rpm):
|
||||
telemetryData.hub.rpm *= (uint8_t)60/(g_model.frsky.blades+2);
|
||||
if (telemetryData.hub.rpm > telemetryData.hub.maxRpm)
|
||||
telemetryData.hub.maxRpm = telemetryData.hub.rpm;
|
||||
break;
|
||||
|
||||
case offsetof(FrskyTelemetryData, temperature1):
|
||||
if (telemetryData.hub.temperature1 > telemetryData.hub.maxTemperature1)
|
||||
telemetryData.hub.maxTemperature1 = telemetryData.hub.temperature1;
|
||||
break;
|
||||
|
||||
case offsetof(FrskyTelemetryData, temperature2):
|
||||
if (telemetryData.hub.temperature2 > telemetryData.hub.maxTemperature2)
|
||||
telemetryData.hub.maxTemperature2 = telemetryData.hub.temperature2;
|
||||
break;
|
||||
|
||||
case offsetof(FrskyTelemetryData, current):
|
||||
if ((int16_t)telemetryData.hub.current > 0 && ((int16_t)telemetryData.hub.current + g_model.frsky.fasOffset) > 0)
|
||||
telemetryData.hub.current += g_model.frsky.fasOffset;
|
||||
else
|
||||
telemetryData.hub.current = 0;
|
||||
if (telemetryData.hub.current > telemetryData.hub.maxCurrent)
|
||||
telemetryData.hub.maxCurrent = telemetryData.hub.current;
|
||||
break;
|
||||
|
||||
case offsetof(FrskyTelemetryData, currentConsumption):
|
||||
// we receive data from openXsensor. stops the calculation of consumption and power
|
||||
telemetryData.hub.openXsensor = 1;
|
||||
break;
|
||||
|
||||
case offsetof(FrskyTelemetryData, volts_ap):
|
||||
#if defined(FAS_PROTOTYPE)
|
||||
telemetryData.hub.vfas = (telemetryData.hub.volts_bp * 10 + telemetryData.hub.volts_ap);
|
||||
#else
|
||||
telemetryData.hub.vfas = ((telemetryData.hub.volts_bp * 10 + telemetryData.hub.volts_ap) * 21) / 11;
|
||||
#endif
|
||||
/* TODO later if (!telemetryData.hub.minVfas || telemetryData.hub.minVfas > telemetryData.hub.vfas)
|
||||
telemetryData.hub.minVfas = telemetryData.hub.vfas; */
|
||||
break;
|
||||
|
||||
case offsetof(FrskyTelemetryData, baroAltitude_bp):
|
||||
// First received barometer altitude => Altitude offset
|
||||
if (!telemetryData.hub.baroAltitudeOffset)
|
||||
telemetryData.hub.baroAltitudeOffset = -telemetryData.hub.baroAltitude_bp;
|
||||
telemetryData.hub.baroAltitude_bp += telemetryData.hub.baroAltitudeOffset;
|
||||
checkMinMaxAltitude();
|
||||
break;
|
||||
|
||||
#if defined(GPS)
|
||||
case offsetof(FrskyTelemetryData, gpsAltitude_ap):
|
||||
if (!telemetryData.hub.gpsAltitudeOffset)
|
||||
telemetryData.hub.gpsAltitudeOffset = -telemetryData.hub.gpsAltitude_bp;
|
||||
telemetryData.hub.gpsAltitude_bp += telemetryData.hub.gpsAltitudeOffset;
|
||||
if (!telemetryData.hub.baroAltitudeOffset) {
|
||||
if (telemetryData.hub.gpsAltitude_bp > telemetryData.hub.maxAltitude)
|
||||
telemetryData.hub.maxAltitude = telemetryData.hub.gpsAltitude_bp;
|
||||
if (telemetryData.hub.gpsAltitude_bp < telemetryData.hub.minAltitude)
|
||||
telemetryData.hub.minAltitude = telemetryData.hub.gpsAltitude_bp;
|
||||
}
|
||||
if (!telemetryData.hub.pilotLatitude && !telemetryData.hub.pilotLongitude) {
|
||||
// First received GPS position => Pilot GPS position
|
||||
getGpsPilotPosition();
|
||||
}
|
||||
else if (telemetryData.hub.gpsDistNeeded || menuHandlers[menuLevel] == menuViewTelemetryFrsky) {
|
||||
getGpsDistance();
|
||||
}
|
||||
break;
|
||||
|
||||
case offsetof(FrskyTelemetryData, gpsSpeed_bp):
|
||||
// Speed => Max speed
|
||||
if (telemetryData.hub.gpsSpeed_bp > telemetryData.hub.maxGpsSpeed)
|
||||
telemetryData.hub.maxGpsSpeed = telemetryData.hub.gpsSpeed_bp;
|
||||
break;
|
||||
#endif
|
||||
|
||||
case offsetof(FrskyTelemetryData, volts):
|
||||
frskyUpdateCells();
|
||||
break;
|
||||
|
||||
case offsetof(FrskyTelemetryData, accelX):
|
||||
case offsetof(FrskyTelemetryData, accelY):
|
||||
case offsetof(FrskyTelemetryData, accelZ):
|
||||
*(int16_t*)(&((uint8_t*)&telemetryData.hub)[structPos]) /= 10;
|
||||
break;
|
||||
|
||||
#if 0
|
||||
case offsetof(FrskyTelemetryData, gpsAltitude_bp):
|
||||
case offsetof(FrskyTelemetryData, fuelLevel):
|
||||
case offsetof(FrskyTelemetryData, gpsLongitude_bp):
|
||||
case offsetof(FrskyTelemetryData, gpsLatitude_bp):
|
||||
case offsetof(FrskyTelemetryData, gpsCourse_bp):
|
||||
case offsetof(FrskyTelemetryData, day):
|
||||
case offsetof(FrskyTelemetryData, year):
|
||||
case offsetof(FrskyTelemetryData, sec):
|
||||
case offsetof(FrskyTelemetryData, gpsSpeed_ap):
|
||||
case offsetof(FrskyTelemetryData, gpsLongitude_ap):
|
||||
case offsetof(FrskyTelemetryData, gpsLatitude_ap):
|
||||
case offsetof(FrskyTelemetryData, gpsCourse_ap):
|
||||
case offsetof(FrskyTelemetryData, gpsLongitudeEW):
|
||||
case offsetof(FrskyTelemetryData, gpsLatitudeNS):
|
||||
case offsetof(FrskyTelemetryData, varioSpeed):
|
||||
case offsetof(FrskyTelemetryData, power): /* because sent by openXsensor */
|
||||
case offsetof(FrskyTelemetryData, vfas):
|
||||
case offsetof(FrskyTelemetryData, volts_bp):
|
||||
break;
|
||||
|
||||
default:
|
||||
*((uint16_t *)(((uint8_t*)&telemetryData.hub) + structPos)) = previousValue;
|
||||
break;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
#endif // #if defined(FRSKY_HUB)
|
||||
|
||||
#if defined(WS_HOW_HIGH)
|
||||
void parseTelemWSHowHighByte(uint8_t byte)
|
||||
{
|
||||
if (wshhStreaming < (WSHH_TIMEOUT10ms - 10)) {
|
||||
((uint8_t*)&telemetryData.hub)[offsetof(FrskyTelemetryData, baroAltitude_bp)] = byte;
|
||||
checkMinMaxAltitude();
|
||||
}
|
||||
else {
|
||||
// At least 100mS passed since last data received
|
||||
((uint8_t*)&telemetryData.hub)[offsetof(FrskyTelemetryData, baroAltitude_bp)+1] = byte;
|
||||
}
|
||||
// baroAltitude_bp unit here is feet!
|
||||
wshhStreaming = WSHH_TIMEOUT10ms; // reset counter
|
||||
}
|
||||
#endif
|
||||
|
||||
void frskyDProcessPacket(const uint8_t *packet)
|
||||
{
|
||||
// What type of packet?
|
||||
switch (packet[0])
|
||||
{
|
||||
case LINKPKT: // A1/A2/RSSI values
|
||||
{
|
||||
telemetryData.analog[TELEM_ANA_A1].set(packet[1], g_model.frsky.channels[TELEM_ANA_A1].type);
|
||||
telemetryData.analog[TELEM_ANA_A2].set(packet[2], g_model.frsky.channels[TELEM_ANA_A2].type);
|
||||
telemetryData.rssi[0].set(packet[3]);
|
||||
telemetryData.rssi[1].set(packet[4] / 2);
|
||||
telemetryStreaming = TELEMETRY_TIMEOUT10ms; // reset counter only if valid packets are being detected
|
||||
link_counter += 256 / FRSKY_D_AVERAGING;
|
||||
#if defined(VARIO)
|
||||
uint8_t varioSource = g_model.frsky.varioSource - VARIO_SOURCE_A1;
|
||||
if (varioSource < 2) {
|
||||
telemetryData.hub.varioSpeed = applyChannelRatio(varioSource, telemetryData.analog[varioSource].value);
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
#if defined(FRSKY_HUB) || defined (WS_HOW_HIGH)
|
||||
case USRPKT: // User Data packet
|
||||
uint8_t numBytes = 3 + (packet[1] & 0x07); // sanitize in case of data corruption leading to buffer overflow
|
||||
for (uint8_t i=3; i<numBytes; i++) {
|
||||
#if defined(FRSKY_HUB)
|
||||
if (IS_USR_PROTO_FRSKY_HUB()) {
|
||||
parseTelemHubByte(packet[i]);
|
||||
}
|
||||
#endif
|
||||
#if defined(WS_HOW_HIGH)
|
||||
if (IS_USR_PROTO_WS_HOW_HIGH()) {
|
||||
parseTelemWSHowHighByte(packet[i]);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
#endif // #if defined(FRSKY_HUB) || defined (WS_HOW_HIGH)
|
||||
}
|
||||
}
|
||||
|
||||
// Alarms level sent to the FrSky module
|
||||
|
||||
uint8_t frskyAlarmsSendState = 0 ;
|
||||
|
||||
void frskyPushValue(uint8_t *&ptr, uint8_t value)
|
||||
{
|
||||
// byte stuff the only byte than might need it
|
||||
bool bytestuff = false;
|
||||
|
||||
if (value == START_STOP) {
|
||||
bytestuff = true;
|
||||
value = 0x5e;
|
||||
}
|
||||
else if (value == BYTESTUFF) {
|
||||
bytestuff = true;
|
||||
value = 0x5d;
|
||||
}
|
||||
|
||||
*ptr++ = value;
|
||||
if (bytestuff)
|
||||
*ptr = BYTESTUFF;
|
||||
}
|
||||
|
||||
void frskySendPacket(uint8_t type, uint8_t value, uint8_t p1, uint8_t p2)
|
||||
{
|
||||
uint8_t *ptr = &frskyTxBuffer[0];
|
||||
|
||||
*ptr++ = START_STOP; // End of packet
|
||||
*ptr++ = 0x00;
|
||||
*ptr++ = 0x00;
|
||||
*ptr++ = 0x00;
|
||||
*ptr++ = 0x00;
|
||||
*ptr++ = 0x00;
|
||||
*ptr++ = (IS_SOUND_OFF() ? alarm_off : p2);
|
||||
*ptr++ = p1;
|
||||
frskyPushValue(ptr, value);
|
||||
*ptr++ = type;
|
||||
*ptr++ = START_STOP; // Start of packet
|
||||
|
||||
frskyTxBufferCount = ptr - &frskyTxBuffer[0];
|
||||
#if !defined(SIMU)
|
||||
telemetryTransmitBuffer();
|
||||
#endif
|
||||
}
|
||||
|
||||
void frskyDSendNextAlarm(void)
|
||||
{
|
||||
if (frskyTxBufferCount)
|
||||
return; // we only have one buffer. If it's in use, then we can't send yet.
|
||||
|
||||
// Now send a packet
|
||||
frskyAlarmsSendState -= 1;
|
||||
uint8_t alarm = 1 - (frskyAlarmsSendState % 2);
|
||||
if (frskyAlarmsSendState < SEND_MODEL_ALARMS) {
|
||||
uint8_t channel = 1 - (frskyAlarmsSendState / 2);
|
||||
frskySendPacket(A22PKT + frskyAlarmsSendState, g_model.frsky.channels[channel].alarms_value[alarm], ALARM_GREATER(channel, alarm), ALARM_LEVEL(channel, alarm));
|
||||
}
|
||||
else {
|
||||
frskySendPacket(RSSI1PKT-alarm, getRssiAlarmValue(alarm), 0, (2+alarm+g_model.frsky.rssiAlarms[alarm].level) % 4);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,207 +0,0 @@
|
|||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include "opentx.h"
|
||||
|
||||
uint8_t jetiRxBuffer[32];
|
||||
uint8_t jetiReady;
|
||||
uint16_t jetiKeys;
|
||||
|
||||
void telemetryInterrupt10ms()
|
||||
{
|
||||
if (jetiReady) {
|
||||
jetiKeys = JETI_KEY_NOCHANGE;
|
||||
if (switchState(KEY_UP)) jetiKeys &= JETI_KEY_UP;
|
||||
if (switchState(KEY_DOWN)) jetiKeys &= JETI_KEY_DOWN;
|
||||
if (switchState(KEY_LEFT)) jetiKeys &= JETI_KEY_LEFT;
|
||||
if (switchState(KEY_RIGHT)) jetiKeys &= JETI_KEY_RIGHT;
|
||||
|
||||
jetiReady = 0; // invalidate buffer
|
||||
|
||||
JETI_EnableTXD();
|
||||
}
|
||||
}
|
||||
|
||||
#ifndef SIMU
|
||||
ISR(USART0_UDRE_vect)
|
||||
{
|
||||
if (jetiKeys != 0xff) {
|
||||
UCSR0B &= ~(1 << TXB80);
|
||||
|
||||
if (jetiKeys & 0x0100)
|
||||
UCSR0B |= (1 << TXB80);
|
||||
|
||||
UDR0 = jetiKeys;
|
||||
|
||||
jetiKeys = 0xff;
|
||||
}
|
||||
else {
|
||||
JETI_DisableTXD();
|
||||
}
|
||||
}
|
||||
|
||||
ISR (USART0_RX_vect)
|
||||
{
|
||||
uint8_t stat;
|
||||
uint8_t rh;
|
||||
uint8_t rl;
|
||||
static uint8_t jbp;
|
||||
|
||||
stat = UCSR0A;
|
||||
rh = UCSR0B;
|
||||
rl = UDR0;
|
||||
|
||||
if (stat & ((1 << FE0) | (1 << DOR0) | (1 << UPE0))) {
|
||||
// discard buffer and start new on any error
|
||||
jetiReady = 0;
|
||||
jbp = 0;
|
||||
}
|
||||
else if ((rh & (1 << RXB80)) == 0) {
|
||||
// control
|
||||
if (rl == 0xfe) {
|
||||
// start condition
|
||||
jetiReady = 0;
|
||||
jbp = 0;
|
||||
}
|
||||
else if (rl == 0xff) {
|
||||
// stop condition
|
||||
jetiReady = 1;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// data
|
||||
if (jbp < 32) {
|
||||
if (rl==0xDF)
|
||||
jetiRxBuffer[jbp++] = '@'; //@ => ° Issue 163
|
||||
else
|
||||
jetiRxBuffer[jbp++] = rl;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void JETI_Init (void)
|
||||
{
|
||||
DDRE &= ~(1 << DDE0); // set RXD0 pin as input
|
||||
PORTE &= ~(1 << PORTE0); // disable pullup on RXD0 pin
|
||||
|
||||
#ifndef SIMU
|
||||
|
||||
#undef BAUD
|
||||
#define BAUD 9600
|
||||
#include <util/setbaud.h>
|
||||
UBRR0H = UBRRH_VALUE;
|
||||
UBRR0L = UBRRL_VALUE;
|
||||
|
||||
UCSR0A &= ~(1 << U2X0); // disable double speed operation
|
||||
|
||||
// set 9O1
|
||||
UCSR0C = (1 << UPM01) | (1 << UPM00) | (1 << UCSZ01) | (1 << UCSZ00);
|
||||
UCSR0B = (1 << UCSZ02);
|
||||
|
||||
// flush receive buffer
|
||||
while ( UCSR0A & (1 << RXC0) ) UDR0;
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void JETI_EnableTXD (void)
|
||||
{
|
||||
UCSR0B |= (1 << TXEN0); // enable TX
|
||||
UCSR0B |= (1 << UDRIE0); // enable UDRE0 interrupt
|
||||
}
|
||||
|
||||
void JETI_DisableTXD (void)
|
||||
{
|
||||
UCSR0B &= ~(1 << TXEN0); // disable TX
|
||||
UCSR0B &= ~(1 << UDRIE0); // disable UDRE0 interrupt
|
||||
}
|
||||
|
||||
void JETI_EnableRXD (void)
|
||||
{
|
||||
UCSR0B |= (1 << RXEN0); // enable RX
|
||||
UCSR0B |= (1 << RXCIE0); // enable Interrupt
|
||||
}
|
||||
|
||||
void JETI_DisableRXD (void)
|
||||
{
|
||||
UCSR0B &= ~(1 << RXEN0); // disable RX
|
||||
UCSR0B &= ~(1 << RXCIE0); // disable Interrupt
|
||||
}
|
||||
|
||||
#if 0
|
||||
void JETI_putw (uint16_t c)
|
||||
{
|
||||
loop_until_bit_is_set(UCSR0A, UDRE0);
|
||||
UCSR0B &= ~(1 << TXB80);
|
||||
if (c & 0x0100)
|
||||
{
|
||||
UCSR0B |= (1 << TXB80);
|
||||
}
|
||||
UDR0 = c;
|
||||
}
|
||||
|
||||
void JETI_putc (uint8_t c)
|
||||
{
|
||||
loop_until_bit_is_set(UCSR0A, UDRE0);
|
||||
// UCSRB &= ~(1 << TXB8);
|
||||
UCSR0B |= (1 << TXB80);
|
||||
UDR0 = c;
|
||||
}
|
||||
|
||||
void JETI_puts (char *s)
|
||||
{
|
||||
while (*s)
|
||||
{
|
||||
JETI_putc (*s);
|
||||
s++;
|
||||
}
|
||||
}
|
||||
|
||||
void JETI_put_start (void)
|
||||
{
|
||||
loop_until_bit_is_set(UCSR0A, UDRE0);
|
||||
UCSR0B &= ~(1 << TXB80);
|
||||
UDR0 = 0xFE;
|
||||
}
|
||||
|
||||
void JETI_put_stop (void)
|
||||
{
|
||||
loop_until_bit_is_set(UCSR0A, UDRE0);
|
||||
UCSR0B &= ~(1 << TXB80);
|
||||
UDR0 = 0xFF;
|
||||
}
|
||||
#endif
|
||||
|
||||
void menuViewTelemetryJeti(event_t event)
|
||||
{
|
||||
drawTelemetryTopBar();
|
||||
|
||||
for (uint8_t i=0; i<16; i++) {
|
||||
lcdDrawChar((i+2)*FW, 3*FH, jetiRxBuffer[i], BSS);
|
||||
lcdDrawChar((i+2)*FW, 4*FH, jetiRxBuffer[i+16], BSS);
|
||||
}
|
||||
|
||||
if (event == EVT_KEY_FIRST(KEY_EXIT)) {
|
||||
JETI_DisableRXD();
|
||||
jetiReady = 0;
|
||||
chainMenu(menuMainView);
|
||||
}
|
||||
}
|
||||
|
|
@ -1,40 +0,0 @@
|
|||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#ifndef _JETI_H_
|
||||
#define _JETI_H_
|
||||
|
||||
#define JETI_KEY_LEFT 0x70
|
||||
#define JETI_KEY_RIGHT 0xe0
|
||||
#define JETI_KEY_UP 0xd0
|
||||
#define JETI_KEY_DOWN 0xb0
|
||||
#define JETI_KEY_NOCHANGE 0xf0
|
||||
|
||||
void JETI_Init(void);
|
||||
void JETI_EnableRXD (void);
|
||||
void JETI_DisableRXD (void);
|
||||
void JETI_EnableTXD (void);
|
||||
void JETI_DisableTXD (void);
|
||||
|
||||
void telemetryInterrupt10ms();
|
||||
void menuViewTelemetryJeti(event_t event);
|
||||
|
||||
#endif // _JETI_H_
|
||||
|
||||
|
|
@ -1,600 +0,0 @@
|
|||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
/*! \file mavlink.cpp
|
||||
* Mavlink telemetry decoder for Arducopter/Arduplane telemetry information.
|
||||
*/
|
||||
|
||||
|
||||
#include "telemetry/mavlink.h"
|
||||
|
||||
// this might need to move to the flight software
|
||||
//static
|
||||
mavlink_system_t mavlink_system = { 7, MAV_COMP_ID_MISSIONPLANNER };
|
||||
|
||||
// Mavlink message decoded Status Text
|
||||
#define PARAM_NB_REPEAT 10
|
||||
char mav_statustext[LEN_STATUSTEXT];
|
||||
int8_t mav_heartbeat = 0;
|
||||
int8_t mav_heartbeat_recv = 0;
|
||||
uint8_t data_stream_start_stop = 0;
|
||||
|
||||
|
||||
// Telemetry data hold
|
||||
Telemetry_Data_t telemetry_data;
|
||||
|
||||
// *****************************************************
|
||||
static void MAVLINK_parse_char(uint8_t c);
|
||||
uint8_t telemetryRxBufferCount = 0;
|
||||
|
||||
void processTelemetryData(uint8_t byte) {
|
||||
MAVLINK_parse_char(byte);
|
||||
}
|
||||
|
||||
/*! \brief Reset basic Mavlink varables
|
||||
* \todo Figure out exact function
|
||||
*
|
||||
*/
|
||||
void MAVLINK_reset(uint8_t warm_reset) {
|
||||
if (warm_reset && telemetry_data.status) {
|
||||
mav_statustext[0] = 0;
|
||||
}
|
||||
|
||||
mavlink_status_t* p_status = mavlink_get_channel_status(MAVLINK_COMM_0);
|
||||
p_status->current_rx_seq = 0;
|
||||
p_status->current_tx_seq = 0;
|
||||
memset(&telemetry_data, 0, sizeof(telemetry_data));
|
||||
telemetry_data.rcv_control_mode = ERROR_NUM_MODES;
|
||||
telemetry_data.req_mode = ERROR_NUM_MODES;
|
||||
|
||||
telemetry_data.type = MAV_TYPE_ENUM_END;
|
||||
telemetry_data.autopilot = MAV_AUTOPILOT_ENUM_END;
|
||||
telemetry_data.type_autopilot = MAVLINK_INVALID_TYPE;
|
||||
|
||||
mav_heartbeat = 0;
|
||||
mav_heartbeat_recv = 0;
|
||||
data_stream_start_stop = 0;
|
||||
}
|
||||
|
||||
//! \brief initalize mavlink extension
|
||||
void MAVLINK_Init(void)
|
||||
{
|
||||
mav_statustext[0] = 0;
|
||||
MAVLINK_reset(0);
|
||||
telemetryPortInit(g_eeGeneral.mavbaud);
|
||||
}
|
||||
|
||||
/*! \brief Status log message
|
||||
* \details Processes the mavlink status messages. This message contains a
|
||||
* severity and a message. The severity is an enum difined by MAV_SEVERITY also
|
||||
* see RFC-5424 for the severity levels.
|
||||
* The original message is maximum 50 characters and is without termination
|
||||
* character. For readablity on the 9x the only the first 15 (LEN_STATUSTEXT)
|
||||
* characters are used. To get the full message you can use the commented
|
||||
* funtion below.
|
||||
*/
|
||||
|
||||
static inline void REC_MAVLINK_MSG_ID_STATUSTEXT(const mavlink_message_t* msg) {
|
||||
_MAV_RETURN_char_array(msg, mav_statustext, LEN_STATUSTEXT, 1);
|
||||
}
|
||||
|
||||
/*! \brief System status including cpu load, battery status and communication status.
|
||||
* \details From this message we use use only the batery infomation. The rest
|
||||
* is not realy of use while flying. The complete message can be decoded in to
|
||||
* a struct with the first two commented lines.
|
||||
* The batery votage is in mV. We devide by 100 to display tenths of volts.'
|
||||
* \todo Add battery remaining variable in telemetry_data struct for estimated
|
||||
* remaining battery. Decoding funtion allready in place.
|
||||
*/
|
||||
|
||||
static inline void REC_MAVLINK_MSG_ID_SYS_STATUS(const mavlink_message_t* msg) {
|
||||
telemetry_data.vbat = mavlink_msg_sys_status_get_voltage_battery(msg) / 100; // Voltage * 10
|
||||
telemetry_data.ibat = mavlink_msg_sys_status_get_current_battery(msg) / 10;
|
||||
telemetry_data.rem_bat = mavlink_msg_sys_status_get_battery_remaining(msg);
|
||||
|
||||
#ifdef MAVLINK_PARAMS
|
||||
telemetry_data.vbat_low = (getMavlinParamsValue(BATT_MONITOR) > 0)
|
||||
&& (((float) telemetry_data.vbat / 10.0) < getMavlinParamsValue(LOW_VOLT)) && (telemetry_data.vbat > 50);
|
||||
#else
|
||||
telemetry_data.vbat_low = (telemetry_data.rem_bat < 10);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*! \brief Receive rc channels
|
||||
*
|
||||
*/
|
||||
static inline void REC_MAVLINK_MSG_ID_RC_CHANNELS(const mavlink_message_t* msg) {
|
||||
uint8_t temp_scale = 5 + g_model.mavlink.rc_rssi_scale;
|
||||
telemetry_data.rc_rssi = mavlink_msg_rc_channels_get_rssi(msg) * 20 / temp_scale;
|
||||
}
|
||||
|
||||
/*! \brief Receive raw rc channels
|
||||
*
|
||||
*/
|
||||
static inline void REC_MAVLINK_MSG_ID_RC_CHANNELS_RAW(const mavlink_message_t* msg) {
|
||||
uint8_t temp_rssi =(mavlink_msg_rc_channels_raw_get_rssi(msg) * 100) / 255;
|
||||
uint8_t temp_scale = 25 + g_model.mavlink.rc_rssi_scale * 5;
|
||||
telemetry_data.rc_rssi = (temp_rssi * 100) / temp_scale;
|
||||
}
|
||||
|
||||
/*! \brief Arducopter specific radio message
|
||||
*
|
||||
*/
|
||||
static inline void REC_MAVLINK_MSG_ID_RADIO(const mavlink_message_t* msg) {
|
||||
if (msg->sysid != 51)
|
||||
return;
|
||||
telemetry_data.pc_rssi = (mavlink_msg_radio_get_rssi(msg) * 100) / 255;
|
||||
telemetry_data.packet_drop = mavlink_msg_radio_get_rxerrors(msg);
|
||||
telemetry_data.packet_fixed = mavlink_msg_radio_get_fixed(msg);
|
||||
telemetry_data.radio_sysid = msg->sysid;
|
||||
telemetry_data.radio_compid = msg->compid;
|
||||
}
|
||||
static inline void REC_MAVLINK_MSG_ID_RADIO_STATUS(const mavlink_message_t* msg) {
|
||||
REC_MAVLINK_MSG_ID_RADIO(msg);
|
||||
}
|
||||
|
||||
//! \brief Navigaion output message
|
||||
static inline void REC_MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT(const mavlink_message_t* msg) {
|
||||
telemetry_data.bearing = mavlink_msg_nav_controller_output_get_target_bearing(msg);
|
||||
}
|
||||
|
||||
//! \brief Hud navigation message
|
||||
static inline void REC_MAVLINK_MSG_ID_VFR_HUD(const mavlink_message_t* msg) {
|
||||
telemetry_data.heading = mavlink_msg_vfr_hud_get_heading(msg);
|
||||
telemetry_data.loc_current.rel_alt = mavlink_msg_vfr_hud_get_alt(msg);
|
||||
}
|
||||
|
||||
/*! \brief Heartbeat message
|
||||
* \details Heartbeat message is used for the following information:
|
||||
* type and autopilot is used to determine if the UAV is an ArduPlane or Arducopter
|
||||
*/
|
||||
static inline void REC_MAVLINK_MSG_ID_HEARTBEAT(const mavlink_message_t* msg) {
|
||||
telemetry_data.mode = mavlink_msg_heartbeat_get_base_mode(msg);
|
||||
telemetry_data.custom_mode = mavlink_msg_heartbeat_get_custom_mode(msg);
|
||||
telemetry_data.status = mavlink_msg_heartbeat_get_system_status(msg);
|
||||
telemetry_data.mav_sysid = msg->sysid;
|
||||
telemetry_data.mav_compid = msg->compid;
|
||||
uint8_t type = mavlink_msg_heartbeat_get_type(msg);
|
||||
uint8_t autopilot = mavlink_msg_heartbeat_get_autopilot(msg);
|
||||
if (type != telemetry_data.type || autopilot != telemetry_data.autopilot) {
|
||||
telemetry_data.type = mavlink_msg_heartbeat_get_type(msg);
|
||||
telemetry_data.autopilot = mavlink_msg_heartbeat_get_autopilot(msg);
|
||||
if (autopilot == MAV_AUTOPILOT_ARDUPILOTMEGA) {
|
||||
if (type == MAV_TYPE_QUADROTOR ||
|
||||
type == MAV_TYPE_COAXIAL ||
|
||||
type == MAV_TYPE_HELICOPTER ||
|
||||
type == MAV_TYPE_HEXAROTOR ||
|
||||
type == MAV_TYPE_OCTOROTOR ||
|
||||
type == MAV_TYPE_TRICOPTER) {
|
||||
telemetry_data.type_autopilot = MAVLINK_ARDUCOPTER;
|
||||
}
|
||||
else if (type == MAV_TYPE_FIXED_WING) {
|
||||
telemetry_data.type_autopilot = MAVLINK_ARDUPLANE;
|
||||
}
|
||||
else {
|
||||
telemetry_data.type_autopilot = MAVLINK_INVALID_TYPE;
|
||||
}
|
||||
}
|
||||
else {
|
||||
telemetry_data.type_autopilot = MAVLINK_INVALID_TYPE;
|
||||
}
|
||||
}
|
||||
telemetry_data.active = (telemetry_data.mode & MAV_MODE_FLAG_SAFETY_ARMED) ? true : false;
|
||||
mav_heartbeat = 3; // 450ms display '*'
|
||||
mav_heartbeat_recv = 1;
|
||||
}
|
||||
|
||||
static inline void REC_MAVLINK_MSG_ID_HIL_CONTROLS(const mavlink_message_t* msg) {
|
||||
telemetry_data.nav_mode = mavlink_msg_hil_controls_get_mode(msg);
|
||||
}
|
||||
|
||||
/*! \brief Process GPS raw intger message
|
||||
* \details This message contains the following data:
|
||||
* - fix type: 0-1: no fix, 2: 2D fix, 3: 3D fix.
|
||||
* - Latitude, longitude in 1E7 * degrees
|
||||
* - Altutude 1E3 * meters above MSL.
|
||||
* - Course over ground in 100 * degrees
|
||||
* - GPS HDOP horizontal dilution of precision in cm (m*100).
|
||||
* - Ground speed in m/s * 100
|
||||
*/
|
||||
static inline void REC_MAVLINK_MSG_ID_GPS_RAW_INT(const mavlink_message_t* msg) {
|
||||
telemetry_data.fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg);
|
||||
telemetry_data.loc_current.lat = mavlink_msg_gps_raw_int_get_lat(msg) / 1E7;
|
||||
telemetry_data.loc_current.lon = mavlink_msg_gps_raw_int_get_lon(msg) / 1E7;
|
||||
telemetry_data.loc_current.gps_alt = mavlink_msg_gps_raw_int_get_alt(msg) / 1E3;
|
||||
telemetry_data.eph = mavlink_msg_gps_raw_int_get_eph(msg) / 100.0;
|
||||
telemetry_data.course = mavlink_msg_gps_raw_int_get_cog(msg) / 100.0;
|
||||
telemetry_data.v = mavlink_msg_gps_raw_int_get_vel(msg) / 100.0 ;
|
||||
telemetry_data.satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(msg);
|
||||
}
|
||||
|
||||
#ifdef MAVLINK_PARAMS
|
||||
const pm_char *getParamId(uint8_t idx) {
|
||||
const pm_char *mav_params_id [((NB_PID_PARAMS / 2) + 4)] = {
|
||||
PSTR("RATE_YAW"), // Rate Yaw
|
||||
PSTR("STB_YAW"), // Stabilize Yaw
|
||||
PSTR("RATE_PIT"), // Rate Pitch
|
||||
PSTR("STB_PIT"), // Stabilize Pitch
|
||||
PSTR("RATE_RLL"), // Rate Roll
|
||||
PSTR("STB_RLL"), // Stabilize Roll
|
||||
PSTR("THR_ALT"), // PSTR("THR_BAR"), // Altitude Hold
|
||||
PSTR("HLD_LON"), // Loiter
|
||||
PSTR("HLD_LAT"), // Loiter
|
||||
PSTR("NAV_LON"), // Nav WP
|
||||
PSTR("NAV_LAT"), // Nav WP
|
||||
PSTR("LOW_VOLT"), // Battery low voltage
|
||||
PSTR("VOLT_DIVIDER"), //
|
||||
PSTR("BATT_MONITOR"), //
|
||||
PSTR("BATT_CAPACITY")
|
||||
};
|
||||
uint8_t i;
|
||||
if (idx < NB_PID_PARAMS) {
|
||||
i = idx / 2;
|
||||
} else {
|
||||
i = idx - (NB_PID_PARAMS / 2);
|
||||
}
|
||||
return mav_params_id[i];
|
||||
}
|
||||
|
||||
void setMavlinParamsValue(uint8_t idx, float val) {
|
||||
MavlinkParam_t *param = getParam(idx);
|
||||
if (idx < NB_PARAMS && val != param->value) {
|
||||
param->value = val;
|
||||
param->repeat = PARAM_NB_REPEAT;
|
||||
uint8_t link_idx = NB_PID_PARAMS;
|
||||
switch (idx) {
|
||||
case RATE_PIT_P:
|
||||
case RATE_PIT_I:
|
||||
case STB_PIT_P:
|
||||
case STB_PIT_I:
|
||||
link_idx = idx + 4;
|
||||
break;
|
||||
case RATE_RLL_P:
|
||||
case RATE_RLL_I:
|
||||
case STB_RLL_P:
|
||||
case STB_RLL_I:
|
||||
link_idx = idx - 4;
|
||||
break;
|
||||
case HLD_LON_P:
|
||||
case HLD_LON_I:
|
||||
case NAV_LON_P:
|
||||
case NAV_LON_I:
|
||||
link_idx = idx + 2;
|
||||
break;
|
||||
case HLD_LAT_P:
|
||||
case HLD_LAT_I:
|
||||
case NAV_LAT_P:
|
||||
case NAV_LAT_I:
|
||||
link_idx = idx - 2;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if (link_idx < NB_PID_PARAMS) {
|
||||
MavlinkParam_t *p = getParam(link_idx);
|
||||
p->value = val;
|
||||
p->repeat = PARAM_NB_REPEAT;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void putsMavlinParams(uint8_t x, uint8_t y, uint8_t idx, uint8_t att) {
|
||||
if (idx < NB_PARAMS) {
|
||||
const pm_char * s = getParamId(idx);
|
||||
char c;
|
||||
while ((c = pgm_read_byte(s++))) {
|
||||
lcdDrawChar(x, y, (c == '_' ? ' ' : c), 0);
|
||||
x += FW;
|
||||
}
|
||||
if (idx < NB_PID_PARAMS) {
|
||||
x = 11 * FW;
|
||||
lcdDrawChar(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, ¶m_value);
|
||||
char *id = param_value.param_id;
|
||||
setParamValue((int8_t*)id, param_value.param_value);
|
||||
data_stream_start_stop = 0; // stop data stream while getting params list
|
||||
watch_mav_req_params_list = mav_req_params_nb_recv < (NB_PARAMS - 5) ? 20 : 0; // stop timeout
|
||||
}
|
||||
#endif
|
||||
|
||||
static inline void handleMessage(mavlink_message_t* p_rxmsg) {
|
||||
switch (p_rxmsg->msgid) {
|
||||
case MAVLINK_MSG_ID_HEARTBEAT:
|
||||
REC_MAVLINK_MSG_ID_HEARTBEAT(p_rxmsg);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_STATUSTEXT:
|
||||
REC_MAVLINK_MSG_ID_STATUSTEXT(p_rxmsg);
|
||||
AUDIO_WARNING1();
|
||||
break;
|
||||
case MAVLINK_MSG_ID_SYS_STATUS:
|
||||
REC_MAVLINK_MSG_ID_SYS_STATUS(p_rxmsg);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS:
|
||||
REC_MAVLINK_MSG_ID_RC_CHANNELS(p_rxmsg);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
|
||||
REC_MAVLINK_MSG_ID_RC_CHANNELS_RAW(p_rxmsg);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_RADIO:
|
||||
REC_MAVLINK_MSG_ID_RADIO(p_rxmsg);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_RADIO_STATUS:
|
||||
REC_MAVLINK_MSG_ID_RADIO_STATUS(p_rxmsg);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT:
|
||||
REC_MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT(p_rxmsg);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_VFR_HUD:
|
||||
REC_MAVLINK_MSG_ID_VFR_HUD(p_rxmsg);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_HIL_CONTROLS:
|
||||
REC_MAVLINK_MSG_ID_HIL_CONTROLS(p_rxmsg);
|
||||
break;
|
||||
case MAVLINK_MSG_ID_GPS_RAW_INT:
|
||||
REC_MAVLINK_MSG_ID_GPS_RAW_INT(p_rxmsg);
|
||||
break;
|
||||
#ifdef MAVLINK_PARAMS
|
||||
case MAVLINK_MSG_ID_PARAM_VALUE:
|
||||
REC_MAVLINK_MSG_ID_PARAM_VALUE(p_rxmsg);
|
||||
break;
|
||||
#endif
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*! \brief Mavlink message parser
|
||||
* \details Parses the characters in to a mavlink message.
|
||||
* Case statement parses each character as it is recieved.
|
||||
* \attention One big change form the 0.9 to 1.0 version is the
|
||||
* MAVLINK_CRC_EXTRA. This requires the mavlink_message_crcs array of 256 bytes.
|
||||
* \todo create dot for the statemachine
|
||||
*/
|
||||
static void MAVLINK_parse_char(uint8_t c) {
|
||||
|
||||
static mavlink_message_t m_mavlink_message;
|
||||
//! The currently decoded message
|
||||
static mavlink_message_t* p_rxmsg = &m_mavlink_message;
|
||||
//! The current decode status
|
||||
mavlink_status_t* p_status = mavlink_get_channel_status(MAVLINK_COMM_0);
|
||||
|
||||
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
static const uint8_t mavlink_message_crcs[256] PROGMEM = MAVLINK_MESSAGE_CRCS;
|
||||
#endif
|
||||
|
||||
// Initializes only once, values keep unchanged after first initialization
|
||||
//mavlink_parse_state_initialize(p_status);
|
||||
|
||||
//p_status->msg_received = 0;
|
||||
|
||||
switch (p_status->parse_state) {
|
||||
case MAVLINK_PARSE_STATE_UNINIT:
|
||||
case MAVLINK_PARSE_STATE_IDLE:
|
||||
if (c == MAVLINK_STX) {
|
||||
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
|
||||
mavlink_start_checksum(p_rxmsg);
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_STX:
|
||||
// NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2
|
||||
p_rxmsg->len = c;
|
||||
p_status->packet_idx = 0;
|
||||
mavlink_update_checksum(p_rxmsg, c);
|
||||
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_LENGTH:
|
||||
p_rxmsg->seq = c;
|
||||
mavlink_update_checksum(p_rxmsg, c);
|
||||
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ;
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_SEQ:
|
||||
p_rxmsg->sysid = c;
|
||||
mavlink_update_checksum(p_rxmsg, c);
|
||||
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID;
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_SYSID:
|
||||
p_rxmsg->compid = c;
|
||||
mavlink_update_checksum(p_rxmsg, c);
|
||||
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_COMPID:
|
||||
p_rxmsg->msgid = c;
|
||||
mavlink_update_checksum(p_rxmsg, c);
|
||||
if (p_rxmsg->len == 0) {
|
||||
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
|
||||
} else {
|
||||
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID:
|
||||
_MAV_PAYLOAD_NON_CONST(p_rxmsg)[p_status->packet_idx++] = (char) c;
|
||||
mavlink_update_checksum(p_rxmsg, c);
|
||||
if (p_status->packet_idx == p_rxmsg->len) {
|
||||
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
|
||||
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
mavlink_update_checksum(p_rxmsg, pgm_read_byte(&(mavlink_message_crcs[p_rxmsg->msgid])));
|
||||
#endif
|
||||
if (c != (p_rxmsg->checksum & 0xFF)) {
|
||||
// Check first checksum byte
|
||||
p_status->parse_error = 3;
|
||||
} else {
|
||||
p_status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_CRC1:
|
||||
if (c != (p_rxmsg->checksum >> 8)) {
|
||||
// Check second checksum byte
|
||||
p_status->parse_error = 4;
|
||||
} else {
|
||||
// Successfully got message
|
||||
if (mav_heartbeat < 0)
|
||||
mav_heartbeat = 0;
|
||||
p_status->current_rx_seq = p_rxmsg->seq;
|
||||
p_status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
#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 = getParam(idx)->value;
|
||||
|
||||
mavlink_channel_t chan = MAVLINK_COMM_0;
|
||||
uint8_t param_type = (uint8_t)MAV_PARAM_TYPE_REAL32;
|
||||
const char* param_id = (const char*)buf;
|
||||
mavlink_msg_param_set_send(chan, mavlink_system.sysid, mavlink_system.compid, param_id, param_value, param_type);
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
__attribute__((unused))
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
__attribute__((unused))
|
||||
//! \brief old mode switch funtion
|
||||
static inline void MAVLINK_msg_set_mode_send(uint8_t mode) {
|
||||
mavlink_channel_t chan = MAVLINK_COMM_0;
|
||||
mavlink_msg_set_mode_send(chan, mavlink_system.sysid, mode, 0);
|
||||
}
|
||||
|
||||
|
||||
/*! \brief Telemetry monitoring, calls \link MAVLINK10mspoll.
|
||||
* \todo Reimplemnt \link MAVLINK10mspoll
|
||||
*
|
||||
*/
|
||||
void telemetryWakeup() {
|
||||
uint16_t tmr10ms = get_tmr10ms();
|
||||
static uint16_t last_time = 0;
|
||||
if (tmr10ms - last_time > 15) {
|
||||
if (mav_heartbeat > -30) {
|
||||
mav_heartbeat--;
|
||||
|
||||
if (mav_heartbeat == -30) {
|
||||
MAVLINK_reset(1);
|
||||
telemetryPortInit(g_eeGeneral.mavbaud);
|
||||
}
|
||||
}
|
||||
last_time = tmr10ms;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1,335 +0,0 @@
|
|||
/*
|
||||
* Copyright (C) OpenTX
|
||||
*
|
||||
* Based on code named
|
||||
* th9x - http://code.google.com/p/th9x
|
||||
* er9x - http://code.google.com/p/er9x
|
||||
* gruvin9x - http://code.google.com/p/gruvin9x
|
||||
*
|
||||
* License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2 as
|
||||
* published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
/*! \file mavlink.h
|
||||
* Mavlink include file
|
||||
*/
|
||||
|
||||
#ifndef _MAVLINK_H_
|
||||
#define _MAVLINK_H_
|
||||
|
||||
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
#define MAVLINK_COMM_NUM_BUFFERS 1
|
||||
|
||||
#include "GCS_MAVLink/include_v1.0/mavlink_types.h"
|
||||
#include "opentx.h"
|
||||
|
||||
extern int8_t mav_heartbeat;
|
||||
extern mavlink_system_t mavlink_system;
|
||||
#define LEN_STATUSTEXT 20
|
||||
extern char mav_statustext[LEN_STATUSTEXT];
|
||||
|
||||
extern void SERIAL_start_uart_send();
|
||||
extern void SERIAL_end_uart_send();
|
||||
extern void SERIAL_send_uart_bytes(const uint8_t * buf, uint16_t len);
|
||||
|
||||
# define MAV_SYSTEM_ID 1
|
||||
//mavlink_system.type = 2; //MAV_QUADROTOR;
|
||||
|
||||
|
||||
#define MAVLINK_START_UART_SEND(chan,len) SERIAL_start_uart_send()
|
||||
#define MAVLINK_END_UART_SEND(chan,len) SERIAL_end_uart_send()
|
||||
#define MAVLINK_SEND_UART_BYTES(chan,buf,len) SERIAL_send_uart_bytes(buf,len)
|
||||
|
||||
#if __clang__
|
||||
// clang does not like packed member access at all. Since mavlink is a 3rd party library, ignore the errors
|
||||
#pragma clang diagnostic push
|
||||
#pragma clang diagnostic ignored "-Waddress-of-packed-member"
|
||||
#endif
|
||||
#include "GCS_MAVLink/include_v1.0/ardupilotmega/mavlink.h"
|
||||
#if __clang__
|
||||
// Restore warnings about packed member access
|
||||
#pragma clang diagnostic pop
|
||||
#endif
|
||||
|
||||
//#define MAVLINK_PARAMS
|
||||
//#define DUMP_RX_TX
|
||||
#define ERROR_NUM_MODES 99
|
||||
#define ERROR_MAV_ACTION_NB 99
|
||||
|
||||
|
||||
// Auto Pilot modes
|
||||
// ----------------
|
||||
#define AC_STABILIZE 0 // hold level position
|
||||
#define AC_ACRO 1 // rate control
|
||||
#define AC_ALT_HOLD 2 // AUTO control
|
||||
#define AC_AUTO 3 // AUTO control
|
||||
#define AC_GUIDED 4 // AUTO control
|
||||
#define AC_LOITER 5 // Hold a single location
|
||||
#define AC_RTL 6 // AUTO control
|
||||
#define AC_CIRCLE 7 // AUTO control
|
||||
#define AC_POSITION 8 // AUTO control
|
||||
#define AC_LAND 9 // AUTO control
|
||||
#define AC_OF_LOITER 10 // Hold a single location using optical flow
|
||||
// sensor
|
||||
#define AC_TOY_A 11 // THOR Enum for Toy mode
|
||||
#define AC_TOY_M 12 // THOR Enum for Toy mode
|
||||
#define AC_NUM_MODES 13
|
||||
|
||||
// Mavlink airframe types
|
||||
#define MAVLINK_ARDUCOPTER 0
|
||||
#define MAVLINK_ARDUPLANE 1
|
||||
#define MAVLINK_INVALID_TYPE 2
|
||||
|
||||
|
||||
#define AP_MANUAL 0
|
||||
#define AP_CIRCLE 1
|
||||
#define AP_STABILIZE 2
|
||||
#define AP_TRAINING 3
|
||||
#define AP_FLY_BY_WIRE_A 5
|
||||
#define AP_FLY_BY_WIRE_B 6
|
||||
#define AP_AUTO 10
|
||||
#define AP_RTL 11
|
||||
#define AP_LOITER 12
|
||||
#define AP_GUIDED 15
|
||||
#define AP_INITIALISING 16
|
||||
#define AP_NUM_MODES 17
|
||||
|
||||
static const uint8_t ap_modes_lut[18] PROGMEM = {0,1,2,3,12,4,5,12,12,12,6,7,8,9,12,12,10,11};
|
||||
|
||||
/*
|
||||
* Type definitions
|
||||
*/
|
||||
|
||||
#ifdef MAVLINK_PARAMS
|
||||
|
||||
enum ACM_PARAMS {
|
||||
RATE_YAW_P, // Rate Yaw
|
||||
RATE_YAW_I, // Rate Yaw
|
||||
STB_YAW_P, // Stabilize Yaw
|
||||
STB_YAW_I, // Stabilize Yaw
|
||||
RATE_PIT_P, // Rate Pitch
|
||||
RATE_PIT_I, // Rate Pitch
|
||||
STB_PIT_P, // Stabilize Pitch
|
||||
STB_PIT_I, // Stabilize Pitch
|
||||
RATE_RLL_P, // Rate Roll
|
||||
RATE_RLL_I, // Rate Roll
|
||||
STB_RLL_P, // Stabilize Roll
|
||||
STB_RLL_I, // Stabilize Roll
|
||||
THR_ALT_P, // THR_BAR, // Altitude Hold
|
||||
THR_ALT_I, // THR_BAR, // Altitude Hold
|
||||
HLD_LON_P, // Loiter
|
||||
HLD_LON_I, // Loiter
|
||||
HLD_LAT_P, // Loiter
|
||||
HLD_LAT_I, // Loiter
|
||||
NAV_LON_P, // Nav WP
|
||||
NAV_LON_I, // Nav WP
|
||||
NAV_LAT_P, // Nav WPs
|
||||
NAV_LAT_I, // Nav WP
|
||||
NB_PID_PARAMS, // Number of PI Parameters
|
||||
LOW_VOLT = NB_PID_PARAMS,
|
||||
IN_VOLT, //
|
||||
BATT_MONITOR, //
|
||||
BATT_CAPACITY, //
|
||||
NB_PARAMS
|
||||
};
|
||||
//#define NB_PID_PARAMS 24
|
||||
#define NB_COL_PARAMS 2
|
||||
#define NB_ROW_PARAMS ((NB_PARAMS+1)/NB_COL_PARAMS)
|
||||
|
||||
typedef struct MavlinkParam_ {
|
||||
uint8_t repeat :4;
|
||||
uint8_t valid :4;
|
||||
float value;
|
||||
} MavlinkParam_t;
|
||||
|
||||
#endif
|
||||
|
||||
typedef struct Location_ {
|
||||
float lat; ///< Latitude in degrees
|
||||
float lon; ///< Longitude in degrees
|
||||
float gps_alt; ///< Altitude in meters
|
||||
float rel_alt;
|
||||
} Location_t;
|
||||
|
||||
typedef struct Telemetry_Data_ {
|
||||
// INFOS
|
||||
uint8_t status; ///< System status flag, see MAV_STATUS ENUM
|
||||
uint8_t type;
|
||||
uint8_t autopilot;
|
||||
uint8_t type_autopilot;
|
||||
uint16_t packet_drop;
|
||||
uint16_t packet_fixed;
|
||||
uint8_t radio_sysid;
|
||||
uint8_t radio_compid;
|
||||
uint8_t mav_sysid;
|
||||
uint8_t mav_compid;
|
||||
uint8_t mode;
|
||||
uint32_t custom_mode;
|
||||
bool active;
|
||||
uint8_t nav_mode;
|
||||
uint8_t rcv_control_mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h
|
||||
uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
|
||||
uint8_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt)
|
||||
uint8_t ibat; ///< Battery voltage, in millivolts (1 = 1 millivolt)
|
||||
uint8_t rem_bat; ///< Battery voltage, in millivolts (1 = 1 millivolt)
|
||||
bool vbat_low;
|
||||
|
||||
uint8_t rc_rssi;
|
||||
uint8_t pc_rssi;
|
||||
|
||||
uint8_t debug;
|
||||
|
||||
// MSG ACTION / ACK
|
||||
uint8_t req_mode;
|
||||
int8_t ack_result;
|
||||
|
||||
// GPS
|
||||
uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
|
||||
uint8_t satellites_visible; ///< Number of satellites visible
|
||||
Location_t loc_current;
|
||||
float eph;
|
||||
uint16_t course;
|
||||
float v; // Ground speed
|
||||
// Navigation
|
||||
uint16_t heading;
|
||||
uint16_t bearing;
|
||||
|
||||
#ifdef MAVLINK_PARAMS
|
||||
// Params
|
||||
MavlinkParam_t params[NB_PARAMS];
|
||||
#endif
|
||||
|
||||
} Telemetry_Data_t;
|
||||
|
||||
// Telemetry data hold
|
||||
extern Telemetry_Data_t telemetry_data;
|
||||
|
||||
/*
|
||||
* Funtion definitions
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
#if 0
|
||||
extern inline uint8_t MAVLINK_CtrlMode2Action(uint8_t mode) {
|
||||
uint8_t action;
|
||||
|
||||
return action;
|
||||
}
|
||||
|
||||
extern inline uint8_t MAVLINK_Action2CtrlMode(uint8_t action) {
|
||||
uint8_t mode = ERROR_NUM_MODES;
|
||||
switch (action) {
|
||||
|
||||
return action;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
void telemetryWakeup();
|
||||
void MAVLINK_Init(void);
|
||||
void menuViewTelemetryMavlink(event_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
|
||||
|
||||
void telemetryPortInit(uint8_t baudrate);
|
||||
|
||||
#endif // _MAVLINK_H_
|
||||
|
||||
Loading…
Add table
Add a link
Reference in a new issue