AP_Motors: Tailsitter output using SRV_Channels instead of rc_write

libraries/AP_Motors/AP_MotorsTailsitter.cpp

@@ -26,20 +26,32 @@
 extern const AP_HAL::HAL& hal;
 
 #define SERVO_OUTPUT_RANGE  4500
-#define THROTTLE_RANGE       100
 
 // init
 void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type frame_type)
 {
-    // make sure 4 output channels are mapped
+    // setup default motor and servo mappings
-    add_motor_num(AP_MOTORS_THROTTLE_LEFT);
+    uint8_t chan;
-    add_motor_num(AP_MOTORS_THROTTLE_RIGHT);
-    add_motor_num(AP_MOTORS_TILT_LEFT);
-    add_motor_num(AP_MOTORS_TILT_RIGHT);
 
-    // set the motor_enabled flag so that the main ESC can be calibrated like other frame types
+    // right throttle defaults to servo output 1
-    motor_enabled[AP_MOTORS_THROTTLE_LEFT] = true;
+    SRV_Channels::set_aux_channel_default(SRV_Channel::k_throttleRight, CH_1);
-    motor_enabled[AP_MOTORS_THROTTLE_RIGHT] = true;
+    if (SRV_Channels::find_channel(SRV_Channel::k_throttleRight, chan)) {
+        motor_enabled[chan] = true;
+    }
+
+    // left throttle defaults to servo output 2
+    SRV_Channels::set_aux_channel_default(SRV_Channel::k_throttleLeft, CH_2);
+    if (SRV_Channels::find_channel(SRV_Channel::k_throttleLeft, chan)) {
+        motor_enabled[chan] = true;
+    }
+
+    // right servo defaults to servo output 3
+    SRV_Channels::set_aux_channel_default(SRV_Channel::k_tiltMotorRight, CH_3);
+    SRV_Channels::set_angle(SRV_Channel::k_tiltMotorRight, SERVO_OUTPUT_RANGE);
+
+    // left servo defaults to servo output 4
+    SRV_Channels::set_aux_channel_default(SRV_Channel::k_tiltMotorLeft, CH_4);
+    SRV_Channels::set_angle(SRV_Channel::k_tiltMotorLeft, SERVO_OUTPUT_RANGE);
 
     // record successful initialisation if what we setup was the desired frame_class
     _flags.initialised_ok = (frame_class == MOTOR_FRAME_TAILSITTER);
@@ -50,8 +62,7 @@ void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type f
 AP_MotorsTailsitter::AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz) :
     AP_MotorsMulticopter(loop_rate, speed_hz)
 {
-    SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleLeft, speed_hz);
+    set_update_rate(speed_hz);
-    SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleRight, speed_hz);
 }
 
 
@@ -61,10 +72,8 @@ void AP_MotorsTailsitter::set_update_rate(uint16_t speed_hz)
     // record requested speed
     _speed_hz = speed_hz;
 
-    uint32_t mask =
+    SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleLeft, speed_hz);
-        1U << AP_MOTORS_THROTTLE_LEFT |
+    SRV_Channels::set_rc_frequency(SRV_Channel::k_throttleRight, speed_hz);
-        1U << AP_MOTORS_THROTTLE_RIGHT;
-    rc_set_freq(mask, _speed_hz);
 }
 
 void AP_MotorsTailsitter::output_to_motors()
@@ -72,59 +81,56 @@ void AP_MotorsTailsitter::output_to_motors()
     if (!_flags.initialised_ok) {
         return;
     }
-    float throttle = 0.0f;
+    float throttle_pwm = 0.0f;
 
     switch (_spool_mode) {
         case SHUT_DOWN:
-            throttle = get_pwm_output_min();
+            throttle_pwm = get_pwm_output_min();
-            rc_write(AP_MOTORS_THROTTLE_LEFT, get_pwm_output_min());
+            SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, get_pwm_output_min());
-            rc_write(AP_MOTORS_THROTTLE_RIGHT, get_pwm_output_min());
+            SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, get_pwm_output_min());
             break;
         case SPIN_WHEN_ARMED:
