fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 21:43:33 +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;
src/modules/commander/px4_custom_mode.h
+2 -1
View File
@@ -63,7 +63,8 @@ enum PX4_CUSTOM_SUB_MODE_AUTO {
PX4_CUSTOM_SUB_MODE_AUTO_LAND,
PX4_CUSTOM_SUB_MODE_AUTO_RESERVED_DO_NOT_USE, // was PX4_CUSTOM_SUB_MODE_AUTO_RTGS, deleted 2020-03-05
PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET,
PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND
PX4_CUSTOM_SUB_MODE_AUTO_PRECLAND,
PX4_CUSTOM_SUB_MODE_AUTO_VTOL_TAKEOFF
};
enum PX4_CUSTOM_SUB_MODE_POSCTL {
src/modules/commander/state_machine_helper.cpp
+10 -1
View File
@@ -368,6 +368,13 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
break;
case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF:
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
ret = TRANSITION_CHANGED;
}
break;
case commander_state_s::MAIN_STATE_AUTO_MISSION:
/* need global position, home position, and a valid mission */
@@ -735,6 +742,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
break;
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
case commander_state_s::MAIN_STATE_AUTO_VTOL_TAKEOFF:
/* require local position */
@@ -769,7 +777,8 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_
set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0);
} else {
status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;
status.nav_state = internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF ?
vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF : vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF;
}
break;