Prepared for baud rate variable in g_eeGeneral
Added detection for ArduPlane and ArduCopter versions. Created different flight mode decoders for plane and copter.
This commit is contained in:
@@ -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;
|
||||
|
||||
+36
-14
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user