dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mc_attitude_control: move attitude setpoint pulling to right before usage 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2024-01-22 16:51:12 +01:00 committed by Daniel Agar
parent e5cfbbb1ee
commit 982c998ab9
1 changed files with 30 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -226,34 +226,6 @@ MulticopterAttitudeControl::Run()
const Quatf q{v_att.q};
// Check for new attitude setpoint
if (_vehicle_attitude_setpoint_sub.updated()) {
vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint)
&& (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) {
_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
_last_attitude_setpoint = vehicle_attitude_setpoint.timestamp;
}
}
// Check for a heading reset
if (_quat_reset_counter != v_att.quat_reset_counter) {
const Quatf delta_q_reset(v_att.delta_q_reset);
// for stabilized attitude generation only extract the heading change from the delta quaternion
_man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi());
if (v_att.timestamp > _last_attitude_setpoint) {
// adapt existing attitude setpoint unless it was generated after the current attitude estimate
_attitude_control.adaptAttitudeSetpoint(delta_q_reset);
}
_quat_reset_counter = v_att.quat_reset_counter;
}
/* check for updates in other topics */
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
_vehicle_control_mode_sub.update(&_vehicle_control_mode);
@ -295,7 +267,8 @@ MulticopterAttitudeControl::Run()
// vehicle is a tailsitter in transition mode
const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode);
bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
const bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering
|| is_tailsitter_transition);
if (run_att_ctrl) {
@ -313,6 +286,34 @@ MulticopterAttitudeControl::Run()
_man_pitch_input_filter.reset(0.f);
}
// Check for new attitude setpoint
if (_vehicle_attitude_setpoint_sub.updated()) {
vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
if (_vehicle_attitude_setpoint_sub.copy(&vehicle_attitude_setpoint)
&& (vehicle_attitude_setpoint.timestamp > _last_attitude_setpoint)) {
_attitude_control.setAttitudeSetpoint(Quatf(vehicle_attitude_setpoint.q_d), vehicle_attitude_setpoint.yaw_sp_move_rate);
_thrust_setpoint_body = Vector3f(vehicle_attitude_setpoint.thrust_body);
_last_attitude_setpoint = vehicle_attitude_setpoint.timestamp;
}
}
// Check for a heading reset
if (_quat_reset_counter != v_att.quat_reset_counter) {
const Quatf delta_q_reset(v_att.delta_q_reset);
// for stabilized attitude generation only extract the heading change from the delta quaternion
_man_yaw_sp = wrap_pi(_man_yaw_sp + Eulerf(delta_q_reset).psi());
if (v_att.timestamp > _last_attitude_setpoint) {
// adapt existing attitude setpoint unless it was generated after the current attitude estimate
_attitude_control.adaptAttitudeSetpoint(delta_q_reset);
}
_quat_reset_counter = v_att.quat_reset_counter;
}
Vector3f rates_sp = _attitude_control.update(q);
const hrt_abstime now = hrt_absolute_time();