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

commander: set home position on takeoff

Browse Source 
instead of when arming and on takeoff.

It is set on the first takeoff or if there was a disarm event between the
last and the current takeoff.
This commit is contained in:
Beat Küng 2020-04-07 13:12:13 +02:00 committed by Julian Oes
parent 80f0892de5
commit 6b0a2649c0
2 changed files with 11 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
src/modules/commander/Commander.cpp
View File

@ -2145,15 +2145,15 @@ Commander::run()
if (!_home_pub.get().manual_home) {
const vehicle_local_position_s &local_position = _local_position_sub.get();
if (armed.armed) {
if ((!_was_armed || (_was_landed && !_land_detector.landed)) &&
(hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
// set the home position when taking off, but only if we were previously disarmed
// and at least 500 ms from commander start spent to avoid setting home on in-air restart
if (_should_set_home_on_takeoff && _was_landed && !_land_detector.landed &&
(hrt_elapsed_time(&_boot_timestamp) > INAIR_RESTART_HOLDOFF_INTERVAL)) {
_should_set_home_on_takeoff = false;
set_home_position();
}
/* update home position on arming if at least 500 ms from commander start spent to avoid setting home on in-air restart */
set_home_position();
}
} else {
if (!armed.armed) {
if (status_flags.condition_home_position_valid) {
if (_land_detector.landed && local_position.xy_valid && local_position.z_valid) {
/* distance from home */
@ -2199,6 +2199,8 @@ Commander::run()
_param_flight_uuid.commit_no_notification();
_last_disarmed_timestamp = hrt_absolute_time();
_should_set_home_on_takeoff = true;
}
}

1
src/modules/commander/Commander.hpp
View File

@ -365,6 +365,7 @@ private:
bool _was_armed{false};
bool _failsafe_old{false}; ///< check which state machines for changes, clear "changed" flag
bool _have_taken_off_since_arming{false};
bool _should_set_home_on_takeoff{true};
bool _flight_termination_printed{false};
main_state_t _main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL};