From edf5ff1bbed6d4e267bd1c73fa0d01e2119adf42 Mon Sep 17 00:00:00 2001 From: squilter Date: Tue, 21 Jul 2015 15:04:25 -0700 Subject: [PATCH] Copter: add capability bitmask --- ArduCopter/Copter.h | 1 + ArduCopter/GCS_Mavlink.cpp | 4 ++-- ArduCopter/capabilities.cpp | 17 +++++++++++++++++ 3 files changed, 20 insertions(+), 2 deletions(-) create mode 100644 ArduCopter/capabilities.cpp diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 15d4719944..448cb667e4 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -946,6 +946,7 @@ private: void print_flight_mode(AP_HAL::BetterStream *port, uint8_t mode); void log_init(void); void run_cli(AP_HAL::UARTDriver *port); + uint64_t get_capabilities(void); public: void mavlink_delay_cb(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index bfd609e91b..f40e552a45 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1416,7 +1416,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1,1.0f)) { - copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); + copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(copter.get_capabilities()); result = MAV_RESULT_ACCEPTED; } break; @@ -1714,7 +1714,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif // AC_RALLY == ENABLED case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: - copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); + copter.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(copter.get_capabilities()); break; case MAVLINK_MSG_ID_LED_CONTROL: diff --git a/ArduCopter/capabilities.cpp b/ArduCopter/capabilities.cpp new file mode 100644 index 0000000000..9c3ff591c4 --- /dev/null +++ b/ArduCopter/capabilities.cpp @@ -0,0 +1,17 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include "Copter.h" + +uint64_t Copter::get_capabilities(void) +{ + uint64_t capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT + | MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT + | MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED + | MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT; + + #if AP_TERRAIN_AVAILABLE + capabilities |= MAV_PROTOCOL_CAPABILITY_TERRAIN; + #endif + + return capabilities; +}