From 74868f8c2bb037cdd2085ee0bcf3649d51c32707 Mon Sep 17 00:00:00 2001 From: sanderux Date: Tue, 19 Dec 2017 01:03:31 +0100 Subject: [PATCH] Reset home position when landed and disarmed --- msg/home_position.msg | 1 + src/modules/commander/commander.cpp | 15 ++++++++++++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/msg/home_position.msg b/msg/home_position.msg index a7d1899985..2d942f010c 100644 --- a/msg/home_position.msg +++ b/msg/home_position.msg @@ -15,3 +15,4 @@ float32 direction_z # Takeoff direction in NED Z bool valid_alt # true when the altitude has been set bool valid_hpos # true when the latitude and longitude have been set +bool manual_home # true when home position was set manually diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74b7c07a97..4e47eeeb45 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3122,6 +3122,13 @@ int commander_thread_main(int argc, char *argv[]) /* Get current timestamp */ const hrt_abstime now = hrt_absolute_time(); + /* distance from home */ + + float home_dist_xy = -1.0f; + float home_dist_z = -1.0f; + get_distance_to_point_global_wgs84(_home.lat, _home.lon, _home.alt, global_position.lat, global_position.lon, + global_position.alt, &home_dist_xy, &home_dist_z); + /* First time home position update - but only if disarmed */ if (!status_flags.condition_home_position_valid && !armed.armed) { commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false); @@ -3129,11 +3136,17 @@ int commander_thread_main(int argc, char *argv[]) /* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */ else if (((!was_armed && armed.armed) || (was_landed && !land_detector.landed)) && - (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { + (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) && !_home.manual_home) { commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false); } + /* update when disarmed, landed and moved away from current home position */ + else if (status_flags.condition_home_position_valid && !armed.armed && land_detector.landed && !_home.manual_home && + (home_dist_xy > global_position.epv * 2 || home_dist_z > global_position.eph * 2)) { + commander_set_home_position(home_pub, _home, local_position, global_position, attitude, false); + } + /* Set home position altitude to EKF origin height if home is not set and the EKF has a global origin. * This allows home atitude to be used in the calculation of height above takeoff location when GPS * use has commenced after takeoff. */