diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index 8c82c86070..2f494f6937 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -21,6 +21,7 @@
#include "Display.h"
#include "ExternalLED.h"
#include "PCA9685LED_I2C.h"
+#include "NCP5623.h"
#include "OreoLED_PX4.h"
#include "RCOutputRGBLed.h"
#include "ToneAlarm.h"
@@ -42,31 +43,35 @@ AP_Notify *AP_Notify::_instance;
#define TOSHIBA_LED_I2C_BUS_INTERNAL 0
#define TOSHIBA_LED_I2C_BUS_EXTERNAL 1
+// all I2C_LEDS
+#define I2C_LEDS (Notify_LED_ToshibaLED_I2C_Internal | Notify_LED_ToshibaLED_I2C_External | \
+ Notify_LED_NCP5623_I2C_Internal | Notify_LED_NCP5623_I2C_External)
+
#ifndef BUILD_DEFAULT_LED_TYPE
// PX4 boards
#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_PX4_V3 // Has enough memory for Oreo LEDs
- #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | Notify_LED_ToshibaLED_I2C_Internal | Notify_LED_ToshibaLED_I2C_External | Notify_LED_OreoLED)
+ #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS | Notify_LED_OreoLED)
#else // All other px4 boards use standard devices
- #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | Notify_LED_ToshibaLED_I2C_Internal | Notify_LED_ToshibaLED_I2C_External)
+ #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS)
#endif
// ChibiOS and VRBrain boards
#elif CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS || \
CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
- #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | Notify_LED_ToshibaLED_I2C_Internal | Notify_LED_ToshibaLED_I2C_External)
+ #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS)
// Linux boards
#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO
- #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | Notify_LED_ToshibaLED_I2C_Internal | Notify_LED_ToshibaLED_I2C_External |\
+ #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS |\
Notify_LED_PCA9685LED_I2C_External)
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2
- #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | Notify_LED_ToshibaLED_I2C_Internal | Notify_LED_ToshibaLED_I2C_External)
+ #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS)
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE
- #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | Notify_LED_ToshibaLED_I2C_Internal | Notify_LED_ToshibaLED_I2C_External |\
+ #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS |\
Notify_LED_UAVCAN)
#elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI || \
CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE || \
@@ -81,7 +86,7 @@ AP_Notify *AP_Notify::_instance;
#define BUILD_DEFAULT_LED_TYPE (Notify_LED_ToshibaLED_I2C_External)
#else // other linux
- #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | Notify_LED_ToshibaLED_I2C_Internal | Notify_LED_ToshibaLED_I2C_External)
+ #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS)
#endif
// F4Light
@@ -90,7 +95,7 @@ AP_Notify *AP_Notify::_instance;
// All other builds
#else
- #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | Notify_LED_ToshibaLED_I2C_Internal | Notify_LED_ToshibaLED_I2C_External)
+ #define BUILD_DEFAULT_LED_TYPE (Notify_LED_Board | I2C_LEDS)
#endif // board selection
@@ -149,7 +154,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:Build in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:UAVCAN
+ // @Bitmask: 0:Build in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:UAVCAN, 6:NCP5623 External, 7:NCP5623 Internal
// @User: Advanced
AP_GROUPINFO("LED_TYPES", 6, AP_Notify, _led_type, BUILD_DEFAULT_LED_TYPE),
@@ -234,6 +239,14 @@ void AP_Notify::add_backends(void)
case Notify_LED_ToshibaLED_I2C_External:
ADD_BACKEND(new ToshibaLED_I2C(TOSHIBA_LED_I2C_BUS_EXTERNAL));
break;
+#if !HAL_MINIMIZE_FEATURES
+ case Notify_LED_NCP5623_I2C_External:
+ ADD_BACKEND(new NCP5623(TOSHIBA_LED_I2C_BUS_EXTERNAL));
+ break;
+ case Notify_LED_NCP5623_I2C_Internal:
+ ADD_BACKEND(new NCP5623(TOSHIBA_LED_I2C_BUS_INTERNAL));
+ break;
+#endif
case Notify_LED_PCA9685LED_I2C_External:
ADD_BACKEND(new PCA9685LED_I2C());
break;
diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h
index 0d167f490e..c7b67630ea 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -66,6 +66,8 @@ public:
Notify_LED_PCA9685LED_I2C_External = (1 << 3), // External PCA9685_I2C
Notify_LED_OreoLED = (1 << 4), // Oreo
Notify_LED_UAVCAN = (1 << 5), // UAVCAN RGB LED
+ Notify_LED_NCP5623_I2C_External = (1 << 6), // External NCP5623
+ Notify_LED_NCP5623_I2C_Internal = (1 << 7), // Internal NCP5623
Notify_LED_MAX
};
diff --git a/libraries/AP_Notify/NCP5623.cpp b/libraries/AP_Notify/NCP5623.cpp
new file mode 100644
index 0000000000..2cf9a127bf
--- /dev/null
+++ b/libraries/AP_Notify/NCP5623.cpp
@@ -0,0 +1,115 @@
+/*
+ NCP5623 I2C LED 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 .
+ */
+
+#include "NCP5623.h"
+#include
+
+#include
+
+extern const AP_HAL::HAL& hal;
+
+#define NCP5623_LED_BRIGHT 0x1f // full brightness
+#define NCP5623_LED_MEDIUM 0x18 // medium brightness
+#define NCP5623_LED_DIM 0x0f // dim
+#define NCP5623_LED_OFF 0x00 // off
+
+#define NCP5623_LED_I2C_ADDR 0x38 // default I2C bus address
+
+#define NCP5623_LED_PWM0 0x40 // pwm0 register
+#define NCP5623_LED_PWM1 0x60 // pwm1 register
+#define NCP5623_LED_PWM2 0x80 // pwm2 register
+#define NCP5623_LED_ENABLE 0x20 // enable register
+
+NCP5623::NCP5623(uint8_t bus)
+ : RGBLed(NCP5623_LED_OFF, NCP5623_LED_BRIGHT, NCP5623_LED_MEDIUM, NCP5623_LED_DIM)
+ , _bus(bus)
+{
+}
+
+bool NCP5623::write(uint8_t reg, uint8_t data)
+{
+ uint8_t msg[1] = { 0x00 };
+ msg[0] = ((reg & 0xe0) | (data & 0x1f));
+ bool ret = _dev->transfer(msg, 1, nullptr, 0);
+ return ret;
+}
+
+bool NCP5623::writes(uint8_t *data, uint8_t len)
+{
+ bool ret = 0;
+ uint8_t reg = data[0];
+ for (uint8_t i = 0; i < (len - 1); i++) {
+ ret = write(reg, data[i + 1]);
+ if (!ret) {
+ return ret;
+ }
+ reg = reg + 0x20;
+ }
+ return ret;
+}
+
+bool NCP5623::hw_init(void)
+{
+ // first look for led on external bus
+ _dev = std::move(hal.i2c_mgr->get_device(_bus, NCP5623_LED_I2C_ADDR));
+ if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+ return false;
+ }
+ _dev->set_retries(10);
+
+ // enable the led
+ bool ret = write(NCP5623_LED_ENABLE, 0x1f);
+ if (!ret) {
+ _dev->get_semaphore()->give();
+ return false;
+ }
+ // update the red, green and blue values to zero
+ uint8_t val[4] = { NCP5623_LED_PWM0, _led_off, _led_off, _led_off };
+ ret = writes(val, sizeof(val));
+ _dev->set_retries(1);
+
+ // give back i2c semaphore
+ _dev->get_semaphore()->give();
+
+ if (ret) {
+ _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&NCP5623::_timer, void));
+ }
+
+ return ret;
+}
+
+// set_rgb - set color as a combination of red, green and blue values
+bool NCP5623::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
+{
+ rgb = { red, green, blue };
+ _need_update = true;
+ return true;
+}
+
+void NCP5623::_timer(void)
+{
+ if (!_need_update) {
+ return;
+ }
+ _need_update = false;
+
+ /* 4-bit for each color */
+ uint8_t val[4] = { NCP5623_LED_PWM0, rgb.r, rgb.g, rgb.b };
+
+ writes(val, sizeof(val));
+}
diff --git a/libraries/AP_Notify/NCP5623.h b/libraries/AP_Notify/NCP5623.h
new file mode 100644
index 0000000000..0101dc267e
--- /dev/null
+++ b/libraries/AP_Notify/NCP5623.h
@@ -0,0 +1,39 @@
+/*
+ NCP5623 Linux 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 .
+ */
+#pragma once
+
+#include
+#include "RGBLed.h"
+
+class NCP5623 : public RGBLed {
+public:
+ NCP5623(uint8_t bus);
+protected:
+ bool hw_init(void) override;
+ bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override;
+private:
+ AP_HAL::OwnPtr _dev;
+ void _timer(void);
+ bool write(uint8_t reg, uint8_t data);
+ bool writes(uint8_t *data, uint8_t len);
+ bool _need_update;
+ struct {
+ uint8_t r, g, b;
+ } rgb;
+ uint8_t _bus;
+};