fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 07:30:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Reset home position when landed and disarmed

Browse Source
This commit is contained in:
sanderux
2017-12-19 01:03:31 +01:00
committed by Sander Smeets
parent 6172315cf7
commit 74868f8c2b
2 changed files with 15 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+14 -1
View File
@@ -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. */