Copter: fixed aux channel outputs in RC failsafe

ArduCopter/esc_calibration.cpp

@@ -100,6 +100,7 @@ void Copter::esc_calibration_passthrough()
     // arm motors
     motors->armed(true);
     motors->enable();
+    SRV_Channels::enable_by_mask(motors->get_motor_mask());
 
     uint32_t last_notify_update_ms = 0;
     while(1) {
@@ -144,6 +145,7 @@ void Copter::esc_calibration_auto()
     // arm and enable motors
     motors->armed(true);
     motors->enable();
+    SRV_Channels::enable_by_mask(motors->get_motor_mask());
 
     // flash LEDS
     AP_Notify::flags.esc_calibration = true;

ArduCopter/motors.cpp

@@ -292,6 +292,12 @@ void Copter::motors_output()
 
     // output any servo channels
     SRV_Channels::calc_pwm();
+
+    // cork now, so that all channel outputs happen at once
+    hal.rcout->cork();
+
+    // update output on any aux channels, for manual passthru
+    SRV_Channels::output_ch_all();
 
     // check if we are performing the motor test
     if (ap.motor_test) {
@@ -309,6 +315,9 @@ void Copter::motors_output()
         // send output signals to motors
         motors->output();
     }
+
+    // push all channels
+    hal.rcout->push();
 }
 
 // check for pilot stick input to trigger lost vehicle alarm

ArduCopter/radio.cpp

@@ -99,9 +99,6 @@ void Copter::read_radio()
             ap.rc_receiver_present = true;
         }
 
-        // update output on any aux channels, for manual passthru
-        SRV_Channels::output_ch_all();
-
         // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters)
         radio_passthrough_to_motors();
 

libraries/AP_HAL_Linux/RCOutput_AeroIO.cpp

@@ -132,6 +132,7 @@ void RCOutput_AeroIO::set_freq(uint32_t chmask, uint16_t freq_hz)
     }
 
     if (!_corking) {
+        _corking = true;
         push();
     }
 }
@@ -150,6 +151,7 @@ void RCOutput_AeroIO::enable_ch(uint8_t ch)
         return;
     }
     _pending_duty_write_mask |= (1U << ch);
+    _corking = true;
     push();
 }
 
@@ -160,6 +162,7 @@ void RCOutput_AeroIO::disable_ch(uint8_t ch)
     }
     _duty_buffer[ch] = 0;
     _pending_duty_write_mask |= (1U << ch);
+    _corking = true;
     push();
 }
 
@@ -169,6 +172,7 @@ void RCOutput_AeroIO::write(uint8_t ch, uint16_t period_us)
     _duty_buffer[ch] = period_us;
 
     if (!_corking) {
+        _corking = true;
         push();
     }
 }
