diff --git a/libraries/AP_HAL_SITL/RCOutput.cpp b/libraries/AP_HAL_SITL/RCOutput.cpp index 29b6f4c830..f9169a5f9f 100644 --- a/libraries/AP_HAL_SITL/RCOutput.cpp +++ b/libraries/AP_HAL_SITL/RCOutput.cpp @@ -84,4 +84,45 @@ void RCOutput::push(void) } } +/* + Serial LED emulation +*/ +bool RCOutput::set_neopixel_num_LEDs(const uint16_t chan, uint8_t num_leds) +{ + if (chan > 15 || num_leds > 32) { + return false; + } + SITL::SITL *sitl = AP::sitl(); + if (sitl) { + sitl->led.num_leds[chan] = num_leds; + return true; + } + return false; +} + +void RCOutput::set_neopixel_rgb_data(const uint16_t chan, uint32_t ledmask, uint8_t red, uint8_t green, uint8_t blue) +{ + if (chan > 15) { + return; + } + SITL::SITL *sitl = AP::sitl(); + if (sitl) { + for (uint8_t i=0; i<32; i++) { + if ((1U<led.rgb[chan][i].rgb[0] = red; + sitl->led.rgb[chan][i].rgb[1] = green; + sitl->led.rgb[chan][i].rgb[2] = blue; + } + } + } +} + +void RCOutput::neopixel_send(void) +{ + SITL::SITL *sitl = AP::sitl(); + if (sitl) { + sitl->led.send_counter++; + } +} + #endif diff --git a/libraries/AP_HAL_SITL/RCOutput.h b/libraries/AP_HAL_SITL/RCOutput.h index 11d4b51a0d..8d7ba99e30 100644 --- a/libraries/AP_HAL_SITL/RCOutput.h +++ b/libraries/AP_HAL_SITL/RCOutput.h @@ -18,6 +18,13 @@ public: void cork(void) override; void push(void) override; + /* + Serial LED emulation + */ + bool set_neopixel_num_LEDs(const uint16_t chan, uint8_t num_leds) override; + void set_neopixel_rgb_data(const uint16_t chan, uint32_t ledmask, uint8_t red, uint8_t green, uint8_t blue) override; + void neopixel_send(void) override; + private: SITL_State *_sitlState; uint16_t _freq_hz;