From acd877abdac7d8b12e73867a768bc5b8accc32d8 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 10 Jan 2012 23:33:07 -0800 Subject: [PATCH] added set_new_altitude call --- ArduCopter/commands.pde | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index c3f5c6243f..cb3767332b 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -147,9 +147,13 @@ static void set_next_WP(struct Location *wp) // --------------------- next_WP = *wp; - // used to control and limit the rate of climb - not used right now! - // ----------------------------------------------------------------- - target_altitude = current_loc.alt; + // used to control and limit the rate of climb + // ------------------------------------------- + // We don't set next WP below 1m + next_WP.alt = max(next_WP.alt, 100); + + // Save new altitude so we can track it for climb_rate + set_new_altitude(next_WP.alt); // this is used to offset the shrinking longitude as we go towards the poles float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;