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

Copter: Of_Loiter uses sensor velocity instead of integrated position

Browse Source
This commit is contained in:
Randy Mackay 2014-07-12 11:36:14 +09:00
parent 3201a8dbca
commit b2e167f9a5
1 changed files with 47 additions and 71 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
ArduCopter/control_ofloiter.pde
View File

@ -9,7 +9,7 @@
// ofloiter_init - initialise ofloiter controller
static bool ofloiter_init(bool ignore_checks)
{
if (g.optflow_enabled || ignore_checks) {
if (optflow.enabled() || ignore_checks) {
// initialize vertical speed and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
@ -18,6 +18,11 @@ static bool ofloiter_init(bool ignore_checks)
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
// initialise of_roll, pitch to current attitude
of_roll = ahrs.roll_sensor;
of_pitch = ahrs.pitch_sensor;
reset_optflow_I();
return true;
}else{
return false;
@ -29,6 +34,7 @@ static bool ofloiter_init(bool ignore_checks)
static void ofloiter_run()
{
int16_t target_roll, target_pitch;
float final_roll, final_pitch;
float target_yaw_rate = 0;
float target_climb_rate = 0;
@ -38,6 +44,8 @@ static void ofloiter_run()
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
pos_control.set_alt_target_to_current_alt();
of_roll = ahrs.roll_sensor;
of_pitch = ahrs.pitch_sensor;
reset_optflow_I();
return;
}
@ -72,14 +80,15 @@ static void ofloiter_run()
// move throttle to between minimum and non-takeoff-throttle to keep us on the ground
attitude_control.set_throttle_out(get_throttle_pre_takeoff(g.rc_3.control_in), false);
pos_control.set_alt_target_to_current_alt();
of_roll = ahrs.roll_sensor;
of_pitch = ahrs.pitch_sensor;
reset_optflow_I();
}else{
// mix in user control with optical flow
target_roll = get_of_roll(target_roll);
target_pitch = get_of_pitch(target_pitch);
get_of_roll_pitch(target_roll, target_pitch, final_roll, final_pitch);
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(final_roll, final_pitch, target_yaw_rate, get_smoothing_gain());
// run altitude controller
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
@ -93,81 +102,50 @@ static void ofloiter_run()
}
}
// calculate modified roll/pitch depending upon optical flow calculated position
static int32_t get_of_roll(int32_t input_roll)
static void get_of_roll_pitch(int16_t input_roll, int16_t input_pitch, float &roll_out, float &pitch_out)
{
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;
static uint32_t last_of_update = 0;
float dt;
// To-Do: pass input_roll, input_pitch through to roll_out, pitch_out if input is non-zero or previous iteration was non-zero
// check if new optflow data available
if( optflow.last_update != last_of_roll_update) {
last_of_roll_update = optflow.last_update;
if (optflow.last_update() != last_of_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{
// calculate dt and sanity check
dt = (optflow.last_update() - last_of_update) / 1000.0f;
if (dt > 0.2) {
dt = 0;
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;
}
static int32_t get_of_pitch(int32_t input_pitch)
{
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;
}
last_of_update = optflow.last_update();
// limit amount of change
of_pitch = constrain_int32(new_pitch, (of_pitch-20), (of_pitch+20));
// get latest velocity from sensor
const Vector2f &vel = optflow.velocity();
// allow pilot override of roll
if (input_roll == 0 && current_loc.alt < 1500) {
roll_out = g.pid_optflow_roll.get_pid(-vel.x, dt);
// limit amount of change and maximum angle
roll_out = constrain_float(roll_out, (of_roll-20), (of_roll+20));
} else {
roll_out = input_roll;
}
of_roll = roll_out;
if (input_pitch == 0 && current_loc.alt < 1500) {
pitch_out = g.pid_optflow_pitch.get_pid(vel.y, dt);
pitch_out = constrain_float(pitch_out, (of_pitch-20), (of_pitch+20));
} else {
pitch_out = input_pitch;
}
of_pitch = pitch_out;
} else {
roll_out = of_roll;
pitch_out = of_pitch;
}
// limit max angle
of_pitch = constrain_int32(of_pitch, -1000, 1000);
return input_pitch+of_pitch;
}
// reset_optflow_I - reset optflow position hold I terms
@ -175,8 +153,6 @@ static void reset_optflow_I(void)
{
g.pid_optflow_roll.reset_I();
g.pid_optflow_pitch.reset_I();
of_roll = 0;
of_pitch = 0;
}
#endif // OPTFLOW == ENABLED