SITL: add simulator for LP5562 RGBLED

libraries/SITL/SIM_I2C.cpp

@@ -29,6 +29,7 @@
 #include "SIM_Temperature_TSYS01.h"
 #include "SIM_Temperature_MCP9600.h"
 #include "SIM_ICM40609.h"
+#include "SIM_LP5562.h"
 #include "SIM_MS5525.h"
 #include "SIM_MS5611.h"
 
@@ -63,6 +64,7 @@ static MCP9600 mcp9600;
 static ICM40609 icm40609;
 static MS5525 ms5525;
 static MS5611 ms5611;
+static LP5562 lp5562;
 
 struct i2c_device_at_address {
     uint8_t bus;
@@ -85,6 +87,7 @@ struct i2c_device_at_address {
     { 2, 0x0B, maxell },        // Maxell: BATTx_MONITOR 16, BATTx_I2C_ADDR 13
     { 3, 0x0B, smbus_generic},  // BATTx_MONITOR 7, BATTx_I2C_ADDR 13
     { 2, 0x28, airspeed_dlvr }, // ARSPD_TYPE = 7 5inch H2O sensor
+    { 2, 0x30, lp5562 },        // LP5562 RGB LED driver
     { 2, 0x77, ms5611 },        // MS5611: BARO_PROBE_EXT = 2
 };
 

libraries/SITL/SIM_LP5562.cpp

@@ -0,0 +1,161 @@
+#include "SIM_LP5562.h"
+
+#if AP_SIM_LP5562_ENABLED
+
+using namespace SITL;
+
+#include <stdio.h>
+
+void LP5562::init()
+{
+    add_register("ENABLE", LP5562DevReg::ENABLE, I2CRegisters::RegMode::RDWR);
+    add_register("OP_MODE", LP5562DevReg::OP_MODE, I2CRegisters::RegMode::RDWR);
+
+    add_register("B_CURRENT", LP5562DevReg::B_CURRENT, I2CRegisters::RegMode::RDWR);
+    add_register("G_CURRENT", LP5562DevReg::G_CURRENT, I2CRegisters::RegMode::RDWR);
+    add_register("R_CURRENT", LP5562DevReg::R_CURRENT, I2CRegisters::RegMode::RDWR);
+
+    add_register("B_PWM", LP5562DevReg::B_PWM, I2CRegisters::RegMode::RDWR);
+    add_register("G_PWM", LP5562DevReg::G_PWM, I2CRegisters::RegMode::RDWR);
+    add_register("R_PWM", LP5562DevReg::R_PWM, I2CRegisters::RegMode::RDWR);
+
+    add_register("CONFIG", LP5562DevReg::CONFIG, I2CRegisters::RegMode::RDWR);
+
+    add_register("STATUS", LP5562DevReg::STATUS, I2CRegisters::RegMode::RDONLY);
+    add_register("RESET", LP5562DevReg::RESET, I2CRegisters::RegMode::RDWR);
+
+    add_register("LED_MAP", LP5562DevReg::LED_MAP, I2CRegisters::RegMode::RDWR);
+
+    reset_registers();
+
+    // create objects for each of the channels to handle the dynamics
+    // of updating each.  The channel gets references to the relevant
+    // configuration bytes.  This seems over-kill for the way we
+    // currently use this device but is a structure we use elsewhere.
+    // Note that this assumed that the LED Map register is set to its
+    // default value.
+    b = new LEDChannel(
+        byte[(uint8_t)LP5562DevReg::B_PWM]
+        );
+    g = new LEDChannel(
+        byte[(uint8_t)LP5562DevReg::G_PWM]
+        );
+    r = new LEDChannel(
+        byte[(uint8_t)LP5562DevReg::R_PWM]
+        );
+
+    rgbled.init();
+}
+
+void LP5562::reset_registers()
+{
+    set_register(LP5562DevReg::LED_MAP, (uint8_t)0b00111001);
+    set_register(LP5562DevReg::B_CURRENT, (uint8_t)0xAF);
+    set_register(LP5562DevReg::G_CURRENT, (uint8_t)0xAF);
+    set_register(LP5562DevReg::R_CURRENT, (uint8_t)0xAF);
+    set_register(LP5562DevReg::RESET, (uint8_t)0);
+}
+
+void LP5562::update(const class Aircraft &aircraft)
+{
+    {    // check the state of the reset register
+        union {
+            struct {
+                uint8_t reset;
+            } bits;
+            uint8_t byte;
+        } reset_reg;
+        reset_reg.byte = get_register(LP5562DevReg::RESET);
+
+        // 0xff in this register resets the device:
+        if (reset_reg.bits.reset == 0xFF) {
+            reset_registers();
+        }
+    }
+
+    {    // check the state of the enable register
+        union {
+            struct {
+                uint8_t eng3_exc : 2;
+                uint8_t eng2_exc : 2;
+                uint8_t eng1_exc : 2;
+                uint8_t chip_en : 1;
+                uint8_t log_en : 1;
+            } bits;
+            uint8_t byte;
+        } enable_reg;
+        enable_reg.byte = get_register(LP5562DevReg::ENABLE);
+
+        // we don't get fancy with the device:
+        if (enable_reg.bits.eng3_exc || enable_reg.bits.eng2_exc || enable_reg.bits.eng1_exc) {
+            AP_HAL::panic("engine execute bits set");
+        }
+
+        if (!enable_reg.bits.chip_en) {
+            // do nothing, maintain outputs; not sure what actual behaviour is
+            return;
+        }
+    }
+
+    {    // check the state of the config register
+        union {
+            struct {
+                uint8_t internal_clock : 1;
+                uint8_t autodetect : 1;
+                uint8_t unused2 : 1;
+                uint8_t unused3 : 1;
+                uint8_t unused4 : 1;
+                uint8_t pwrsav_en : 1;
+                uint8_t pwm_hf : 1;
+            } bits;
+            uint8_t byte;
+        } config_reg;
+        config_reg.byte = get_register(LP5562DevReg::CONFIG);
+        // we don't get fancy with the device.  Until the driver sets
+        // internal clock enabled we do nothing (assume no external clock)
+        if (config_reg.bits.internal_clock != 1) {
+            fprintf(stderr, "Waiting for internal clock bits to be set (current=0x%02x)\n", config_reg.byte);
+            return;
+        }
+    }
+
+    {    // check the state of the LED Map register
+        union {
+            struct {
+                uint8_t b_eng_sel : 2;
+                uint8_t g_eng_sel : 2;
+                uint8_t r_eng_sel : 2;
+                uint8_t w_eng_sel : 2;
+            } bits;
+            uint8_t byte;
+        } led_map_reg;
+        led_map_reg.byte = get_register(LP5562DevReg::LED_MAP);
+        // we don't get fancy with the device.  We expect this to be
+        // at its reset value
+        if (led_map_reg.bits.b_eng_sel != 0x00 ||
+            led_map_reg.bits.g_eng_sel != 0x00 ||
+            led_map_reg.bits.r_eng_sel != 0x00
+            ) {
+            fprintf(stderr, "Waiting for LED mode to be set to take-from-register\n");
+            return;
+        }
+    }
+
+    // update each channel.
+    b->update();
+    g->update();
+    r->update();
+
+    rgbled.set_colours(
+        r->current_value(),
+        g->current_value(),
+        b->current_value()
+        );
+}
+
+void LP5562::LEDChannel::update()
+{
+    // nothing to update here ATM; the output value is just the input value
+}
+
+#endif

libraries/SITL/SIM_LP5562.h

@@ -0,0 +1,127 @@
+#include <AP_HAL/AP_HAL_Boards.h>
+
+/*
+
+  ./Tools/autotest/sim_vehicle.py -v ArduCopter --gdb --debug --rgbled
+
+  param set NTF_LED_TYPES 8198  # enable ToshibaLED and LP5562
+  reboot
+
+  param set NTF_LED_OVERRIDE 1
+  led 255 0 0   # red
+  led 0 255 0   # green
+  led 0 0 255   # blue
+
+ */
+
+
+#ifndef AP_SIM_LP5562_ENABLED
+#define AP_SIM_LP5562_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif
+
+#if AP_SIM_LP5562_ENABLED
+
+#include "SIM_I2CDevice.h"
+#include "SIM_RGBLED.h"
+
+namespace SITL {
+
+class LP5562DevReg : public I2CRegEnum {
+public:
+    static constexpr uint8_t ENABLE = 0x00;
+    static constexpr uint8_t OP_MODE = 0x01;
+
+    static constexpr uint8_t B_PWM = 0x02;
+    static constexpr uint8_t G_PWM = 0x03;
+    static constexpr uint8_t R_PWM = 0x04;
+
+    static constexpr uint8_t B_CURRENT = 0x05;
+    static constexpr uint8_t G_CURRENT = 0x06;
+    static constexpr uint8_t R_CURRENT = 0x07;
+
+    static constexpr uint8_t CONFIG = 0x08;
+
+    static constexpr uint8_t ENG1_PC = 0x09;
+    static constexpr uint8_t ENG2_PC = 0x0A;
+    static constexpr uint8_t ENG3_PC = 0x0B;
+
+    static constexpr uint8_t STATUS = 0x0C;
+
+    static constexpr uint8_t RESET = 0x0D;
+
+    static constexpr uint8_t W_PWM = 0x0E;
+    static constexpr uint8_t W_CURRENT = 0x0F;
+
+    static constexpr uint8_t LED_MAP = 0x70;
+
+    static constexpr uint8_t PROG_MEM_ENG1_01H = 0x10;
+    static constexpr uint8_t PROG_MEM_ENG1_01L = 0x11;
+    static constexpr uint8_t PROG_MEM_ENG1_02H = 0x12;
+    static constexpr uint8_t PROG_MEM_ENG1_02L = 0x13;
+    // .
+    // .
+    static constexpr uint8_t PROG_MEM_ENG1_16H = 0x2E;
+    static constexpr uint8_t PROG_MEM_ENG1_16L = 0x2F;
+
+    static constexpr uint8_t PROG_MEM_ENG2_01H = 0x30;
+    static constexpr uint8_t PROG_MEM_ENG2_01L = 0x31;
+    static constexpr uint8_t PROG_MEM_ENG2_02H = 0x32;
+    static constexpr uint8_t PROG_MEM_ENG2_02L = 0x43;
+    // .
+    // .
+    static constexpr uint8_t PROG_MEM_ENG2_16H = 0x4E;
+    static constexpr uint8_t PROG_MEM_ENG2_16L = 0x4F;
+
+    static constexpr uint8_t PROG_MEM_ENG3_01H = 0x50;
+    static constexpr uint8_t PROG_MEM_ENG3_01L = 0x51;
+    static constexpr uint8_t PROG_MEM_ENG3_02H = 0x52;
+    static constexpr uint8_t PROG_MEM_ENG3_02L = 0x53;
+    // .
+    // .
+    static constexpr uint8_t PROG_MEM_ENG3_16H = 0x6E;
+    static constexpr uint8_t PROG_MEM_ENG3_16L = 0x6F;
+};
+
+class LP5562 : public I2CDevice, protected I2CRegisters_8Bit
+{
+public:
+
+    void init() override;
+
+    void update(const class Aircraft &aircraft) override;
+
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
+        return I2CRegisters_8Bit::rdwr(data);
+    }
+
+private:
+
+    // nested class to hold calculations for a single channel:
+    class LEDChannel {
+    public:
+        LEDChannel(uint8_t &_direct_pwm_value) :
+            direct_pwm_value{_direct_pwm_value}
+            { }
+
+        void update();
+
+        // returns a value 0-255 for LED brightness:
+        uint8_t current_value() const { return direct_pwm_value; }
+
+    private:
+        uint8_t &direct_pwm_value;
+    };
+
+    // these could come in as rgb or bgr?
+    LEDChannel *b;
+    LEDChannel *g;
+    LEDChannel *r;
+
+    void reset_registers();
+
+    SIM_RGBLED rgbled{"LP5562"};
+};
+
+} // namespace SITL
+
+#endif  // AP_SIM_LP5562_ENABLED