diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 43a8d1b186..292133aa93 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1330,30 +1330,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) } break; - case MAV_CMD_DO_SET_SERVO: - if (plane.ServoRelayEvents.do_set_servo(packet.param1, packet.param2)) { - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_DO_REPEAT_SERVO: - if (plane.ServoRelayEvents.do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4*1000)) { - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_DO_SET_RELAY: - if (plane.ServoRelayEvents.do_set_relay(packet.param1, packet.param2)) { - result = MAV_RESULT_ACCEPTED; - } - break; - - case MAV_CMD_DO_REPEAT_RELAY: - if (plane.ServoRelayEvents.do_repeat_relay(packet.param1, packet.param2, packet.param3*1000)) { - result = MAV_RESULT_ACCEPTED; - } - break; - case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: result = handle_preflight_reboot(packet, plane.quadplane.enable != 0); break; @@ -2061,6 +2037,11 @@ void GCS_MAVLINK_Plane::handle_mission_set_current(AP_Mission &mission, mavlink_ } } +AP_ServoRelayEvents *GCS_MAVLINK_Plane::get_servorelayevents() const +{ + return &plane.ServoRelayEvents; +} + AP_Rally *GCS_MAVLINK_Plane::get_rally() const { return &plane.rally; diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index d501b89008..497b91ff43 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -21,6 +21,7 @@ protected: AP_Mission *get_mission() override; void handle_mission_set_current(AP_Mission &mission, mavlink_message_t *msg) override; + AP_ServoRelayEvents *get_servorelayevents() const override; AP_Rally *get_rally() const override; uint8_t sysid_my_gcs() const override;