Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

OpticalFlow - replaced PI controller with PID controller.

Browse Source 
Modified optical flow hold to use aggregated position instead of speed.
This commit is contained in:
Randy Mackay 2012-01-25 21:55:14 +09:00
parent cd21c5905b
commit 2f84b54bbb
5 changed files with 51 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/ArduCopter.pde
View File

@ -69,6 +69,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
#include <AP_TimerProcess.h> // TimerProcess is the scheduler for MPU6000 reads.
#include <AP_DCM.h> // ArduPilot Mega DCM Library
#include <APM_PI.h> // PI library
#include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library
#include <AP_OpticalFlow.h> // 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;
}
}

44
ArduCopter/Attitude.pde
View File

@ -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

8
ArduCopter/Parameters.h
View File

@ -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
{

17
ArduCopter/config.h
View File

@ -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

1
ArduCopter/defines.h
View File

@ -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