diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index c26b4199bf..6e0598733b 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -117,27 +117,6 @@ static void guided_posvel_control_start() set_auto_yaw_mode(AUTO_YAW_HOLD); } -// initialise guided mode's spline controller -static void guided_spline_control_start() -{ - // set guided_mode to velocity controller - guided_mode = Guided_Spline; - - // initialise waypoint and spline controller - wp_nav.wp_and_spline_init(); - - // initialise wpnav to stopping point at current altitude - // To-Do: set to current location if disarmed? - // To-Do: set to stopping point altitude? - Vector3f stopping_point; - stopping_point.z = inertial_nav.get_altitude(); - wp_nav.get_wp_stopping_point_xy(stopping_point); - wp_nav.set_wp_destination(stopping_point); - - // initialise yaw - set_auto_yaw_mode(get_default_auto_yaw_mode(false)); -} - // guided_set_destination - sets guided mode's target destination static void guided_set_destination(const Vector3f& destination) { @@ -175,16 +154,6 @@ static void guided_set_destination_posvel(const Vector3f& destination, const Vec pos_control.set_pos_target(posvel_pos_target_cm); } -// set guided mode spline target -static void guided_set_destination_spline(const Vector3f& destination, const Vector3f& velocity) { - // check we are in velocity control mode - if (guided_mode != Guided_Spline) { - guided_spline_control_start(); - } - - wp_nav.set_spline_dest_and_vel(destination, velocity); -} - // guided_run - runs the guided controller // should be called at 100hz or more static void guided_run() @@ -217,11 +186,6 @@ static void guided_run() guided_vel_control_run(); break; - case Guided_Spline: - // run spline controller - guided_spline_control_run(); - break; - case Guided_PosVel: // run position-velocity controller guided_posvel_control_run(); @@ -362,36 +326,6 @@ static void guided_posvel_control_run() } } -// guided_spline_control_run - runs the guided spline controller -// called from guided_run -static void guided_spline_control_run() -{ - // process pilot's yaw input - float target_yaw_rate = 0; - if (!failsafe.radio) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in); - if (target_yaw_rate != 0) { - set_auto_yaw_mode(AUTO_YAW_HOLD); - } - } - - // run waypoint controller - wp_nav.update_spline(); - - // call z-axis position controller (wpnav should have already updated it's alt target) - pos_control.update_z_controller(); - - // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { - // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control.angle_ef_roll_pitch_rate_ef_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); - }else{ - // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control.angle_ef_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true); - } -} - // Guided Limit code // guided_limit_clear - clear/turn off guided limits diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 1c97cb7ba8..e6a10ac191 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -189,7 +189,6 @@ enum GuidedMode { Guided_TakeOff, Guided_WP, Guided_Velocity, - Guided_Spline, Guided_PosVel };