Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: add IS31FL3195 simulator

This commit is contained in:
Peter Barker 2023-06-08 14:13:51 +10:00 committed by Andrew Tridgell
parent c92f71842b
commit 7952e99813
6 changed files with 194 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/SITL/SIM_I2C.cpp
View File

@ -17,6 +17,7 @@
*/
#include "SIM_config.h"
#include <GCS_MAVLink/GCS.h>
#include <SITL/SITL.h>
@ -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; i<ARRAY_SIZE(i2c_devices)-1; i++) {

2
libraries/SITL/SIM_I2CDevice.cpp
View File

@ -147,7 +147,7 @@ int SITL::I2CRegisters_8Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
}
const uint8_t register_value = data->msgs[0].buf[1+bytes_copied];
byte[reg_addr] = register_value;
rdwr_store_register_value(reg_addr, register_value);
bytes_copied++;
}
return 0;

5
libraries/SITL/SIM_I2CDevice.h
View File

@ -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];
};

94
libraries/SITL/SIM_IS31FL3195.cpp Normal file
View File

@ -0,0 +1,94 @@
#include "SIM_config.h"
#if AP_SIM_IS31FL3195_ENABLED
#include "SIM_IS31FL3195.h"
using namespace SITL;
#include <stdio.h>
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

78
libraries/SITL/SIM_IS31FL3195.h Normal file
View File

@ -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

4
libraries/SITL/SIM_config.h
View File

@ -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