aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ArduCopter - Attitude.pde - added logging of optical flow pid controller

Browse Source
This commit is contained in:
rmackay9 2012-04-21 20:17:12 +09:00
parent 3e95ade004
commit 533772339e
1 changed files with 42 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduCopter/Attitude.pde
View File

@ -586,9 +586,11 @@ static int32_t
get_of_roll(int32_t control_roll)
{
#ifdef OPTFLOW_ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
static float tot_x_cm = 0; // total distance from target
static uint32_t last_of_roll_update = 0;
int32_t new_roll = 0;
int32_t p,i,d;
// check if new optflow data available
if( optflow.last_update != last_of_roll_update) {
@ -599,17 +601,35 @@ 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); // we could use the last update time to calculate the time change
p = g.pid_optflow_roll.get_p(-tot_x_cm);
i = g.pid_optflow_roll.get_i(-tot_x_cm,1.0); // we could use the last update time to calculate the time change
d = g.pid_optflow_roll.get_d(-tot_x_cm,1.0);
new_roll = p+i+d;
}else{
g.pid_optflow_roll.reset_I();
tot_x_cm = 0;
p = 0; // for logging
i = 0;
d = 0;
}
// limit amount of change and maximum angle
of_roll = constrain(new_roll, (of_roll-20), (of_roll+20));
#if LOGGING_ENABLED == ENABLED
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) {
log_counter++;
if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
Log_Write_PID(CH6_OPTFLOW_KP, tot_x_cm, p, i, d, of_roll, tuning_value);
}
}
#endif // LOGGING_ENABLED == ENABLED
}
// limit max angle
of_roll = constrain(of_roll, -1000, 1000);
return control_roll+of_roll;
#else
return control_roll;
@ -620,9 +640,11 @@ static int32_t
get_of_pitch(int32_t control_pitch)
{
#ifdef OPTFLOW_ENABLED
static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash
static float tot_y_cm = 0; // total distance from target
static uint32_t last_of_pitch_update = 0;
int32_t new_pitch = 0;
int32_t p,i,d;
// check if new optflow data available
if( optflow.last_update != last_of_pitch_update ) {
@ -633,18 +655,36 @@ 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); // we could use the last update time to calculate the time change
p = g.pid_optflow_pitch.get_p(tot_y_cm);
i = g.pid_optflow_pitch.get_i(tot_y_cm, 1.0); // we could use the last update time to calculate the time change
d = g.pid_optflow_pitch.get_d(tot_y_cm, 1.0);
new_pitch = p + i + d;
}else{
tot_y_cm = 0;
g.pid_optflow_pitch.reset_I();
p = 0; // for logging
i = 0;
d = 0;
}
// limit amount of change
of_pitch = constrain(new_pitch, (of_pitch-20), (of_pitch+20));
#if LOGGING_ENABLED == ENABLED
// log output if PID logging is on and we are tuning the rate P, I or D gains
if( g.log_bitmask & MASK_LOG_PID && (g.radio_tuning == CH6_OPTFLOW_KP || g.radio_tuning == CH6_OPTFLOW_KI || g.radio_tuning == CH6_OPTFLOW_KD) ) {
log_counter++;
if( log_counter >= 5 ) { // (update rate / desired output rate) = (100hz / 10hz) = 10
log_counter = 0;
Log_Write_PID(CH6_OPTFLOW_KP+100, tot_y_cm, p, i, d, of_pitch, tuning_value);
}
}
#endif // LOGGING_ENABLED == ENABLED
}
// limit max angle
of_pitch = constrain(of_pitch, -1000, 1000);
return control_pitch+of_pitch;
#else
return control_pitch;