From 1a1f6cac5d8f4f5a95f67774f19d51ab1b3365f8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 1 Feb 2012 08:04:18 +0900 Subject: [PATCH] OptFlow - reenable OF_LOITER pid controller and reduce I term --- ArduCopter/Attitude.pde | 4 ++-- ArduCopter/config.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 26e12f3610..5942e69583 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -441,7 +441,7 @@ get_of_roll(int32_t control_roll) // only stop roll if caller isn't modifying roll if( control_roll == 0 && current_loc.alt < 1500) { - //new_roll = g.pid_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change + new_roll = g.pid_optflow_roll.get_pid(-tot_x_cm, 1.0); // we could use the last update time to calculate the time change }else{ g.pid_optflow_roll.reset_I(); tot_x_cm = 0; @@ -475,7 +475,7 @@ get_of_pitch(int32_t control_pitch) // only stop roll if caller isn't modifying pitch if( control_pitch == 0 && current_loc.alt < 1500 ) { - //new_pitch = g.pid_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change + new_pitch = g.pid_optflow_pitch.get_pid(tot_y_cm, 1.0); // we could use the last update time to calculate the time change }else{ tot_y_cm = 0; g.pid_optflow_pitch.reset_I(); diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 43abc0c984..2bf3a93d60 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -324,7 +324,7 @@ #define OPTFLOW_ROLL_P 2.5 #endif #ifndef OPTFLOW_ROLL_I - #define OPTFLOW_ROLL_I 6.2 + #define OPTFLOW_ROLL_I 3.2 #endif #ifndef OPTFLOW_ROLL_D #define OPTFLOW_ROLL_D 0.12 @@ -333,7 +333,7 @@ #define OPTFLOW_PITCH_P 2.5 #endif #ifndef OPTFLOW_PITCH_I - #define OPTFLOW_PITCH_I 6.2 + #define OPTFLOW_PITCH_I 3.2 #endif #ifndef OPTFLOW_PITCH_D #define OPTFLOW_PITCH_D 0.12