forked from Archive/PX4-Autopilot
Multicopter attitude setpoint flag absolute yaw control
This allows to disable absolute yaw control while still using the attitude quaternion.
This commit is contained in:
parent
0b74076265
commit
3684cfee74
|
@ -4,7 +4,8 @@ float32 roll_body # body angle in NED frame (can be NaN for FW)
|
|||
float32 pitch_body # body angle in NED frame (can be NaN for FW)
|
||||
float32 yaw_body # body angle in NED frame (can be NaN for FW)
|
||||
|
||||
float32 yaw_sp_move_rate # rad/s (commanded by user)
|
||||
float32 yaw_sp_move_rate # feed-forward NED yaw angular rate in rad/s
|
||||
bool absolute_heading_valid # True if absolute heading of attitude quaternion should be tracked
|
||||
|
||||
# For quaternion-based attitude control
|
||||
float32[4] q_d # Desired quaternion for quaternion control
|
||||
|
|
|
@ -246,6 +246,9 @@ MulticopterAttitudeControl::Run()
|
|||
if (_vehicle_attitude_setpoint_sub.updated()) {
|
||||
vehicle_attitude_setpoint_s vehicle_attitude_setpoint;
|
||||
_vehicle_attitude_setpoint_sub.update(&vehicle_attitude_setpoint);
|
||||
const float yaw_weight = vehicle_attitude_setpoint.absolute_heading_valid ? _param_mc_yaw_weight.get() : 0.f;
|
||||
_attitude_control.setProportionalGain(Vector3f(_param_mc_roll_p.get(), _param_mc_pitch_p.get(), _param_mc_yaw_p.get()),
|
||||
yaw_weight);
|
||||
_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);
|
||||
}
|
||||
|
|
|
@ -120,7 +120,6 @@ bool PositionControl::update(const float dt)
|
|||
_velocityControl(dt);
|
||||
|
||||
_yawspeed_sp = PX4_ISFINITE(_yawspeed_sp) ? _yawspeed_sp : 0.f;
|
||||
_yaw_sp = PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw; // TODO: better way to disable yaw control
|
||||
|
||||
return valid && _updateSuccessful();
|
||||
}
|
||||
|
@ -240,6 +239,7 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
|
|||
|
||||
void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint) const
|
||||
{
|
||||
ControlMath::thrustToAttitude(_thr_sp, _yaw_sp, attitude_setpoint);
|
||||
ControlMath::thrustToAttitude(_thr_sp, PX4_ISFINITE(_yaw_sp) ? _yaw_sp : _yaw, attitude_setpoint);
|
||||
attitude_setpoint.absolute_heading_valid = PX4_ISFINITE(_yaw_sp);
|
||||
attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue