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

vtol_att_control: avoid using non-recent attitude setpoint during beginning

Browse Source 
of transition

Signed-off-by: RomanBapst <bapstroman@gmail.com>
This commit is contained in:
RomanBapst
2022-09-20 16:50:07 +02:00
committed by Silvan Fuhrer
parent 5aa873aed5
commit 2a6cd4a409
1 changed files with 7 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/vtol_att_control_main.cpp
+7 -2
View File
@@ -322,6 +322,11 @@ VtolAttitudeControl::Run()
const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp); const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp); const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
// for transition code to publish an attitude setpoint require both mc and fw virtual attitude setpoint to not contain data older than one second.
// this prevents either topic from being used with old data at the time when we switch into transition mode
const bool mc_and_fw_att_sp_are_recent = _mc_virtual_att_sp.timestamp > (now - 1_s)
&& _fw_virtual_att_sp.timestamp > (now - 1_s);
// 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(); _vtol_type->update_vtol_state();
@@ -331,7 +336,7 @@ VtolAttitudeControl::Run()
// vehicle is doing a transition to FW // vehicle is doing a transition to FW
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW; _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_FW;
if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) { if (mc_and_fw_att_sp_are_recent && (mc_att_sp_updated || fw_att_sp_updated)) {
_vtol_type->update_transition_state(); _vtol_type->update_transition_state();
_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp); _vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
} }
@@ -342,7 +347,7 @@ VtolAttitudeControl::Run()
// vehicle is doing a transition to MC // vehicle is doing a transition to MC
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC; _vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_TRANSITION_TO_MC;
if (!_vtol_type->was_in_trans_mode() || mc_att_sp_updated || fw_att_sp_updated) { if (mc_and_fw_att_sp_are_recent && (mc_att_sp_updated || fw_att_sp_updated)) {
_vtol_type->update_transition_state(); _vtol_type->update_transition_state();
_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp); _vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
} }