mirror of https://github.com/ArduPilot/ardupilot
39 lines
873 B
C++
39 lines
873 B
C++
|
#include "SIM_config.h"
|
||
|
|
|
||
|
|
#if AP_SIM_GPIO_LED_RGB_ENABLED
|
||
|
|
|
||
|
|
#include "SIM_GPIO_LED_RGB.h"
|
||
|
|
|
||
|
|
#include <SITL/SITL.h>
|
||
|
|
|
||
|
|
using namespace SITL;
|
||
|
|
|
||
|
|
void GPIO_LED_RGB::init()
|
||
|
|
{
|
||
|
|
leds.init();
|
||
|
|
rgbled.init();
|
||
|
|
}
|
||
|
|
|
||
|
|
void GPIO_LED_RGB::update(const class Aircraft &aircraft)
|
||
|
|
{
|
||
|
|
if (!init_done) {
|
||
|
|
init();
|
||
|
|
init_done = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
const uint16_t pin_mask = AP::sitl()->pin_mask.get();
|
||
|
|
|
||
|
|
const bool red = ((pin_mask & uint16_t((1U<<LED_RED_PIN))) != 0);
|
||
|
|
const bool green = ((pin_mask & uint16_t((1U<<LED_GREEN_PIN))) != 0);
|
||
|
|
const bool blue = ((pin_mask & uint16_t((1U<<LED_BLUE_PIN))) != 0);
|
||
|
|
|
||
|
|
const bool new_led_states[3] { red, green, blue };
|
||
|
|
leds.set_state(new_led_states);
|
||
|
|
|
||
|
|
// FIXME: check why we need to "!" here; do we need to move
|
||
|
|
// ON_VALUE into here?
|
||
|
|
rgbled.set_colours((!red)*255, (!green)*255, (!blue)*255);
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|