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

mc_att_control: only apply quat reset if estimate is newer than setpoint

Browse Source
This commit is contained in:
Daniel Agar
2022-04-25 21:25:29 -04:00
parent 23c89429ac
commit 3d590af115
3 changed files with 40 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/AttitudeControl/AttitudeControl.hpp
+11 -2
View File
@@ -75,14 +75,23 @@ public:
* @param qd desired vehicle attitude setpoint
* @param yawspeed_setpoint [rad/s] yaw feed forward angular rate in world frame
*/
void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint) { _attitude_setpoint_q = qd; _attitude_setpoint_q.normalize(); _yawspeed_setpoint = yawspeed_setpoint; }
void setAttitudeSetpoint(const matrix::Quatf &qd, const float yawspeed_setpoint)
{
_attitude_setpoint_q = qd;
_attitude_setpoint_q.normalize();
_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; }
void adaptAttitudeSetpoint(const matrix::Quatf &q_delta)
{
_attitude_setpoint_q = q_delta * _attitude_setpoint_q;
_attitude_setpoint_q.normalize();
}
/**
* Run one control loop cycle calculation