From c1169eb38b8aefa4d3afe28c272b09bfdab59821 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Sat, 19 May 2018 14:12:31 -0400 Subject: [PATCH] mc_pos_control : update to use new navigation limits architechture --- .../mc_pos_control/mc_pos_control_main.cpp | 96 ++++++++++--------- .../mc_pos_control/mc_pos_control_params.c | 7 +- 2 files changed, 54 insertions(+), 49 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 cd15055b70..eee17546b3 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -229,9 +229,7 @@ private: (ParamInt) _alt_mode, (ParamFloat) _rc_flt_cutoff, (ParamFloat) _rc_flt_smp_rate, - (ParamFloat) _acc_max_flow_xy, - (ParamFloat) _flow_min_hgt, - (ParamFloat) _flow_max_hgt + (ParamFloat) _acc_max_estimator_xy ); @@ -396,6 +394,8 @@ private: bool manual_wants_takeoff(); + bool manual_wants_landing(); + void set_takeoff_velocity(float &vel_sp_z); void landdetection_thrust_limit(matrix::Vector3f &thrust_sp); @@ -842,26 +842,29 @@ MulticopterPositionControl::reset_alt_sp() void MulticopterPositionControl::limit_altitude() { - float altitude_above_home = -(_pos(2) - _home_pos.z); - bool applying_flow_height_limit = false; + // TODO : This limit calculationshould be moved to updateConstraints in FlightTask + // Calculate altitude limit + float altitude_limit = INFINITY; - if (_terrain_follow && _local_pos.limit_hagl) { - // Don't allow the height setpoint to exceed the optical flow limits - if (_pos_sp(2) < -_flow_max_hgt.get()) { - _pos_sp(2) = -_flow_max_hgt.get(); - } + if (_terrain_follow) { + altitude_limit = fmaxf(-_local_pos.hagl_max, -_vehicle_land_detected.alt_max); - applying_flow_height_limit = true; + //printf("terrain follow alt limit : %f\n", altitude_limit); - } else if (_run_alt_control && (_vehicle_land_detected.alt_max > 0.0f) - && (altitude_above_home > _vehicle_land_detected.alt_max)) { - // we are above maximum altitude - _pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z; + } else { + altitude_limit = fmaxf(-_local_pos.hagl_max + _pos(2) + _local_pos.dist_bottom, + -_vehicle_land_detected.alt_max + _home_pos.z); + //printf("altitude follow alt limit : %f\n", altitude_limit); } + // Don't allow the height setpoint to exceed the limits + if (_pos_sp(2) < altitude_limit) { + _pos_sp(2) = altitude_limit; + } + + // We want to fly upwards. Check that vehicle does not exceed altitude if (!_run_alt_control && _vel_sp(2) <= 0.0f) { - // we want to fly upwards: check if vehicle does not exceed altitude // time to reach zero velocity float delta_t = -_vel(2) / _acceleration_z_max_down.get(); @@ -869,16 +872,9 @@ MulticopterPositionControl::limit_altitude() // predict next position based on current position, velocity, max acceleration downwards and time to reach zero velocity float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _acceleration_z_max_down.get() * delta_t *delta_t; - - if (!applying_flow_height_limit && (-(pos_z_next - _home_pos.z) > _vehicle_land_detected.alt_max) - && (_vehicle_land_detected.alt_max > 0.0f)) { + if (pos_z_next < altitude_limit) { // prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint - _pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z; - _run_alt_control = true; - - } else if (applying_flow_height_limit && (pos_z_next < -_flow_max_hgt.get())) { - // prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint - _pos_sp(2) = -_flow_max_hgt.get(); + _pos_sp(2) = altitude_limit; _run_alt_control = true; } } @@ -2432,13 +2428,14 @@ MulticopterPositionControl::calculate_velocity_setpoint() _vel_sp(2) = math::max(_vel_sp(2), -vel_limit); } - // encourage pilot to respect flow sensor minimum height limitations - if (_terrain_follow && _local_pos.limit_hagl - && _control_mode.flag_control_manual_enabled - && _control_mode.flag_control_altitude_enabled) { - // If distance to ground is less than limit, increment set point upwards at up to the landing descent rate - if (_local_pos.dist_bottom < _flow_min_hgt.get()) { - float climb_rate_bias = fminf(1.5f * _pos_p(2) * (_flow_min_hgt.get() - _local_pos.dist_bottom), _land_speed.get()); + /* encourage pilot to respect estimator height limitations when in manually controlled modes and not landing */ + if (PX4_ISFINITE(_local_pos.hagl_min) // We need height limiting + && _control_mode.flag_control_manual_enabled // Vehicle is under manual control + && _control_mode.flag_control_altitude_enabled // Altitude controller is running + && !manual_wants_landing()) { // Operator is not trying to land + if (_local_pos.dist_bottom < _local_pos.hagl_min) { + // If distance to ground is less than limit, increment setpoint upwards at up to the landing descent rate + float climb_rate_bias = fminf(1.5f * _pos_p(2) * (_local_pos.hagl_min - _local_pos.dist_bottom), _land_speed.get()); _vel_sp(2) -= climb_rate_bias; _pos_sp(2) -= climb_rate_bias * _dt; @@ -2919,6 +2916,14 @@ bool MulticopterPositionControl::manual_wants_takeoff() return (has_manual_control_present && (_manual.z > 0.65f || !_control_mode.flag_control_climb_rate_enabled)); } +bool MulticopterPositionControl::manual_wants_landing() +{ + const bool has_manual_control_present = _control_mode.flag_control_manual_enabled && _manual.timestamp > 0; + + // Operator is trying to land if the throttle stick is under 15%. + return (has_manual_control_present && (_manual.z < 0.15f || !_control_mode.flag_control_climb_rate_enabled)); +} + void MulticopterPositionControl::task_main() { @@ -2982,21 +2987,20 @@ MulticopterPositionControl::task_main() /* set dt for control blocks */ setDt(dt); - /* set default max velocity in xy to vel_max - * Apply estimator limits if applicable */ - if (_local_pos.vxy_max > 0.001f) { - // use the minimum of the estimator and user specified limit - _vel_max_xy = fminf(_vel_max_xy_param.get(), _local_pos.vxy_max); - // Allow for a minimum of 0.3 m/s for repositioning - _vel_max_xy = fmaxf(_vel_max_xy, 0.3f); + /* Update velocity limits. Use the minimum of the estimator demanded and vehicle + * limits, and allow a minimum of 0.3 m/s for repositioning */ + if (PX4_ISFINITE(_local_pos.vxy_max)) { + _vel_max_xy = fmaxf(fminf(_vel_max_xy_param.get(), _local_pos.vxy_max), 0.3f); - } else if (_vel_sp_significant) { - // raise the limit at a constant rate up to the user specified value - if (_vel_max_xy >= _vel_max_xy_param.get()) { - _vel_max_xy = _vel_max_xy_param.get(); + } else { + /* If the estimator stopped demanding a limit, release the limit gradually */ + if (_vel_sp_significant) { + if (_vel_max_xy < _vel_max_xy_param.get()) { + _vel_max_xy += dt * _acc_max_estimator_xy.get(); - } else { - _vel_max_xy += dt * _acc_max_flow_xy.get(); + } else { + _vel_max_xy = _vel_max_xy_param.get(); + } } } @@ -3338,7 +3342,7 @@ MulticopterPositionControl::set_idle_state() void MulticopterPositionControl::updateConstraints(Controller::Constraints &constraints) { - /* _contstraints */ + /* _constraints */ constraints.tilt_max = NAN; // Default no maximum tilt /* Set maximum tilt */ diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 95af9eac4b..a472ff11b9 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -434,8 +434,9 @@ PARAM_DEFINE_FLOAT(MPC_ACC_HOR, 5.0f); PARAM_DEFINE_FLOAT(MPC_DEC_HOR_SLOW, 5.0f); /** - * Horizontal acceleration in manual modes when optical flow ground speed limit is removed. - * If full stick is being applied and the EKF starts using GPS whilst using optical flow, + * Horizontal acceleration in manual modes when te estimator speed limit is removed. + * If full stick is being applied and the estimator stops demanding a speed limit, + * which it had been before (e.g if GPS is gained while flying on optical flow/vision only), * the vehicle will accelerate at this rate until the normal position control speed is achieved. * * @unit m/s/s @@ -445,7 +446,7 @@ PARAM_DEFINE_FLOAT(MPC_DEC_HOR_SLOW, 5.0f); * @decimal 1 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_ACC_HOR_FLOW, 0.5f); +PARAM_DEFINE_FLOAT(MPC_ACC_HOR_ESTM, 0.5f); /** * Maximum vertical acceleration in velocity controlled modes upward