diff --git a/ArduCopter/control_althold.pde b/ArduCopter/control_althold.pde index b1b9beb084..265758c563 100644 --- a/ArduCopter/control_althold.pde +++ b/ArduCopter/control_althold.pde @@ -69,10 +69,4 @@ static void althold_run() pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); pos_control.update_z_controller(); } - - // refetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } diff --git a/ArduCopter/control_auto.pde b/ArduCopter/control_auto.pde index dc6315d3e9..d54a1b8059 100644 --- a/ArduCopter/control_auto.pde +++ b/ArduCopter/control_auto.pde @@ -71,6 +71,9 @@ static void auto_takeoff_start(float final_alt) // initialise yaw set_auto_yaw_mode(AUTO_YAW_HOLD); + + // tell motors to do a slow start + motors.slow_start(true); } // auto_takeoff_run - takeoff in auto mode @@ -82,6 +85,8 @@ static void auto_takeoff_run() // reset attitude control targets attitude_control.init_targets(); attitude_control.set_throttle_out(0, false); + // tell motors to do a slow start + motors.slow_start(true); // To-Do: re-initialise wpnav targets return; } @@ -101,12 +106,6 @@ static void auto_takeoff_run() // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); - - // refetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } // auto_wp_start - initialises waypoint controller to implement flying to a particular destination @@ -129,9 +128,10 @@ static void auto_wp_run() if(!ap.auto_armed) { // 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) - // To-Do: set motors to slow start attitude_control.init_targets(); attitude_control.set_throttle_out(0, false); + // tell motors to do a slow start + motors.slow_start(true); return; } @@ -159,12 +159,6 @@ static void auto_wp_run() // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true); } - - // refetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } // auto_land_start - initialises controller to implement a landing @@ -222,12 +216,6 @@ static void auto_land_run() // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.angleef_rp_rateef_y(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); - - // refetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } // auto_rtl_start - initialises RTL in AUTO flight mode @@ -268,12 +256,6 @@ void auto_circle_run() // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.angleef_rpy(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true); - - // refetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } // get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter diff --git a/ArduCopter/control_autotune.pde b/ArduCopter/control_autotune.pde index 56a4fa8c3a..530aff4560 100644 --- a/ArduCopter/control_autotune.pde +++ b/ArduCopter/control_autotune.pde @@ -271,12 +271,6 @@ static void autotune_run() pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); pos_control.update_z_controller(); } - - // refetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } // autotune_attitude_controller - sets attitude control targets during tuning diff --git a/ArduCopter/control_circle.pde b/ArduCopter/control_circle.pde index a296352075..3fb70ca99d 100644 --- a/ArduCopter/control_circle.pde +++ b/ArduCopter/control_circle.pde @@ -69,10 +69,4 @@ static void circle_run() // 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(); - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } diff --git a/ArduCopter/control_drift.pde b/ArduCopter/control_drift.pde index e8b2bb87bc..ccf6c0d705 100644 --- a/ArduCopter/control_drift.pde +++ b/ArduCopter/control_drift.pde @@ -66,10 +66,4 @@ static void drift_run() // call attitude controller attitude_control.angleef_rp_rateef_y(target_roll, target_pitch, target_yaw_rate); - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } diff --git a/ArduCopter/control_guided.pde b/ArduCopter/control_guided.pde index 064565b827..56eefe8c97 100644 --- a/ArduCopter/control_guided.pde +++ b/ArduCopter/control_guided.pde @@ -88,10 +88,4 @@ static void guided_run() // roll, pitch from waypoint controller, yaw heading from auto_heading() attitude_control.angleef_rpy(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true); } - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } diff --git a/ArduCopter/control_land.pde b/ArduCopter/control_land.pde index c19687e822..d6de4a89ca 100644 --- a/ArduCopter/control_land.pde +++ b/ArduCopter/control_land.pde @@ -84,12 +84,6 @@ static void land_gps_run() // update altitude target and call position controller pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt); pos_control.update_z_controller(); - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } // land_nogps_run - runs the land controller @@ -136,12 +130,6 @@ static void land_nogps_run() // call position controller pos_control.set_alt_target_from_climb_rate(get_throttle_land(), G_Dt); pos_control.update_z_controller(); - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } // get_throttle_land - high level landing logic diff --git a/ArduCopter/control_loiter.pde b/ArduCopter/control_loiter.pde index daf4c552b6..a1a977328e 100644 --- a/ArduCopter/control_loiter.pde +++ b/ArduCopter/control_loiter.pde @@ -81,10 +81,4 @@ static void loiter_run() pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); pos_control.update_z_controller(); } - - // refetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } diff --git a/ArduCopter/control_ofloiter.pde b/ArduCopter/control_ofloiter.pde index 87dc103653..2b3c3076c1 100644 --- a/ArduCopter/control_ofloiter.pde +++ b/ArduCopter/control_ofloiter.pde @@ -80,12 +80,6 @@ static void ofloiter_run() pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); pos_control.update_z_controller(); } - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } diff --git a/ArduCopter/control_rtl.pde b/ArduCopter/control_rtl.pde index f68ea4664a..65b5248701 100644 --- a/ArduCopter/control_rtl.pde +++ b/ArduCopter/control_rtl.pde @@ -161,12 +161,6 @@ static void rtl_climb_return_descent_run() // check if we've completed this stage of RTL rtl_state_complete = wp_nav.reached_wp_destination(); - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } // rtl_return_start - initialise return to home @@ -225,12 +219,6 @@ static void rtl_loiterathome_run() // check if we've completed this stage of RTL // To-Do: add extra check that we've reached the target yaw rtl_state_complete = ((millis() - rtl_loiter_start_time) > (uint32_t)g.rtl_loiter_time.get()); - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } // rtl_loiterathome_start - initialise controllers to loiter over home @@ -282,11 +270,5 @@ static void rtl_land_run() // check if we've completed this stage of RTL rtl_state_complete = ap.land_complete; - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde index fafb5b813b..f30cdba823 100644 --- a/ArduCopter/control_sport.pde +++ b/ArduCopter/control_sport.pde @@ -64,10 +64,4 @@ static void sport_run() pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt); pos_control.update_z_controller(); } - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } diff --git a/ArduCopter/control_stabilize.pde b/ArduCopter/control_stabilize.pde index c3b359142e..cfa5ba1034 100644 --- a/ArduCopter/control_stabilize.pde +++ b/ArduCopter/control_stabilize.pde @@ -55,10 +55,4 @@ static void stabilize_run() // output pilot's throttle attitude_control.set_throttle_out(pilot_throttle_scaled, true); - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } diff --git a/ArduCopter/heli_control_stabilize.pde b/ArduCopter/heli_control_stabilize.pde index c89d81fbf9..12e83d3082 100644 --- a/ArduCopter/heli_control_stabilize.pde +++ b/ArduCopter/heli_control_stabilize.pde @@ -41,12 +41,6 @@ static void heli_stabilize_run() // output pilot's throttle - note that TradHeli does not used angle-boost attitude_control.set_throttle_out(pilot_throttle_scaled, false); - - // re-fetch angle targets for reporting - const Vector3f angle_target = attitude_control.angle_ef_targets(); - control_roll = angle_target.x; - control_pitch = angle_target.y; - control_yaw = angle_target.z; } #endif //HELI_FRAME