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