From d2322c738d180327bad1f6c99c7032fb88370904 Mon Sep 17 00:00:00 2001 From: "pienk@planet.nl" Date: Sat, 17 Aug 2013 15:33:44 +0000 Subject: [PATCH] Prepared for baud rate variable in g_eeGeneral Added detection for ArduPlane and ArduCopter versions. Created different flight mode decoders for plane and copter. --- src/gui/view_mavlink.cpp | 148 ++++++++++++++++++++++++++------------ src/serial.cpp | 58 ++++++++++++--- src/serial.h | 10 +++ src/telemetry/mavlink.cpp | 37 +++++++++- src/telemetry/mavlink.h | 50 +++++++++---- 5 files changed, 233 insertions(+), 70 deletions(-) diff --git a/src/gui/view_mavlink.cpp b/src/gui/view_mavlink.cpp index 80c07e834..86ce943db 100644 --- a/src/gui/view_mavlink.cpp +++ b/src/gui/view_mavlink.cpp @@ -164,6 +164,108 @@ void lcd_outdezFloat(uint8_t x, uint8_t y, float val, uint8_t precis, uint8_t mo } } +/*! \brief Fightmode decode helper + * \details Decodes the flight mode from Mavlink custom mode enum to a string. + * This funtion can handle ArduPilot and ArduCoper code. + */ + +const char * mav_mode_to_string(uint32_t custom_mode) //, const char * mode_text_p) +{ + const char * mode_text_p; + switch (telemetry_data.type_autopilot) { + case MAVLINK_ARDUCOPTER: + switch(custom_mode) { + case AC_STABILIZE: + mode_text_p = PSTR("Stabilize"); + break; + case AC_ACRO: + mode_text_p = PSTR("Acro"); + break; + case AC_ALT_HOLD: + mode_text_p = PSTR("Alt Hold"); + break; + case AC_AUTO: + mode_text_p = PSTR("Auto"); + break; + case AC_GUIDED: + mode_text_p = PSTR("Guided"); + break; + case AC_LOITER: + mode_text_p = PSTR("Loiter"); + break; + case AC_RTL: + mode_text_p = PSTR("RTL"); + break; + case AC_CIRCLE: + mode_text_p = PSTR("Circle"); + break; + case AC_POSITION: + mode_text_p = PSTR("Pos Hold"); + break; + case AC_LAND: + mode_text_p = PSTR("Land"); + break; + case AC_OF_LOITER: + mode_text_p = PSTR("OF Loiter"); + break; + case AC_TOY_A: + mode_text_p = PSTR("Toy A"); + break; + case AC_TOY_M: + mode_text_p = PSTR("Toy M"); + break; + default: + mode_text_p = PSTR("INVALID"); + break; + } + break; + case MAVLINK_ARDUPLANE: + switch(custom_mode) { + case AP_MANUAL: + mode_text_p = PSTR("Manual"); + break; + case AP_CIRCLE: + mode_text_p = PSTR("Circle"); + break; + case AP_STABILIZE: + mode_text_p = PSTR("Stabilize"); + break; + case AP_TRAINING: + mode_text_p = PSTR("Training"); + break; + case AP_FLY_BY_WIRE_A: + mode_text_p = PSTR("Fly by Wire A"); + break; + case AP_FLY_BY_WIRE_B: + mode_text_p = PSTR("Fly by Wire A"); + break; + case AP_AUTO: + mode_text_p = PSTR("Auto"); + break; + case AP_RTL: + mode_text_p = PSTR("RTL"); + break; + case AP_LOITER: + mode_text_p = PSTR("Loiter"); + break; + case AP_GUIDED: + mode_text_p = PSTR("Guided"); + break; + case AP_INITIALISING: + mode_text_p = PSTR("Initialising"); + break; + default: + mode_text_p = PSTR("INVALID"); + break; + } + break; + default: + mode_text_p = PSTR("INVALID MAV TYPE"); + break; + } + return mode_text_p; +} + /*! \brief Menu header * \details Small helper function to print the standard header on the screen. */ @@ -232,51 +334,7 @@ void menuTelemetryMavlinkFlightMode(void) { mav_title(PSTR("MODE"), MAVLINK_menu); - const char * mode_text_p; - switch(telemetry_data.custom_mode) { - case AP_STABILIZE: - mode_text_p = PSTR("Stabilize"); - break; - case AP_ACRO: - mode_text_p = PSTR("Acro"); - break; - case AP_ALT_HOLD: - mode_text_p = PSTR("Alt Hold"); - break; - case AP_AUTO: - mode_text_p = PSTR("Auto"); - break; - case AP_GUIDED: - mode_text_p = PSTR("Guided"); - break; - case AP_LOITER: - mode_text_p = PSTR("Loiter"); - break; - case AP_RTL: - mode_text_p = PSTR("RTL"); - break; - case AP_CIRCLE: - mode_text_p = PSTR("Circle"); - break; - case AP_POSITION: - mode_text_p = PSTR("Pos Hold"); - break; - case AP_LAND: - mode_text_p = PSTR("Land"); - break; - case AP_OF_LOITER: - mode_text_p = PSTR("OF Loiter"); - break; - case AP_TOY_A: - mode_text_p = PSTR("Toy A"); - break; - case AP_TOY_M: - mode_text_p = PSTR("Toy M"); - break; - default: - mode_text_p = PSTR("INVALID"); - break; - } + const char * mode_text_p = mav_mode_to_string(telemetry_data.custom_mode); uint8_t x, y; x = 0; diff --git a/src/serial.cpp b/src/serial.cpp index baf5a538e..516897479 100644 --- a/src/serial.cpp +++ b/src/serial.cpp @@ -137,6 +137,19 @@ void SERIAL_startTX(void) { } } +static void uart_9600(void) { +#ifndef SIMU +#define BAUD 9600 +#include + UBRR0H = UBRRH_VALUE; + UBRR0L = UBRRL_VALUE; +#if USE_2X + UCSR0A |= (1 << U2X0); +#else + UCSR0A &= ~(1 << U2X0); +#endif +#endif +} static void uart_19200(void) { #ifndef SIMU @@ -182,6 +195,23 @@ static void uart_57600(void) { #endif } + +static void uart_115200(void) { +#ifndef SIMU +#undef BAUD // avoid compiler warning +#define BAUD 115200 +#include + UBRR0H = UBRRH_VALUE; + UBRR0L = UBRRL_VALUE; +#if USE_2X + UCSR0A |= (1 << U2X0); +#else + UCSR0A &= ~(1 << U2X0); +#endif +#endif +} + + inline void SERIAL_EnableTXD(void) { //UCSR0B |= (1 << TXEN0); // enable TX UCSR0B |= (1 << TXEN0) | (1 << UDRIE0); // enable TX and TX interrupt @@ -199,19 +229,29 @@ void SERIAL_Init(void) { PORTE &= ~(1 << PORTE0); // disable pullup on RXD0 pin uint8_t b = 2; // TODO g_eeGeneral.baudRate; - - if (b == 0) { + g_eeGeneral.spare2 = BAUD_57600; + switch (g_eeGeneral.spare2) { + case BAUD_9600: + uart_9600(); + break; + case BAUD_19200: uart_19200(); - } - else if (b == 1) { + break; + case BAUD_38400: uart_38400(); - } - else if (b == 2) { + break; + case BAUD_57600: uart_57600(); + break; + case BAUD_115200: + uart_115200(); + break; +// default: +// g_eeGeneral.spare2 = BAUD_57600; +// eeDirty(EE_GENERAL); +// break; } -#if 0 - uart_57600(); -#endif + // UCSR0A &= ~(1 << U2X0); // disable double speed operation // set 8N1 diff --git a/src/serial.h b/src/serial.h index dbea8debb..6c3f4b0a6 100644 --- a/src/serial.h +++ b/src/serial.h @@ -24,6 +24,16 @@ typedef enum serial_tx_state_ { } serial_tx_state_t; extern serial_tx_state_t serialTxState; +enum SERIAL_BAUDS { + BAUD_4800 = 0, + BAUD_9600, + BAUD_14400, + BAUD_19200, + BAUD_38400, + BAUD_57600, + BAUD_76800, + BAUD_115200 +}; typedef void (*SerialFuncP)(uint8_t event); diff --git a/src/telemetry/mavlink.cpp b/src/telemetry/mavlink.cpp index b4b2ede36..5a5c9bdac 100644 --- a/src/telemetry/mavlink.cpp +++ b/src/telemetry/mavlink.cpp @@ -119,6 +119,10 @@ void MAVLINK_reset(uint8_t warm_reset) { 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; @@ -202,12 +206,41 @@ static inline void REC_MAVLINK_MSG_ID_VFR_HUD(const mavlink_message_t* msg) { telemetry_data.heading = mavlink_msg_vfr_hud_get_heading(msg); } -/*! \brief Hardbeat message +/*! \brief Heartbeat message + * \details Heartbeat message is used for the following information: + * type and autopilot is used to determain if the UAV is a 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.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); + 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; diff --git a/src/telemetry/mavlink.h b/src/telemetry/mavlink.h index 623ec9ebb..0bb3956a5 100644 --- a/src/telemetry/mavlink.h +++ b/src/telemetry/mavlink.h @@ -64,21 +64,40 @@ extern void SERIAL_send_uart_bytes(const uint8_t * buf, uint16_t len); // Auto Pilot modes // ---------------- -#define AP_STABILIZE 0 // hold level position -#define AP_ACRO 1 // rate control -#define AP_ALT_HOLD 2 // AUTO control -#define AP_AUTO 3 // AUTO control -#define AP_GUIDED 4 // AUTO control -#define AP_LOITER 5 // Hold a single location -#define AP_RTL 6 // AUTO control -#define AP_CIRCLE 7 // AUTO control -#define AP_POSITION 8 // AUTO control -#define AP_LAND 9 // AUTO control -#define AP_OF_LOITER 10 // Hold a single location using optical flow +#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 AP_TOY_A 11 // THOR Enum for Toy mode -#define AP_TOY_M 12 // THOR Enum for Toy mode -#define AP_NUM_MODES 13 +#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 AC_NUM_MODES 17 /* * Type definitions @@ -137,6 +156,9 @@ typedef struct Location_ { 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; uint8_t mode; uint32_t custom_mode;