From 56180089351cac15ebf3eb87fa36d217d915f033 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Mon, 11 Jul 2016 15:04:54 -0300
Subject: [PATCH] AP_Notify: ToshibaLED_I2C: Fix driver after I2CDevice
 conversion

    - Add missing semaphore take on bus
    - Initialize device on init function rather than constructor: the
      constructor may run before I2CDeviceManager is initialized since our
      AP_Notify objects are static so it can't be used.
---
 libraries/AP_Notify/ToshibaLED_I2C.cpp | 26 +++++++++++++++-----------
 libraries/AP_Notify/ToshibaLED_I2C.h   |  1 -
 2 files changed, 15 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_Notify/ToshibaLED_I2C.cpp b/libraries/AP_Notify/ToshibaLED_I2C.cpp
index 4093653215..bc05b9c303 100644
--- a/libraries/AP_Notify/ToshibaLED_I2C.cpp
+++ b/libraries/AP_Notify/ToshibaLED_I2C.cpp
@@ -17,6 +17,8 @@
  */
 #include "ToshibaLED_I2C.h"
 
+#include <utility>
+
 #include <AP_HAL/AP_HAL.h>
 
 extern const AP_HAL::HAL& hal;
@@ -29,15 +31,12 @@ extern const AP_HAL::HAL& hal;
 #define TOSHIBA_LED_PWM2    0x03    // pwm2 register
 #define TOSHIBA_LED_ENABLE  0x04    // enable register
 
-ToshibaLED_I2C::ToshibaLED_I2C()
-    : _dev(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS, TOSHIBA_LED_I2C_ADDR))
-{
-}
-
 bool ToshibaLED_I2C::hw_init()
 {
+    _dev = std::move(hal.i2c_mgr->get_device(TOSHIBA_LED_I2C_BUS, TOSHIBA_LED_I2C_ADDR));
+
     // take i2c bus sempahore
-    if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+    if (!_dev || !_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         return false;
     }
 
@@ -57,11 +56,16 @@ bool ToshibaLED_I2C::hw_init()
 // set_rgb - set color as a combination of red, green and blue values
 bool ToshibaLED_I2C::hw_set_rgb(uint8_t red, uint8_t green, uint8_t blue)
 {
-    // update the red value
-    uint8_t val[4] = { TOSHIBA_LED_PWM0, (uint8_t)(blue>>4), (uint8_t)(green>>4), (uint8_t)(red>>4) };
-    bool success = _dev->transfer(val, sizeof(val), nullptr, 0);
+    if (!_dev || !_dev->get_semaphore()->take(5)) {
+        return false;
+    }
+
+    /* 4-bit for each color */
+    uint8_t val[4] = { TOSHIBA_LED_PWM0, (uint8_t)(blue >> 4),
+                       (uint8_t)(green / 16), (uint8_t)(red / 16) };
+    bool ret = _dev->transfer(val, sizeof(val), nullptr, 0);
 
-    // give back i2c semaphore
     _dev->get_semaphore()->give();
-    return success;
+
+    return ret;
 }
diff --git a/libraries/AP_Notify/ToshibaLED_I2C.h b/libraries/AP_Notify/ToshibaLED_I2C.h
index 3ddfc5e7b1..25a7f62745 100644
--- a/libraries/AP_Notify/ToshibaLED_I2C.h
+++ b/libraries/AP_Notify/ToshibaLED_I2C.h
@@ -22,7 +22,6 @@
 class ToshibaLED_I2C : public ToshibaLED
 {
 public:
-    ToshibaLED_I2C();
     bool hw_init(void);
     bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b);