Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-29 03:36:07 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

VTOL attitude controller: run update_vtol_state only if mc or fw att sp changed, such that pusher support in hover works again

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2019-09-06 14:44:31 +02:00
committed by Roman Bapst
parent 1b2403a87f
commit 92b824e218
1 changed files with 26 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_att_control_main.cpp
+26 -20
View File
@@ -354,25 +354,33 @@ VtolAttitudeControl::Run()
_land_detected_sub.update(&_land_detected); _land_detected_sub.update(&_land_detected);
vehicle_cmd_poll(); vehicle_cmd_poll();
// check if mc and fw sp were updated
bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
// update the vtol state machine which decides which mode we are in // update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state(); if (mc_att_sp_updated || fw_att_sp_updated) {
_vtol_type->update_vtol_state();
// reset transition command if not auto control // reset transition command if not auto control
if (_v_control_mode.flag_control_manual_enabled) { if (_v_control_mode.flag_control_manual_enabled) {
if (_vtol_type->get_mode() == mode::ROTARY_WING) { if (_vtol_type->get_mode() == mode::ROTARY_WING) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
} else if (_vtol_type->get_mode() == mode::FIXED_WING) { } else if (_vtol_type->get_mode() == mode::FIXED_WING) {
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW; _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
} else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) { } else if (_vtol_type->get_mode() == mode::TRANSITION_TO_MC) {
/* We want to make sure that a mode change (manual>auto) during the back transition /* We want to make sure that a mode change (manual>auto) during the back transition
* doesn't result in an unsafe state. This prevents the instant fall back to * doesn't result in an unsafe state. This prevents the instant fall back to
* fixed-wing on the switch from manual to auto */ * fixed-wing on the switch from manual to auto */
_transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC; _transition_command = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
}
} }
} }
// check in which mode we are in and call mode specific functions // check in which mode we are in and call mode specific functions
switch (_vtol_type->get_mode()) { switch (_vtol_type->get_mode()) {
case mode::TRANSITION_TO_FW: case mode::TRANSITION_TO_FW:
@@ -384,7 +392,7 @@ VtolAttitudeControl::Run()
_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
if (_mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp)) { if (mc_att_sp_updated || fw_att_sp_updated) {
// reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept
if (!_v_control_mode.flag_armed) { if (!_v_control_mode.flag_armed) {
@@ -406,9 +414,7 @@ VtolAttitudeControl::Run()
_vtol_vehicle_status.vtol_in_trans_mode = false; _vtol_vehicle_status.vtol_in_trans_mode = false;
_vtol_vehicle_status.in_transition_to_fw = false; _vtol_vehicle_status.in_transition_to_fw = false;
// got data from mc attitude controller if (mc_att_sp_updated) {
if (_mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp)) {
// reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept // reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept
if (!_v_control_mode.flag_armed) { if (!_v_control_mode.flag_armed) {
Quatf().copyTo(_mc_virtual_att_sp.q_d); Quatf().copyTo(_mc_virtual_att_sp.q_d);
@@ -416,11 +422,11 @@ VtolAttitudeControl::Run()
Quatf().copyTo(_v_att_sp.q_d); Quatf().copyTo(_v_att_sp.q_d);
Vector3f().copyTo(_v_att_sp.thrust_body); Vector3f().copyTo(_v_att_sp.thrust_body);
} }
_vtol_type->update_mc_state();
_v_att_sp_pub.publish(_v_att_sp);
} }
_vtol_type->update_mc_state();
_v_att_sp_pub.publish(_v_att_sp);
break; break;
case mode::FIXED_WING: case mode::FIXED_WING:
@@ -429,7 +435,7 @@ VtolAttitudeControl::Run()
_vtol_vehicle_status.vtol_in_trans_mode = false; _vtol_vehicle_status.vtol_in_trans_mode = false;
_vtol_vehicle_status.in_transition_to_fw = false; _vtol_vehicle_status.in_transition_to_fw = false;
if (_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp)) { if (fw_att_sp_updated) {
_vtol_type->update_fw_state(); _vtol_type->update_fw_state();
_v_att_sp_pub.publish(_v_att_sp); _v_att_sp_pub.publish(_v_att_sp);
} }