From 2c906b7b270fa36c7cbdf9d55f00351b4ee44210 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 25 Jan 2012 21:55:14 +0900 Subject: [PATCH] OpticalFlow - replaced PI controller with PID controller. Modified optical flow hold to use aggregated position instead of speed. --- ArduCopter/ArduCopter.pde | 10 +++++++-- ArduCopter/Attitude.pde | 44 ++++++++++++++++++++++----------------- ArduCopter/Parameters.h | 8 +++---- ArduCopter/config.h | 17 +++++++++++---- ArduCopter/defines.h | 1 + 5 files changed, 51 insertions(+), 29 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 86bd6e060f..193a112b8a 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -69,6 +69,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // TimerProcess is the scheduler for MPU6000 reads. #include // ArduPilot Mega DCM Library #include // PI library +#include // PID library #include // RC Channel Library #include // Range finder library #include // Optical Flow library @@ -1989,17 +1990,22 @@ static void tuning(){ break; case CH6_OPTFLOW_KP: - g.rc_6.set_range(0,10000); // 0 to 10 + g.rc_6.set_range(0,5000); // 0 to 5 g.pi_optflow_roll.kP(tuning_value); g.pi_optflow_pitch.kP(tuning_value); break; case CH6_OPTFLOW_KI: - g.rc_6.set_range(0,100); // 0 to 0.1 + g.rc_6.set_range(0,10000); // 0 to 10 g.pi_optflow_roll.kI(tuning_value); g.pi_optflow_pitch.kI(tuning_value); break; + case CH6_OPTFLOW_KD: + g.rc_6.set_range(0,200); // 0 to 0.2 + g.pi_optflow_roll.kD(tuning_value); + g.pi_optflow_pitch.kD(tuning_value); + break; } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 16c6cd59fe..0851d90333 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -258,6 +258,8 @@ static void reset_optflow_I(void) { g.pi_optflow_roll.reset_I(); g.pi_optflow_pitch.reset_I(); + of_roll = 0; + of_pitch = 0; } static void reset_wind_I(void) @@ -462,34 +464,35 @@ static void init_z_damper() } #endif -// calculate modified roll/pitch depending upon optical flow values +// calculate modified roll/pitch depending upon optical flow calculated position static int32_t get_of_roll(int32_t control_roll) { #ifdef OPTFLOW_ENABLED - //static int32_t of_roll = 0; // we use global variable to make logging easier + static float tot_x_cm = 0; // total distance from target static unsigned long last_of_roll_update = 0; - static float prev_value = 0; - float x_cm; + int32_t new_roll = 0; // check if new optflow data available if( optflow.last_update != last_of_roll_update) { last_of_roll_update = optflow.last_update; - // filter movement - x_cm = (optflow.x_cm + prev_value) / 2.0 * 50.0; + // add new distance moved + tot_x_cm += optflow.x_cm; // only stop roll if caller isn't modifying roll if( control_roll == 0 && current_loc.alt < 1500) { - of_roll = g.pi_optflow_roll.get_pi(-x_cm, 1.0); // we could use the last update time to calculate the time change + new_roll = g.pi_optflow_roll.get_pid(-tot_x_cm, 1.0, 1.0); // we could use the last update time to calculate the time change }else{ g.pi_optflow_roll.reset_I(); - prev_value = 0; + tot_x_cm = 0; } + // limit amount of change and maximum angle + of_roll = constrain(new_roll, (of_roll-20), (of_roll+20)); } - // limit maximum angle - of_roll = constrain(of_roll, -1000, 1000); + // limit max angle + of_roll = constrain(of_roll, -1000, 1000); return control_roll+of_roll; #else return control_roll; @@ -500,27 +503,30 @@ static int32_t get_of_pitch(int32_t control_pitch) { #ifdef OPTFLOW_ENABLED - //static int32_t of_pitch = 0; // we use global variable to make logging easier + static float tot_y_cm = 0; // total distance from target static unsigned long last_of_pitch_update = 0; - static float prev_value = 0; - float y_cm; + int32_t new_pitch = 0; // check if new optflow data available if( optflow.last_update != last_of_pitch_update ) { last_of_pitch_update = optflow.last_update; - // filter movement - y_cm = (optflow.y_cm + prev_value) / 2.0 * 50.0; + // add new distance moved + tot_y_cm += optflow.y_cm; - // only stop roll if caller isn't modifying roll + // only stop roll if caller isn't modifying pitch if( control_pitch == 0 && current_loc.alt < 1500 ) { - of_pitch = g.pi_optflow_pitch.get_pi(y_cm, 1.0); // we could use the last update time to calculate the time change + new_pitch = g.pi_optflow_pitch.get_pid(tot_y_cm, 1.0, 1.0); // we could use the last update time to calculate the time change }else{ + tot_y_cm = 0; g.pi_optflow_pitch.reset_I(); - prev_value = 0; } + + // limit amount of change + of_pitch = constrain(new_pitch, (of_pitch-20), (of_pitch+20)); } - // limit maximum angle + + // limit max angle of_pitch = constrain(of_pitch, -1000, 1000); return control_pitch+of_pitch; #else diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index a94f36417f..4c061a6026 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -309,8 +309,8 @@ public: APM_PI pi_acro_roll; APM_PI pi_acro_pitch; - APM_PI pi_optflow_roll; - APM_PI pi_optflow_pitch; + PID pi_optflow_roll; + PID pi_optflow_pitch; uint8_t junk; @@ -435,8 +435,8 @@ public: pi_acro_roll (k_param_pi_acro_roll, PSTR("ACRO_RLL_"), ACRO_ROLL_P, ACRO_ROLL_I, ACRO_ROLL_IMAX * 100), pi_acro_pitch (k_param_pi_acro_pitch, PSTR("ACRO_PIT_"), ACRO_PITCH_P, ACRO_PITCH_I, ACRO_PITCH_IMAX * 100), - pi_optflow_roll (k_param_pi_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_IMAX * 100), - pi_optflow_pitch (k_param_pi_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_IMAX * 100), + pi_optflow_roll (k_param_pi_optflow_roll, PSTR("OF_RLL_"), OPTFLOW_ROLL_P, OPTFLOW_ROLL_I, OPTFLOW_ROLL_D, OPTFLOW_IMAX * 100), + pi_optflow_pitch (k_param_pi_optflow_pitch, PSTR("OF_PIT_"), OPTFLOW_PITCH_P, OPTFLOW_PITCH_I, OPTFLOW_PITCH_D, OPTFLOW_IMAX * 100), junk(0) // XXX just so that we can add things without worrying about the trailing comma { diff --git a/ArduCopter/config.h b/ArduCopter/config.h index b309f5d6fa..defab3c1a0 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -313,21 +313,30 @@ #ifndef OPTFLOW_ORIENTATION # define OPTFLOW_ORIENTATION AP_OPTICALFLOW_ADNS3080_PINS_FORWARD #endif +#ifndef OPTFLOW_RESOLUTION +# define OPTFLOW_RESOLUTION ADNS3080_RESOLUTION_1600 +#endif #ifndef OPTFLOW_FOV # define OPTFLOW_FOV AP_OPTICALFLOW_ADNS3080_08_FOV #endif // optical flow based loiter PI values #ifndef OPTFLOW_ROLL_P - #define OPTFLOW_ROLL_P 6.4 + #define OPTFLOW_ROLL_P 2.5 #endif #ifndef OPTFLOW_ROLL_I - #define OPTFLOW_ROLL_I 0.068 + #define OPTFLOW_ROLL_I 6.2 +#endif +#ifndef OPTFLOW_ROLL_D + #define OPTFLOW_ROLL_D 0.12 #endif #ifndef OPTFLOW_PITCH_P - #define OPTFLOW_PITCH_P 6.4 + #define OPTFLOW_PITCH_P 2.5 #endif #ifndef OPTFLOW_PITCH_I - #define OPTFLOW_PITCH_I 0.068 + #define OPTFLOW_PITCH_I 6.2 +#endif +#ifndef OPTFLOW_PITCH_D + #define OPTFLOW_PITCH_D 0.12 #endif #ifndef OPTFLOW_IMAX #define OPTFLOW_IMAX 4 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 6c0720799d..c0121912fb 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -163,6 +163,7 @@ // optical flow controller #define CH6_OPTFLOW_KP 17 #define CH6_OPTFLOW_KI 18 +#define CH6_OPTFLOW_KD 19 // nav byte mask