From 470c3fdc0654e299ffd8fb336c008e48dda2eea1 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 21 Mar 2017 10:45:05 +0100 Subject: [PATCH] mc_pos_control: refactoring only in manual velocity setpoint generation --- .../mc_pos_control/mc_pos_control_main.cpp | 79 ++++++++----------- 1 file changed, 35 insertions(+), 44 deletions(-) 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 3a533144e4..2fc4dbf81b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -922,18 +922,15 @@ MulticopterPositionControl::control_manual(float dt) /* - * Map from stick input to velocity setpoint - * xy in local frame, z in global frame - */ + * Map from stick input to velocity setpoint + */ - /* setpoint in normalized range [-1,1] */ - math::Vector<2> req_vel_sp_xy; - req_vel_sp_xy.zero(); - float req_vel_sp_z = 0.0f; + /* velocity setpoint commanded by user stick input */ + matrix::Vector3f man_vel_sp; if (_control_mode.flag_control_altitude_enabled) { /* set vertical velocity setpoint with throttle stick */ - req_vel_sp_z = -math::expo_deadzone((_manual.z - 0.5f) * 2.f, _z_vel_man_expo.get(), _params.hold_dz); + man_vel_sp(2) = -math::expo_deadzone((_manual.z - 0.5f) * 2.f, _z_vel_man_expo.get(), _params.hold_dz); /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); @@ -941,55 +938,53 @@ MulticopterPositionControl::control_manual(float dt) if (_control_mode.flag_control_position_enabled) { /* set horizontal velocity setpoint with roll/pitch stick */ - req_vel_sp_xy(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_dz); - req_vel_sp_xy(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_dz); + man_vel_sp(0) = math::expo_deadzone(_manual.x, _params.xy_vel_man_expo, _params.hold_dz); + man_vel_sp(1) = math::expo_deadzone(_manual.y, _params.xy_vel_man_expo, _params.hold_dz); - /* saturarate such that magnitude is never larger than 1 */ - if (req_vel_sp_xy.length() > 1.0f) { - req_vel_sp_xy = req_vel_sp_xy.normalized(); + const float man_vel_hor_length = ((matrix::Vector2f)man_vel_sp.slice<2, 1>(0, 0)).length(); + + /* saturate such that magnitude is never larger than 1 */ + if (man_vel_hor_length > 1.0f) { + man_vel_sp(0) /= man_vel_hor_length; + man_vel_sp(1) /= man_vel_hor_length; } /* reset position setpoint to current position if needed */ reset_pos_sp(); } - /* scale requested velocity setpoint to cruisespeed and rotate around yaw */ - math::Vector<3> cruising_scale(_params.vel_cruise_xy, - _params.vel_cruise_xy, - (req_vel_sp_z > 0.0f) ? _params.vel_max_down : _params.vel_max_up); - math::Vector<3> req_vel_sp_scaled(req_vel_sp_xy(0), req_vel_sp_xy(1), req_vel_sp_z); + /* prepare yaw to rotate into NED frame */ + float yaw_input_fame = _control_mode.flag_control_fixed_hdg_enabled ? _yaw_takeoff : _local_pos.yaw; - /* scale velocity setpoint to cruise speed (m/s) and rotate around yaw to NED frame */ - math::Matrix<3, 3> R_input_fame; + /* prepare cruise speed (m/s) vector to scale the velocity setpoint */ + matrix::Vector3f vel_cruise_scale(_params.vel_cruise_xy, + _params.vel_cruise_xy, + (man_vel_sp(2) > 0.0f) ? _params.vel_max_down : _params.vel_max_up); - if (_control_mode.flag_control_fixed_hdg_enabled) { - R_input_fame.from_euler(0.0f, 0.0f, _yaw_takeoff); + /* setpoint in NED frame and scaled to cruise velocity */ + man_vel_sp = matrix::Dcmf(matrix::Eulerf(0.0f, 0.0f, yaw_input_fame)) * man_vel_sp.emult(vel_cruise_scale); - } else { - R_input_fame.from_euler(0.0f, 0.0f, _local_pos.yaw); - - } - - req_vel_sp_scaled = R_input_fame * req_vel_sp_scaled.emult( - cruising_scale); // in NED and scaled to actual velocity; /* - * assisted velocity mode: user controls velocity, but if velocity is small enough, position + * assisted velocity mode: user controls velocity, but if velocity is small enough, position * hold is activated for the corresponding axis */ + /* want to get/stay in altitude hold if user has z stick in the middle (accounted for deadzone already) */ + const bool alt_hold_desired = _control_mode.flag_control_altitude_enabled && fabsf(man_vel_sp(2)) < FLT_EPSILON; + + /* want to get/stay in position hold if user has xy stick in the middle (accounted for deadzone already) */ + const bool pos_hold_desired = _control_mode.flag_control_position_enabled && + fabsf(man_vel_sp(0)) < FLT_EPSILON && fabsf(man_vel_sp(1)) < FLT_EPSILON; /* check vertical hold engaged flag */ if (_alt_hold_engaged) { - - /* only switch back to velocity control if user moves stick */ - _alt_hold_engaged = _control_mode.flag_control_altitude_enabled && (fabsf(req_vel_sp_z) < FLT_EPSILON); + _alt_hold_engaged = alt_hold_desired; } else { /* check if we switch to alt_hold_engaged */ - bool smooth_alt_transition = _control_mode.flag_control_altitude_enabled && - fabsf(req_vel_sp_z) < FLT_EPSILON && + bool smooth_alt_transition = alt_hold_desired && (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z); /* during transition predict setpoint forward */ @@ -1011,17 +1006,13 @@ MulticopterPositionControl::control_manual(float dt) /* check horizontal hold engaged flag */ if (_pos_hold_engaged) { - - /* only switch back to velocity control if user moves stick */ - _pos_hold_engaged = _control_mode.flag_control_position_enabled && (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON) - && (fabsf(req_vel_sp_xy(1)) < FLT_EPSILON); + _pos_hold_engaged = pos_hold_desired; } else { /* check if we switch to pos_hold_engaged */ float vel_xy_mag = sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)); - bool smooth_pos_transition = _control_mode.flag_control_position_enabled && - (fabsf(req_vel_sp_xy(0)) < FLT_EPSILON && fabsf(req_vel_sp_xy(1)) < FLT_EPSILON) && + bool smooth_pos_transition = pos_hold_desired && (_params.hold_max_xy < FLT_EPSILON || vel_xy_mag < _params.hold_max_xy); /* during transition predict setpoint forward */ @@ -1046,15 +1037,15 @@ MulticopterPositionControl::control_manual(float dt) if (!_alt_hold_engaged) { _pos_sp(2) = _pos(2); _run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */ - _vel_sp(2) = req_vel_sp_scaled(2); + _vel_sp(2) = man_vel_sp(2); } if (!_pos_hold_engaged) { _pos_sp(0) = _pos(0); _pos_sp(1) = _pos(1); _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ - _vel_sp(0) = req_vel_sp_scaled(0); - _vel_sp(1) = req_vel_sp_scaled(1); + _vel_sp(0) = man_vel_sp(0); + _vel_sp(1) = man_vel_sp(1); } if (_vehicle_land_detected.landed) {