From 20a57b8932b8c2e836d46cc0a2244c0239f54190 Mon Sep 17 00:00:00 2001 From: jasonshort Date: Sun, 6 Feb 2011 07:04:26 +0000 Subject: [PATCH] added error limit to alt hold git-svn-id: https://arducopter.googlecode.com/svn/trunk@1599 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArduCopterMega/flight_control.pde | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/ArduCopterMega/flight_control.pde b/ArduCopterMega/flight_control.pde index 5b669e47cb..24c04648ee 100644 --- a/ArduCopterMega/flight_control.pde +++ b/ArduCopterMega/flight_control.pde @@ -20,16 +20,19 @@ void output_auto_throttle() } void calc_nav_throttle() -{ +{ + // limit error + long error = constrain(altitude_error, -300, 300); + if(altitude_sensor == BARO) { - nav_throttle = pid_baro_throttle.get_pid(altitude_error, delta_ms_fast_loop, 1.0); + nav_throttle = pid_baro_throttle.get_pid(error, delta_ms_fast_loop, 1.0); // limit output of throttle control nav_throttle = throttle_cruise + constrain(nav_throttle, -50, 100); } else { // SONAR - nav_throttle = pid_sonar_throttle.get_pid(altitude_error, delta_ms_fast_loop, 1.0); + nav_throttle = pid_sonar_throttle.get_pid(error, delta_ms_fast_loop, 1.0); // limit output of throttle control nav_throttle = throttle_cruise + constrain(nav_throttle, -60, 100);