forked from Archive/PX4-Autopilot
tailsitter: correctly use roll/pitch inputs when optimal recovery is enabled
This commit is contained in:
parent
8c9f4e8ab8
commit
e1e410783a
|
@ -2019,47 +2019,48 @@ MulticopterPositionControl::task_main()
|
|||
}
|
||||
}
|
||||
|
||||
/* control roll and pitch directly if no aiding velocity controller is active
|
||||
* and only if optimal recovery is not used */
|
||||
if (!_control_mode.flag_control_velocity_enabled
|
||||
&& _params.opt_recover <= 0) {
|
||||
math::Matrix<3, 3> R_sp;
|
||||
/* control roll and pitch directly if no aiding velocity controller is active */
|
||||
if (!_control_mode.flag_control_velocity_enabled) {
|
||||
_att_sp.roll_body = _manual.y * _params.man_roll_max;
|
||||
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
|
||||
|
||||
// construct attitude setpoint rotation matrix. modify the setpoints for roll
|
||||
// and pitch such that they reflect the user's intention even if a yaw error
|
||||
// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
|
||||
// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
|
||||
// the user's view as the euler angle sequence uses the yaw setpoint and not the current
|
||||
// heading of the vehicle.
|
||||
/* only if optimal recovery is not used, modify roll/pitch */
|
||||
if (_params.opt_recover <= 0) {
|
||||
// construct attitude setpoint rotation matrix. modify the setpoints for roll
|
||||
// and pitch such that they reflect the user's intention even if a yaw error
|
||||
// (yaw_sp - yaw) is present. In the presence of a yaw error constructing a rotation matrix
|
||||
// from the pure euler angle setpoints will lead to unexpected attitude behaviour from
|
||||
// the user's view as the euler angle sequence uses the yaw setpoint and not the current
|
||||
// heading of the vehicle.
|
||||
|
||||
// calculate our current yaw error
|
||||
float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw);
|
||||
// calculate our current yaw error
|
||||
float yaw_error = _wrap_pi(_att_sp.yaw_body - _yaw);
|
||||
|
||||
// compute the vector obtained by rotating a z unit vector by the rotation
|
||||
// given by the roll and pitch commands of the user
|
||||
math::Vector<3> zB = {0, 0, 1};
|
||||
math::Matrix<3, 3> R_sp_roll_pitch;
|
||||
R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
|
||||
math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;
|
||||
// compute the vector obtained by rotating a z unit vector by the rotation
|
||||
// given by the roll and pitch commands of the user
|
||||
math::Vector<3> zB = {0, 0, 1};
|
||||
math::Matrix<3, 3> R_sp_roll_pitch;
|
||||
R_sp_roll_pitch.from_euler(_att_sp.roll_body, _att_sp.pitch_body, 0);
|
||||
math::Vector<3> z_roll_pitch_sp = R_sp_roll_pitch * zB;
|
||||
|
||||
|
||||
// transform the vector into a new frame which is rotated around the z axis
|
||||
// by the current yaw error. this vector defines the desired tilt when we look
|
||||
// into the direction of the desired heading
|
||||
math::Matrix<3, 3> R_yaw_correction;
|
||||
R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
|
||||
z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
|
||||
// transform the vector into a new frame which is rotated around the z axis
|
||||
// by the current yaw error. this vector defines the desired tilt when we look
|
||||
// into the direction of the desired heading
|
||||
math::Matrix<3, 3> R_yaw_correction;
|
||||
R_yaw_correction.from_euler(0.0f, 0.0f, -yaw_error);
|
||||
z_roll_pitch_sp = R_yaw_correction * z_roll_pitch_sp;
|
||||
|
||||
// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
|
||||
// to calculate the new desired roll and pitch angles
|
||||
// R_tilt can be written as a function of the new desired roll and pitch
|
||||
// angles. we get three equations and have to solve for 2 unknowns
|
||||
float pitch_new = asinf(z_roll_pitch_sp(0));
|
||||
float roll_new = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2));
|
||||
// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
|
||||
// to calculate the new desired roll and pitch angles
|
||||
// R_tilt can be written as a function of the new desired roll and pitch
|
||||
// angles. we get three equations and have to solve for 2 unknowns
|
||||
_att_sp.pitch_body = asinf(z_roll_pitch_sp(0));
|
||||
_att_sp.roll_body = -atan2f(z_roll_pitch_sp(1), z_roll_pitch_sp(2));
|
||||
}
|
||||
|
||||
R_sp.from_euler(roll_new, pitch_new, _att_sp.yaw_body);
|
||||
math::Matrix<3, 3> R_sp;
|
||||
R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
|
||||
memcpy(&_att_sp.R_body[0], R_sp.data, sizeof(_att_sp.R_body));
|
||||
|
||||
/* copy quaternion setpoint to attitude setpoint topic */
|
||||
|
|
Loading…
Reference in New Issue