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

SITL: add simulator for LP5562 RGBLED

This commit is contained in:
Peter Barker 2023-02-27 10:23:04 +11:00 committed by Peter Barker
parent 52d5fbad39
commit 19d2659179
3 changed files with 291 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/SITL/SIM_I2C.cpp
View File

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

161
libraries/SITL/SIM_LP5562.cpp Normal file
View File

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

127
libraries/SITL/SIM_LP5562.h Normal file
View File

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