diff --git a/ArduCopter/control_ofloiter.pde b/ArduCopter/control_ofloiter.pde deleted file mode 100644 index f591f38b54..0000000000 --- a/ArduCopter/control_ofloiter.pde +++ /dev/null @@ -1,168 +0,0 @@ -/// -*- 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 - */ - -#if OPTFLOW == ENABLED - -#define OPTFLOW_ALT_MAX_CM 1500 // maximum altitude above home that optical flow sensor will be used -#define OPTFLOW_TIMEOUT_MS 200 // timeout in milliseconds after which we will give up on optical flow readings and return control to the pilot -#define OPTFLOW_RP_RATE_LIM (2000/MAIN_LOOP_RATE) // limit in centi-degrees/sec on rate of change of roll-pitch target. Equal to 20deg/sec - -// ofloiter_init - initialise ofloiter controller -static bool ofloiter_init(bool 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); - pos_control.set_accel_z(g.pilot_accel_z); - - // 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; - } -} - -// 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 final_roll, final_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); - pos_control.set_alt_target_to_current_alt(); - of_roll = ahrs.roll_sensor; - of_pitch = ahrs.pitch_sensor; - 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(); - // 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; - }else{ - // mix in user control with optical flow - 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(final_roll, final_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 void get_of_roll_pitch(int16_t input_roll, int16_t input_pitch, float &roll_out, float &pitch_out) -{ - static uint32_t last_of_update = 0; - float dt; - Vector2f vel; - - // 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_update) { - - // calculate dt and sanity check - dt = (optflow.last_update() - last_of_update) / 1000.0f; - if (dt > 0.2f) { - dt = 0.0f; - g.pid_optflow_roll.reset_I(); - g.pid_optflow_pitch.reset_I(); - } - last_of_update = optflow.last_update(); - - // get latest velocity from sensor - vel = optflow.velocity(); - } - - // calculate time since last update - uint32_t time_since_update_ms = millis() - last_of_update; - - // use pilot roll input if input is non-zero, altitude above 15m or optical flow sensor has timed out - if (input_roll != 0 || current_loc.alt > OPTFLOW_ALT_MAX_CM || time_since_update_ms > OPTFLOW_TIMEOUT_MS) { - roll_out = input_roll; - } else { - // run velocity through pid controller - roll_out = g.pid_optflow_roll.get_pid(-vel.x, dt); - - // limit amount of change and maximum angle - // To-Do: replace reliance on of_roll, of_pitch within this function - roll_out = constrain_float(roll_out, (of_roll-OPTFLOW_RP_RATE_LIM), (of_roll+OPTFLOW_RP_RATE_LIM)); - } - - // use pilot pitch input if input is non-zero, altitude above 15m or optical flow sensor has timed out - if (input_pitch != 0 || current_loc.alt > OPTFLOW_ALT_MAX_CM || time_since_update_ms > OPTFLOW_TIMEOUT_MS) { - pitch_out = input_pitch; - } else { - // run velocity through pid controller - pitch_out = g.pid_optflow_pitch.get_pid(vel.y, dt); - - // limit amount of change and maximum angle - // To-Do: replace reliance on of_roll, of_pitch within this function - pitch_out = constrain_float(pitch_out, (of_pitch-OPTFLOW_RP_RATE_LIM), (of_pitch+OPTFLOW_RP_RATE_LIM)); - } -} - -// 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(); -} - -#endif // OPTFLOW == ENABLED