mirror of https://github.com/ArduPilot/ardupilot
171 lines
7.2 KiB
C++
171 lines
7.2 KiB
C++
#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_NOTHROW 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_NOTHROW 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_NOTHROW 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
|