ardupilot/ArduCopter/control_ofloiter.pde

188 lines
5.9 KiB
Plaintext

/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
* control_ofloiter.pde - init and run calls for of_loiter (optical flow loiter) flight mode
*/
// ofloiter_init - initialise ofloiter controller
static bool ofloiter_init(bool ignore_checks)
{
#if OPTFLOW == ENABLED
if (g.optflow_enabled || ignore_checks) {
// initialize vertical speed and acceleration
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
pos_control.set_accel_z(g.pilot_accel_z);
// initialise altitude target to stopping point
pos_control.set_target_to_stopping_point_z();
return true;
}else{
return false;
}
#else
return false;
#endif
}
// ofloiter_run - runs the optical flow loiter controller
// should be called at 100hz or more
static void ofloiter_run()
{
int16_t target_roll, target_pitch;
float target_yaw_rate = 0;
float target_climb_rate = 0;
// if not auto armed set throttle to zero and exit immediately
if(!ap.auto_armed) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
reset_optflow_I();
return;
}
// process pilot inputs
if (!failsafe.radio) {
// apply SIMPLE mode transform to pilot inputs
update_simple_mode();
// convert pilot input to lean angles
get_pilot_desired_lean_angles(g.rc_1.control_in, g.rc_2.control_in, target_roll, target_pitch);
// get pilot's desired yaw rate
target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);
// get pilot desired climb rate
target_climb_rate = get_pilot_desired_climb_rate(g.rc_3.control_in);
// check for pilot requested take-off
if (ap.land_complete && target_climb_rate > 0) {
// indicate we are taking off
set_land_complete(false);
// clear i term when we're taking off
set_throttle_takeoff();
}
}
// when landed reset targets and output zero throttle
if (ap.land_complete) {
attitude_control.relax_bf_rate_controller();
attitude_control.set_yaw_target_to_current_heading();
attitude_control.set_throttle_out(0, false);
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);
// call attitude controller
attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
// run altitude controller
if (sonar_alt_health >= SONAR_ALT_HEALTH_MAX) {
// if sonar is ok, use surface tracking
target_climb_rate = get_throttle_surface_tracking(target_climb_rate, pos_control.get_alt_target(), G_Dt);
}
// update altitude target and call position controller
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt);
pos_control.update_z_controller();
}
}
// 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
}
// reset_optflow_I - reset optflow position hold I terms
static void reset_optflow_I(void)
{
g.pid_optflow_roll.reset_I();
g.pid_optflow_pitch.reset_I();
of_roll = 0;
of_pitch = 0;
}