-            throttle = constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min;
+            throttle_pwm = calc_spin_up_to_pwm();
-            rc_write(AP_MOTORS_THROTTLE_LEFT, calc_spin_up_to_pwm());
+            SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, calc_spin_up_to_pwm());
-            rc_write(AP_MOTORS_THROTTLE_RIGHT, calc_spin_up_to_pwm());
+            SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, calc_spin_up_to_pwm());
             break;
         case SPOOL_UP:
         case THROTTLE_UNLIMITED:
         case SPOOL_DOWN:
-            throttle = calc_thrust_to_pwm(_throttle);
+            throttle_pwm = calc_thrust_to_pwm(_throttle);
-            rc_write(AP_MOTORS_THROTTLE_LEFT, calc_thrust_to_pwm(_thrust_left));
+            SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, calc_thrust_to_pwm(_thrust_left));
-            rc_write(AP_MOTORS_THROTTLE_RIGHT, calc_thrust_to_pwm(_thrust_right));
+            SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, calc_thrust_to_pwm(_thrust_right));
             break;
     }
 
-    // Always output to tilts
+    // Always output to tilt servos
-    rc_write_angle(AP_MOTORS_TILT_LEFT, _tilt_left*SERVO_OUTPUT_RANGE);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, _tilt_left*SERVO_OUTPUT_RANGE);
-    rc_write_angle(AP_MOTORS_TILT_RIGHT, _tilt_right*SERVO_OUTPUT_RANGE);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, _tilt_right*SERVO_OUTPUT_RANGE);
 
     // plane outputs for Qmodes are setup here, and written to the HAL by the plane servos loop
     SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, -_yaw_in*SERVO_OUTPUT_RANGE);
     SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, _pitch_in*SERVO_OUTPUT_RANGE);
     SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, _roll_in*SERVO_OUTPUT_RANGE);
-    SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, throttle);
+    SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, throttle_pwm);
-
-#if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
-    SRV_Channels::calc_pwm();
-    SRV_Channels::output_ch_all();
-#endif
 }
 
-// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
+// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
 //  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
 uint16_t AP_MotorsTailsitter::get_motor_mask()
 {
-    uint32_t motor_mask =
+    uint32_t motor_mask = 0;
-        1U << AP_MOTORS_THROTTLE_LEFT |
+    uint8_t chan;
-        1U << AP_MOTORS_THROTTLE_RIGHT |
+    if (SRV_Channels::find_channel(SRV_Channel::k_throttleLeft, chan)) {
-        1U << AP_MOTORS_TILT_LEFT |
+        motor_mask |= 1U << chan;
-        1U << AP_MOTORS_TILT_RIGHT;
+    }
-    uint16_t mask = rc_map_mask(motor_mask);
+    if (SRV_Channels::find_channel(SRV_Channel::k_throttleRight, chan)) {
+        motor_mask |= 1U << chan;
+    }
 
     // add parent's mask
-    mask |= AP_MotorsMulticopter::get_motor_mask();
+    motor_mask |= AP_MotorsMulticopter::get_motor_mask();
 
-    return mask;
+    return motor_mask;
 }
 
 // calculate outputs to the motors
@@ -189,20 +195,20 @@ void AP_MotorsTailsitter::output_test_seq(uint8_t motor_seq, int16_t pwm)
     // output to motors and servos
     switch (motor_seq) {
         case 1:
-            // throttle left
+            // right throttle
-            rc_write(AP_MOTORS_THROTTLE_LEFT, pwm);
+            SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, pwm);
             break;
         case 2:
-            // throttle right
+            // right tilt servo
-            rc_write(AP_MOTORS_THROTTLE_RIGHT, pwm);
+            SRV_Channels::set_output_pwm(SRV_Channel::k_tiltMotorRight, pwm);
             break;
         case 3:
-            // tilt left
+            // left throttle
-            rc_write(AP_MOTORS_TILT_LEFT, pwm);
+            SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, pwm);
             break;
         case 4:
-            // tilt right
+            // left tilt servo
-            rc_write(AP_MOTORS_TILT_RIGHT, pwm);
+            SRV_Channels::set_output_pwm(SRV_Channel::k_tiltMotorLeft, pwm);
             break;
         default:
             // do nothing