ardupilot/libraries/SITL/SIM_LM2755.cpp

171 lines
7.1 KiB
C++
Raw Normal View History

2023-02-26 19:23:04 -04:00
#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