From 19d265917996a6688b404fd26eaa6a1d6a243fff Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 27 Feb 2023 10:23:04 +1100 Subject: [PATCH] SITL: add simulator for LP5562 RGBLED --- libraries/SITL/SIM_I2C.cpp | 3 + libraries/SITL/SIM_LP5562.cpp | 161 ++++++++++++++++++++++++++++++++++ libraries/SITL/SIM_LP5562.h | 127 +++++++++++++++++++++++++++ 3 files changed, 291 insertions(+) create mode 100644 libraries/SITL/SIM_LP5562.cpp create mode 100644 libraries/SITL/SIM_LP5562.h diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index ac4c186022..d23150f656 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/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 }; diff --git a/libraries/SITL/SIM_LP5562.cpp b/libraries/SITL/SIM_LP5562.cpp new file mode 100644 index 0000000000..7d0e88087a --- /dev/null +++ b/libraries/SITL/SIM_LP5562.cpp @@ -0,0 +1,161 @@ +#include "SIM_LP5562.h" + +#if AP_SIM_LP5562_ENABLED + +using namespace SITL; + +#include + +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 diff --git a/libraries/SITL/SIM_LP5562.h b/libraries/SITL/SIM_LP5562.h new file mode 100644 index 0000000000..9d335cf082 --- /dev/null +++ b/libraries/SITL/SIM_LP5562.h @@ -0,0 +1,127 @@ +#include + +/* + + ./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