@@ -180,6 +184,9 @@ void RCOutput_AeroIO::cork()
 
 void RCOutput_AeroIO::push()
 {
+    if (!_corking) {
+        return;
+    }
     _corking = false;
 
     for (uint8_t i = 0; i < PWM_CHAN_COUNT; i++) {

libraries/AP_HAL_Linux/RCOutput_AioPRU.cpp

@@ -142,6 +142,9 @@ void RCOutput_AioPRU::cork(void)
 
 void RCOutput_AioPRU::push(void)
 {
+    if (!corked) {
+        return;
+    }
     corked = false;
     for (uint8_t i=0; i<PWM_CHAN_COUNT; i++) {
         if (pending_mask & (1U<<i)) {

libraries/AP_HAL_Linux/RCOutput_Bebop.cpp

@@ -418,6 +418,9 @@ void RCOutput_Bebop::cork()
 
 void RCOutput_Bebop::push()
 {
+    if (!_corking) {
+        return;
+    }
     _corking = false;
     pthread_mutex_lock(&_mutex);
     memcpy(_period_us, _request_period_us, sizeof(_period_us));

libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp

@@ -176,8 +176,10 @@ void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us)
     _pulses_buffer[ch] = period_us;
     _pending_write_mask |= (1U << ch);
 
-    if (!_corking)
+    if (!_corking) {
+        _corking = true;
         push();
+    }
 }
 
 void RCOutput_PCA9685::cork()
@@ -187,6 +189,9 @@ void RCOutput_PCA9685::cork()
 
 void RCOutput_PCA9685::push()
 {
+    if (!_corking) {
+        return;
+    }
     _corking = false;
 
     if (_pending_write_mask == 0)

libraries/AP_HAL_Linux/RCOutput_PRU.cpp

@@ -99,6 +99,9 @@ void RCOutput_PRU::cork(void)
 
 void RCOutput_PRU::push(void)
 {
+    if (!corked) {
+        return;
+    }
     corked = false;
     for (uint8_t i=0; i<ARRAY_SIZE(pending); i++) {
         if (pending_mask & (1U << i)) {

libraries/AP_HAL_Linux/RCOutput_Sysfs.cpp

@@ -136,6 +136,9 @@ void RCOutput_Sysfs::cork(void)
 
 void RCOutput_Sysfs::push(void)
 {
+    if (!_corked) {
+        return;
+    }
     for (uint8_t i=0; i<_channel_count; i++) {
         if ((1U<<i) & _pending_mask) {
             _pwm_channels[i]->set_duty_cycle(usec_to_nsec(_pending[i]));

libraries/AP_HAL_Linux/RCOutput_ZYNQ.cpp

@@ -119,6 +119,9 @@ void RCOutput_ZYNQ::cork(void)
 
 void RCOutput_ZYNQ::push(void)
 {
+    if (!corked) {
+        return;
+    }
     corked = false;
     for (uint8_t i=0; i<MAX_ZYNQ_PWMS; i++) {
         if (pending_mask & (1U << i)) {

libraries/AP_HAL_Linux/RCOutput_qflight.cpp

@@ -180,8 +180,10 @@ void RCOutput_QFLIGHT::cork(void)
 
 void RCOutput_QFLIGHT::push(void)
 {
-    corked = false;
-    need_write = true;
+    if (corked) {
+        corked = false;
+        need_write = true;
+    }
 }
 
 #endif // CONFIG_HAL_BOARD_SUBTYPE

libraries/AP_HAL_PX4/RCOutput.cpp

@@ -543,10 +543,12 @@ void PX4RCOutput::push()
     hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
     hal.gpio->write(55, 0);
 #endif
-    _corking = false;
-    if (_output_mode == MODE_PWM_ONESHOT) {
-        // run timer immediately in oneshot mode
-        _send_outputs();
+    if (_corking) {
+        _corking = false;
+        if (_output_mode == MODE_PWM_ONESHOT) {
+            // run timer immediately in oneshot mode
+            _send_outputs();
+        }
     }
 }
 

libraries/AP_HAL_QURT/RCOutput.cpp

@@ -116,8 +116,10 @@ void RCOutput::cork(void)
 
 void RCOutput::push(void)
 {
-    need_write = true;
-    corked = false;
+    if (corked) {
+        need_write = true;
+        corked = false;
+    }
 }
 
 #endif // CONFIG_HAL_BOARD_SUBTYPE

libraries/AP_HAL_SITL/RCOutput.cpp

@@ -69,14 +69,18 @@ void RCOutput::read(uint16_t* period_us, uint8_t len)
 
 void RCOutput::cork(void)
 {
-    memcpy(_pending, _sitlState->pwm_output, SITL_NUM_CHANNELS * sizeof(uint16_t));
-    _corked = true;
+    if (!_corked) {
+        memcpy(_pending, _sitlState->pwm_output, SITL_NUM_CHANNELS * sizeof(uint16_t));
+        _corked = true;
+    }
 }
 
 void RCOutput::push(void)
 {
-    memcpy(_sitlState->pwm_output, _pending, SITL_NUM_CHANNELS * sizeof(uint16_t));
-    _corked = false;
+    if (_corked) {
+        memcpy(_sitlState->pwm_output, _pending, SITL_NUM_CHANNELS * sizeof(uint16_t));
+        _corked = false;
+    }
 }
 
 #endif

libraries/AP_HAL_VRBRAIN/RCOutput.cpp

@@ -531,6 +531,9 @@ void VRBRAINRCOutput::cork()
 
 void VRBRAINRCOutput::push()
 {
+    if (!_corking) {
+        return;
+    }
 #if RCOUT_DEBUG_LATENCY
     hal.gpio->pinMode(55, HAL_GPIO_OUTPUT);
     hal.gpio->write(55, 0);

libraries/AP_Motors/AP_Motors6DOF.cpp

@@ -183,13 +183,11 @@ void AP_Motors6DOF::output_min()
 
     // fill the motor_out[] array for HIL use and send minimum value to each motor
     // ToDo find a field to store the minimum pwm instead of hard coding 1500
-    hal.rcout->cork();
     for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
             rc_write(i, 1500);
         }
     }
-    hal.rcout->push();
 }
 
 int16_t AP_Motors6DOF::calc_thrust_to_pwm(float thrust_in) const
@@ -233,13 +231,11 @@ void AP_Motors6DOF::output_to_motors()
     }
 
     // send output to each motor
-    hal.rcout->cork();
     for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
             rc_write(i, motor_out[i]);
         }
     }
-    hal.rcout->push();
 }
 
 // output_armed - sends commands to the motors

libraries/AP_Motors/AP_MotorsCoax.cpp

@@ -89,38 +89,32 @@ void AP_MotorsCoax::output_to_motors()
     switch (_spool_mode) {
         case SHUT_DOWN:
             // sends minimum values out to the motors
-            hal.rcout->cork();
             rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_roll_radio_passthrough, _servo1));
             rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough, _servo2));
             rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough, _servo3));
             rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough, _servo4));
             rc_write(AP_MOTORS_MOT_5, get_pwm_output_min());
             rc_write(AP_MOTORS_MOT_6, get_pwm_output_min());
-            hal.rcout->push();
             break;
         case SPIN_WHEN_ARMED:
             // sends output to motors when armed but not flying
-            hal.rcout->cork();
             rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1));
             rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2));
             rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3));
             rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4));
             rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
             rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
