From f0a1cd197ef3981744c4beec2576760fc7049d20 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 7 Jun 2018 08:05:27 +1000 Subject: [PATCH] mc_pos_control: formatting fixes --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 7682ec277f..b15780931f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -846,6 +846,7 @@ MulticopterPositionControl::limit_altitude() // Calculate vertical position limit float poz_z_min_limit; + if (_terrain_follow) { poz_z_min_limit = fmaxf(-_local_pos.hagl_max, -_vehicle_land_detected.alt_max); @@ -853,7 +854,7 @@ MulticopterPositionControl::limit_altitude() } else { poz_z_min_limit = fmaxf(-_local_pos.hagl_max + _pos(2) + _local_pos.dist_bottom, - -_vehicle_land_detected.alt_max + _home_pos.z); + -_vehicle_land_detected.alt_max + _home_pos.z); //printf("altitude follow alt limit : %f\n", altitude_limit); } @@ -868,6 +869,7 @@ MulticopterPositionControl::limit_altitude() // 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