Method push

libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp:230–287

}

void RCOutput_PCA9685::push()
{
    if (!_corking) {
        return;
    }
    _corking = false;

    if (_pending_write_mask == 0)
        return;

    // Calculate the number of channels for this transfer.
    uint8_t max_ch = (sizeof(unsigned) * 8) - __builtin_clz(_pending_write_mask);
    uint8_t min_ch = __builtin_ctz(_pending_write_mask);

    /*
     * scratch buffer size is always for all the channels, but we write only
     * from min_ch to max_ch
     */
    struct PACKED pwm_values {
        uint8_t reg;
        uint8_t data[PWM_CHAN_COUNT * 4];
    } pwm_values;

    for (unsigned ch = min_ch; ch < max_ch; ch++) {
        uint16_t period_us = _pulses_buffer[ch];
        uint16_t length = 0;

        if (period_us) {
            length = round((period_us * 4096) / (1000000.f / _frequency)) - 1;
        }

        uint8_t *d = &pwm_values.data[(ch - min_ch) * 4];

        if (_is_gpio_mask & (1 << ch)) {
            *d++ = 0;                                                // LEDn_ON_L
            *d++ = period_us ? PCA9685_LED_ON_H_ALWAYS_ON_BIT : 0;   // LEDn_ON_H
            *d++ = 0;                                                // LEDn_OFF_L
            *d++ = period_us ? 0 : PCA9685_LED_OFF_H_ALWAYS_OFF_BIT; // LEDn_OFF_H
        } else {
            *d++ = 0;               // LEDn_ON_L
            *d++ = 0;               // LEDn_ON_H
            *d++ = length & 0xFF;   // LEDn_OFF_L
            *d++ = length >> 8;     // LEDn_OFF_H
        }
    }

    if (!_dev || !_dev->get_semaphore()->take_nonblocking()) {
        return;
    }

    pwm_values.reg = PCA9685_RA_LED0_ON_L + 4 * (_channel_offset + min_ch);
    /* reg + all the channels we are going to write */
    size_t payload_size = 1 + (max_ch - min_ch) * 4;

    _dev->transfer((uint8_t *)&pwm_values, payload_size, nullptr, 0);
    _dev->get_semaphore()->give();
    _pending_write_mask = 0;
}