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

Copter: get_of_roll, get_of_pitch moved to control_ofloiter

Browse Source
This commit is contained in:
Randy Mackay 2014-01-29 13:35:44 +09:00 committed by Andrew Tridgell
parent 837061fd65
commit c632211c8c
1 changed files with 0 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
ArduCopter/Attitude.pde
View File

@ -472,91 +472,6 @@ void set_yaw_rate_target( int32_t desired_rate, uint8_t earth_or_body_frame ) {
}
}
// calculate modified roll/pitch depending upon optical flow calculated position
static int32_t
get_of_roll(int32_t input_roll)
{
#if OPTFLOW == ENABLED
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) {
last_of_roll_update = optflow.last_update;
// add new distance moved
tot_x_cm += optflow.x_cm;
// only stop roll if caller isn't modifying roll
if( input_roll == 0 && current_loc.alt < 1500) {
p = g.pid_optflow_roll.get_p(-tot_x_cm);
i = g.pid_optflow_roll.get_i(-tot_x_cm,1.0f); // we could use the last update time to calculate the time change
d = g.pid_optflow_roll.get_d(-tot_x_cm,1.0f);
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_int32(new_roll, (of_roll-20), (of_roll+20));
}
// limit max angle
of_roll = constrain_int32(of_roll, -1000, 1000);
return input_roll+of_roll;
#else
return input_roll;
#endif
}
static int32_t
get_of_pitch(int32_t input_pitch)
{
#if OPTFLOW == ENABLED
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 ) {
last_of_pitch_update = optflow.last_update;
// add new distance moved
tot_y_cm += optflow.y_cm;
// only stop roll if caller isn't modifying pitch
if( input_pitch == 0 && current_loc.alt < 1500 ) {
p = g.pid_optflow_pitch.get_p(tot_y_cm);
i = g.pid_optflow_pitch.get_i(tot_y_cm, 1.0f); // we could use the last update time to calculate the time change
d = g.pid_optflow_pitch.get_d(tot_y_cm, 1.0f);
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_int32(new_pitch, (of_pitch-20), (of_pitch+20));
}
// limit max angle
of_pitch = constrain_int32(of_pitch, -1000, 1000);
return input_pitch+of_pitch;
#else
return input_pitch;
#endif
}
/*************************************************************
* yaw controllers