diff --git a/ArduCopter/control_acro.pde b/ArduCopter/control_acro.pde index 57e07f8283..e9ee38a459 100644 --- a/ArduCopter/control_acro.pde +++ b/ArduCopter/control_acro.pde @@ -21,6 +21,7 @@ static void acro_run() // if motors not running reset angle targets if(!motors.armed() || g.rc_3.control_in <= 0) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); return; } diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde index 7316186c50..bfc553cb94 100644 --- a/ArduCopter/control_althold.pde +++ b/ArduCopter/control_althold.pde @@ -29,6 +29,7 @@ static void althold_run() if(!ap.auto_armed) { // To-Do: reset altitude target if we're somehow not landed? attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); return; } @@ -57,6 +58,7 @@ static void althold_run() // reset target lean angles and heading while landed if (ap.land_complete) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); // move throttle to minimum to keep us on the ground attitude_control.set_throttle_out(0, false); }else{ diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index 47d07d1337..718662eacb 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -98,6 +98,7 @@ static void auto_takeoff_run() if(!ap.auto_armed) { // reset attitude control targets attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // tell motors to do a slow start motors.slow_start(true); @@ -146,6 +147,7 @@ static void auto_wp_run() // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // tell motors to do a slow start motors.slow_start(true); @@ -203,6 +205,7 @@ static void auto_spline_run() // To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off // (of course it would be better if people just used take-off) attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // tell motors to do a slow start motors.slow_start(true); @@ -268,6 +271,7 @@ static void auto_land_run() // 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); // set target to current position wp_nav.init_loiter_target(); diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index 7be7c687f7..b12f12688d 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -214,6 +214,7 @@ static void autotune_run() // this should not actually be possible because of the autotune_init() checks 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); return; } @@ -241,6 +242,7 @@ static void autotune_run() // reset target lean angles and heading while landed if (ap.land_complete) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); // move throttle to minimum to keep us on the ground attitude_control.set_throttle_out(0, false); }else{ diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index 1e219f1960..fb2b52ee84 100644 --- a/ArduCopter/control_circle.pde +++ b/ArduCopter/control_circle.pde @@ -36,6 +36,7 @@ static void circle_run() if(!ap.auto_armed || ap.land_complete) { // To-Do: add some initialisation of position controllers attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); return; } diff --git a/ArduCopter/control_drift.pde b/ArduCopter/control_drift.pde index 0faee9eb2c..f7bb079fe6 100644 --- a/ArduCopter/control_drift.pde +++ b/ArduCopter/control_drift.pde @@ -45,6 +45,7 @@ static void drift_run() // if not armed or landed and throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || (ap.land_complete && g.rc_3.control_in <= 0)) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); return; } diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index 4a0303e862..aad568b2d8 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -54,6 +54,7 @@ static void guided_run() if(!ap.auto_armed) { // To-Do: reset waypoint controller? attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true return; diff --git a/ArduCopter/control_hybrid.pde b/ArduCopter/control_hybrid.pde index 8e3df6e361..754ec5d282 100644 --- a/ArduCopter/control_hybrid.pde +++ b/ArduCopter/control_hybrid.pde @@ -158,6 +158,7 @@ static void hybrid_run() if(!ap.auto_armed || !inertial_nav.position_ok()) { wp_nav.init_loiter_target(); attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); return; } @@ -186,6 +187,7 @@ static void hybrid_run() if (ap.land_complete) { wp_nav.init_loiter_target(); attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); return; }else{ diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index 2139a3c762..8751a9e1f3 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -48,6 +48,7 @@ static void land_gps_run() // if not auto armed or landed set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); wp_nav.init_loiter_target(); @@ -103,6 +104,7 @@ static void land_nogps_run() // if not auto armed or landed set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED // disarm when the landing detector says we've landed and throttle is at minimum diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index 2c11c57921..8d1575b45a 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -36,6 +36,7 @@ static void loiter_run() if(!ap.auto_armed || !inertial_nav.position_ok()) { wp_nav.init_loiter_target(); attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); return; } @@ -70,6 +71,7 @@ static void loiter_run() if (ap.land_complete) { wp_nav.init_loiter_target(); attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); }else{ // run loiter controller diff --git a/ArduCopter/control_ofloiter.pde b/ArduCopter/control_ofloiter.pde index 4e23f35ad6..23d1abf670 100644 --- a/ArduCopter/control_ofloiter.pde +++ b/ArduCopter/control_ofloiter.pde @@ -37,6 +37,7 @@ static void ofloiter_run() // 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; @@ -68,6 +69,7 @@ static void ofloiter_run() // 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{ diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index 36d6850325..77d0d1a4ce 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -128,6 +128,7 @@ static void rtl_climb_return_run() if(!ap.auto_armed) { // reset attitude control targets attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // To-Do: re-initialise wpnav targets return; @@ -185,6 +186,7 @@ static void rtl_loiterathome_run() if(!ap.auto_armed) { // reset attitude control targets attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // To-Do: re-initialise wpnav targets return; @@ -252,6 +254,7 @@ static void rtl_descent_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed || !inertial_nav.position_ok()) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // set target to current position wp_nav.init_loiter_target(); @@ -311,6 +314,7 @@ static void rtl_land_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed || !inertial_nav.position_ok()) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); // set target to current position wp_nav.init_loiter_target(); diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde index 0dba3a270d..179400af74 100644 --- a/ArduCopter/control_sport.pde +++ b/ArduCopter/control_sport.pde @@ -27,6 +27,7 @@ static void sport_run() // if not armed or throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || g.rc_3.control_in <= 0) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); return; } @@ -76,6 +77,7 @@ static void sport_run() // reset target lean angles and heading while landed if (ap.land_complete) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); // move throttle to minimum to keep us on the ground attitude_control.set_throttle_out(0, false); }else{ diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index c5635b6b4b..44c194e2a3 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -26,6 +26,7 @@ static void stabilize_run() // if not armed or throttle at zero, set throttle to zero and exit immediately if(!motors.armed() || g.rc_3.control_in <= 0) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); attitude_control.set_throttle_out(0, false); return; } @@ -46,6 +47,7 @@ static void stabilize_run() // reset target lean angles and heading while landed if (ap.land_complete) { attitude_control.relax_bf_rate_controller(); + attitude_control.set_yaw_target_to_current_heading(); }else{ // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());