From bc77302fc932aaf9bbe2a41e929f6784de1e2d5e Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Sat, 11 May 2019 13:34:59 +0200 Subject: [PATCH] mc_pos_control: refactor smooth takeoff names The comments and variable names were partly misleading. I grouped all members, hopefully gave them more understandable names and adjusted the comments. --- .../mc_pos_control/mc_pos_control_main.cpp | 101 +++++++++--------- 1 file changed, 49 insertions(+), 52 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 740684dc0b..aa95366494 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -102,8 +102,11 @@ public: int print_status() override; private: - - bool _in_smooth_takeoff = false; /**< true if takeoff ramp is applied */ + // smooth takeoff vertical velocity ramp state + bool _in_takeoff_ramp = false; /**< true while takeoff ramp is applied */ + bool _velocity_triggered_takeoff = false; /**< true if takeoff was triggered by a velocity setpoint */ + float _takeoff_ramp_velocity = -1.f; /**< current value of the smooth takeoff ramp */ + float _takeoff_reference_z; /**< z-position when takeoff ramp was initiated */ orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */ orb_advert_t _traj_sp_pub{nullptr}; /**< trajectory setpoints publication */ @@ -122,11 +125,6 @@ private: int _task_failure_count{0}; /**< counter for task failures */ - float _takeoff_speed = -1.f; /**< For flighttask interface used only. It can be thrust or velocity setpoints */ - float _takeoff_reference_z; /**< Z-position when takeoff was initiated */ - bool _smooth_velocity_takeoff = - false; /**< Smooth velocity takeoff can be initiated either through position or velocity setpoint */ - vehicle_status_s _vehicle_status{}; /**< vehicle status */ /**< vehicle-land-detection: initialze to landed */ vehicle_land_detected_s _vehicle_land_detected = { @@ -233,13 +231,13 @@ private: void publish_trajectory_sp(const vehicle_local_position_setpoint_s &traj); /** - * Update smooth takeoff setpoint ramp to bring the vehicle off the ground without step. + * Update smooth takeoff vertical velocity ramp to bring the vehicle off the ground without step * @param z_sp position setpoint in the z-Direction * @param vz_sp velocity setpoint in the z-Direction * @param jz_sp jerk setpoint in the z-Direction * @param min_ground_clearance minimal distance to the ground in which e.g. optical flow works correctly */ - void update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance); + void update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground); /** * Adjust the setpoint during landing. @@ -649,14 +647,14 @@ MulticopterPositionControl::run() // do smooth takeoff after delay if there's a valid vertical velocity or position if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) { - update_smooth_takeoff(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground); + update_takeoff_ramp(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints.min_distance_to_ground); } // disable horizontal / yaw control during smooth takeoff and limit maximum speed upwards - if (_in_smooth_takeoff) { + if (_in_takeoff_ramp) { - // during smooth takeoff, constrain speed to takeoff speed - constraints.speed_up = _takeoff_speed; + // during smooth takeoff, constrain speed to ramp state + constraints.speed_up = _takeoff_ramp_velocity; // altitude above reference takeoff const float alt_above_tko = -(_states.position(2) - _takeoff_reference_z); @@ -675,7 +673,7 @@ MulticopterPositionControl::run() } } - if (_vehicle_land_detected.landed && !_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) { + if (_vehicle_land_detected.landed && !_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) { // Keep throttle low when landed and NOT in smooth takeoff reset_setpoint_to_nan(setpoint); setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = 0.0f; @@ -734,7 +732,7 @@ MulticopterPositionControl::run() // Part of landing logic: if ground-contact/maybe landed was detected, turn off // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic. // Note: only adust thrust output if there was not thrust-setpoint demand in D-direction. - if (!_in_smooth_takeoff && !PX4_ISFINITE(setpoint.thrust[2])) { + if (!_in_takeoff_ramp && !PX4_ISFINITE(setpoint.thrust[2])) { limit_thrust_during_landing(local_pos_sp); } @@ -980,61 +978,60 @@ MulticopterPositionControl::start_flight_task() } void -MulticopterPositionControl::update_smooth_takeoff(const float z_sp, const float vz_sp, const float jz_sp, const float min_ground_clearance) +MulticopterPositionControl::update_takeoff_ramp(const float z_sp, const float vz_sp, const float jz_sp, const float min_distance_to_ground) { - // Check for smooth takeoff - if (_vehicle_land_detected.landed && !_in_smooth_takeoff) { - // Vehicle is still landed and no takeoff was initiated yet. - // Adjust for different takeoff cases. - // The minimum takeoff altitude needs to be at least 20cm above minimum distance or, if valid, above minimum distance - // above ground. - float min_altitude = PX4_ISFINITE(min_ground_clearance) ? (min_ground_clearance + 0.05f) : - 0.2f; + // check if takeoff is triggered + if (_vehicle_land_detected.landed && !_in_takeoff_ramp) { + // vehicle is still landed and no takeoff was initiated yet, check if takeoff is triggered + // minimum takeoff altitude either 20cm or if valid above minimum altitude specified by flight task/estimator + float min_altitude = 0.2f; + if (PX4_ISFINITE(min_distance_to_ground)) { + min_altitude = min_distance_to_ground + 0.05f; + } - // takeoff was initiated through velocity setpoint - _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f; - bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON; - _smooth_velocity_takeoff |= jerk_triggered_takeoff; + // upwards velocity setpoint larger than a minimal speed + _velocity_triggered_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f; + // upwards jerk setpoint + const bool jerk_triggered_takeoff = PX4_ISFINITE(jz_sp) && jz_sp < -FLT_EPSILON; + // position setpoint above the minimum altitude + const bool position_triggered_takeoff = PX4_ISFINITE(z_sp) && (z_sp < (_states.position(2) - min_altitude)); - if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) { - // There is a position setpoint above current position or velocity setpoint larger than - // takeoff speed. Enable smooth takeoff. - _in_smooth_takeoff = true; - _takeoff_speed = 0.f; + _velocity_triggered_takeoff |= jerk_triggered_takeoff; + if (_velocity_triggered_takeoff || position_triggered_takeoff) { + // takeoff was triggered, start velocity ramp + _takeoff_ramp_velocity = 0.f; + _in_takeoff_ramp = true; _takeoff_reference_z = _states.position(2); - } } - // If in smooth takeoff, adjust setpoints based on what is valid: - // 1. position setpoint is valid -> go with takeoffspeed to specific altitude - // 2. position setpoint not valid but velocity setpoint valid: ramp up velocity - if (_in_smooth_takeoff) { - float desired_tko_speed = -vz_sp; + // if in smooth takeoff limit upwards velocity setpoint to a smooth ramp + if (_in_takeoff_ramp) { + float takeoff_desired_velocity = -vz_sp; - // If there is a valid position setpoint, then set the desired speed to the takeoff speed. - if (!_smooth_velocity_takeoff) { - desired_tko_speed = _param_mpc_tko_speed.get(); + // if takeoff was triggered by a position setpoint, ramp to the takeoff speed parameter + if (!_velocity_triggered_takeoff) { + takeoff_desired_velocity = _param_mpc_tko_speed.get(); } - // Ramp up takeoff speed. + // ramp up vrtical velocity in TKO_RAMP_T seconds if (_param_mpc_tko_ramp_t.get() > _dt) { - _takeoff_speed += desired_tko_speed * _dt / _param_mpc_tko_ramp_t.get(); + _takeoff_ramp_velocity += takeoff_desired_velocity * _dt / _param_mpc_tko_ramp_t.get(); } else { - // No ramp, directly go to desired takeoff speed - _takeoff_speed = desired_tko_speed; + // no ramp, directly go to desired takeoff speed + _takeoff_ramp_velocity = takeoff_desired_velocity; } - _takeoff_speed = math::min(_takeoff_speed, _param_mpc_tko_speed.get()); + _takeoff_ramp_velocity = math::min(_takeoff_ramp_velocity, _param_mpc_tko_speed.get()); - // Smooth takeoff is achieved once desired altitude/velocity setpoint is reached. - if (!_smooth_velocity_takeoff) { - _in_smooth_takeoff = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get()); + // smooth takeoff is achieved once desired altitude/velocity setpoint is reached + if (!_velocity_triggered_takeoff) { + _in_takeoff_ramp = _states.position(2) - 0.2f > math::max(z_sp, _takeoff_reference_z - _param_mpc_land_alt2.get()); } else { - // Make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector - _in_smooth_takeoff = (_takeoff_speed < -vz_sp) || _vehicle_land_detected.landed; + // make sure to stay in smooth takeoff if takeoff has not been detected yet by the land detector + _in_takeoff_ramp = (_takeoff_ramp_velocity < -vz_sp) || _vehicle_land_detected.landed; } } }