From ae6adbea1edd06597df6df40cc7419e8774cf61b Mon Sep 17 00:00:00 2001 From: Johan Jansen Date: Wed, 18 Feb 2015 12:36:10 +0100 Subject: mc_pos_control: Fix yaw in PosHold and reset yaw setpoints --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 70 ++++++++++++---------- 1 file changed, 39 insertions(+), 31 deletions(-) (limited to 'src/modules/mc_pos_control') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 5f76b714c..7f87e3532 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -73,8 +73,7 @@ #include #include #include -// #include -// #include + #include #include #include @@ -746,8 +745,7 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f } } -void -MulticopterPositionControl::control_auto(float dt) +void MulticopterPositionControl::control_auto(float dt) { if (!_mode_auto) { _mode_auto = true; @@ -924,7 +922,7 @@ MulticopterPositionControl::task_main() bool reset_int_z = true; bool reset_int_z_manual = false; bool reset_int_xy = true; - bool reset_yaw_sp = false; + bool reset_yaw_sp = true; bool was_armed = false; hrt_abstime t_prev = 0; @@ -968,15 +966,12 @@ MulticopterPositionControl::task_main() _reset_alt_sp = true; reset_int_z = true; reset_int_xy = true; + reset_yaw_sp = true; } + //Update previous arming state was_armed = _control_mode.flag_armed; - /* check if should reset yaw setpoint for manual attitude control */ - if(!_arming.armed || !_control_mode.flag_control_manual_enabled || (!_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_manual_enabled)) { - reset_yaw_sp = true; - } - update_ref(); if (_control_mode.flag_control_altitude_enabled || @@ -1137,7 +1132,7 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.tilt_max_land; @@ -1350,40 +1345,53 @@ MulticopterPositionControl::task_main() reset_int_xy = true; } - if(!_control_mode.flag_control_velocity_enabled) { - - /* generate attitude setpoint from manual controls */ - if(_control_mode.flag_control_manual_enabled) { - /* move yaw setpoint */ - float yaw_sp_move_rate = _manual.r * _params.man_yaw_max; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); - float yaw_offs_max = _params.man_yaw_max / _params.mc_att_yaw_p; + // 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 + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp.yaw_body = _att.yaw; + } + + // 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; + + _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); float yaw_offs = _wrap_pi(_att_sp.yaw_body - _att.yaw); - if (yaw_offs < - yaw_offs_max) { - _att_sp.yaw_body = _wrap_pi(_att.yaw - yaw_offs_max); + if (yaw_offs < - YAW_OFFSET_MAX) { + _att_sp.yaw_body = _wrap_pi(_att.yaw - YAW_OFFSET_MAX); - } else if (yaw_offs > yaw_offs_max) { - _att_sp.yaw_body = _wrap_pi(_att.yaw + yaw_offs_max); - } + } else if (yaw_offs > YAW_OFFSET_MAX) { + _att_sp.yaw_body = _wrap_pi(_att.yaw + YAW_OFFSET_MAX); + } + } + //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; - _att_sp.yaw_sp_move_rate = yaw_sp_move_rate; - _att_sp.thrust = _control_mode.flag_control_altitude_enabled ? _att_sp.thrust : _manual.z; } - /* reset yaw setpoint to current position if needed */ - if (reset_yaw_sp) { - reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; + //Control climb rate directly if no aiding altitude controller is active + if(!_control_mode.flag_control_climb_rate_enabled) { + _att_sp.thrust = _manual.z; } + //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)); _att_sp.timestamp = hrt_absolute_time(); } - /* publish attitude setpoint */ + else { + reset_yaw_sp = true; + } + + // publish attitude setpoint if (_att_sp_pub > 0) { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); -- cgit v1.2.3