fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 02:31:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added VTOL Takeoff navigation mode (#19027)

Browse Source 
* Commander: added support for MAIN_STATE_AUTO_VTOL_TAKEOFF
* navigator: added support for vtol_takeoff navigation mode
This commit is contained in:
Roman Bapst
2022-02-15 14:56:57 +01:00
committed by GitHub
parent 374bcb105a
commit 6de5d24e00
13 changed files with 388 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+25 -2
View File
@@ -629,9 +629,12 @@ static inline navigation_mode_t navigation_mode(uint8_t main_state)
case commander_state_s::MAIN_STATE_AUTO_PRECLAND: return navigation_mode_t::auto_precland;
case commander_state_s::MAIN_STATE_ORBIT: return navigation_mode_t::orbit;
case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF: return navigation_mode_t::auto_vtol_takeoff;
}
static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::orbit, "enum definition mismatch");
static_assert(commander_state_s::MAIN_STATE_MAX - 1 == (int)navigation_mode_t::auto_vtol_takeoff,
"enum definition mismatch");
return navigation_mode_t::unknown;
}
@@ -1149,6 +1152,24 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
break;
case vehicle_command_s::VEHICLE_CMD_NAV_VTOL_TAKEOFF:
/* ok, home set, use it to take off */
if (TRANSITION_CHANGED == main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF,
_status_flags,
_internal_state)) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else if (_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(&_mavlink_log_pub, "VTOL Takeoff denied! Please disarm and retry");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
break;
case vehicle_command_s::VEHICLE_CMD_NAV_LAND: {
if (TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LAND, _status_flags,
_internal_state)) {
@@ -2393,7 +2414,8 @@ 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)
&& (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
&& (mission_result.timestamp >= _status.nav_state_timestamp)
&& mission_result.finished) {
@@ -3332,6 +3354,7 @@ Commander::update_control_mode()
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
case vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF:
_vehicle_control_mode.flag_control_auto_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;