diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp index c35cece98a..78262619d8 100644 --- a/libraries/AP_Notify/AP_Notify.cpp +++ b/libraries/AP_Notify/AP_Notify.cpp @@ -28,6 +28,7 @@ #include "RCOutputRGBLed.h" #include "ToneAlarm.h" #include "ToshibaLED_I2C.h" +#include "LP5562.h" #include "VRBoard_LED.h" #include "DiscreteRGBLed.h" #include "DiscoLED.h" @@ -60,8 +61,14 @@ AP_Notify *AP_Notify::_singleton; #define ALL_NCP5623_I2C 0 #endif +#if AP_NOTIFY_LP5562_ENABLED +#define ALL_LP5562_I2C (Notify_LED_LP5562_I2C_Internal | Notify_LED_LP5562_I2C_External) +#else +#define ALL_LP5562_I2C 0 +#endif + // all I2C_LEDS -#define I2C_LEDS (ALL_TOSHIBALED_I2C | ALL_NCP5623_I2C) +#define I2C_LEDS (ALL_TOSHIBALED_I2C | ALL_NCP5623_I2C | ALL_LP5562_I2C) #ifndef DEFAULT_NTF_LED_TYPES @@ -194,7 +201,7 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = { // @Param: LED_TYPES // @DisplayName: LED Driver Types // @Description: Controls what types of LEDs will be enabled - // @Bitmask: 0:Built-in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:DroneCAN, 6:NCP5623 External, 7:NCP5623 Internal, 8:NeoPixel, 9:ProfiLED, 10:Scripting, 11:DShot, 12:ProfiLED_SPI + // @Bitmask: 0:Built-in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:DroneCAN, 6:NCP5623 External, 7:NCP5623 Internal, 8:NeoPixel, 9:ProfiLED, 10:Scripting, 11:DShot, 12:ProfiLED_SPI, 13:LP5562 External, 14: LP5562 Internal // @User: Advanced AP_GROUPINFO("LED_TYPES", 6, AP_Notify, _led_type, DEFAULT_NTF_LED_TYPES), @@ -363,6 +370,18 @@ void AP_Notify::add_backends(void) ADD_BACKEND(new DShotLED()); #endif break; +#if AP_NOTIFY_LP5562_ENABLED + case Notify_LED_LP5562_I2C_External: + FOREACH_I2C_EXTERNAL(b) { + ADD_BACKEND(new LP5562(b, 0x30)); + } + break; + case Notify_LED_LP5562_I2C_Internal: + FOREACH_I2C_INTERNAL(b) { + ADD_BACKEND(new LP5562(b, 0x30)); + } + break; +#endif } } diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h index 2c5346b645..4d8f3395a4 100644 --- a/libraries/AP_Notify/AP_Notify.h +++ b/libraries/AP_Notify/AP_Notify.h @@ -83,6 +83,10 @@ public: Notify_LED_Scripting = (1 << 10),// Colour accessor for scripting Notify_LED_DShot = (1 << 11),// Use dshot commands to set ESC LEDs Notify_LED_ProfiLED_SPI = (1 << 12), // ProfiLED +#if AP_NOTIFY_LP5562_ENABLED + Notify_LED_LP5562_I2C_External = (1 << 13), // LP5562 + Notify_LED_LP5562_I2C_Internal = (1 << 14), // LP5562 +#endif Notify_LED_MAX }; diff --git a/libraries/AP_Notify/AP_Notify_config.h b/libraries/AP_Notify/AP_Notify_config.h index 9d6eec2e03..e2a4ea6695 100644 --- a/libraries/AP_Notify/AP_Notify_config.h +++ b/libraries/AP_Notify/AP_Notify_config.h @@ -8,6 +8,10 @@ #endif #include +#ifndef AP_NOTIFY_LP5562_ENABLED +#define AP_NOTIFY_LP5562_ENABLED 1 +#endif + #ifndef AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED #define AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED HAL_GCS_ENABLED #endif diff --git a/libraries/AP_Notify/LP5562.cpp b/libraries/AP_Notify/LP5562.cpp new file mode 100644 index 0000000000..aad8c03db5 --- /dev/null +++ b/libraries/AP_Notify/LP5562.cpp @@ -0,0 +1,147 @@ +/* + LP5562 I2C driver +*/ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ + +/* LED driver for LP5562 */ + +#include "LP5562.h" + +#if AP_NOTIFY_LP5562_ENABLED + +#include + +#include + +extern const AP_HAL::HAL& hal; + +#define LP5562_LED_BRIGHT 255 // full brightness +#define LP5562_LED_MEDIUM 170 // medium brightness +#define LP5562_LED_DIM 85 // dim +#define LP5562_LED_OFF 0 // off + +enum class Register { + ENABLE = 0x00, + B_PWM = 0x02, + G_PWM = 0x03, + R_PWM = 0x04, + B_CURRENT= 0x05, + G_CURRENT= 0x06, + R_CURRENT= 0x07, + CONFIG = 0x08, + RESET = 0x0D, + LED_MAP = 0x70, +}; + +LP5562::LP5562(uint8_t bus, uint8_t addr) + : RGBLed(LP5562_LED_OFF, LP5562_LED_BRIGHT, LP5562_LED_MEDIUM, LP5562_LED_DIM) + , _bus(bus) + , _addr(addr) +{ +} + +bool LP5562::init(void) +{ + _dev = std::move(hal.i2c_mgr->get_device(_bus, _addr)); + if (!_dev) { + return false; + } + WITH_SEMAPHORE(_dev->get_semaphore()); + + _dev->set_retries(10); + + // reset the device and probe to see if this device looks like an LP5662: + if (!_dev->write_register((uint8_t)Register::RESET, 0xff)) { + return false; + } + + // reset delay; unsure if this is really required: + hal.scheduler->delay_microseconds(100); + + // check the GBR PWM control registers have their reset values: + for (uint8_t i=(uint8_t)Register::B_CURRENT; i<=(uint8_t)Register::R_CURRENT; i++) { + uint8_t value; + if (!_dev->read_registers(i, &value, 1)) { + return false; + } + if (value != 0xAF) { // 0xAF is the startup value for these registers per datasheet + return false; + } + } + + // values here are taken literally from 7.3.2 in the datasheet. + // See the simulator for register breakdown. + + // chip enable: + if (!_dev->write_register((uint8_t)Register::ENABLE, 0b1000000)) { + return false; + } + + // start-up-delay: + hal.scheduler->delay_microseconds(500); + + // use internal clock: + if (!_dev->write_register((uint8_t)Register::CONFIG, 0b00000001)) { + return false; + } + + // set direct PWM control: + if (!_dev->write_register((uint8_t)Register::LED_MAP, 0b00000000)) { + return false; + } + + _dev->set_retries(1); + + _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&LP5562::_timer, void)); + + return true; +} + +// set_rgb - set color as a combination of red, green and blue values +bool LP5562::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue) +{ + bgr[0] = blue; + bgr[1] = green; + bgr[2] = red; + _need_update = true; + return true; +} + +void LP5562::_timer(void) +{ + if (!_need_update) { + return; + } + _need_update = false; + + for (uint8_t i=0; i< ARRAY_SIZE(bgr); i++) { + const uint8_t new_colour = bgr[i]; + const uint8_t last_sent = last_sent_bgr[i]; + if (new_colour == last_sent) { + continue; + } + // note that new_colour is already scaled by supplying + // brightness values in the constructor. + + // take advantage of the linear layout of the registers. The + // direct PWM registers start at 0x02 for blue: + _dev->write_register((uint8_t)0x02 + i, new_colour); + + last_sent_bgr[i] = new_colour; + } +} + +#endif // AP_NOTIFY_LP5562_ENABLED diff --git a/libraries/AP_Notify/LP5562.h b/libraries/AP_Notify/LP5562.h new file mode 100644 index 0000000000..edaf1138b4 --- /dev/null +++ b/libraries/AP_Notify/LP5562.h @@ -0,0 +1,48 @@ +/* + LP5562 I2C driver + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + Datasheet: https://www.ti.com/lit/ds/symlink/lp5562.pdf?ts=1680437709368&ref_url=https%253A%252F%252Fwww.ti.com%252Fproduct%252FLP5562 + + */ +#pragma once + +#include "AP_Notify_config.h" + +#if AP_NOTIFY_LP5562_ENABLED + +#include +#include "RGBLed.h" + +class LP5562 : public RGBLed +{ +public: + LP5562(uint8_t bus, uint8_t addr); + bool init(void) override; +protected: + bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override; + +private: + AP_HAL::OwnPtr _dev; + uint8_t _bus; + uint8_t _addr; + + void _timer(void); + bool _need_update; + uint8_t bgr[3]; + uint8_t last_sent_bgr[3]; +}; + +#endif // AP_NOTIFY_LP5562_ENABLED