From 364afe8da0446957dbfde434c330364c896c0e39 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Thu, 23 Feb 2012 09:14:27 -0800 Subject: [PATCH] added a constraint to D term --- ArduCopter/Attitude.pde | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index eaf4d75b38..5b3c3c024c 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -134,6 +134,7 @@ get_rate_roll(int32_t target_rate) // MultiWii uses a filter of the last three to get around noise and get a stronger signal. // Works well! Thanks! int16_t d_temp = (rate_d1 + rate_d2 + rate_d3) * g.stabilize_d; + d_temp = constrain(d_temp, -400, 400); target_rate -= d_temp; // output control: @@ -167,6 +168,7 @@ get_rate_pitch(int32_t target_rate) // D term int16_t d_temp = (rate_d1 + rate_d2 + rate_d3) * g.stabilize_d; + d_temp = constrain(d_temp, -400, 400); target_rate -= d_temp; // output control: