From 4fee26437b4b080c915fc04b1d7a3dd5f1bcbd7f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 24 Apr 2012 22:17:56 +1000 Subject: [PATCH] MAVLink: make ACM build with MAVLink 1.0 possible in arduino GUI --- ArduCopter/APM_Config.h | 4 ++++ ArduCopter/ArduCopter.pde | 3 ++- ArduCopter/GCS.h | 1 - ArduCopter/GCS_Mavlink.pde | 43 ++++++++++++++++++++++++++++++++------ ArduCopter/Makefile | 4 ++-- ArduCopter/defines.h | 2 -- 6 files changed, 45 insertions(+), 12 deletions(-) diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index a92afc0963..9a9aaec63f 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -89,3 +89,7 @@ // #define MOT_6 CH_4 // #define MOT_7 CH_7 // #define MOT_8 CH_8 + +// use this to enable the new MAVLink 1.0 protocol, instead of the +// older 0.9 protocol +// #define MAVLINK10 ENABLED diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 19b34a6082..319f941566 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -94,7 +94,6 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // Mode Filter from Filter library #include // Mode Filter from Filter library #include // APM relay -#include // MAVLink GCS definitions #include // Configuration @@ -102,6 +101,8 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include "config.h" #include "config_channels.h" +#include // MAVLink GCS definitions + // Local modules #include "Parameters.h" #include "GCS.h" diff --git a/ArduCopter/GCS.h b/ArduCopter/GCS.h index 607fe197dd..c10a7e0d67 100644 --- a/ArduCopter/GCS.h +++ b/ArduCopter/GCS.h @@ -8,7 +8,6 @@ #include #include -#include #include #include #include diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 8732be5839..8c9d2b2ba6 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -29,7 +29,7 @@ static bool mavlink_active; static NOINLINE void send_heartbeat(mavlink_channel_t chan) { -#ifdef MAVLINK10 +#if MAVLINK10 == ENABLED uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; uint8_t system_status = MAV_STATE_ACTIVE; uint32_t custom_mode = control_mode; @@ -102,7 +102,7 @@ static NOINLINE void send_attitude(mavlink_channel_t chan) static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) { -#ifdef MAVLINK10 +#if MAVLINK10 == ENABLED uint32_t control_sensors_present = 0; uint32_t control_sensors_enabled; uint32_t control_sensors_health; @@ -265,6 +265,21 @@ static void NOINLINE send_ahrs(mavlink_channel_t chan) #endif // HIL_MODE != HIL_MODE_ATTITUDE #ifdef DESKTOP_BUILD +void mavlink_simstate_send(uint8_t chan, + float roll, + float pitch, + float yaw, + float xAcc, + float yAcc, + float zAcc, + float p, + float q, + float r) +{ + mavlink_msg_simstate_send((mavlink_channel_t)chan, + roll, pitch, yaw, xAcc, yAcc, zAcc, p, q, r); +} + // report simulator state static void NOINLINE send_simstate(mavlink_channel_t chan) { @@ -286,7 +301,7 @@ static void NOINLINE send_hwstatus(mavlink_channel_t chan) static void NOINLINE send_gps_raw(mavlink_channel_t chan) { -#ifdef MAVLINK10 +#if MAVLINK10 == ENABLED uint8_t fix = g_gps->status(); if (fix == GPS::GPS_OK) { fix = 3; @@ -554,7 +569,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, break; case MSG_GPS_RAW: -#ifdef MAVLINK10 +#if MAVLINK10 == ENABLED CHECK_PAYLOAD_SIZE(GPS_RAW_INT); #else CHECK_PAYLOAD_SIZE(GPS_RAW); @@ -1050,7 +1065,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } -#ifdef MAVLINK10 +#if MAVLINK10 == ENABLED case MAVLINK_MSG_ID_COMMAND_LONG: { // decode @@ -1251,7 +1266,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_set_mode_t packet; mavlink_msg_set_mode_decode(msg, &packet); -#ifdef MAVLINK10 +#if MAVLINK10 == ENABLED if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) { // we ignore base_mode as there is no sane way to map // from that bitmap to a APM flight mode. We rely on @@ -2172,3 +2187,19 @@ static void gcs_send_text_fmt(const prog_char_t *fmt, ...) mavlink_send_message(MAVLINK_COMM_1, MSG_STATUSTEXT, 0); } } + +// this code was moved from libraries/GCS_MAVLink to allow compile +// time selection of MAVLink 1.0 +BetterStream *mavlink_comm_0_port; +BetterStream *mavlink_comm_1_port; + +mavlink_system_t mavlink_system = {7,1,0,0}; + +uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) +{ + if (sysid != mavlink_system.sysid) + return 1; + // Currently we are not checking for correct compid since APM is not passing mavlink info to any subsystem + // If it is addressed to our system ID we assume it is for us + return 0; // no error +} diff --git a/ArduCopter/Makefile b/ArduCopter/Makefile index 32e2e44a17..2beb16563a 100644 --- a/ArduCopter/Makefile +++ b/ArduCopter/Makefile @@ -28,13 +28,13 @@ apm2beta: make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DAPM2_BETA_HARDWARE" mavlink10: - make -f Makefile EXTRAFLAGS="-DMAVLINK10" + make -f Makefile EXTRAFLAGS="-DMAVLINK10=1" sitl: make -f ../libraries/Desktop/Makefile.desktop sitl-mavlink10: - make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DMAVLINK10" + make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DMAVLINK10=1" sitl-octa: make -f ../libraries/Desktop/Makefile.desktop octa diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 123d843a13..69803555e7 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -224,8 +224,6 @@ #define RELAY_TOGGLE 5 #define STOP_REPEAT 10 -//#define MAV_CMD_CONDITION_YAW 23 - // GCS Message ID's /// NOTE: to ensure we never block on sending MAVLink messages /// please keep each MSG_ to a single MAVLink message. If need be