Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: fixed DroneCAN LEDs

This commit is contained in:
Andrew Tridgell 2023-06-24 20:01:15 +10:00
parent 93c1674e1f
commit 7f177abae8
2 changed files with 10 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Notify/DroneCAN_RGB_LED.cpp
View File

@ -44,39 +44,31 @@ DroneCAN_RGB_LED::DroneCAN_RGB_LED(uint8_t led_off,
bool DroneCAN_RGB_LED::init() bool DroneCAN_RGB_LED::init()
{ {
const uint8_t can_num_drivers = AP::can().get_num_drivers(); // LEDs can turn up later
for (uint8_t i = 0; i < can_num_drivers; i++) { return true;
AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i);
if (uavcan != nullptr) {
return true;
}
}
// no UAVCAN drivers
return false;
} }
bool DroneCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) bool DroneCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{ {
bool success = false; uavcan_equipment_indication_LightsCommand msg {};
msg.commands.len = 1; msg.commands.len = 1;
msg.commands.data[0].light_id =0; msg.commands.data[0].light_id =0;
msg.commands.data[0].color.red = red >> 3; msg.commands.data[0].color.red = red >> 3;
msg.commands.data[0].color.green = green >> 2; msg.commands.data[0].color.green = green >> 2;
msg.commands.data[0].color.blue = blue >> 3; msg.commands.data[0].color.blue = blue >> 3;
return success;
}
void DroneCAN_RGB_LED::update()
{
// broadcast the message on all ifaces // broadcast the message on all ifaces
uint8_t can_num_drivers = AP::can().get_num_drivers(); uint8_t can_num_drivers = AP::can().get_num_drivers();
bool ok = false;
for (uint8_t i = 0; i < can_num_drivers; i++) { for (uint8_t i = 0; i < can_num_drivers; i++) {
AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i); auto *dronecan = AP_DroneCAN::get_dronecan(i);
if (uavcan != nullptr) { if (dronecan != nullptr) {
uavcan->rgb_led.broadcast(msg); ok |= dronecan->rgb_led.broadcast(msg);
} }
} }
return ok;
} }
#endif #endif // HAL_ENABLE_DRONECAN_DRIVERS

2
libraries/AP_Notify/DroneCAN_RGB_LED.h
View File

@ -11,9 +11,7 @@ public:
uint8_t led_medium, uint8_t led_dim); uint8_t led_medium, uint8_t led_dim);
DroneCAN_RGB_LED(); DroneCAN_RGB_LED();
bool init() override; bool init() override;
void update() override;
protected: protected:
virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) override; virtual bool hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) override;
uavcan_equipment_indication_LightsCommand msg;
}; };
#endif #endif