From 8b2e69162dc6eb07dc816aff7efe80336d47ba59 Mon Sep 17 00:00:00 2001
From: "David B. Bitton" <david@codenoevil.com>
Date: Mon, 28 Dec 2020 14:49:31 -0700
Subject: [PATCH] AP_Periph: add support for Toshiba LED

---
 Tools/AP_Periph/AP_Periph.h |  2 +-
 Tools/AP_Periph/can.cpp     | 26 ++++++++++++++++++++++++++
 2 files changed, 27 insertions(+), 1 deletion(-)

diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h
index fd783e4bb4..a6ca8e384d 100644
--- a/Tools/AP_Periph/AP_Periph.h
+++ b/Tools/AP_Periph/AP_Periph.h
@@ -14,7 +14,7 @@
 #include "../AP_Bootloader/app_comms.h"
 #include "hwing_esc.h"
 
-#if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_NCP5623_LED) || defined(HAL_PERIPH_ENABLE_NCP5623_BGR_LED)
+#if defined(HAL_PERIPH_NEOPIXEL_COUNT) || defined(HAL_PERIPH_ENABLE_NCP5623_LED) || defined(HAL_PERIPH_ENABLE_NCP5623_BGR_LED) || defined(HAL_PERIPH_ENABLE_TOSHIBA_LED)
 #define AP_PERIPH_HAVE_LED
 #endif
 
diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index e7038f9390..033d97de28 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -583,6 +583,32 @@ static void set_rgb_led(uint8_t red, uint8_t green, uint8_t blue)
         dev->transfer(&v, 1, nullptr, 0);
     }
 #endif // HAL_PERIPH_ENABLE_NCP5623_BGR_LED
+#ifdef HAL_PERIPH_ENABLE_TOSHIBA_LED
+    {
+#define TOSHIBA_LED_PWM0    0x01    // pwm0 register
+#define TOSHIBA_LED_ENABLE  0x04    // enable register
+#define TOSHIBA_LED_I2C_ADDR 0x55   // default I2C bus address
+
+        static AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev_toshiba;
+        if (!dev_toshiba) {
+            dev_toshiba = std::move(hal.i2c_mgr->get_device(0, TOSHIBA_LED_I2C_ADDR));
+        }
+        WITH_SEMAPHORE(dev_toshiba->get_semaphore());
+        dev_toshiba->set_retries(0); // use 0 because this is running on main thread.
+
+        // enable the led
+        dev_toshiba->write_register(TOSHIBA_LED_ENABLE, 0x03);
+
+        /* 4-bit for each color */
+        uint8_t val[4] = { 
+            TOSHIBA_LED_PWM0, 
+            (uint8_t)(blue >> 4),
+            (uint8_t)(green / 16), 
+            (uint8_t)(red / 16) 
+        };
+        dev_toshiba->transfer(val, sizeof(val), nullptr, 0);
+    }
+#endif // HAL_PERIPH_ENABLE_TOSHIBA_LED
 }
 
 /*