void MulticopterPositionControl::control_manual(float dt) { _sp_move_rate.zero(); if (_control_mode.flag_control_altitude_enabled) { if(_reset_mission) { _reset_mission = false; _mode_mission = 1 ; _hover_time = 0.0 ; } float height_hover_constant=-1.0; float hover_time_constant = 20.0; switch(_mode_mission) { case 1: _sp_move_rate(2) = -0.8; if(_pos_sp(2)<=height_hover_constant) _mode_mission=2; break; case 2: _hover_time += dt; if(_hover_time>hover_time_constant) { _hover_time=0.0; _mode_mission=3; } break; case 3: _pos_sp_triplet.current.type =position_setpoint_s::SETPOINT_TYPE_LAND; break; default: /* move altitude setpoint with throttle stick */ _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); break; } } if (_control_mode.flag_control_position_enabled) { /* move position setpoint with roll/pitch stick */ _sp_move_rate(0) = _manual.x; _sp_move_rate(1) = _manual.y; } /* limit setpoint move rate */ float sp_move_norm = _sp_move_rate.length(); if (sp_move_norm > 1.0f) { _sp_move_rate /= sp_move_norm; } /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ math::Matrix<3, 3> R_yaw_sp; R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); } if (_control_mode.flag_control_position_enabled) { /* reset position setpoint to current position if needed */ reset_pos_sp(); } /* feed forward setpoint move rate with weight vel_ff */ _vel_ff = _sp_move_rate.emult(_params.vel_ff); /* move position setpoint */ _pos_sp += _sp_move_rate * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); if (_control_mode.flag_control_position_enabled) { pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } }
void MulticopterPositionControl::control_manual(float dt) { _sp_move_rate.zero(); if (_control_mode.flag_control_altitude_enabled) { /* move altitude setpoint with throttle stick */ _sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz); } if (_control_mode.flag_control_position_enabled) { /* move position setpoint with roll/pitch stick */ _sp_move_rate(0) = _manual.x; _sp_move_rate(1) = _manual.y; } /* limit setpoint move rate */ float sp_move_norm = _sp_move_rate.length(); if (sp_move_norm > 1.0f) { _sp_move_rate /= sp_move_norm; } /* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */ math::Matrix<3, 3> R_yaw_sp; R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body); _sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max); if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); } if (_control_mode.flag_control_position_enabled) { /* reset position setpoint to current position if needed */ reset_pos_sp(); } /* feed forward setpoint move rate with weight vel_ff */ _vel_ff = _sp_move_rate.emult(_params.vel_ff); /* move position setpoint */ _pos_sp += _sp_move_rate * dt; /* check if position setpoint is too far from actual position */ math::Vector<3> pos_sp_offs; pos_sp_offs.zero(); if (_control_mode.flag_control_position_enabled) { pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); } if (_control_mode.flag_control_altitude_enabled) { pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); } }