bool RCOutputRGBLed::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
const uint16_t freq_motor = hal.rcout->get_freq(0);
const uint16_t freq = hal.rcout->get_freq(_red_channel);
const uint16_t usec_period = hz_to_usec(freq);
if (freq_motor != freq) {
/*
* keep at same frequency as the first RCOutput channel, some RCOutput
* drivers can not operate in different frequency between channels
*/
const uint32_t mask = 1 << _red_channel | 1 << _green_channel
| 1 << _blue_channel;
hal.rcout->set_freq(mask, freq_motor);
}
/*
* Not calling push() to have a better performance on RCOutput's that
* implements cork()/push(), so this changes will be committed together
* with the motors.
*/
hal.rcout->cork();
uint16_t usec_duty = usec_period * red / _led_bright;
hal.rcout->write(_red_channel, usec_duty);
usec_duty = usec_period * green / _led_bright;
hal.rcout->write(_green_channel, usec_duty);
usec_duty = usec_period * blue / _led_bright;
hal.rcout->write(_blue_channel, usec_duty);
return true;
}
bool PeriodicThread::set_rate(uint32_t rate_hz)
{
if (_started || rate_hz == 0) {
return false;
}
_period_usec = hz_to_usec(rate_hz);
return true;
}
