diff --git a/radio/src/definitions.h b/radio/src/definitions.h index 29ac760f3..6145fd175 100644 --- a/radio/src/definitions.h +++ b/radio/src/definitions.h @@ -48,8 +48,6 @@ #define __NOINIT #endif -typedef int32_t int24_t; - #if __GNUC__ #define PACK( __Declaration__ ) __Declaration__ __attribute__((__packed__)) #else diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 0f3b45b94..773c3db18 100644 --- a/radio/src/mixer.cpp +++ b/radio/src/mixer.cpp @@ -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; diff --git a/radio/src/opentx.h b/radio/src/opentx.h index 487640f68..9e02444d7 100644 --- a/radio/src/opentx.h +++ b/radio/src/opentx.h @@ -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) diff --git a/radio/src/telemetry/frsky_d.cpp b/radio/src/telemetry/frsky_d.cpp deleted file mode 100644 index 256919acd..000000000 --- a/radio/src/telemetry/frsky_d.cpp +++ /dev/null @@ -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 ° 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 - 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); - } -} diff --git a/radio/src/telemetry/jeti.h b/radio/src/telemetry/jeti.h deleted file mode 100644 index d545798e7..000000000 --- a/radio/src/telemetry/jeti.h +++ /dev/null @@ -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_ - diff --git a/radio/src/telemetry/mavlink.cpp b/radio/src/telemetry/mavlink.cpp deleted file mode 100644 index 514cacc50..000000000 --- a/radio/src/telemetry/mavlink.cpp +++ /dev/null @@ -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; - } -} - diff --git a/radio/src/telemetry/mavlink.h b/radio/src/telemetry/mavlink.h deleted file mode 100644 index 40540ecb6..000000000 --- a/radio/src/telemetry/mavlink.h +++ /dev/null @@ -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_ -