Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Notify: send dronecan led commands directly from driver

Browse Source
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; break;
case Notify_LED_DroneCAN: case Notify_LED_DroneCAN:
#if HAL_ENABLE_DRONECAN_DRIVERS #if HAL_ENABLE_DRONECAN_DRIVERS
ADD_BACKEND(new DroneCAN_RGB_LED(0)); ADD_BACKEND(new DroneCAN_RGB_LED());
#endif // HAL_ENABLE_DRONECAN_DRIVERS #endif // HAL_ENABLE_DRONECAN_DRIVERS
break; 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_MEDIUM ((LED_FULL_BRIGHT / 5) * 4)
#define LED_DIM ((LED_FULL_BRIGHT / 5) * 2) #define LED_DIM ((LED_FULL_BRIGHT / 5) * 2)
DroneCAN_RGB_LED::DroneCAN_RGB_LED(uint8_t led_index) DroneCAN_RGB_LED::DroneCAN_RGB_LED()
: DroneCAN_RGB_LED(led_index, LED_OFF, : DroneCAN_RGB_LED(LED_OFF,
LED_FULL_BRIGHT, LED_MEDIUM, LED_DIM) 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_full, uint8_t led_medium,
uint8_t led_dim) uint8_t led_dim)
: RGBLed(led_off, led_full, led_medium, 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 DroneCAN_RGB_LED::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
{ {
bool success = false; 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++) { for (uint8_t i = 0; i < can_num_drivers; i++) {
AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i); AP_DroneCAN *uavcan = AP_DroneCAN::get_dronecan(i);
if (uavcan != nullptr) { if (uavcan != nullptr) {
success = uavcan->led_write(_led_index, red, green, blue) || success; uavcan->rgb_led.broadcast(msg);
} }
} }
return success;
} }
#endif #endif

15
libraries/AP_Notify/DroneCAN_RGB_LED.h
View File

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