forked from Archive/PX4-Autopilot
mc_att_control: adapt setpoint on estimation reset
There was a gap where the attitude controller already used the estimate with a new reference but the last known attitude setpoint was still based on the old reference. This leads to a big glitch on reset because until the attitude setpoint gets updated the error is wrong and as large as the attitude delta of the reset.
This commit is contained in:
parent
cf658494ad
commit
12ee75700a
|
@ -77,6 +77,13 @@ public:
|
|||
*/
|
||||
void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _yawspeed_setpoint = yawspeed_setpoint; }
|
||||
|
||||
/**
|
||||
* Adjust last known attitude setpoint by a delta rotation
|
||||
* Optional use to avoid glitches when attitude estimate reference e.g. heading changes.
|
||||
* @param q_delta delta rotation to apply
|
||||
*/
|
||||
void adaptAttitudeSetpoint(const matrix::Quatf &q_delta) { _attitude_setpoint_q = q_delta * _attitude_setpoint_q; }
|
||||
|
||||
/**
|
||||
* Run one control loop cycle calculation
|
||||
* @param q estimation of the current vehicle attitude unit quaternion
|
||||
|
|
|
@ -286,7 +286,9 @@ MulticopterAttitudeControl::Run()
|
|||
// 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
|
||||
_man_yaw_sp += Eulerf(Quatf(_v_att.delta_q_reset)).psi();
|
||||
const Quatf delta_q_reset(_v_att.delta_q_reset);
|
||||
_man_yaw_sp += Eulerf(delta_q_reset).psi();
|
||||
_attitude_control.adaptAttitudeSetpoint(delta_q_reset);
|
||||
}
|
||||
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
|
Loading…
Reference in New Issue