diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 6624e25d8f..d463983218 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -860,14 +860,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) } break; - case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { - if (is_equal(packet.param1, 1.0f)) { - send_autopilot_version(FIRMWARE_VERSION); - result = MAV_RESULT_ACCEPTED; - } - break; - } - case MAV_CMD_DO_SET_HOME: { // param1 : use current (1=use current location, 0=use specified location) @@ -1278,10 +1270,6 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) rover.rangefinder.handle_msg(msg); break; - case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: - send_autopilot_version(FIRMWARE_VERSION); - break; - case MAVLINK_MSG_ID_VISION_POSITION_DELTA: rover.g2.visual_odom.handle_msg(msg); break; @@ -1415,3 +1403,8 @@ bool GCS_MAVLINK_Rover::set_mode(const uint8_t mode) } return rover.set_mode(*new_mode, MODE_REASON_GCS_COMMAND); } + +const AP_FWVersion &GCS_MAVLINK_Rover::get_fwver() const +{ + return rover.fwver; +} diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index a3c523fe95..3ef77246af 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -23,6 +23,7 @@ protected: AP_ServoRelayEvents *get_servorelayevents() const override; AP_GPS *get_gps() const override; AP_AdvancedFailsafe *get_advanced_failsafe() const override; + const AP_FWVersion &get_fwver() const override; uint8_t sysid_my_gcs() const override;