mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-21 23:33:57 -04:00
SITL: add simulator for LM2755 RGBLED
This commit is contained in:
Peter Barker
Peter Barker
parent
f67f025c92
commit
b7f3d25bed
@ -30,6 +30,7 @@
|
||||
#include "SIM_Temperature_MCP9600.h"
|
||||
#include "SIM_ICM40609.h"
|
||||
#include "SIM_LP5562.h"
|
||||
#include "SIM_LM2755.h"
|
||||
#include "SIM_MS5525.h"
|
||||
#include "SIM_MS5611.h"
|
||||
|
||||
@ -67,6 +68,9 @@ static MS5611 ms5611;
|
||||
#if AP_SIM_LP5562_ENABLED
|
||||
static LP5562 lp5562;
|
||||
#endif
|
||||
#if AP_SIM_LM2755_ENABLED
|
||||
static LM2755 lm2755;
|
||||
#endif
|
||||
|
||||
struct i2c_device_at_address {
|
||||
uint8_t bus;
|
||||
@ -91,6 +95,9 @@ struct i2c_device_at_address {
|
||||
{ 2, 0x28, airspeed_dlvr }, // ARSPD_TYPE = 7 5inch H2O sensor
|
||||
#if AP_SIM_LP5562_ENABLED
|
||||
{ 2, 0x30, lp5562 }, // LP5562 RGB LED driver
|
||||
#endif
|
||||
#if AP_SIM_LM2755_ENABLED
|
||||
{ 2, 0x67, lm2755 }, // LM2755 RGB LED driver
|
||||
#endif
|
||||
{ 2, 0x77, ms5611 }, // MS5611: BARO_PROBE_EXT = 2
|
||||
};
|
||||
|
||||
170
libraries/SITL/SIM_LM2755.cpp
Normal file
170
libraries/SITL/SIM_LM2755.cpp
Normal file
@ -0,0 +1,170 @@
|
||||
#include "SIM_LM2755.h"
|
||||
|
||||
#if AP_SIM_LM2755_ENABLED
|
||||
|
||||
using namespace SITL;
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
void LM2755::init()
|
||||
{
|
||||
add_register("GENERAL_PURPOSE", LM2755DevReg::GENERAL_PURPOSE, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("TIME_STEP", LM2755DevReg::TIME_STEP, I2CRegisters::RegMode::WRONLY);
|
||||
|
||||
add_register("D1_HIGH_LEVEL", LM2755DevReg::D1_HIGH_LEVEL, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D1_LOW_LEVEL", LM2755DevReg::D1_LOW_LEVEL, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D1_DELAY", LM2755DevReg::D1_DELAY, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D1_RAMP_UP_STEP_TIME", LM2755DevReg::D1_RAMP_UP_STEP_TIME, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D1_TIME_HIGH", LM2755DevReg::D1_TIME_HIGH, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D1_RAMP_DOWN_STEP_TIME", LM2755DevReg::D1_RAMP_DOWN_STEP_TIME, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D1_TIMING", LM2755DevReg::D1_TIMING, I2CRegisters::RegMode::WRONLY);
|
||||
|
||||
add_register("D2_HIGH_LEVEL", LM2755DevReg::D2_HIGH_LEVEL, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D2_LOW_LEVEL", LM2755DevReg::D2_LOW_LEVEL, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D2_DELAY", LM2755DevReg::D2_DELAY, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D2_RAMP_UP_STEP_TIME", LM2755DevReg::D2_RAMP_UP_STEP_TIME, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D2_TIME_HIGH", LM2755DevReg::D2_TIME_HIGH, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D2_RAMP_DOWN_STEP_TIME", LM2755DevReg::D2_RAMP_DOWN_STEP_TIME, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D2_TIMING", LM2755DevReg::D2_TIMING, I2CRegisters::RegMode::WRONLY);
|
||||
|
||||
add_register("D3_HIGH_LEVEL", LM2755DevReg::D3_HIGH_LEVEL, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D3_LOW_LEVEL", LM2755DevReg::D3_LOW_LEVEL, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D3_DELAY", LM2755DevReg::D3_DELAY, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D3_RAMP_UP_STEP_TIME", LM2755DevReg::D3_RAMP_UP_STEP_TIME, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D3_TIME_HIGH", LM2755DevReg::D3_TIME_HIGH, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D3_RAMP_DOWN_STEP_TIME", LM2755DevReg::D3_RAMP_DOWN_STEP_TIME, I2CRegisters::RegMode::WRONLY);
|
||||
add_register("D3_TIMING", LM2755DevReg::D3_TIMING, I2CRegisters::RegMode::WRONLY);
|
||||
|
||||
|
||||
// set startup register values. Note that a lot of these are zero
|
||||
// in the datasheet and are zero when we allocate this object, so
|
||||
// code is commented out here
|
||||
|
||||
// set_register(LM2755DevReg::GENERAL_PURPOSE, 0b00000000);
|
||||
set_register(LM2755DevReg::TIME_STEP, (uint8_t)0b10001000);
|
||||
|
||||
set_register(LM2755DevReg::D1_HIGH_LEVEL, (uint8_t)0b11100000);
|
||||
set_register(LM2755DevReg::D1_LOW_LEVEL, (uint8_t)0b11100000);
|
||||
// set_register(LM2755DevReg::D1_DELAY, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D1_RAMP_UP_STEP_TIME, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D1_TIME_HIGH, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D1_RAMP_DOWN_STEP_TIME, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D1_TIMING, (uint8_t)0b00000000);
|
||||
|
||||
set_register(LM2755DevReg::D2_HIGH_LEVEL, (uint8_t)0b11100000);
|
||||
set_register(LM2755DevReg::D2_LOW_LEVEL, (uint8_t)0b11100000);
|
||||
// set_register(LM2755DevReg::D2_DELAY, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D2_RAMP_UP_STEP_TIME, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D2_TIME_HIGH, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D2_RAMP_DOWN_STEP_TIME, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D2_TIMING, (uint8_t)0b00000000);
|
||||
|
||||
set_register(LM2755DevReg::D3_HIGH_LEVEL, (uint8_t)0b11100000);
|
||||
set_register(LM2755DevReg::D3_LOW_LEVEL, (uint8_t)0b11100000);
|
||||
// set_register(LM2755DevReg::D3_DELAY, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D3_RAMP_UP_STEP_TIME, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D3_TIME_HIGH, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D3_RAMP_DOWN_STEP_TIME, (uint8_t)0b00000000);
|
||||
// set_register(LM2755DevReg::D3_TIMING, (uint8_t)0b00000000);
|
||||
|
||||
// create objects for each of the channels to handle the dynamics
|
||||
// of updating each. The channel gets references to the relevant
|
||||
// configuration bytes.
|
||||
d1 = new LEDChannel(
|
||||
byte[(uint8_t)LM2755DevReg::D1_HIGH_LEVEL],
|
||||
byte[(uint8_t)LM2755DevReg::D1_LOW_LEVEL],
|
||||
byte[(uint8_t)LM2755DevReg::D1_DELAY],
|
||||
byte[(uint8_t)LM2755DevReg::D1_RAMP_UP_STEP_TIME],
|
||||
byte[(uint8_t)LM2755DevReg::D1_TIME_HIGH],
|
||||
byte[(uint8_t)LM2755DevReg::D1_RAMP_DOWN_STEP_TIME],
|
||||
byte[(uint8_t)LM2755DevReg::D1_TIMING]
|
||||
);
|
||||
d2 = new LEDChannel(
|
||||
byte[(uint8_t)LM2755DevReg::D2_HIGH_LEVEL],
|
||||
byte[(uint8_t)LM2755DevReg::D2_LOW_LEVEL],
|
||||
byte[(uint8_t)LM2755DevReg::D2_DELAY],
|
||||
byte[(uint8_t)LM2755DevReg::D2_RAMP_UP_STEP_TIME],
|
||||
byte[(uint8_t)LM2755DevReg::D2_TIME_HIGH],
|
||||
byte[(uint8_t)LM2755DevReg::D2_RAMP_DOWN_STEP_TIME],
|
||||
byte[(uint8_t)LM2755DevReg::D2_TIMING]
|
||||
);
|
||||
d3 = new LEDChannel(
|
||||
byte[(uint8_t)LM2755DevReg::D3_HIGH_LEVEL],
|
||||
byte[(uint8_t)LM2755DevReg::D3_LOW_LEVEL],
|
||||
byte[(uint8_t)LM2755DevReg::D3_DELAY],
|
||||
byte[(uint8_t)LM2755DevReg::D3_RAMP_UP_STEP_TIME],
|
||||
byte[(uint8_t)LM2755DevReg::D3_TIME_HIGH],
|
||||
byte[(uint8_t)LM2755DevReg::D3_RAMP_DOWN_STEP_TIME],
|
||||
byte[(uint8_t)LM2755DevReg::D3_TIMING]
|
||||
);
|
||||
|
||||
rgbled.init();
|
||||
}
|
||||
|
||||
void LM2755::update(const class Aircraft &aircraft)
|
||||
{
|
||||
union {
|
||||
struct {
|
||||
uint8_t d1_high_current : 1;
|
||||
uint8_t d2_high_current : 1;
|
||||
uint8_t d3_high_current : 1;
|
||||
uint8_t d1_dimming : 1;
|
||||
uint8_t d2_dimming : 1;
|
||||
uint8_t d3_dimming : 1;
|
||||
uint8_t external_clock : 1;
|
||||
uint8_t charge_pump : 1;
|
||||
} bits;
|
||||
uint8_t byte;
|
||||
} gp;
|
||||
gp.byte = get_register(LM2755DevReg::GENERAL_PURPOSE);
|
||||
|
||||
if (gp.byte & 0x7) {
|
||||
// disallow high current
|
||||
AP_HAL::panic("High current bits set");
|
||||
}
|
||||
if (gp.bits.charge_pump) {
|
||||
AP_HAL::panic("Charge pump set");
|
||||
}
|
||||
if (gp.bits.external_clock) {
|
||||
AP_HAL::panic("external clock set");
|
||||
}
|
||||
|
||||
// update each channel.
|
||||
if (gp.bits.d1_dimming) {
|
||||
d1->update();
|
||||
}
|
||||
if (gp.bits.d2_dimming) {
|
||||
d2->update();
|
||||
}
|
||||
if (gp.bits.d3_dimming) {
|
||||
d3->update();
|
||||
}
|
||||
|
||||
rgbled.set_colours(
|
||||
d1->current_value(),
|
||||
d2->current_value(),
|
||||
d3->current_value()
|
||||
);
|
||||
}
|
||||
|
||||
void LM2755::LEDChannel::update()
|
||||
{
|
||||
// calculate the brightness for this LED:
|
||||
|
||||
// this is an awful simulation, which assumes that ArduPilot only
|
||||
// step-changtes the input for this device. AP could do better in
|
||||
// terms of passing through the "throbber" logic down to the
|
||||
// device itself - at which point a *lot* of these sanity checks
|
||||
// will fail.
|
||||
|
||||
if (high_level != low_level) {
|
||||
// we should not be dimming when high != low
|
||||
AP_HAL::panic("high != low");
|
||||
}
|
||||
if (high_level > 31) {
|
||||
AP_HAL::panic("Invalid high value %u", high_level);
|
||||
}
|
||||
output_value = high_level * 8; // OK, so this is 0-248. So Sue me.
|
||||
}
|
||||
|
||||
#endif
|
||||
110
libraries/SITL/SIM_LM2755.h
Normal file
110
libraries/SITL/SIM_LM2755.h
Normal file
@ -0,0 +1,110 @@
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
|
||||
/*
|
||||
* To test in SITL:
|
||||
*
|
||||
* ./Tools/autotest/sim_vehicle.py -v ArduCopter --rgbled
|
||||
*/
|
||||
|
||||
#ifndef AP_SIM_LM2755_ENABLED
|
||||
#define AP_SIM_LM2755_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#if AP_SIM_LM2755_ENABLED
|
||||
|
||||
#include "SIM_I2CDevice.h"
|
||||
#include "SIM_RGBLED.h"
|
||||
|
||||
namespace SITL {
|
||||
|
||||
class LM2755DevReg : public I2CRegEnum {
|
||||
public:
|
||||
static constexpr uint8_t GENERAL_PURPOSE = 0x10;
|
||||
static constexpr uint8_t TIME_STEP = 0x20;
|
||||
|
||||
static constexpr uint8_t D1_HIGH_LEVEL = 0xA9;
|
||||
static constexpr uint8_t D1_LOW_LEVEL = 0xA8;
|
||||
static constexpr uint8_t D1_DELAY = 0xA1;
|
||||
static constexpr uint8_t D1_RAMP_UP_STEP_TIME = 0xA5;
|
||||
static constexpr uint8_t D1_TIME_HIGH = 0xA3;
|
||||
static constexpr uint8_t D1_RAMP_DOWN_STEP_TIME = 0xA4;
|
||||
static constexpr uint8_t D1_TIMING = 0xA2;
|
||||
|
||||
static constexpr uint8_t D2_HIGH_LEVEL = 0xB9;
|
||||
static constexpr uint8_t D2_LOW_LEVEL = 0xB8;
|
||||
static constexpr uint8_t D2_DELAY = 0xB1;
|
||||
static constexpr uint8_t D2_RAMP_UP_STEP_TIME = 0xB5;
|
||||
static constexpr uint8_t D2_TIME_HIGH = 0xB3;
|
||||
static constexpr uint8_t D2_RAMP_DOWN_STEP_TIME = 0xB4;
|
||||
static constexpr uint8_t D2_TIMING = 0xB2;
|
||||
|
||||
static constexpr uint8_t D3_HIGH_LEVEL = 0xC9;
|
||||
static constexpr uint8_t D3_LOW_LEVEL = 0xC8;
|
||||
static constexpr uint8_t D3_DELAY = 0xC1;
|
||||
static constexpr uint8_t D3_RAMP_UP_STEP_TIME = 0xC5;
|
||||
static constexpr uint8_t D3_TIME_HIGH = 0xC3;
|
||||
static constexpr uint8_t D3_RAMP_DOWN_STEP_TIME = 0xC4;
|
||||
static constexpr uint8_t D3_TIMING = 0xC2;
|
||||
};
|
||||
|
||||
class LM2755 : 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 &_high_level,
|
||||
uint8_t &_low_level,
|
||||
uint8_t &_delay,
|
||||
uint8_t &_ramp_up_step_time,
|
||||
uint8_t &_time_high,
|
||||
uint8_t &_ramp_down_step_time,
|
||||
uint8_t &_timing) :
|
||||
high_level{_high_level},
|
||||
low_level{_low_level},
|
||||
delay{_delay},
|
||||
ramp_up_step_time{_ramp_up_step_time},
|
||||
time_high{_time_high},
|
||||
ramp_down_step_time{_ramp_down_step_time},
|
||||
timing{_timing}
|
||||
{ }
|
||||
|
||||
void update();
|
||||
|
||||
// returns a value 0-255 for LED brightness:
|
||||
uint8_t current_value() const { return output_value; }
|
||||
|
||||
private:
|
||||
uint8_t &high_level;
|
||||
uint8_t &low_level;
|
||||
uint8_t &delay;
|
||||
uint8_t &ramp_up_step_time;
|
||||
uint8_t &time_high;
|
||||
uint8_t &ramp_down_step_time;
|
||||
uint8_t &timing;
|
||||
|
||||
uint8_t output_value;
|
||||
};
|
||||
|
||||
// these could come in as rgb or bgr?
|
||||
LEDChannel *d1;
|
||||
LEDChannel *d2;
|
||||
LEDChannel *d3;
|
||||
|
||||
SIM_RGBLED rgbled{"LM2755"};
|
||||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
#endif // AP_SIM_LM2755_ENABLED
|
||||
Loading…
Reference in New Issue
Block a user