diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index a281cec56b..abca6bd455 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -945,7 +945,9 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) mavlink_set_home_position_t packet; mavlink_msg_set_home_position_decode(msg, &packet); if ((packet.latitude == 0) && (packet.longitude == 0) && (packet.altitude == 0)) { - sub.set_home_to_current_location(true); + if (!sub.set_home_to_current_location(true)) { + // ignore this failure + } } else { // sanity check location if (!check_latlng(packet.latitude, packet.longitude)) { @@ -958,7 +960,9 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg) if (sub.far_from_EKF_origin(new_home_loc)) { break; } - sub.set_home(new_home_loc, true); + if (!sub.set_home(new_home_loc, true)) { + // silently ignored + } } break; } diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h index cb88c49b96..ef6ca90e8c 100644 --- a/ArduSub/GCS_Mavlink.h +++ b/ArduSub/GCS_Mavlink.h @@ -32,8 +32,8 @@ protected: bool vehicle_initialised() const override; - bool set_home_to_current_location(bool lock) override; - bool set_home(const Location& loc, bool lock) override; + bool set_home_to_current_location(bool lock) override WARN_IF_UNUSED; + bool set_home(const Location& loc, bool lock) override WARN_IF_UNUSED; private: diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h index 04a78a5bee..8a5f0d4182 100644 --- a/ArduSub/Sub.h +++ b/ArduSub/Sub.h @@ -507,8 +507,8 @@ private: void userhook_SuperSlowLoop(); void update_home_from_EKF(); void set_home_to_current_location_inflight(); - bool set_home_to_current_location(bool lock); - bool set_home(const Location& loc, bool lock); + bool set_home_to_current_location(bool lock) WARN_IF_UNUSED; + bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED; bool far_from_EKF_origin(const Location& loc); void exit_mission(); bool verify_loiter_unlimited(); diff --git a/ArduSub/commands.cpp b/ArduSub/commands.cpp index f11fc36760..f77af7ba51 100644 --- a/ArduSub/commands.cpp +++ b/ArduSub/commands.cpp @@ -13,7 +13,9 @@ void Sub::update_home_from_EKF() set_home_to_current_location_inflight(); } else { // move home to current ekf location (this will set home_state to HOME_SET) - set_home_to_current_location(false); + if (!set_home_to_current_location(false)) { + // ignore this failure + } } } @@ -25,7 +27,9 @@ void Sub::set_home_to_current_location_inflight() if (inertial_nav.get_location(temp_loc)) { const struct Location &ekf_origin = inertial_nav.get_origin(); temp_loc.alt = ekf_origin.alt; - set_home(temp_loc, false); + if (!set_home(temp_loc, false)) { + // ignore this failure + } } } @@ -65,7 +69,9 @@ bool Sub::set_home(const Location& loc, bool lock) const bool home_was_set = ahrs.home_is_set(); // set ahrs home (used for RTL) - ahrs.set_home(loc); + if (!ahrs.set_home(loc)) { + return false; + } // init inav and compass declination if (!home_was_set) { diff --git a/ArduSub/commands_logic.cpp b/ArduSub/commands_logic.cpp index a7b18d84f0..44218fd18b 100644 --- a/ArduSub/commands_logic.cpp +++ b/ArduSub/commands_logic.cpp @@ -765,10 +765,14 @@ void Sub::do_change_speed(const AP_Mission::Mission_Command& cmd) void Sub::do_set_home(const AP_Mission::Mission_Command& cmd) { if (cmd.p1 == 1 || (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0)) { - set_home_to_current_location(false); + if (!set_home_to_current_location(false)) { + // silently ignore this failure + } } else { if (!far_from_EKF_origin(cmd.content.location)) { - set_home(cmd.content.location, false); + if (!set_home(cmd.content.location, false)) { + // silently ignore this failure + } } } } diff --git a/ArduSub/motors.cpp b/ArduSub/motors.cpp index 459f760b14..8086c4c67f 100644 --- a/ArduSub/motors.cpp +++ b/ArduSub/motors.cpp @@ -52,7 +52,9 @@ bool Sub::init_arm_motors(AP_Arming::ArmingMethod method) // Log_Write_Event(DATA_EKF_ALT_RESET); } else if (ahrs.home_is_set() && !ahrs.home_is_locked()) { // Reset home position if it has already been set before (but not locked) - set_home_to_current_location(false); + if (!set_home_to_current_location(false)) { + // ignore this failure + } } // enable gps velocity based centrefugal force compensation