diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 08cf70f91a..0408e95c07 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -620,55 +620,11 @@ MAV_RESULT GCS_MAVLINK_Sub::handle_command_do_set_roi(const Location &roi_loc) return MAV_RESULT_ACCEPTED; } -MAV_RESULT GCS_MAVLINK_Sub::handle_command_int_packet(const mavlink_command_int_t &packet) -{ - switch (packet.command) { - - case MAV_CMD_DO_SET_HOME: { - // assume failure - if (is_equal(packet.param1, 1.0f)) { - // if param1 is 1, use current location - if (sub.set_home_to_current_location(true)) { - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; - } - // ensure param1 is zero - if (!is_zero(packet.param1)) { - return MAV_RESULT_FAILED; - } - // check frame type is supported - if (packet.frame != MAV_FRAME_GLOBAL && - packet.frame != MAV_FRAME_GLOBAL_INT && - packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT && - packet.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { - return MAV_RESULT_FAILED; - } - // sanity check location - if (!check_latlng(packet.x, packet.y)) { - return MAV_RESULT_FAILED; - } - Location new_home_loc {}; - new_home_loc.lat = packet.x; - new_home_loc.lng = packet.y; - new_home_loc.alt = packet.z * 100; - // handle relative altitude - if (packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT || packet.frame == MAV_FRAME_GLOBAL_RELATIVE_ALT_INT) { - if (!AP::ahrs().home_is_set()) { - // cannot use relative altitude if home is not set - return MAV_RESULT_FAILED; - } - new_home_loc.alt += sub.ahrs.get_home().alt; - } - if (sub.set_home(new_home_loc, true)) { - return MAV_RESULT_ACCEPTED; - } - return MAV_RESULT_FAILED; - } - - default: - return GCS_MAVLINK::handle_command_int_packet(packet); - } +bool GCS_MAVLINK_Sub::set_home_to_current_location(bool lock) { + return sub.set_home_to_current_location(lock); +} +bool GCS_MAVLINK_Sub::set_home(const Location& loc, bool lock) { + return sub.set_home(loc, lock); } diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index 9ebf6a5f12..cb88c49b96 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -22,7 +22,6 @@ protected: MAV_RESULT handle_command_do_set_roi(const Location &roi_loc) override; MAV_RESULT _handle_command_preflight_calibration_baro() override; MAV_RESULT _handle_command_preflight_calibration(const mavlink_command_long_t &packet) override; - MAV_RESULT handle_command_int_packet(const mavlink_command_int_t &packet) override; MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet) override; // override sending of scaled_pressure3 to send on-board temperature: @@ -33,6 +32,9 @@ protected: bool vehicle_initialised() const override; + bool set_home_to_current_location(bool lock) override; + bool set_home(const Location& loc, bool lock) override; + private: void handleMessage(mavlink_message_t * msg) override;