diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 4cc7284526..366ec90930 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -1093,6 +1093,16 @@ void Aircraft::update_external_payload(const struct sitl_input &input) dronecan->update(); } #endif + +#if AP_SIM_GPIO_LED_2_ENABLED + sim_led2.update(*this); +#endif +#if AP_SIM_GPIO_LED_3_ENABLED + sim_led3.update(*this); +#endif +#if AP_SIM_GPIO_LED_RGB_ENABLED + sim_ledrgb.update(*this); +#endif } void Aircraft::add_shove_forces(Vector3f &rot_accel, Vector3f &body_accel) diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 56ed55860d..4af43bb80c 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -38,6 +38,9 @@ #include #include "SIM_JSON_Master.h" #include "ServoModel.h" +#include "SIM_GPIO_LED_2.h" +#include "SIM_GPIO_LED_3.h" +#include "SIM_GPIO_LED_RGB.h" namespace SITL { @@ -372,6 +375,18 @@ private: #if AP_TEST_DRONECAN_DRIVERS DroneCANDevice *dronecan; #endif + + +#if AP_SIM_GPIO_LED_2_ENABLED + GPIO_LED_2 sim_led2{13, 14}; // pins to match sitl.h +#endif +#if AP_SIM_GPIO_LED_3_ENABLED + GPIO_LED_3 sim_led3{13, 14, 15}; // pins to match sitl.h +#endif +#if AP_SIM_GPIO_LED_RGB_ENABLED + GPIO_LED_RGB sim_ledrgb{8, 9, 10}; // pins to match sitl.h +#endif + }; } // namespace SITL diff --git a/libraries/SITL/SIM_GPIO_LED_2.cpp b/libraries/SITL/SIM_GPIO_LED_2.cpp new file mode 100644 index 0000000000..429e3d8875 --- /dev/null +++ b/libraries/SITL/SIM_GPIO_LED_2.cpp @@ -0,0 +1,32 @@ +#include "SIM_config.h" + +#if AP_SIM_GPIO_LED_2_ENABLED + +#include "SIM_GPIO_LED_2.h" + +#include + +using namespace SITL; + +void GPIO_LED_2::init() +{ + leds.init(); +} + +void GPIO_LED_2::update(const class Aircraft &aircraft) +{ + if (!init_done) { + init(); + init_done = true; + } + + const uint16_t pin_mask = AP::sitl()->pin_mask.get(); + const bool new_led_states[2] { + ((pin_mask & uint16_t((1U<::LEDColour colours[2] { + SIM_LED_n<2>::LEDColour::RED, + SIM_LED_n<2>::LEDColour::BLUE, + }; + + SIM_LED_n<2> leds{"GPIO_LED_2", colours}; + + uint8_t LED_A_PIN; + uint8_t LED_B_PIN; +}; + +} // namespace SITL + +#endif // AP_SIM_GPIO_LED_2_ENABLED diff --git a/libraries/SITL/SIM_GPIO_LED_3.cpp b/libraries/SITL/SIM_GPIO_LED_3.cpp new file mode 100644 index 0000000000..d078f30d30 --- /dev/null +++ b/libraries/SITL/SIM_GPIO_LED_3.cpp @@ -0,0 +1,33 @@ +#include "SIM_config.h" + +#if AP_SIM_GPIO_LED_3_ENABLED + +#include "SIM_GPIO_LED_3.h" + +#include + +using namespace SITL; + +void GPIO_LED_3::init() +{ + leds.init(); +} + +void GPIO_LED_3::update(const class Aircraft &aircraft) +{ + if (!init_done) { + init(); + init_done = true; + } + + const uint16_t pin_mask = AP::sitl()->pin_mask.get(); + const bool new_led_states[3] { + ((pin_mask & uint16_t((1U<::LEDColour colours[3] { + SIM_LED_n<3>::LEDColour::RED, + SIM_LED_n<3>::LEDColour::BLUE, + SIM_LED_n<3>::LEDColour::YELLOW, + }; + + SIM_LED_n<3> leds{"GPIO_LED_3", colours}; + + uint8_t LED_A_PIN; + uint8_t LED_B_PIN; + uint8_t LED_C_PIN; +}; + +} // namespace SITL + +#endif // AP_SIM_GPIO_LED_3_ENABLED diff --git a/libraries/SITL/SIM_GPIO_LED_RGB.cpp b/libraries/SITL/SIM_GPIO_LED_RGB.cpp new file mode 100644 index 0000000000..940240c6c4 --- /dev/null +++ b/libraries/SITL/SIM_GPIO_LED_RGB.cpp @@ -0,0 +1,38 @@ +#include "SIM_config.h" + +#if AP_SIM_GPIO_LED_RGB_ENABLED + +#include "SIM_GPIO_LED_RGB.h" + +#include + +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<::LEDColour colours[3] { + SIM_LED_n<3>::LEDColour::RED, + SIM_LED_n<3>::LEDColour::GREEN, + SIM_LED_n<3>::LEDColour::BLUE, + }; + + SIM_LED_n<3> leds{"GPIO_LED_RGB", colours}; + SIM_RGBLED rgbled{"GPIO_LED_RGB Mixed"}; + + uint8_t LED_RED_PIN; + uint8_t LED_GREEN_PIN; + uint8_t LED_BLUE_PIN; +}; + +} // namespace SITL + +#endif // AP_SIM_GPIO_LED_RGB_ENABLED diff --git a/libraries/SITL/SIM_LED_n.cpp b/libraries/SITL/SIM_LED_n.cpp new file mode 100644 index 0000000000..30cd3b90ef --- /dev/null +++ b/libraries/SITL/SIM_LED_n.cpp @@ -0,0 +1,96 @@ +#include "SIM_config.h" + +#if AP_SIM_LED_N_ENABLED + +#include "SIM_LED_n.h" + +#include + +#ifdef HAVE_SFML_GRAPHICS_H +#include +#else +#include +#endif + +#include + +template +void SIM_LED_n::update_thread(void) +{ + sf::RenderWindow *w = nullptr; + { + WITH_SEMAPHORE(AP::notify().sf_window_mutex); + w = NEW_NOTHROW sf::RenderWindow(sf::VideoMode(total_width, height), name); + w->clear(sf::Color(0, 0, 0, 255)); // blacken + } + + if (w == nullptr) { + AP_HAL::panic("Unable to create SIM_LED_n window"); + } + + while (true) { + { + WITH_SEMAPHORE(AP::notify().sf_window_mutex); + sf::Event event; + while (w->pollEvent(event)) { + if (event.type == sf::Event::Closed) { + w->close(); + break; + } + } + if (!w->isOpen()) { + break; + } + if (memcmp(new_state, last_state, sizeof(new_state)) != 0) { + memcpy(last_state, new_state, sizeof(last_state)); + w->clear(sf::Color(0, 0, 0, 255)); // blacken + sf::RectangleShape rectangle(sf::Vector2f(width, height)); + for (uint8_t i=0; idraw(rectangle); + } + w->display(); + } + } + usleep(10000); + } +} + +// trampoline for update thread +template +void *SIM_LED_n::update_thread_start(void *obj) +{ + ((SIM_LED_n *)obj)->update_thread(); + return nullptr; +} + +template +void SIM_LED_n::init() +{ + pthread_create(&thread, NULL, update_thread_start, this); +} + +template class SIM_LED_n<2>; +template class SIM_LED_n<3>; + +#endif // AP_SIM_LED_N_ENABLED diff --git a/libraries/SITL/SIM_LED_n.h b/libraries/SITL/SIM_LED_n.h new file mode 100644 index 0000000000..c6f5da09b3 --- /dev/null +++ b/libraries/SITL/SIM_LED_n.h @@ -0,0 +1,62 @@ +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_LED_N_ENABLED + +#include +#include + +/* + A class to create output of some description or another for a group + of N LEDs. + + Hopefully something visual, but perhaps just text +*/ + +template +class SIM_LED_n +{ +public: + enum class LEDColour : uint8_t { + RED, + GREEN, + BLUE, + YELLOW, + }; + + SIM_LED_n(const char *_name, const LEDColour _led_colours[NUM_LEDS]) : + name{_name} + { + memcpy(led_colours, _led_colours, sizeof(led_colours)); + } + + void set_state(const bool state[NUM_LEDS]) { + memcpy(new_state, state, sizeof(new_state)); + } + + void init(); + +private: + + const char *name; + LEDColour led_colours[NUM_LEDS]; + + static constexpr uint8_t height = 50; + static constexpr uint8_t width = height; + static constexpr uint8_t total_width = width * NUM_LEDS; + + pthread_t thread; + static void *update_thread_start(void *obj); + void update_thread(void); + + // state to be written to LEDs; note lack of thread protection. + bool new_state[NUM_LEDS]; + + // avoid too-frequent display updates: + bool last_state[NUM_LEDS]; + + const bool ON_VALUE = 0; // so if the pin is low we are lit +}; + +#endif // AP_SIM_LED_N_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index d5f6a00192..99a53bfb0f 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -23,6 +23,22 @@ #define AP_SIM_IS31FL3195_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif +#ifndef AP_SIM_LED_N_ENABLED +#define AP_SIM_LED_N_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) && defined(WITH_SITL_RGBLED) +#endif + +#ifndef AP_SIM_GPIO_LED_2_ENABLED +#define AP_SIM_GPIO_LED_2_ENABLED AP_SIM_LED_N_ENABLED && 0 +#endif + +#ifndef AP_SIM_GPIO_LED_3_ENABLED +#define AP_SIM_GPIO_LED_3_ENABLED AP_SIM_LED_N_ENABLED && 0 +#endif + +#ifndef AP_SIM_GPIO_LED_RGB_ENABLED +#define AP_SIM_GPIO_LED_RGB_ENABLED AP_SIM_LED_N_ENABLED +#endif + #ifndef AP_SIM_LOWEHEISER_ENABLED #define AP_SIM_LOWEHEISER_ENABLED AP_SIM_ENABLED && HAL_MAVLINK_BINDINGS_ENABLED #endif