-            hal.rcout->push();
             break;
         case SPOOL_UP:
         case THROTTLE_UNLIMITED:
         case SPOOL_DOWN:
             // set motor output based on thrust requests
-            hal.rcout->cork();
             rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1));
             rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2));
             rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_actuator_out[2], _servo3));
             rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_actuator_out[3], _servo4));
             rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_yt_ccw));
             rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_yt_cw));
-            hal.rcout->push();
             break;
     }
 }

libraries/AP_Motors/AP_MotorsHeli_Dual.cpp

@@ -543,16 +543,12 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
     servo6_out = 2*servo6_out - 1;
 
     // actually move the servos
-    hal.rcout->cork();
-
     rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(servo1_out, _swash_servo_1));
     rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(servo2_out, _swash_servo_2));
     rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(servo3_out, _swash_servo_3));
     rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(servo4_out, _swash_servo_4));
     rc_write(AP_MOTORS_MOT_5, calc_pwm_output_1to1(servo5_out, _swash_servo_5));
     rc_write(AP_MOTORS_MOT_6, calc_pwm_output_1to1(servo6_out, _swash_servo_6));
-
-    hal.rcout->push();
 }
 
 

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -449,8 +449,6 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
     }
     float servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * coll_out_scaled;
 
-    hal.rcout->cork();
-
     // rescale from -1..1, so we can use the pwm calc that includes trim
     servo1_out = 2*servo1_out - 1;
     servo2_out = 2*servo2_out - 1;
@@ -463,8 +461,6 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
 
     // update the yaw rate using the tail rotor/servo
     move_yaw(yaw_out + yaw_offset);
-
-    hal.rcout->push();
 }
 
 // move_yaw

libraries/AP_Motors/AP_MotorsMatrix.cpp

@@ -126,13 +126,11 @@ void AP_MotorsMatrix::output_to_motors()
     }
 
     // send output to each motor
-    hal.rcout->cork();
     for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
             rc_write(i, motor_out[i]);
         }
     }
-    hal.rcout->push();
 }
 
 
@@ -302,14 +300,12 @@ void AP_MotorsMatrix::output_test(uint8_t motor_seq, int16_t pwm)
     }
 
     // loop through all the possible orders spinning any motors that match that description
-    hal.rcout->cork();
     for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i] && _test_order[i] == motor_seq) {
             // turn on this motor
             rc_write(i, pwm);
         }
     }
-    hal.rcout->push();
 }
 
 // add_motor

libraries/AP_Motors/AP_MotorsMulticopter.cpp

@@ -582,13 +582,11 @@ void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float th
     if (armed()) {
         uint16_t pwm_out = get_pwm_output_min() + constrain_float(throttle_input, 0.0f, 1.0f) * (get_pwm_output_max() - get_pwm_output_min());
         // send the pilot's input directly to each enabled motor
-        hal.rcout->cork();
         for (uint16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
             if (motor_enabled[i]) {
                 rc_write(i, pwm_out);
             }
         }
-        hal.rcout->push();
     }
 }
 
@@ -597,7 +595,6 @@ void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float th
 // the range 0 to 1
 void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask)
 {
-    hal.rcout->cork();
     for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
         if (motor_enabled[i]) {
             int16_t motor_out;
@@ -609,7 +606,6 @@ void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask)
             rc_write(i, motor_out);
         }
     }
-    hal.rcout->push();
 }
 
 // save parameters as part of disarming