Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 04:06:33 +08:00
Code Issues Actions 9 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
+12 -6
View File
@@ -354,7 +354,12 @@ 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
if (mc_att_sp_updated || fw_att_sp_updated) {
_vtol_type->update_vtol_state(); _vtol_type->update_vtol_state();
// reset transition command if not auto control // reset transition command if not auto control
@@ -372,6 +377,9 @@ VtolAttitudeControl::Run()
_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()) {
@@ -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,10 +422,10 @@ 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(); _vtol_type->update_mc_state();
_v_att_sp_pub.publish(_v_att_sp); _v_att_sp_pub.publish(_v_att_sp);
}
break; break;
@@ -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);
} }