diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 913b897004..dc11c8907d 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -375,12 +375,12 @@ bool Copter::Mode::_TakeOff::triggered(const float target_climb_rate) const void Copter::Mode::zero_throttle_and_relax_ac() { + motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); #if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); attitude_control->set_throttle_out(0.0f, false, copter.g.throttle_filt); #else - motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); // multicopters do not stabilize roll/pitch/yaw when disarmed attitude_control->set_throttle_out_unstabilized(0.0f, true, copter.g.throttle_filt); #endif diff --git a/ArduCopter/mode_acro_heli.cpp b/ArduCopter/mode_acro_heli.cpp index 69b8282172..94461acf71 100644 --- a/ArduCopter/mode_acro_heli.cpp +++ b/ArduCopter/mode_acro_heli.cpp @@ -45,6 +45,8 @@ void Copter::ModeAcro_Heli::run() set_land_complete(false); } + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + if (!motors->has_flybar()){ // convert the input to the desired body frame rate get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw); diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index c160598675..0bd49de4f5 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -103,19 +103,22 @@ void Copter::ModeAltHold::run() break; case AltHold_Landed: + +#if FRAME_CONFIG == HELI_FRAME + // helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot. + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + + if (motors->init_targets_on_arming()) { + attitude_control->reset_rate_controller_I_terms(); + attitude_control->set_yaw_target_to_current_heading(); + } +#else // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); } else { motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } - -#if FRAME_CONFIG == HELI_FRAME - if (motors->init_targets_on_arming()) { - attitude_control->reset_rate_controller_I_terms(); - attitude_control->set_yaw_target_to_current_heading(); - } -#else attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); #endif diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 977b669be2..c18687714f 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -334,13 +334,17 @@ void Copter::ModeFlowHold::run() break; case FlowHold_Landed: +#if FRAME_CONFIG == HELI_FRAME + // helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot. + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); +#else // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { copter.motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); } else { copter.motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } - +#endif copter.attitude_control->reset_rate_controller_I_terms(); copter.attitude_control->set_yaw_target_to_current_heading(); copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(bf_angles.x, bf_angles.y, target_yaw_rate); diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index b89c3ebc9b..f63c593cd1 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -180,12 +180,17 @@ void Copter::ModeLoiter::run() break; case Loiter_Landed: +#if FRAME_CONFIG == HELI_FRAME + // helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot. + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); +#else // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); } else { motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } +#endif loiter_nav->init_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index b22d020cc6..d5a8d5ead8 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -184,12 +184,17 @@ void Copter::ModePosHold::run() // if landed initialise loiter targets, set throttle to zero and exit if (ap.land_complete) { +#if FRAME_CONFIG == HELI_FRAME + // helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot. + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); +#else // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); } else { motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } +#endif loiter_nav->init_target(); attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index 77b2ca1aeb..9c95cfcd83 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -130,13 +130,17 @@ void Copter::ModeSport::run() break; case Sport_Landed: +#if FRAME_CONFIG == HELI_FRAME + // helicopters do not spool down when landed. Only when commanded to go to ground idle by pilot. + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); +#else // set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle) if (target_climb_rate < 0.0f) { motors->set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); } else { motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); } - +#endif attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate); diff --git a/ArduCopter/mode_stabilize_heli.cpp b/ArduCopter/mode_stabilize_heli.cpp index 6e720c83a2..696ad9e3ac 100644 --- a/ArduCopter/mode_stabilize_heli.cpp +++ b/ArduCopter/mode_stabilize_heli.cpp @@ -38,6 +38,8 @@ void Copter::ModeStabilize_Heli::run() set_land_complete(false); } + motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + // apply SIMPLE mode transform to pilot inputs update_simple_mode();