From b77845a3c04dbda6902b456bcb657272ae73006f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 4 Jun 2018 18:46:08 +1000 Subject: [PATCH] mc_pos_control: Fix bug in calculation of altitude limit The correction for stopping distance applied to the maximum altitude limiter uses the vertical velocity estimate and gives the same offset for both positive (down) and negative (up) velocity. This calculation has been corrected and simplified and variable names changes to make the functionality clearer. --- .../mc_pos_control/mc_pos_control_main.cpp | 34 ++++++++++--------- 1 file changed, 18 insertions(+), 16 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 eee17546b3..7682ec277f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -842,39 +842,41 @@ MulticopterPositionControl::reset_alt_sp() void MulticopterPositionControl::limit_altitude() { - // TODO : This limit calculationshould be moved to updateConstraints in FlightTask - // Calculate altitude limit - float altitude_limit = INFINITY; + // TODO : This limit calculation should be moved to updateConstraints in FlightTask + // Calculate vertical position limit + float poz_z_min_limit; if (_terrain_follow) { - altitude_limit = fmaxf(-_local_pos.hagl_max, -_vehicle_land_detected.alt_max); + poz_z_min_limit = fmaxf(-_local_pos.hagl_max, -_vehicle_land_detected.alt_max); //printf("terrain follow alt limit : %f\n", altitude_limit); } else { - altitude_limit = fmaxf(-_local_pos.hagl_max + _pos(2) + _local_pos.dist_bottom, + poz_z_min_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; + // Don't allow the z position setpoint to exceed the limit + if (_pos_sp(2) < poz_z_min_limit) { + _pos_sp(2) = poz_z_min_limit; } - // We want to fly upwards. Check that vehicle does not exceed altitude + // We want to fly upwards. Check that vehicle does not exceed altitude limit if (!_run_alt_control && _vel_sp(2) <= 0.0f) { - // time to reach zero velocity - float delta_t = -_vel(2) / _acceleration_z_max_down.get(); + // predict future position based on current position, velocity, max acceleration downwards and time to reach zero velocity + float pos_z_next = _pos(2); + if (_vel(2) < 0.0f) { + // allow for stopping distance only when approaching the altitude limit from below + // TODO better calculation that allows for delay in braking + pos_z_next -= 0.5f * _vel(2) * _vel(2) / _acceleration_z_max_down.get(); + } - // 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 (pos_z_next < altitude_limit) { + if (pos_z_next < poz_z_min_limit) { // prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint - _pos_sp(2) = altitude_limit; + _pos_sp(2) = poz_z_min_limit; _run_alt_control = true; } }