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

AP_Notify: send dronecan led commands directly from driver

This commit is contained in:
bugobliterator 2023-05-02 16:04:21 +10:00 committed by Andrew Tridgell
parent 5026b3d08c
commit a1ba65a654
3 changed files with 25 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Notify/AP_Notify.cpp
View File

@ -355,7 +355,7 @@ void AP_Notify::add_backends(void)
break;
case Notify_LED_DroneCAN:
#if HAL_ENABLE_DRONECAN_DRIVERS
ADD_BACKEND(new DroneCAN_RGB_LED(0));
ADD_BACKEND(new DroneCAN_RGB_LED());
#endif // HAL_ENABLE_DRONECAN_DRIVERS
break;

23
libraries/AP_Notify/DroneCAN_RGB_LED.cpp
View File

@ -29,17 +29,16 @@
#define LED_MEDIUM ((LED_FULL_BRIGHT / 5) * 4)
#define LED_DIM ((LED_FULL_BRIGHT / 5) * 2)
DroneCAN_RGB_LED::DroneCAN_RGB_LED(uint8_t led_index)
: DroneCAN_RGB_LED(led_index, LED_OFF,
DroneCAN_RGB_LED::DroneCAN_RGB_LED()
: DroneCAN_RGB_LED(LED_OFF,
LED_FULL_BRIGHT, LED_MEDIUM, LED_DIM)
{
}
DroneCAN_RGB_LED::DroneCAN_RGB_LED(uint8_t led_index, uint8_t led_off,
DroneCAN_RGB_LED::DroneCAN_RGB_LED(uint8_t led_off,
uint8_t led_full, uint8_t led_medium,
uint8_t led_dim)
: RGBLed(led_off, led_full, led_medium, led_dim)
, _led_index(led_index)
{
}
@ -60,14 +59,24 @@ bool DroneCAN_RGB_LED::init()
bool DroneCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{
bool success = false;
uint8_t can_num_drivers = AP::can().get_num_drivers();
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();
for (uint8_t i = 0; i < can_num_drivers; i++) {
AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i);
if (uavcan != nullptr) {
success = uavcan->led_write(_led_index, red, green, blue) || success;
uavcan->rgb_led.broadcast(msg);
}
}
return success;
}
#endif

15
libraries/AP_Notify/DroneCAN_RGB_LED.h
View File

@ -1,18 +1,19 @@
#pragma once
#include "RGBLed.h"
#if HAL_ENABLE_DRONECAN_DRIVERS
#include <AP_DroneCAN/AP_DroneCAN.h>
class DroneCAN_RGB_LED: public RGBLed {
public:
DroneCAN_RGB_LED(uint8_t led_index, uint8_t led_off, uint8_t led_full,
DroneCAN_RGB_LED(uint8_t led_off, uint8_t led_full,
uint8_t led_medium, uint8_t led_dim);
DroneCAN_RGB_LED(uint8_t led_index);
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;
private:
uint8_t _led_index;
uavcan_equipment_indication_LightsCommand msg;
};
#endif