fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 05:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

VTOL: in update_transition_state(): check that needed virtual attitude setpoints are recent

Browse Source 
This is to prevent that that data from non-recent publications of the virtual
attitude setpoints are used in the transition code.
It removes the previous implementation where the update_transition_state()
method was only run when both mc and fw virtual att sp where recent, independetly
of whether both are used or not.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-09-29 16:01:43 +02:00
parent dd2e6bd416
commit 9c5a423753
4 changed files with 35 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/standard.cpp
+13 -2
View File
@@ -203,18 +203,29 @@ void Standard::update_vtol_state()
void Standard::update_transition_state()
{
const hrt_abstime now = hrt_absolute_time();
float mc_weight = 1.0f;
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
float time_since_trans_start = (float)(now - _vtol_schedule.transition_start) * 1e-6f;
VtolType::update_transition_state();
// we get attitude setpoint from a multirotor flighttask if altitude is controlled.
// we get attitude setpoint from a multirotor flighttask if climbrate is controlled.
// in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input.
if (_v_control_mode->flag_control_climb_rate_enabled) {
// we need the incoming (virtual) attitude setpoints (both mc and fw) to be recent, otherwise return (means the previous setpoint stays active)
if (_mc_virtual_att_sp->timestamp < (now - 1_s) && _fw_virtual_att_sp->timestamp < (now - 1_s)) {
return;
}
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_v_att_sp->roll_body = _fw_virtual_att_sp->roll_body;
} else {
// we need a recent incoming (fw virtual) attitude setpoint, otherwise return (means the previous setpoint stays active)
if (_fw_virtual_att_sp->timestamp < (now - 1_s)) {
return;
}
memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
_v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0];
}