int
RGBLED::probe()
{
int ret;
bool on, powersave;
uint8_t r, g, b;
/**
this may look strange, but is needed. There is a serial
EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to
a bunch of I2C addresses, including the 0x55 used by this
LED device. So we need to do enough operations to be sure
we are talking to the right device. These 3 operations seem
to be enough, as the 3rd one consistently fails if no
RGBLED is on the bus.
*/
unsigned prevretries = _retries;
_retries = 4;
if ((ret=get(on, powersave, r, g, b)) != OK ||
(ret=send_led_enable(false) != OK) ||
(ret=send_led_enable(false) != OK)) {
return ret;
}
_retries = prevretries;
return ret;
}
/**
* Main loop function
*/
void
RGBLED::led()
{
if (!_should_run) {
_running = false;
return;
}
switch (_mode) {
case RGBLED_MODE_BLINK_SLOW:
case RGBLED_MODE_BLINK_NORMAL:
case RGBLED_MODE_BLINK_FAST:
if (_counter >= 2)
_counter = 0;
send_led_enable(_counter == 0);
break;
case RGBLED_MODE_BREATHE:
if (_counter >= 62)
_counter = 0;
int n;
if (_counter < 32) {
n = _counter;
} else {
n = 62 - _counter;
}
_brightness = n * n / (31.0f * 31.0f);
send_led_rgb();
break;
case RGBLED_MODE_PATTERN:
/* don't run out of the pattern array and stop if the next frame is 0 */
if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0)
_counter = 0;
set_color(_pattern.color[_counter]);
send_led_rgb();
_led_interval = _pattern.duration[_counter];
break;
default:
break;
}
_counter++;
/* re-queue ourselves to run again later */
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, _led_interval);
}
int
RGBLED::init()
{
int ret;
ret = I2C::init();
if (ret != OK) {
return ret;
}
/* switch off LED on start */
send_led_enable(false);
send_led_rgb();
return OK;
}
/**
* Set mode, if mode not changed has no any effect (doesn't reset blinks phase)
*/
void
RGBLED::set_mode(rgbled_mode_t mode)
{
if (mode != _mode) {
_mode = mode;
switch (mode) {
case RGBLED_MODE_OFF:
_should_run = false;
send_led_enable(false);
break;
case RGBLED_MODE_ON:
_brightness = 1.0f;
send_led_rgb();
send_led_enable(true);
break;
case RGBLED_MODE_BLINK_SLOW:
_should_run = true;
_counter = 0;
_led_interval = 2000;
_brightness = 1.0f;
send_led_rgb();
break;
case RGBLED_MODE_BLINK_NORMAL:
_should_run = true;
_counter = 0;
_led_interval = 500;
_brightness = 1.0f;
send_led_rgb();
break;
case RGBLED_MODE_BLINK_FAST:
_should_run = true;
_counter = 0;
_led_interval = 100;
_brightness = 1.0f;
send_led_rgb();
break;
case RGBLED_MODE_BREATHE:
_should_run = true;
_counter = 0;
_led_interval = 25;
send_led_enable(true);
break;
case RGBLED_MODE_PATTERN:
_should_run = true;
_counter = 0;
_brightness = 1.0f;
send_led_enable(true);
break;
default:
warnx("mode unknown");
break;
}
/* if it should run now, start the workq */
if (_should_run && !_running) {
_running = true;
work_queue(LPWORK, &_work, (worker_t)&RGBLED::led_trampoline, this, 1);
}
}
}
