void
MulticopterPositionControl::generate_attitude_setpoint(float dt)
{
/* reset yaw setpoint to current position if needed */
if (_reset_yaw_sp) {
_reset_yaw_sp = false;
_att_sp.yaw_body = _yaw;
}
/* do not move yaw while sitting on the ground */
else if (!_vehicle_land_detected.landed &&
!(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) {
/* we want to know the real constraint, and global overrides manual */
const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max :
_params.global_yaw_max;
const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p;
_att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max;
float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(yaw_target - _yaw);
// If the yaw offset became too big for the system to track stop
// shifting it, only allow if it would make the offset smaller again.
if (fabsf(yaw_offs) < yaw_offset_max ||
(_att_sp.yaw_sp_move_rate > 0 && yaw_offs < 0) ||
(_att_sp.yaw_sp_move_rate < 0 && yaw_offs > 0)) {
_att_sp.yaw_body = yaw_target;
}
}
/* control throttle directly if no climb rate controller is active */
if (!_control_mode.flag_control_climb_rate_enabled) {
float thr_val = throttle_curve(_manual.z, _params.thr_hover);
_att_sp.thrust = math::min(thr_val, _manual_thr_max.get());
/* enforce minimum throttle if not landed */
if (!_vehicle_land_detected.landed) {
_att_sp.thrust = math::max(_att_sp.thrust, _manual_thr_min.get());
}
}
/* 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;
/* 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);
// 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;
// use the formula z_roll_pitch_sp = R_tilt * [0;0;1]
// R_tilt is computed from_euler; only true if cos(roll) not equal zero
// -> valid if roll is not +-pi/2;
_att_sp.roll_body = -asinf(z_roll_pitch_sp(1));
_att_sp.pitch_body = atan2f(z_roll_pitch_sp(0), z_roll_pitch_sp(2));
}
/* copy quaternion setpoint to attitude setpoint topic */
matrix::Quatf q_sp = matrix::Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
_att_sp.q_d_valid = true;
}
if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
!_vehicle_land_detected.landed) {
_att_sp.landing_gear = 1.0f;
} else if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
_att_sp.landing_gear = -1.0f;
}
_att_sp.timestamp = hrt_absolute_time();
}