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

commander: auto TAKEOFF to LOITER/MISSION minor fix to avoid failsafe confusion

Browse Source 
- base on nav_state rather than main_state
This commit is contained in:
Daniel Agar
2021-02-16 13:02:14 -05:00
committed by GitHub
parent 64d0ce43b6
commit 5abf29d93c
1 changed files with 18 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+18 -23
View File
@@ -1944,11 +1944,12 @@ Commander::run()
}
/* start mission result check */
const auto prev_mission_instance_count = _mission_result_sub.get().instance_count;
if (_mission_result_sub.update()) {
if (_mission_result_sub.updated()) {
const mission_result_s &mission_result = _mission_result_sub.get();
const auto prev_mission_instance_count = mission_result.instance_count;
_mission_result_sub.update();
// if mission_result is valid for the current mission
const bool mission_result_ok = (mission_result.timestamp > _boot_timestamp)
&& (mission_result.instance_count > 0);
@@ -1956,7 +1957,6 @@ Commander::run()
_status_flags.condition_auto_mission_available = mission_result_ok && mission_result.valid;
if (mission_result_ok) {
if (_status.mission_failure != mission_result.failure) {
_status.mission_failure = mission_result.failure;
_status_changed = true;
@@ -1984,6 +1984,20 @@ Commander::run()
}
}
}
// Transition main state to loiter or auto-mission after takeoff is completed.
if (_armed.armed && !_land_detector.landed
&& (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
&& (mission_result.timestamp >= _status.nav_state_timestamp)
&& mission_result.finished) {
if ((_param_takeoff_finished_action.get() == 1) && _status_flags.condition_auto_mission_available) {
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state);
} else {
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
}
}
}
// update manual_control_setpoint before geofence (which might check sticks or switches)
@@ -2381,25 +2395,6 @@ Commander::run()
}
}
/* Reset main state to loiter or auto-mission after takeoff is completed.
* Sometimes, the mission result topic is outdated and the mission is still signaled
* as finished even though we only just started with the takeoff. Therefore, we also
* check the timestamp of the mission_result topic. */
if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
&& (_mission_result_sub.get().timestamp >= _internal_state.timestamp)
&& _mission_result_sub.get().finished) {
const bool mission_available = (_mission_result_sub.get().timestamp > _boot_timestamp)
&& (_mission_result_sub.get().instance_count > 0) && _mission_result_sub.get().valid;
if ((_param_takeoff_finished_action.get() == 1) && mission_available) {
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags, &_internal_state);
} else {
main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
}
}
/* check if we are disarmed and there is a better mode to wait in */
if (!_armed.armed) {
/* if there is no radio control but GPS lock the user might want to fly using