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

mc_att_control: grab attitude setpoint before processing estimate reset

Browse Source
This commit is contained in:
Matthias Grob
2020-05-26 15:07:45 +03:00
committed by Roman Bapst
parent 12ee75700a
commit e482081f91
1 changed files with 9 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control_main.cpp
+9 -8
View File
@@ -233,13 +233,6 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
void
MulticopterAttitudeControl::control_attitude()
{
if (_vehicle_attitude_setpoint_sub.updated()) {
vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
_vehicle_attitude_setpoint_sub.update(&vehicle_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);
}
_rates_sp = _attitude_control.update(Quatf(_v_att.q));
}
@@ -283,10 +276,18 @@ MulticopterAttitudeControl::Run()
if (_vehicle_attitude_sub.update(&_v_att)) {
// Check for new attitude setpoint
if (_vehicle_attitude_setpoint_sub.updated()) {
vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
_vehicle_attitude_setpoint_sub.update(&vehicle_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);
}
// Check for a heading reset
if (prev_quat_reset_counter != _v_att.quat_reset_counter) {
// we only extract the heading change from the delta quaternion
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 += Eulerf(delta_q_reset).psi();
_attitude_control.adaptAttitudeSetpoint(delta_q_reset);
}