From f68d23d91e438ea1260b52abdd3a0de482b9e84c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 9 Jan 2012 13:53:54 +0900 Subject: [PATCH] ArduCopter - added ROLL_PITCH_STABLE_OF (i.e. Stabilised Roll/Pitch + adjustments based on optical flow) Removed optical flow from regular loiter for now until it's tested. --- ArduCopter/ArduCopter.pde | 75 ++++++++++++++++++++++++++++++--------- ArduCopter/Attitude.pde | 70 +++++++++++++++++++++++++++++++++++- ArduCopter/Log.pde | 20 ++++++++--- ArduCopter/Parameters.h | 8 +++++ ArduCopter/config.h | 17 +++++++++ ArduCopter/defines.h | 5 +++ ArduCopter/system.pde | 2 ++ 7 files changed, 175 insertions(+), 22 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f7a015efe4..9368f051ae 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -665,6 +665,11 @@ static int16_t nav_lon; static int16_t nav_lat_p; static int16_t nav_lon_p; +// The Commanded ROll from the autopilot based on optical flow sensor. +static int32_t of_roll = 0; +// The Commanded pitch from the autopilot based on optical flow sensor. negative Pitch means go forward. +static int32_t of_pitch = 0; + //////////////////////////////////////////////////////////////////////////////// // Navigation Throttle control @@ -926,12 +931,6 @@ static void medium_loop() update_GPS(); } - #ifdef OPTFLOW_ENABLED - if(g.optflow_enabled){ - update_optical_flow(); - } - #endif - #if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode if(g.compass_enabled){ if (compass.read()) { @@ -975,13 +974,13 @@ static void medium_loop() // If we have optFlow enabled we can grab a more accurate speed // here and override the speed from the GPS // ---------------------------------------- - #ifdef OPTFLOW_ENABLED - if(g.optflow_enabled && current_loc.alt < 500){ - // optflow wont be enabled on 1280's - x_GPS_speed = optflow.x_cm; - y_GPS_speed = optflow.y_cm; - } - #endif + //#ifdef OPTFLOW_ENABLED + //if(g.optflow_enabled && current_loc.alt < 500){ + // // optflow wont be enabled on 1280's + // x_GPS_speed = optflow.x_cm; + // y_GPS_speed = optflow.y_cm; + //} + //#endif // control mode specific updates // ----------------------------- @@ -1096,6 +1095,13 @@ static void fifty_hz_loop() } #endif + // syncronise optical flow reads with altitude reads + #ifdef OPTFLOW_ENABLED + if(g.optflow_enabled){ + update_optical_flow(); + } + #endif + // agmatthews - USERHOOKS #ifdef USERHOOK_50HZLOOP USERHOOK_50HZLOOP @@ -1222,15 +1228,21 @@ static void super_slow_loop() #ifdef OPTFLOW_ENABLED static void update_optical_flow(void) { + static int log_counter = 0; + optflow.update(); optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow // write to log - if (g.log_bitmask & MASK_LOG_OPTFLOW){ - Log_Write_Optflow(); + log_counter++; + if( log_counter >= 5 ) { + log_counter = 0; + if (g.log_bitmask & MASK_LOG_OPTFLOW){ + Log_Write_Optflow(); + } } - if(g.optflow_enabled && current_loc.alt < 500){ + /*if(g.optflow_enabled && current_loc.alt < 500){ if(GPS_enabled){ // if we have a GPS, we add some detail to the GPS // XXX this may not ne right @@ -1247,7 +1259,7 @@ static void update_optical_flow(void) } // OK to run the nav routines nav_ok = true; - } + }*/ } #endif @@ -1414,6 +1426,22 @@ void update_roll_pitch_mode(void) g.rc_1.servo_out = get_stabilize_roll(control_roll); g.rc_2.servo_out = get_stabilize_pitch(control_pitch); break; + + case ROLL_PITCH_STABLE_OF: + // apply SIMPLE mode transform + if(do_simple && new_radio_frame){ + update_simple_mode(); + } + + // in this mode, nav_roll and nav_pitch = the iterm + #if WIND_COMP_STAB == 1 + g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in + nav_roll)); + g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in + nav_pitch)); + #else + g.rc_1.servo_out = get_stabilize_roll(get_of_roll(g.rc_1.control_in)); + g.rc_2.servo_out = get_stabilize_pitch(get_of_pitch(g.rc_2.control_in)); + #endif + break; } // clear new radio frame info @@ -1950,6 +1978,19 @@ static void tuning(){ g.rc_6.set_range(0,1000); // 0 to 1 g.pi_alt_hold.kP(tuning_value); break; + + case CH6_OPTFLOW_KP: + g.rc_6.set_range(0,10000); // 0 to 10 + 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.pi_optflow_roll.kI(tuning_value); + g.pi_optflow_pitch.kI(tuning_value); + break; + } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index cdf552d099..8c075f3305 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -258,6 +258,8 @@ static void reset_rate_I() g.pi_rate_pitch.reset_I(); g.pi_acro_roll.reset_I(); g.pi_acro_pitch.reset_I(); + g.pi_optflow_roll.reset_I(); + g.pi_optflow_pitch.reset_I(); } @@ -429,4 +431,70 @@ static int get_z_damping() static void init_z_damper() { } -#endif \ No newline at end of file +#endif + +// calculate modified roll/pitch depending upon optical flow values +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 unsigned long last_of_roll_update = 0; + static float prev_value = 0; + float x_cm; + + // 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; + + // 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 + }else{ + g.pi_optflow_roll.reset_I(); + prev_value = 0; + } + } + // limit maximum angle + of_roll = constrain(of_roll, -1000, 1000); + + return control_roll+of_roll; +#else + return control_roll; +#endif +} + +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 unsigned long last_of_pitch_update = 0; + static float prev_value = 0; + float y_cm; + + // 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; + + // only stop roll if caller isn't modifying roll + 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 + }else{ + g.pi_optflow_pitch.reset_I(); + prev_value = 0; + } + } + // limit maximum angle + of_pitch = constrain(of_pitch, -1000, 1000); + return control_pitch+of_pitch; +#else + return control_pitch; +#endif +} diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index cfc71a7454..61197d4186 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -482,8 +482,12 @@ static void Log_Write_Optflow() DataFlash.WriteInt((int)optflow.dx); DataFlash.WriteInt((int)optflow.dy); DataFlash.WriteInt((int)optflow.surface_quality); + DataFlash.WriteInt((int)optflow.x_cm); + DataFlash.WriteInt((int)optflow.y_cm); DataFlash.WriteLong(optflow.vlat);//optflow_offset.lat + optflow.lat); DataFlash.WriteLong(optflow.vlon);//optflow_offset.lng + optflow.lng); + DataFlash.WriteLong(of_roll); + DataFlash.WriteLong(of_pitch); DataFlash.WriteByte(END_BYTE); #endif } @@ -495,15 +499,23 @@ static void Log_Read_Optflow() int16_t temp1 = DataFlash.ReadInt(); // 1 int16_t temp2 = DataFlash.ReadInt(); // 2 int16_t temp3 = DataFlash.ReadInt(); // 3 - float temp4 = DataFlash.ReadLong(); // 4 - float temp5 = DataFlash.ReadLong(); // 5 + int16_t temp4 = DataFlash.ReadInt(); // 4 + int16_t temp5 = DataFlash.ReadInt(); // 5 + float temp6 = DataFlash.ReadLong(); // 6 + float temp7 = DataFlash.ReadLong(); // 7 + int32_t temp8 = DataFlash.ReadLong(); // 8 + int32_t temp9 = DataFlash.ReadLong(); // 9 - Serial.printf_P(PSTR("OF, %d, %d, %d, %4.7f, %4.7f\n"), + Serial.printf_P(PSTR("OF, %d, %d, %d, %d, %d, %4.7f, %4.7f, %d, %d\n"), temp1, temp2, temp3, temp4, - temp5); + temp5, + temp6, + temp7, + temp8, + temp9); #endif } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e5e68693ec..4895a39b52 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -178,6 +178,8 @@ public: k_param_pi_throttle, k_param_pi_acro_roll, k_param_pi_acro_pitch, + k_param_pi_optflow_roll, + k_param_pi_optflow_pitch, // 250 // 254,255: reserved @@ -298,6 +300,9 @@ public: APM_PI pi_acro_roll; APM_PI pi_acro_pitch; + APM_PI pi_optflow_roll; + APM_PI pi_optflow_pitch; + uint8_t junk; // Note: keep initializers here in the same order as they are declared above. @@ -417,6 +422,9 @@ 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), + 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 0ebd2b0c36..02b3548c79 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -310,6 +310,23 @@ #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 +#endif +#ifndef OPTFLOW_ROLL_I + #define OPTFLOW_ROLL_I 0.068 +#endif +#ifndef OPTFLOW_PITCH_P + #define OPTFLOW_PITCH_P 6.4 +#endif +#ifndef OPTFLOW_PITCH_I + #define OPTFLOW_PITCH_I 0.068 +#endif +#ifndef OPTFLOW_IMAX + #define OPTFLOW_IMAX 4 +#endif + ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index cd0efba163..efa8fe9f70 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -18,6 +18,7 @@ #define ROLL_PITCH_STABLE 0 #define ROLL_PITCH_ACRO 1 #define ROLL_PITCH_AUTO 2 +#define ROLL_PITCH_STABLE_OF 3 #define THROTTLE_MANUAL 0 #define THROTTLE_HOLD 1 @@ -155,6 +156,10 @@ #define CH6_Z_GAIN 15 #define CH6_DAMP 16 +// optical flow controller +#define CH6_OPTFLOW_KP 17 +#define CH6_OPTFLOW_KI 18 + // nav byte mask // ------------- diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index a57413b9eb..6c610d7137 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -380,6 +380,8 @@ static void reset_I_all(void) g.pi_throttle.reset_I(); g.pi_acro_roll.reset_I(); g.pi_acro_pitch.reset_I(); + g.pi_optflow_roll.reset_I(); + g.pi_optflow_pitch.reset_I(); }