|
|
|
@ -1380,16 +1380,16 @@ MulticopterPositionControl::task_main()
@@ -1380,16 +1380,16 @@ MulticopterPositionControl::task_main()
|
|
|
|
|
reset_int_xy = true; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// generate attitude setpoint from manual controls
|
|
|
|
|
/* generate attitude setpoint from manual controls */ |
|
|
|
|
if(_control_mode.flag_control_manual_enabled && _control_mode.flag_control_attitude_enabled) { |
|
|
|
|
|
|
|
|
|
// reset yaw setpoint to current position if needed
|
|
|
|
|
/* reset yaw setpoint to current position if needed */ |
|
|
|
|
if (reset_yaw_sp) { |
|
|
|
|
reset_yaw_sp = false; |
|
|
|
|
_att_sp.yaw_body = _att.yaw; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// do not move yaw while arming
|
|
|
|
|
/* do not move yaw while arming */ |
|
|
|
|
else if (_manual.z > 0.1f) |
|
|
|
|
{ |
|
|
|
|
const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p; |
|
|
|
@ -1405,19 +1405,19 @@ MulticopterPositionControl::task_main()
@@ -1405,19 +1405,19 @@ MulticopterPositionControl::task_main()
|
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//Control roll and pitch directly if we no aiding velocity controller is active
|
|
|
|
|
/* control roll and pitch directly if we 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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//Control climb rate directly if no aiding altitude controller is active
|
|
|
|
|
/* control throttle directly if no climb rate controller is active */ |
|
|
|
|
if (!_control_mode.flag_control_climb_rate_enabled) { |
|
|
|
|
_att_sp.thrust = math::min(_manual.z, _params.thr_max); |
|
|
|
|
_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
//Construct attitude setpoint rotation matrix
|
|
|
|
|
/* construct attitude setpoint rotation matrix */ |
|
|
|
|
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)); |
|
|
|
|