From ad600fff68b1d89f855771bfb85ecf9b7b96127d Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 18 May 2018 12:59:27 +1000 Subject: [PATCH] Plane: split home-set and home-locked state --- ArduPlane/GCS_Mavlink.cpp | 3 --- ArduPlane/commands.cpp | 3 +-- ArduPlane/commands_logic.cpp | 1 - 3 files changed, 1 insertion(+), 6 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 1ec5448631..88c7f6cd48 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -810,7 +810,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) new_home_loc.alt += plane.ahrs.get_home().alt; } plane.ahrs.set_home(new_home_loc); - AP::ahrs().set_home_status(HOME_SET_NOT_LOCKED); AP::ahrs().Log_Write_Home_And_Origin(); gcs().send_home(); result = MAV_RESULT_ACCEPTED; @@ -1096,7 +1095,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) new_home_loc.lng = (int32_t)(packet.param6 * 1.0e7f); new_home_loc.alt = (int32_t)(packet.param7 * 100.0f); plane.ahrs.set_home(new_home_loc); - AP::ahrs().set_home_status(HOME_SET_NOT_LOCKED); AP::ahrs().Log_Write_Home_And_Origin(); gcs().send_home(); result = MAV_RESULT_ACCEPTED; @@ -1514,7 +1512,6 @@ void GCS_MAVLINK_Plane::handleMessage(mavlink_message_t* msg) new_home_loc.lng = packet.longitude; new_home_loc.alt = packet.altitude / 10; plane.ahrs.set_home(new_home_loc); - plane.ahrs.set_home_status(HOME_SET_NOT_LOCKED); plane.ahrs.Log_Write_Home_And_Origin(); gcs().send_home(); gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %um", diff --git a/ArduPlane/commands.cpp b/ArduPlane/commands.cpp index 3b04e37f99..da8991badb 100644 --- a/ArduPlane/commands.cpp +++ b/ArduPlane/commands.cpp @@ -110,7 +110,6 @@ void Plane::init_home() gcs().send_text(MAV_SEVERITY_INFO, "Init HOME"); ahrs.set_home(gps.location()); - ahrs.set_home_status(HOME_SET_NOT_LOCKED); ahrs.Log_Write_Home_And_Origin(); gcs().send_home(); @@ -138,7 +137,7 @@ void Plane::update_home() // significantly return; } - if (ahrs.home_status() == HOME_SET_NOT_LOCKED) { + if (ahrs.home_is_set() && !ahrs.home_is_locked()) { Location loc; if(ahrs.get_position(loc)) { ahrs.set_home(loc); diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 2553182332..0e5c479a46 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -937,7 +937,6 @@ void Plane::do_set_home(const AP_Mission::Mission_Command& cmd) init_home(); } else { ahrs.set_home(cmd.content.location); - ahrs.set_home_status(HOME_SET_NOT_LOCKED); ahrs.Log_Write_Home_And_Origin(); gcs().send_home(); gcs().send_ekf_origin();