1
0
Fork 0
mirror of https://github.com/opentx/opentx.git synced 2025-07-24 00:35:18 +03:00
opentx/src/mavlink.cpp
bsongis 2e5cf5a4f3 Small RAM saving.
Some code ported for the ersky9x board
Mavlink port commited
2012-03-21 18:31:00 +00:00

928 lines
23 KiB
C++

/*
* 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;
}
}