ardupilot/libraries/SITL/SIM_LP5562.cpp

162 lines
5.1 KiB
C++

#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