diff --git a/libraries/AP_Notify/AP_Notify.cpp b/libraries/AP_Notify/AP_Notify.cpp
index a04dbef242..bb2f6c4c4e 100644
--- a/libraries/AP_Notify/AP_Notify.cpp
+++ b/libraries/AP_Notify/AP_Notify.cpp
@@ -20,6 +20,7 @@
#include "Buzzer.h"
#include "Display.h"
#include "ExternalLED.h"
+#include "IS31FL3195.h"
#include "PCA9685LED_I2C.h"
#include "NavigatorLED.h"
#include "NeoPixel.h"
@@ -384,6 +385,18 @@ void AP_Notify::add_backends(void)
ADD_BACKEND(new LP5562(b, 0x30));
}
break;
+#endif
+#if AP_NOTIFY_IS31FL3195_ENABLED
+ case Notify_LED_IS31FL3195_I2C_External:
+ FOREACH_I2C_EXTERNAL(b) {
+ ADD_BACKEND(new IS31FL3195(b, 0x54));
+ }
+ break;
+ case Notify_LED_IS31FL3195_I2C_Internal:
+ FOREACH_I2C_INTERNAL(b) {
+ ADD_BACKEND(new IS31FL3195(b, 0x54));
+ }
+ break;
#endif
}
}
diff --git a/libraries/AP_Notify/AP_Notify.h b/libraries/AP_Notify/AP_Notify.h
index 23f5e5f2c2..29a5542bda 100644
--- a/libraries/AP_Notify/AP_Notify.h
+++ b/libraries/AP_Notify/AP_Notify.h
@@ -96,6 +96,10 @@ public:
#if AP_NOTIFY_LP5562_ENABLED
Notify_LED_LP5562_I2C_External = (1 << 13), // LP5562
Notify_LED_LP5562_I2C_Internal = (1 << 14), // LP5562
+#endif
+#if AP_NOTIFY_IS31FL3195_ENABLED
+ Notify_LED_IS31FL3195_I2C_External = (1 << 15), // IS31FL3195
+ Notify_LED_IS31FL3195_I2C_Internal = (1 << 16), // IS31FL3195
#endif
Notify_LED_MAX
};
diff --git a/libraries/AP_Notify/AP_Notify_config.h b/libraries/AP_Notify/AP_Notify_config.h
index 73b5f7cc90..2c759ae016 100644
--- a/libraries/AP_Notify/AP_Notify_config.h
+++ b/libraries/AP_Notify/AP_Notify_config.h
@@ -25,6 +25,10 @@
#define AP_NOTIFY_LP5562_ENABLED 1
#endif
+#ifndef AP_NOTIFY_IS31FL3195_ENABLED
+#define AP_NOTIFY_IS31FL3195_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/IS31FL3195.cpp b/libraries/AP_Notify/IS31FL3195.cpp
new file mode 100644
index 0000000000..bf6c12ce60
--- /dev/null
+++ b/libraries/AP_Notify/IS31FL3195.cpp
@@ -0,0 +1,122 @@
+/*
+ IS31FL3195 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 IS31FL3195 */
+
+#include "IS31FL3195.h"
+
+#if AP_NOTIFY_IS31FL3195_ENABLED
+
+#include
+
+#include
+
+extern const AP_HAL::HAL& hal;
+
+#define IS31FL3195_LED_BRIGHT 255 // full brightness
+#define IS31FL3195_LED_MEDIUM 170 // medium brightness
+#define IS31FL3195_LED_DIM 85 // dim
+#define IS31FL3195_LED_OFF 0 // off
+
+#define REGISTER_MAGIC_VLAUE 0xc5
+
+enum class Register {
+ PRODUCT_ID = 0x00, // not really product ID, rather address of this LED
+ SHUTDOWN_CONTROL = 0x01,
+ OUT1 = 0x10,
+ OUT2 = 0x21,
+ OUT3 = 0x32,
+ COLOR_UPDATE = 0x50,
+ RESET = 0x5f,
+};
+
+IS31FL3195::IS31FL3195(uint8_t bus, uint8_t addr)
+ : RGBLed(IS31FL3195_LED_OFF, IS31FL3195_LED_BRIGHT, IS31FL3195_LED_MEDIUM, IS31FL3195_LED_DIM)
+ , _bus(bus)
+ , _addr(addr)
+{
+}
+
+bool IS31FL3195::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);
+
+ // check the PRODUCT_ID register to check we have found the device. The product ID register
+ // should contain the 8 bit I2C address
+ uint8_t v;
+ if (!_dev->read_registers((uint8_t)Register::PRODUCT_ID, &v, 1) || v != _addr<<1U) {
+ return false;
+ }
+
+ // reset the device and probe to see if this device looks like an IS31FL3195:
+ if (!_dev->write_register((uint8_t)Register::RESET, REGISTER_MAGIC_VLAUE)) {
+ return false;
+ }
+
+ // reset delay; unsure if this is really required:
+ hal.scheduler->delay_microseconds(100);
+
+ // check SHUTDOWN_CONTROL as an additional ID after reset
+ if (!_dev->read_registers((uint8_t)Register::SHUTDOWN_CONTROL, &v, 1) || v != 0xf0) {
+ return false;
+ }
+
+ // come out of shutdown mode
+ if (!_dev->write_register((uint8_t)Register::SHUTDOWN_CONTROL, 0xf1)) {
+ return false;
+ }
+
+ _dev->set_retries(1);
+
+ // update at 50Hz
+ _dev->register_periodic_callback(20000, FUNCTOR_BIND_MEMBER(&IS31FL3195::_timer, void));
+
+ return true;
+}
+
+// set_rgb - set color as a combination of red, green and blue values
+bool IS31FL3195::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
+{
+ rgb[0] = red;
+ rgb[1] = green;
+ rgb[2] = blue;
+ _need_update = true;
+ return true;
+}
+
+void IS31FL3195::_timer(void)
+{
+ if (!_need_update) {
+ return;
+ }
+ _need_update = false;
+
+ // write color values
+ _dev->write_register(uint8_t(Register::OUT1), rgb[0]);
+ _dev->write_register(uint8_t(Register::OUT2), rgb[1]);
+ _dev->write_register(uint8_t(Register::OUT3), rgb[2]);
+ _dev->write_register(uint8_t(Register::COLOR_UPDATE), REGISTER_MAGIC_VLAUE);
+}
+
+#endif // AP_NOTIFY_IS31FL3195_ENABLED
diff --git a/libraries/AP_Notify/IS31FL3195.h b/libraries/AP_Notify/IS31FL3195.h
new file mode 100644
index 0000000000..78c0e8ac42
--- /dev/null
+++ b/libraries/AP_Notify/IS31FL3195.h
@@ -0,0 +1,47 @@
+/*
+ IS31FL3195 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.lumissil.com/assets/pdf/core/IS31FL3195_DS.pdf
+
+ */
+#pragma once
+
+#include "AP_Notify_config.h"
+
+#if AP_NOTIFY_IS31FL3195_ENABLED
+
+#include
+#include "RGBLed.h"
+
+class IS31FL3195 : public RGBLed
+{
+public:
+ IS31FL3195(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 rgb[3];
+};
+
+#endif // AP_NOTIFY_IS31FL3195_ENABLED