diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 057972f1da..d2dfeb1e06 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -1070,6 +1070,19 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) } // end switch } // end handle mavlink +uint64_t GCS_MAVLINK_Rover::capabilities() const +{ + return (MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT | + MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | + MAV_PROTOCOL_CAPABILITY_MISSION_INT | + MAV_PROTOCOL_CAPABILITY_COMMAND_INT | + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED | + MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT | + MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET | + MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION | + GCS_MAVLINK::capabilities()); +} + /* * a delay() callback that processes MAVLink packets. We set this as the * callback in long running library initialisation routines to allow diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index 4fd0344062..97efcc98c3 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -34,6 +34,7 @@ protected: bool set_home_to_current_location(bool lock) override; bool set_home(const Location& loc, bool lock) override; + uint64_t capabilities() const override; void send_nav_controller_output() const override; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index acf018ee69..ff938b792c 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -396,9 +396,6 @@ private: void balancebot_pitch_control(float &throttle); bool is_balancebot() const; - // capabilities.cpp - void init_capabilities(void); - // commands_logic.cpp void update_mission(void); diff --git a/APMrover2/capabilities.cpp b/APMrover2/capabilities.cpp deleted file mode 100644 index 7bef67ef98..0000000000 --- a/APMrover2/capabilities.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include "Rover.h" - -void Rover::init_capabilities(void) -{ - hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT | - MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | - MAV_PROTOCOL_CAPABILITY_MISSION_INT | - MAV_PROTOCOL_CAPABILITY_COMMAND_INT | - MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED | - MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_GLOBAL_INT | - MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET | - MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION); -} diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 557daccf79..6bbba0ef3b 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -27,8 +27,6 @@ void Rover::init_ardupilot() AP::fwversion().fw_string, (unsigned)hal.util->available_memory()); - init_capabilities(); - // // Check the EEPROM format version before loading any parameters from EEPROM. //