Copter: heli: Update swashplate behavior

change swashplate behavior on ground in Acro, Stabilize, and AltHold
flight modes. See discussion here:

https://discuss.ardupilot.org/t/tradheli-swashplate-behavior-while-on-the-ground-and-potential-fix-to-issue-5396/22463/18

ArduCopter/control_althold.cpp

@@ -74,18 +74,24 @@ void Copter::althold_run()
 
         motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
+        attitude_control->reset_rate_controller_I_terms();
+        attitude_control->set_yaw_target_to_current_heading();
 #if FRAME_CONFIG == HELI_FRAME    
         // force descent rate and call position controller
         pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
+        heli_flags.init_targets_on_arming=true;
 #else
-        attitude_control->reset_rate_controller_I_terms();
-        attitude_control->set_yaw_target_to_current_heading();
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
 #endif
         pos_control->update_z_controller();
         break;
 
     case AltHold_Takeoff:
+#if FRAME_CONFIG == HELI_FRAME    
+        if (heli_flags.init_targets_on_arming) {
+            heli_flags.init_targets_on_arming=false;
+        }
+#endif
         // set motors to full range
         motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
 
@@ -121,8 +127,18 @@ void Copter::althold_run()
             motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
         }
 
+#if FRAME_CONFIG == HELI_FRAME    
+        if (heli_flags.init_targets_on_arming) {
+            attitude_control->reset_rate_controller_I_terms();
+            attitude_control->set_yaw_target_to_current_heading();
+            if (motors->get_interlock()) {
+                heli_flags.init_targets_on_arming=false;
+            }
+        }
+#else
         attitude_control->reset_rate_controller_I_terms();
         attitude_control->set_yaw_target_to_current_heading();
+#endif
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
         pos_control->relax_alt_hold_controllers(0.0f);   // forces throttle output to go to zero
         pos_control->update_z_controller();

ArduCopter/heli_control_acro.cpp

@@ -35,12 +35,14 @@ void Copter::heli_acro_run()
     
     if(!motors->armed()) {
         heli_flags.init_targets_on_arming=true;
-        attitude_control->set_yaw_target_to_current_heading();
+        attitude_control->set_attitude_target_to_current_attitude();
+        attitude_control->reset_rate_controller_I_terms();
     }
     
     if(motors->armed() && heli_flags.init_targets_on_arming) {
-        attitude_control->set_yaw_target_to_current_heading();
-        if (motors->rotor_speed_above_critical()) {
+        attitude_control->set_attitude_target_to_current_attitude();
+        attitude_control->reset_rate_controller_I_terms();
+        if (motors->get_interlock()) {
             heli_flags.init_targets_on_arming=false;
         }
     }   

ArduCopter/heli_control_stabilize.cpp

@@ -35,11 +35,13 @@ void Copter::heli_stabilize_run()
     if(!motors->armed()) {
         heli_flags.init_targets_on_arming=true;
         attitude_control->set_yaw_target_to_current_heading();
+        attitude_control->reset_rate_controller_I_terms();
     }
     
     if(motors->armed() && heli_flags.init_targets_on_arming) {
         attitude_control->set_yaw_target_to_current_heading();
-        if (motors->rotor_speed_above_critical()) {
+        attitude_control->reset_rate_controller_I_terms();
+        if (motors->get_interlock()) {
             heli_flags.init_targets_on_arming=false;
         }
     }