From 9d56d286155636305fa1e1b097995bf60e31c691 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 24 Apr 2012 22:18:30 +1000 Subject: [PATCH] MAVLink: make APM build with MAVLink 1.0 possible with arduino GUI --- ArduPlane/APM_Config.h | 3 +++ ArduPlane/ArduPlane.pde | 5 ++-- ArduPlane/GCS.h | 1 - ArduPlane/GCS_Mavlink.pde | 51 +++++++++++++++++++++++++++++++-------- ArduPlane/Makefile | 4 +-- ArduPlane/defines.h | 2 -- 6 files changed, 49 insertions(+), 17 deletions(-) diff --git a/ArduPlane/APM_Config.h b/ArduPlane/APM_Config.h index 18b5794ca7..36e10ac9d1 100644 --- a/ArduPlane/APM_Config.h +++ b/ArduPlane/APM_Config.h @@ -28,3 +28,6 @@ */ +// use this to enable the new MAVLink 1.0 protocol, instead of the +// older 0.9 protocol +// #define MAVLINK10 ENABLED diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 7740128199..fcf4af17bf 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -47,13 +47,14 @@ version 2.1 of the License, or (at your option) any later version. #include // Filter library #include // Mode Filter from Filter library #include // APM relay -#include // Camera/Antenna mount -#include // MAVLink GCS definitions #include // Configuration #include "config.h" +#include // MAVLink GCS definitions +#include // Camera/Antenna mount + // Local modules #include "defines.h" #include "Parameters.h" diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index 607fe197dd..c10a7e0d67 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -8,7 +8,6 @@ #include #include -#include #include #include #include diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 5cd4841ddc..c1313c37ba 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -27,7 +27,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; @@ -130,7 +130,7 @@ static NOINLINE void send_fence_status(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; @@ -337,7 +337,7 @@ static void NOINLINE send_nav_controller_output(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; @@ -501,6 +501,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) { @@ -592,7 +607,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); @@ -1082,7 +1097,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } -#ifdef MAVLINK10 +#if MAVLINK10 == ENABLED case MAVLINK_MSG_ID_COMMAND_LONG: { // decode @@ -1272,7 +1287,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 @@ -1328,7 +1343,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } -#ifndef MAVLINK10 +#if MAVLINK10 == DISABLED case MAVLINK_MSG_ID_SET_NAV_MODE: { // decode @@ -1680,7 +1695,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; default: -#ifdef MAVLINK10 +#if MAVLINK10 == ENABLED result = MAV_MISSION_UNSUPPORTED; #endif break; @@ -1896,7 +1911,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #if HIL_MODE != HIL_MODE_DISABLED // This is used both as a sensor and to pass the location // in HIL_ATTITUDE mode. -#ifdef MAVLINK10 +#if MAVLINK10 == ENABLED case MAVLINK_MSG_ID_GPS_RAW_INT: { // decode @@ -1934,7 +1949,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) airspeed = 100*packet.airspeed; break; } -#ifdef MAVLINK10 +#if MAVLINK10 == ENABLED case MAVLINK_MSG_ID_HIL_STATE: { mavlink_hil_state_t packet; @@ -2257,3 +2272,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/ArduPlane/Makefile b/ArduPlane/Makefile index 8f7c0a9050..d8ed558b99 100644 --- a/ArduPlane/Makefile +++ b/ArduPlane/Makefile @@ -25,13 +25,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-mount: make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DMOUNT=ENABLED" diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 9144bf3cfd..f88576b2b8 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -95,8 +95,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