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

ArduCopter - added ROLL_PITCH_STABLE_OF (i.e. Stabilised Roll/Pitch + adjustments based on optical flow)

Browse Source 
Removed optical flow from regular loiter for now until it's tested.
This commit is contained in:
Randy Mackay 2012-01-09 13:53:54 +09:00
parent ee18748565
commit f68d23d91e
7 changed files with 175 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
ArduCopter/ArduCopter.pde
View File

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

70
ArduCopter/Attitude.pde
View File

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

20
ArduCopter/Log.pde
View File

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

8
ArduCopter/Parameters.h
View File

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

17
ArduCopter/config.h
View File

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

5
ArduCopter/defines.h
View File

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

2
ArduCopter/system.pde
View File

@ -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();
}