530 lines
14 KiB
C++
530 lines
14 KiB
C++
/*
|
|
* Authors (alphabetical order)
|
|
* - Andre Bernet <bernet.andre@gmail.com>
|
|
* - Andreas Weitl
|
|
* - Bertrand Songis <bsongis@gmail.com>
|
|
* - Bryan J. Rentoul (Gruvin) <gruvin@gmail.com>
|
|
* - Cameron Weeks <th9xer@gmail.com>
|
|
* - Erez Raviv
|
|
* - Gabriel Birkus
|
|
* - Gerard Valade <gerard.valade@gmail.com>
|
|
* - Jean-Pierre Parisy
|
|
* - Karl Szmutny
|
|
* - Michael Blandford
|
|
* - Michal Hlavinka
|
|
* - Pat Mackenzie
|
|
* - Philip Moss
|
|
* - Rienk de Jong
|
|
* - Rob Thomson
|
|
* - Romolo Manfredini <romolo.manfredini@gmail.com>
|
|
* - Thomas Husterer
|
|
*
|
|
* opentx 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.
|
|
*
|
|
*/
|
|
|
|
/*! \file view_mavlink.cpp
|
|
* Mavlink menu
|
|
* Contains the menu specific code for Mavlink support.
|
|
*/
|
|
|
|
#include "gui/9X/view_mavlink.h"
|
|
|
|
// Globals declaration
|
|
|
|
|
|
/*! \brief Top Mavlink Menu definition
|
|
* \details Registers button events and handles that info. Buttons select menus,
|
|
* these are launched from the MAVLINK_menu switch statement. Setup menu is
|
|
* lanuched by the menu button. On exit (with exit button) the mavlink
|
|
* extension is reinitialized.
|
|
*/
|
|
void menuTelemetryMavlink(uint8_t event) {
|
|
|
|
switch (event) // new event received, branch accordingly
|
|
{
|
|
case EVT_ENTRY:
|
|
MAVLINK_menu = MENU_INFO;
|
|
break;
|
|
|
|
case EVT_KEY_FIRST(KEY_UP):
|
|
if (MAVLINK_menu > 0)
|
|
MAVLINK_menu--;
|
|
else
|
|
chainMenu(menuMainView);
|
|
break;
|
|
case EVT_KEY_FIRST(KEY_DOWN):
|
|
if (MAVLINK_menu < MAX_MAVLINK_MENU - 1)
|
|
MAVLINK_menu++;
|
|
break;
|
|
case EVT_KEY_FIRST(KEY_MENU):
|
|
return;
|
|
case EVT_KEY_FIRST(KEY_EXIT):
|
|
chainMenu(menuMainView);
|
|
break;
|
|
}
|
|
|
|
switch (MAVLINK_menu) {
|
|
case MENU_INFO:
|
|
menuTelemetryMavlinkInfos();
|
|
break;
|
|
case MENU_MODE:
|
|
menuTelemetryMavlinkFlightMode();
|
|
break;
|
|
case MENU_BATT:
|
|
menuTelemetryMavlinkBattery();
|
|
break;
|
|
case MENU_NAV:
|
|
menuTelemetryMavlinkNavigation();
|
|
break;
|
|
case MENU_GPS:
|
|
menuTelemetryMavlinkGPS();
|
|
break;
|
|
#ifdef DUMP_RX_TX
|
|
case MENU_DUMP_TX:
|
|
case MENU_DUMP_RX:
|
|
menuTelemetryMavlinkDump(event);
|
|
break;
|
|
#endif
|
|
default:
|
|
break;
|
|
}
|
|
|
|
}
|
|
|
|
/*! \brief Float variable display helper
|
|
* \param x x position on the screen
|
|
* \param y y position on the screen
|
|
* \param val value to display
|
|
* \param percis precision to display
|
|
* \param mode Use one of the defines in lcd.h line 81
|
|
* \details The maximum value is 9999. The position is the position of the ones, see below for explanation "*" marks
|
|
* the x position.
|
|
* \verbatim
|
|
*
|
|
9999.9999 \endverbatim
|
|
*/
|
|
void lcd_outdezFloat(uint8_t x, uint8_t y, float val, uint8_t precis, uint8_t mode) {
|
|
char c;
|
|
int16_t lnum = val;
|
|
uint8_t x1 = x;
|
|
val -= lnum;
|
|
uint8_t xinc = FWNUM;
|
|
if (mode & DBLSIZE)
|
|
xinc *= 2;
|
|
|
|
int8_t i = 0;
|
|
lnum = abs(lnum);
|
|
for (; i < 4; i++) {
|
|
c = (lnum % 10) + '0';
|
|
x1 -= xinc;
|
|
lcdDrawChar(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;
|
|
lcdDrawChar(x1, y, '?', mode);
|
|
}
|
|
} else {
|
|
if (val < 0) {
|
|
val = -val;
|
|
x1 -= xinc;
|
|
lcdDrawChar(x1, y, '-', mode);
|
|
}
|
|
if (precis)
|
|
{
|
|
uint8_t y_temp = y;
|
|
if (mode & DBLSIZE)
|
|
y_temp += FH;
|
|
lcdDrawChar(x, y_temp, '.', (mode & (~DBLSIZE)));
|
|
x -= (xinc / 2);
|
|
}
|
|
for (i = 0; i < precis; i++) {
|
|
val *= 10;
|
|
int a = val;
|
|
c = a + '0';
|
|
x += xinc;
|
|
lcdDrawChar(x, y, c, mode);
|
|
val -= a;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*! \brief Fightmode string printer
|
|
* \details Decodes the flight mode from Mavlink custom mode enum to a string.
|
|
* This funtion can handle ArduPilot and ArduCoper code.
|
|
* To support new autopilot pilots add a STR_MAVLINK_... to the translations,
|
|
* and if requred a lut (see arduplane for examle) if there are unused modes
|
|
* in the sequence.
|
|
*/
|
|
|
|
void print_mav_mode(uint8_t x, uint8_t y, uint32_t custom_mode, uint8_t attr) //, const char * mode_text_p)
|
|
{
|
|
uint8_t mode = (uint8_t) custom_mode;
|
|
switch (telemetry_data.type_autopilot) {
|
|
case MAVLINK_ARDUCOPTER:
|
|
lcdDrawTextAtIndex(x,y,STR_MAVLINK_AC_MODES,mode,attr);
|
|
break;
|
|
case MAVLINK_ARDUPLANE:
|
|
lcdDrawTextAtIndex(x,y,STR_MAVLINK_AP_MODES,ap_modes_lut[custom_mode],attr);
|
|
break;
|
|
default:
|
|
lcdDrawText (FW, y, PSTR("INV. MAV TYPE"), attr);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/*! \brief Menu header
|
|
* \details Small helper function to print the standard header on the screen.
|
|
*/
|
|
void mav_title(const pm_char * s, uint8_t index)
|
|
{
|
|
lcdDrawText(0, 0, PSTR("MAVLINK"), INVERS);
|
|
lcdDrawText(10 * FW, 0, s);
|
|
displayScreenIndex(index, MAX_MAVLINK_MENU, INVERS);
|
|
lcdDrawChar(7 * FW, 0, (mav_heartbeat > 0) ? '*' : ' ');
|
|
if (telemetry_data.active)
|
|
lcdDrawChar(8 * FW, 0, 'A', BLINK);
|
|
}
|
|
|
|
/*! \brief Global info menu
|
|
* \details Quick status overview menu. The menu should contain current mode,
|
|
* armed | disarmed, battery status and RSSI info. Menu must be clean and
|
|
* readable with a quick glance.
|
|
* \todo Make menu as described as above.
|
|
*/
|
|
void menuTelemetryMavlinkInfos(void) {
|
|
|
|
mav_title(STR_MAVLINK_INFOS, MAVLINK_menu);
|
|
|
|
uint8_t x1, x2, xnum, y;
|
|
x1 = FW;
|
|
x2 = 7 * FW;
|
|
xnum = x2 + 5 * FWNUM;
|
|
y = FH;
|
|
/*
|
|
char * ptr = mav_statustext;
|
|
for (uint8_t j = 0; j < LEN_STATUSTEXT; j++) {
|
|
if (*ptr == 0) {
|
|
lcdDrawChar(x1, y, ' ');
|
|
} else {
|
|
lcdDrawChar(x1, y, *ptr++);
|
|
}
|
|
x1 += FW;
|
|
}
|
|
x1 = FW;
|
|
y += FH;
|
|
*/
|
|
if (telemetry_data.status) {
|
|
|
|
lcdDrawSizedText(x1, y, STR_MAVLINK_MODE, 4, 0);
|
|
if (telemetry_data.active)
|
|
lcdDrawSizedText(x2, y, PSTR("A"), 1, 0);
|
|
lcdDrawNumber(xnum, y, telemetry_data.mode, 0);
|
|
|
|
y += FH;
|
|
lcdDrawText(x1, y, PSTR("BATT"));
|
|
lcdDrawNumber(xnum, y, telemetry_data.vbat, PREC1, 5);
|
|
|
|
y += FH;
|
|
lcdDrawText(x1, y, PSTR("DROP"));
|
|
lcdDrawNumber(xnum, y, telemetry_data.packet_drop, 0);
|
|
/* y += FH;
|
|
lcdDrawText(x1, y, PSTR("FIX"));
|
|
lcdDrawNumber(xnum, y, telemetry_data.packet_fixed, 0);
|
|
y += FH;
|
|
lcdDrawText(x1, y, PSTR("MAV Comp"));
|
|
lcdDrawNumber(xnum, y, telemetry_data.mav_compid, 0);
|
|
y += FH;
|
|
lcdDrawText(x1, y, PSTR("MAV Sys"));
|
|
lcdDrawNumber(xnum, y, telemetry_data.mav_sysid, 0);
|
|
y += FH;
|
|
lcdDrawText(x1, y, PSTR("Rad Comp"));
|
|
lcdDrawNumber(xnum, y, telemetry_data.radio_compid, 0);
|
|
y += FH;
|
|
lcdDrawText(x1, y, PSTR("Rad Sys"));
|
|
lcdDrawNumber(xnum, y, telemetry_data.radio_sysid, 0);
|
|
*/
|
|
|
|
}
|
|
}
|
|
|
|
/*! \brief Flight mode menu
|
|
* \details Clear display of current flight mode.
|
|
* \todo Add functionality to change flight mode.
|
|
*/
|
|
void menuTelemetryMavlinkFlightMode(void) {
|
|
|
|
mav_title(STR_MAVLINK_MODE, MAVLINK_menu);
|
|
|
|
uint8_t x, y;
|
|
x = 0;
|
|
y = FH;
|
|
|
|
lcdDrawText(x, y, STR_MAVLINK_CUR_MODE);
|
|
y += FH;
|
|
print_mav_mode(FW, y, telemetry_data.custom_mode, DBLSIZE);
|
|
y += 2 * FH;
|
|
|
|
char * ptr = mav_statustext;
|
|
for (uint8_t j = 0; j < LEN_STATUSTEXT; j++) {
|
|
if (*ptr == 0) {
|
|
lcdDrawChar(x, y, ' ');
|
|
} else {
|
|
lcdDrawChar(x, y, *ptr++);
|
|
}
|
|
x += FW;
|
|
}
|
|
y += FH;
|
|
x = 0;
|
|
|
|
if (telemetry_data.active)
|
|
lcdDrawText (FW, y, STR_MAVLINK_ARMED, DBLSIZE);
|
|
}
|
|
|
|
/*! \brief Batterystatus dislplay
|
|
* \details Shows flight batery status.
|
|
* Also RC and PC RSSI are in this menu.
|
|
*/
|
|
void menuTelemetryMavlinkBattery(void) {
|
|
|
|
mav_title(STR_MAVLINK_BAT_MENU_TITLE, MAVLINK_menu);
|
|
|
|
uint8_t x, y, ynum;
|
|
x = 7 * FWNUM;
|
|
// x = xnum + 0 * FW;
|
|
ynum = 2 * FH;
|
|
y = 3 * FH;
|
|
|
|
lcdDrawText(0, 1*FH, STR_MAVLINK_BATTERY_LABEL);
|
|
|
|
lcdDrawNumber(x, ynum, telemetry_data.vbat, (DBLSIZE | PREC1 | UNSIGN));
|
|
lcdDrawText(x, y, PSTR("V"));
|
|
x += 4 * (2 * FWNUM);
|
|
lcdDrawNumber(x, ynum, telemetry_data.ibat, (DBLSIZE | PREC1 | UNSIGN));
|
|
lcdDrawText(x, y, PSTR("A"));
|
|
x += 4 * (2 * FWNUM);
|
|
lcdDrawNumber(x, ynum, telemetry_data.rem_bat, (DBLSIZE | UNSIGN));
|
|
lcdDrawText(x, y, PSTR("%"));
|
|
y += FH;
|
|
ynum += 3 * FH;
|
|
|
|
x = 0;
|
|
lcdDrawText(x, y, STR_MAVLINK_RC_RSSI_LABEL);
|
|
lcdDrawNumber(x + 7 * FWNUM, ynum, telemetry_data.rc_rssi, (DBLSIZE | UNSIGN));
|
|
lcdDrawText(x + 7 * FWNUM, ynum + FH, PSTR("%"));
|
|
if (g_model.mavlink.pc_rssi_en)
|
|
{
|
|
x += 8 * (2 * FWNUM);
|
|
lcdDrawText(x, y, STR_MAVLINK_PC_RSSI_LABEL);
|
|
lcdDrawNumber(x + 7 * FWNUM, ynum, telemetry_data.pc_rssi, (DBLSIZE));
|
|
lcdDrawText(x + 7 * FWNUM, ynum + FH, PSTR("%"));
|
|
}
|
|
|
|
}
|
|
|
|
/*! \brief Navigation dislplay
|
|
* \details Shows Navigation telemetry.
|
|
* Altitude in this menu is the relative (to the home location) altitude. This
|
|
* is the same altitude used by the waypoints.
|
|
* \todo Add a similar menu to fly back to the home location.
|
|
*/
|
|
void menuTelemetryMavlinkNavigation(void) {
|
|
|
|
mav_title(STR_MAVLINK_NAV_MENU_TITLE, MAVLINK_menu);
|
|
|
|
uint8_t x, y, ynum;
|
|
x = 7 * FWNUM;
|
|
// x = xnum + 0 * FW;
|
|
ynum = 2 * FH;
|
|
y = FH;
|
|
|
|
|
|
x = 0;
|
|
lcdDrawText(x, y, STR_MAVLINK_COURSE);
|
|
lcdDrawNumber(x + 7 * FWNUM, ynum, telemetry_data.course, (DBLSIZE | UNSIGN));
|
|
lcdDrawText(x + 7 * FWNUM, ynum, PSTR("o"));
|
|
x += 8 * (2 * FWNUM);
|
|
lcdDrawText(x, y, STR_MAVLINK_HEADING);
|
|
lcdDrawNumber(x + 7 * FWNUM, ynum, telemetry_data.heading, (DBLSIZE | UNSIGN));
|
|
lcdDrawText(x + 7 * FWNUM, ynum, PSTR("o"));
|
|
y += 3 * FH;
|
|
ynum += 3 * FH;
|
|
|
|
x = 0;
|
|
lcdDrawText(x, y, STR_MAVLINK_BEARING);
|
|
lcdDrawNumber(x + 7 * FWNUM, ynum, telemetry_data.bearing, (DBLSIZE | UNSIGN));
|
|
lcdDrawText(x + 7 * FWNUM, ynum, PSTR("o"));
|
|
x += 8 * (2 * FWNUM);
|
|
lcdDrawText(x, y, STR_MAVLINK_ALTITUDE);
|
|
lcd_outdezFloat(x + 4 * FWNUM - 1, ynum, telemetry_data.loc_current.rel_alt, 1, (DBLSIZE));
|
|
lcdDrawText(x + 7 * FWNUM, ynum + FH, PSTR("m"));
|
|
|
|
}
|
|
|
|
|
|
|
|
/*! \brief GPS information menu
|
|
* \details Menu gives a lot of info from the gps like fix type, position,
|
|
* attitude, heading and velocity. Text is small and the user must focus to
|
|
* read it.
|
|
* \todo Text is small. Should we do something about this or leaf it like this.
|
|
* I don't think will be used much when a user is concentrated on flying.
|
|
*/
|
|
void menuTelemetryMavlinkGPS(void) {
|
|
mav_title(STR_MAVLINK_GPS, MAVLINK_menu);
|
|
|
|
uint8_t x1, x2, xnum, xnum2, y;
|
|
x1 = FW;
|
|
x2 = x1 + 12 * FW;
|
|
xnum = 7 * FW + 3 * FWNUM;
|
|
xnum2 = xnum + 11 * FWNUM;
|
|
y = FH;
|
|
|
|
lcdDrawSizedText(x1, y, STR_MAVLINK_GPS, 3, 0);
|
|
if (telemetry_data.fix_type < 2) {
|
|
lcdDrawSizedText(xnum, y, STR_MAVLINK_NO_FIX, 6, 0);
|
|
} else {
|
|
lcdDrawNumber(xnum, y, telemetry_data.fix_type, 0, 3);
|
|
lcdDrawText(xnum, y, PSTR("D"));
|
|
}
|
|
lcdDrawText(x2, y, STR_MAVLINK_SAT);
|
|
lcdDrawNumber(xnum2, y, telemetry_data.satellites_visible, 0, 2);
|
|
|
|
// if (telemetry_data.fix_type > 0) {
|
|
y += FH;
|
|
lcdDrawText(x1, y, STR_MAVLINK_HDOP);
|
|
lcd_outdezFloat(xnum, y, telemetry_data.eph, 2);
|
|
|
|
y += FH;
|
|
lcdDrawText(x1, y, STR_MAVLINK_LAT);
|
|
lcd_outdezFloat(xnum, y, telemetry_data.loc_current.lat, 2);
|
|
|
|
lcdDrawSizedText(x2, y, STR_MAVLINK_LON, 3, 0);
|
|
lcd_outdezFloat(xnum2, y, telemetry_data.loc_current.lon, 2);
|
|
|
|
y += FH;
|
|
lcdDrawSizedText(x1, y, STR_MAVLINK_ALTITUDE, 3, 0);
|
|
lcdDrawNumber(xnum, y, telemetry_data.loc_current.gps_alt, 0);
|
|
|
|
y += FH;
|
|
lcdDrawSizedText(x1, y, STR_MAVLINK_COURSE, 6, 0);
|
|
lcd_outdezFloat(xnum, y, telemetry_data.course, 2);
|
|
|
|
y += FH;
|
|
lcdDrawSizedText(x1, y, PSTR("V"), 1, 0);
|
|
lcdDrawNumber(xnum, y, telemetry_data.v, 0);
|
|
//}
|
|
}
|
|
|
|
#ifdef DUMP_RX_TX
|
|
//! \brief Display one byte as hex.
|
|
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';
|
|
lcdDrawChar(x, y, c, c >= 'A' ? CONDENSED : 0);
|
|
val >>= 4;
|
|
}
|
|
}
|
|
|
|
//! \brief Hex dump of the current mavlink message.
|
|
void menuTelemetryMavlinkDump(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
|
|
|
|
|
|
/*! \brief Mavlink General setup menu.
|
|
* \details Setup menu for generic mavlink settings.
|
|
* Current menu items
|
|
* - RC RSSI scale item. Used to adjust the scale of the RSSI indicator to match
|
|
* the actual rssi value
|
|
* - PC RSSI enable item. Can be used to dissable PC RSSI display if not used.
|
|
* This funcion is called from the model setup menus, not directly by the
|
|
* telemetry menus
|
|
*/
|
|
void menuTelemetryMavlinkSetup(uint8_t event) {
|
|
|
|
MENU(STR_MAVMENUSETUP_TITLE, menuTabModel, e_MavSetup, ITEM_MAVLINK_MAX + 1, {0, 0, 1/*to force edit mode*/});
|
|
|
|
uint8_t sub = menuVerticalPosition - 1;
|
|
|
|
for (uint8_t i=0; i<LCD_LINES-1; i++) {
|
|
uint8_t y = 1 + 1*FH + i*FH;
|
|
uint8_t k = i+menuVerticalOffset;
|
|
uint8_t blink = ((s_editMode>0) ? BLINK|INVERS : INVERS);
|
|
uint8_t attr = (sub == k ? blink : 0);
|
|
switch(k) {
|
|
case ITEM_MAVLINK_RC_RSSI_SCALE:
|
|
lcd_putsLeft(y, STR_MAVLINK_RC_RSSI_SCALE_LABEL);
|
|
lcdDrawNumber(RADIO_SETUP_2ND_COLUMN, y, (25 + g_model.mavlink.rc_rssi_scale * 5), attr|LEFT);
|
|
lcdDrawChar(lcdLastPos, y, '%');
|
|
if (attr) CHECK_INCDEC_MODELVAR(event, g_model.mavlink.rc_rssi_scale, 0, 15);
|
|
break;
|
|
case ITEM_MAVLINK_PC_RSSI_EN:
|
|
g_model.mavlink.pc_rssi_en = onoffMenuItem(g_model.mavlink.pc_rssi_en,
|
|
RADIO_SETUP_2ND_COLUMN,
|
|
y,
|
|
STR_MAVLINK_PC_RSSI_EN_LABEL,
|
|
attr,
|
|
event);
|
|
break;
|
|
}
|
|
}
|
|
}
|