From 7952e998133cd94d49f5293bd7445e9815b5f473 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 8 Jun 2023 14:13:51 +1000 Subject: [PATCH] SITL: add IS31FL3195 simulator --- libraries/SITL/SIM_I2C.cpp | 12 ++++ libraries/SITL/SIM_I2CDevice.cpp | 2 +- libraries/SITL/SIM_I2CDevice.h | 5 ++ libraries/SITL/SIM_IS31FL3195.cpp | 94 +++++++++++++++++++++++++++++++ libraries/SITL/SIM_IS31FL3195.h | 78 +++++++++++++++++++++++++ libraries/SITL/SIM_config.h | 4 ++ 6 files changed, 194 insertions(+), 1 deletion(-) create mode 100644 libraries/SITL/SIM_IS31FL3195.cpp create mode 100644 libraries/SITL/SIM_IS31FL3195.h diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index a8b32c3fae..1f8f76413a 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/libraries/SITL/SIM_I2C.cpp @@ -17,6 +17,7 @@ */ +#include "SIM_config.h" #include #include @@ -29,6 +30,7 @@ #include "SIM_Temperature_TSYS01.h" #include "SIM_Temperature_MCP9600.h" #include "SIM_ICM40609.h" +#include "SIM_IS31FL3195.h" #include "SIM_LP5562.h" #include "SIM_LM2755.h" #include "SIM_MS5525.h" @@ -71,6 +73,10 @@ static LP5562 lp5562; #if AP_SIM_LM2755_ENABLED static LM2755 lm2755; #endif +#if AP_SIM_IS31FL3195_ENABLED +static IS31FL3195 is31fl3195; +#define SIM_IS31FL3195_ADDR 0x54 +#endif struct i2c_device_at_address { uint8_t bus; @@ -98,6 +104,9 @@ struct i2c_device_at_address { #endif #if AP_SIM_LM2755_ENABLED { 2, 0x67, lm2755 }, // LM2755 RGB LED driver +#endif +#if AP_SIM_IS31FL3195_ENABLED + { 2, SIM_IS31FL3195_ADDR, is31fl3195 }, // IS31FL3195 RGB LED driver; see page 9 #endif { 2, 0x77, ms5611 }, // MS5611: BARO_PROBE_EXT = 2 }; @@ -108,6 +117,9 @@ void I2C::init() i.device.init(); } + // IS31FL3195 needs to know its own address: + is31fl3195.set_product_id(SIM_IS31FL3195_ADDR); + // sanity check the i2c_devices structure to ensure we don't have // two devices at the same address on the same bus: for (uint8_t i=0; imsgs[0].buf[1+bytes_copied]; - byte[reg_addr] = register_value; + rdwr_store_register_value(reg_addr, register_value); bytes_copied++; } return 0; diff --git a/libraries/SITL/SIM_I2CDevice.h b/libraries/SITL/SIM_I2CDevice.h index 4e193d9ec2..d5c5ed2f34 100644 --- a/libraries/SITL/SIM_I2CDevice.h +++ b/libraries/SITL/SIM_I2CDevice.h @@ -91,6 +91,11 @@ public: protected: + // this method is called by rdwr to store values into a register + virtual void rdwr_store_register_value(uint8_t reg, uint8_t value) { + byte[reg] = value; + } + uint8_t byte[256]; }; diff --git a/libraries/SITL/SIM_IS31FL3195.cpp b/libraries/SITL/SIM_IS31FL3195.cpp new file mode 100644 index 0000000000..3adfcebb33 --- /dev/null +++ b/libraries/SITL/SIM_IS31FL3195.cpp @@ -0,0 +1,94 @@ +#include "SIM_config.h" + +#if AP_SIM_IS31FL3195_ENABLED + +#include "SIM_IS31FL3195.h" + +using namespace SITL; + +#include + +void IS31FL3195::init() +{ + add_register("PRODUCT_ID", IS31FL3195DevReg::PRODUCT_ID, I2CRegisters::RegMode::RDONLY); + add_register("SHUTDOWN_CONTROL", IS31FL3195DevReg::SHUTDOWN_CONTROL, I2CRegisters::RegMode::RDWR); + + add_register("P1_STATE", IS31FL3195DevReg::P1_STATE, I2CRegisters::RegMode::RDONLY); + add_register("P2_STATE", IS31FL3195DevReg::P2_STATE, I2CRegisters::RegMode::RDWR); + add_register("P3_STATE", IS31FL3195DevReg::P3_STATE, I2CRegisters::RegMode::RDWR); + add_register("P4_STATE", IS31FL3195DevReg::P4_STATE, I2CRegisters::RegMode::RDWR); + add_register("COLOUR_UPDATE", IS31FL3195DevReg::COLOUR_UPDATE, I2CRegisters::RegMode::WRONLY); + + add_register("OUT1", IS31FL3195DevReg::OUT1, I2CRegisters::RegMode::WRONLY); + add_register("OUT2", IS31FL3195DevReg::OUT2, I2CRegisters::RegMode::WRONLY); + add_register("OUT3", IS31FL3195DevReg::OUT3, I2CRegisters::RegMode::WRONLY); + add_register("OUT4", IS31FL3195DevReg::OUT4, I2CRegisters::RegMode::WRONLY); + + add_register("RESET_REGISTER", IS31FL3195DevReg::RESET_REGISTER, I2CRegisters::RegMode::WRONLY); + + // set_register(IS31FL3195DevReg::GENERAL_PURPOSE, 0b00000000); + + reset_registers(); + + rgbled.init(); +} + +void IS31FL3195::rdwr_store_register_value(uint8_t reg, uint8_t value) +{ + if (reg == IS31FL3195DevReg::COLOUR_UPDATE && value == 0xc5) { + colour_update_register_poked = true; + } + I2CRegisters_8Bit::rdwr_store_register_value(reg, value); +} + +int IS31FL3195::rdwr(I2C::i2c_rdwr_ioctl_data *&data) { + return I2CRegisters_8Bit::rdwr(data); +} + +void IS31FL3195::set_product_id(uint8_t product_id) +{ + set_register(IS31FL3195DevReg::PRODUCT_ID, uint8_t(product_id << 1U)); +} + + +void IS31FL3195::reset_registers() +{ + // see page 11 of datasheet for reset states + set_register(IS31FL3195DevReg::SHUTDOWN_CONTROL, (uint8_t)0b11110000); + + set_register(IS31FL3195DevReg::P1_STATE, (uint8_t)0); + set_register(IS31FL3195DevReg::P2_STATE, (uint8_t)0); + set_register(IS31FL3195DevReg::P3_STATE, (uint8_t)0); + set_register(IS31FL3195DevReg::P4_STATE, (uint8_t)0); + + set_register(IS31FL3195DevReg::OUT1, (uint8_t)0); + set_register(IS31FL3195DevReg::OUT2, (uint8_t)0); + set_register(IS31FL3195DevReg::OUT3, (uint8_t)0); + set_register(IS31FL3195DevReg::OUT4, (uint8_t)0); + + set_register(IS31FL3195DevReg::RESET_REGISTER, (uint8_t)0); +} + +void IS31FL3195::update(const class Aircraft &aircraft) +{ + if (get_register(IS31FL3195DevReg::RESET_REGISTER) == 0xC5) { + reset_registers(); + } + if (get_register(IS31FL3195DevReg::SHUTDOWN_CONTROL) == 0xf0) { + // in shutdown + return; + } + + if (!colour_update_register_poked) { + return; + } + colour_update_register_poked = false; + + const uint8_t red = get_register(IS31FL3195DevReg::OUT1); + const uint8_t green = get_register(IS31FL3195DevReg::OUT2); + const uint8_t blue = get_register(IS31FL3195DevReg::OUT3); + + rgbled.set_colours(red, green, blue); +} + +#endif diff --git a/libraries/SITL/SIM_IS31FL3195.h b/libraries/SITL/SIM_IS31FL3195.h new file mode 100644 index 0000000000..e51484a480 --- /dev/null +++ b/libraries/SITL/SIM_IS31FL3195.h @@ -0,0 +1,78 @@ +/* + + DataSheet: https://www.lumissil.com/assets/pdf/core/IS31FL3195_DS.pdf + + ./Tools/autotest/sim_vehicle.py -v ArduCopter --gdb --debug --rgbled + + param set NTF_LED_TYPES 32772 # enable ToshibaLED and IS31FL3195 + reboot + + param set NTF_LED_OVERRIDE 1 + led 255 0 0 # red + led 0 255 0 # green + led 0 0 255 # blue + + */ + + +#include "SIM_config.h" + +#if AP_SIM_IS31FL3195_ENABLED + +#include "SIM_I2CDevice.h" +#include "SIM_RGBLED.h" + +namespace SITL { + +class IS31FL3195DevReg : public I2CRegEnum { +public: + static constexpr uint8_t PRODUCT_ID = 0x00; // not really; actually i2c addr! + static constexpr uint8_t SHUTDOWN_CONTROL = 0x01; + // . + // . + static constexpr uint8_t P1_STATE = 0x0C; + static constexpr uint8_t P2_STATE = 0x0D; + static constexpr uint8_t P3_STATE = 0x0E; + static constexpr uint8_t P4_STATE = 0x0F; + static constexpr uint8_t OUT1 = 0x10; + // . + // . + static constexpr uint8_t OUT2 = 0x21; + // . + // . + static constexpr uint8_t OUT3 = 0x32; + // . + // . + static constexpr uint8_t OUT4 = 0x40; + // . + // . + static constexpr uint8_t COLOUR_UPDATE = 0x50; + // . + // . + static constexpr uint8_t RESET_REGISTER = 0x5f; // reset the registers value to default +}; + +class IS31FL3195 : public I2CDevice, protected I2CRegisters_8Bit +{ +public: + + void init() override; + void set_product_id(uint8_t product_id); + + void update(const class Aircraft &aircraft) override; + + void rdwr_store_register_value(uint8_t reg, uint8_t value) override; + int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override; + +private: + + void reset_registers(); + + bool colour_update_register_poked; + + SIM_RGBLED rgbled{"IS31FL3195"}; +}; + +} // namespace SITL + +#endif // AP_SIM_IS31FL3195_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index e9c55f17aa..9a2fddd18e 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -13,3 +13,7 @@ #ifndef HAL_SIM_PS_RPLIDARA2_ENABLED #define HAL_SIM_PS_RPLIDARA2_ENABLED HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED #endif + +#ifndef AP_SIM_IS31FL3195_ENABLED +#define AP_SIM_IS31FL3195_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif