diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 1802d92b23..fb402ab424 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -115,8 +115,14 @@ void Copter::auto_takeoff_run() if(!ap.auto_armed || !motors.get_interlock()) { // initialise wpnav targets wp_nav.shift_wp_origin_to_current_pos(); +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // tell motors to do a slow start motors.slow_start(true); return; @@ -162,7 +168,13 @@ void Copter::auto_wp_run() if(!ap.auto_armed || !motors.get_interlock()) { // 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) +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // tell motors to do a slow start motors.slow_start(true); return; @@ -220,7 +232,13 @@ void Copter::auto_spline_run() if(!ap.auto_armed || !motors.get_interlock()) { // 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) +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // tell motors to do a slow start motors.slow_start(true); return; @@ -287,7 +305,13 @@ void Copter::auto_land_run() // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete) { +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // set target to current position wp_nav.init_loiter_target(); return; @@ -453,7 +477,13 @@ void Copter::auto_loiter_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif return; } diff --git a/ArduCopter/control_brake.cpp b/ArduCopter/control_brake.cpp index ddd3fb29cb..b993b04d28 100644 --- a/ArduCopter/control_brake.cpp +++ b/ArduCopter/control_brake.cpp @@ -37,7 +37,13 @@ void Copter::brake_run() // if not auto armed set throttle to zero and exit immediately if(!ap.auto_armed) { wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE); +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif pos_control.relax_alt_hold_controllers(get_throttle_pre_takeoff(0)-throttle_average); return; } diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index 2e6670678f..54ca7e84cf 100644 --- a/ArduCopter/control_circle.cpp +++ b/ArduCopter/control_circle.cpp @@ -37,7 +37,13 @@ void Copter::circle_run() // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { // To-Do: add some initialisation of position controllers +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif pos_control.set_alt_target_to_current_alt(); return; } diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index a8b28d5937..8ca63a975b 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -163,7 +163,13 @@ void Copter::guided_run() // if not auto armed or motors not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { // To-Do: reset waypoint controller? +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // To-Do: handle take-offs - these may not only be immediately after auto_armed becomes true return; } @@ -201,8 +207,14 @@ void Copter::guided_takeoff_run() if(!ap.auto_armed || !motors.get_interlock()) { // initialise wpnav targets wp_nav.shift_wp_origin_to_current_pos(); +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // tell motors to do a slow start motors.slow_start(true); return; diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 638765cbb4..bf78c84215 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -54,7 +54,13 @@ void Copter::land_gps_run() // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif wp_nav.init_loiter_target(); #if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED @@ -125,23 +131,6 @@ void Copter::land_nogps_run() float target_roll = 0.0f, target_pitch = 0.0f; float target_yaw_rate = 0; - // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately - if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { - attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); -#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED - // disarm when the landing detector says we've landed and throttle is at minimum - if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { - init_disarm_motors(); - } -#else - // disarm when the landing detector says we've landed - if (ap.land_complete) { - init_disarm_motors(); - } -#endif - return; - } - // process pilot inputs if (!failsafe.radio) { if (g.land_repositioning) { @@ -156,6 +145,30 @@ void Copter::land_nogps_run() target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->control_in); } + // if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately + if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed + attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif + +#if LAND_REQUIRE_MIN_THROTTLE_TO_DISARM == ENABLED + // disarm when the landing detector says we've landed and throttle is at minimum + if (ap.land_complete && (ap.throttle_zero || failsafe.radio)) { + init_disarm_motors(); + } +#else + // disarm when the landing detector says we've landed + if (ap.land_complete) { + init_disarm_motors(); + } +#endif + return; + } + // call attitude controller attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain()); diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index d4cd787e8b..d784732ece 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -135,8 +135,14 @@ void Copter::rtl_climb_return_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // To-Do: re-initialise wpnav targets return; } @@ -191,8 +197,14 @@ void Copter::rtl_loiterathome_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed // reset attitude control targets attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // To-Do: re-initialise wpnav targets return; } @@ -261,7 +273,13 @@ void Copter::rtl_descent_run() // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || !motors.get_interlock()) { +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // set target to current position wp_nav.init_loiter_target(); return; @@ -323,7 +341,13 @@ void Copter::rtl_land_run() float target_yaw_rate = 0; // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately if(!ap.auto_armed || ap.land_complete || !motors.get_interlock()) { +#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw + // call attitude controller + attitude_control.angle_ef_roll_pitch_rate_ef_yaw_smooth(0, 0, 0, get_smoothing_gain()); + attitude_control.set_throttle_out(0,false,g.throttle_filt); +#else // Multirotors do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); +#endif // set target to current position wp_nav.init_loiter_target();