Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 02:36:37 +08:00
Code Issues Actions 32 Packages Projects Releases Wiki Activity

mc_attitude_control: move attitude setpoint pulling to right before usage

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+30 -29
View File
@@ -226,34 +226,6 @@ MulticopterAttitudeControl::Run()
const Quatf q{v_att.q}; 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 */ /* check for updates in other topics */
_manual_control_setpoint_sub.update(&_manual_control_setpoint); _manual_control_setpoint_sub.update(&_manual_control_setpoint);
_vehicle_control_mode_sub.update(&_vehicle_control_mode); _vehicle_control_mode_sub.update(&_vehicle_control_mode);
@@ -295,7 +267,8 @@ MulticopterAttitudeControl::Run()
// vehicle is a tailsitter in transition mode // vehicle is a tailsitter in transition mode
const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_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) { if (run_att_ctrl) {
@@ -313,6 +286,34 @@ MulticopterAttitudeControl::Run()
_man_pitch_input_filter.reset(0.f); _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); Vector3f rates_sp = _attitude_control.update(q);
const hrt_abstime now = hrt_absolute_time(); const hrt_abstime now = hrt_absolute_time();