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

abort front transition in vtol module instead of in navigator/rtl.cpp

Browse Source
This commit is contained in:
thomas
2022-10-24 13:55:41 +02:00
committed by Silvan Fuhrer
parent 26c36a96f2
commit f9f466854b
3 changed files with 24 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_att_control_main.cpp
+18 -6
View File
@@ -105,6 +105,19 @@ VtolAttitudeControl::init()
return true;
}
void VtolAttitudeControl::vehicle_status_poll()
{
_vehicle_status_sub.copy(&_vehicle_status);
// abort front transition when RTL is triggered
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
&& _nav_state_prev != vehicle_status_s::NAVIGATION_STATE_AUTO_RTL && _vtol_type->get_mode() == mode::TRANSITION_TO_FW) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
}
_nav_state_prev = _vehicle_status.nav_state;
}
void VtolAttitudeControl::action_request_poll()
{
while (_action_request_sub.updated()) {
@@ -132,8 +145,6 @@ void VtolAttitudeControl::vehicle_cmd_poll()
while (_vehicle_cmd_sub.update(&vehicle_command)) {
if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) {
vehicle_status_s vehicle_status{};
_vehicle_status_sub.copy(&vehicle_status);
uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
@@ -141,10 +152,10 @@ void VtolAttitudeControl::vehicle_cmd_poll()
// deny transition from MC to FW in Takeoff, Land, RTL and Orbit
if (transition_command_param1 == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW &&
(vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
(_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
|| _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT)) {
result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
@@ -309,6 +320,7 @@ VtolAttitudeControl::Run()
_airspeed_validated_sub.update(&_airspeed_validated);
_tecs_status_sub.update(&_tecs_status);
_land_detected_sub.update(&_land_detected);
vehicle_status_poll();
action_request_poll();
vehicle_cmd_poll();