Copter: GCS_MAVLink takes care of mavlink capabilities

This commit is contained in:
Peter Barker 2018-03-29 11:51:26 +11:00 committed by Peter Barker
parent 8efda2b792
commit f759c7ac7e
5 changed files with 18 additions and 22 deletions

View File

@ -658,9 +658,6 @@ private:
// baro_ground_effect.cpp
void update_ground_effect_detector(void);
// capabilities.cpp
void init_capabilities(void);
// commands.cpp
void update_home_from_EKF();
void set_home_to_current_location_inflight();

View File

@ -1359,3 +1359,20 @@ float GCS_MAVLINK_Copter::vfr_hud_alt() const
}
return GCS_MAVLINK::vfr_hud_alt();
}
uint64_t GCS_MAVLINK_Copter::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_FLIGHT_TERMINATION |
MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
(copter.terrain.enabled() ? MAV_PROTOCOL_CAPABILITY_TERRAIN : 0) |
#endif
MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION |
GCS_MAVLINK::capabilities());
}

View File

@ -40,6 +40,7 @@ protected:
bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED;
bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED;
void send_nav_controller_output() const override;
uint64_t capabilities() const override;
private:

View File

@ -1,17 +0,0 @@
#include "Copter.h"
void Copter::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_FLIGHT_TERMINATION |
MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET |
MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION);
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
hal.util->set_capabilities(MAV_PROTOCOL_CAPABILITY_TERRAIN);
#endif
}

View File

@ -28,8 +28,6 @@ void Copter::init_ardupilot()
AP::fwversion().fw_string,
(unsigned)hal.util->available_memory());
init_capabilities();
//
// Report firmware version code expect on console (check of actual EEPROM format version is done in load_parameters function)
//