Copter: implement safety spin for Tricopters

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -71,23 +71,30 @@ void AP_MotorsTri::output_min()
 // output_armed - sends commands to the motors
 void AP_MotorsTri::output_armed()
 {
-    int16_t out_min = _rc_throttle->radio_min;
+    int16_t out_min = _rc_throttle->radio_min + _min_throttle;
     int16_t out_max = _rc_throttle->radio_max;
 
     // Throttle is 0 to 1000 only
     _rc_throttle->servo_out = constrain_int16(_rc_throttle->servo_out, 0, _max_throttle);
 
-    if(_rc_throttle->servo_out > 0)
-        out_min = _rc_throttle->radio_min + _min_throttle;
-
     // capture desired roll, pitch, yaw and throttle from receiver
     _rc_roll->calc_pwm();
     _rc_pitch->calc_pwm();
     _rc_throttle->calc_pwm();
     _rc_yaw->calc_pwm();
 
-    int roll_out            = (float)_rc_roll->pwm_out * 0.866f;
-    int pitch_out           = _rc_pitch->pwm_out / 2;
+    // if we are not sending a throttle output, we cut the motors
+    if(_rc_throttle->servo_out == 0) {
+        // range check spin_when_armed
+        if (_spin_when_armed < 0) {
+            _spin_when_armed = 0;
+        }
+        motor_out[AP_MOTORS_MOT_1] = _rc_throttle->radio_min + _spin_when_armed;
+        motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_min + _spin_when_armed;
+        motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min + _spin_when_armed;
+    }else{
+        int16_t roll_out            = (float)_rc_roll->pwm_out * 0.866f;
+        int16_t pitch_out           = _rc_pitch->pwm_out / 2;
 
         //left front
         motor_out[AP_MOTORS_MOT_2] = _rc_throttle->radio_out + roll_out + pitch_out;
@@ -126,15 +133,7 @@ void AP_MotorsTri::output_armed()
         motor_out[AP_MOTORS_MOT_1] = max(motor_out[AP_MOTORS_MOT_1],    out_min);
         motor_out[AP_MOTORS_MOT_2] = max(motor_out[AP_MOTORS_MOT_2],    out_min);
         motor_out[AP_MOTORS_MOT_4] = max(motor_out[AP_MOTORS_MOT_4],    out_min);
-
-#if CUT_MOTORS == ENABLED
-    // if we are not sending a throttle output, we cut the motors
-    if(_rc_throttle->servo_out == 0) {
-        motor_out[AP_MOTORS_MOT_1]      = _rc_throttle->radio_min;
-        motor_out[AP_MOTORS_MOT_2]      = _rc_throttle->radio_min;
-        motor_out[AP_MOTORS_MOT_4] = _rc_throttle->radio_min;
     }
-#endif
 
     // send output to each motor
     hal.rcout->write(_motor_to_channel_map[AP_MOTORS_MOT_1], motor_out[AP_MOTORS_MOT_1]);