aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: make ToshibaLED an 8-bit register device

Browse Source
This commit is contained in:
Peter Barker 2020-11-12 15:19:13 +11:00 committed by Peter Barker
parent e21e55ba51
commit 545a086865
2 changed files with 38 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/SITL/SIM_ToshibaLED.cpp
View File

@ -2,45 +2,18 @@
#include <stdio.h>
#define TOSHIBA_LED_PWM0 0x01 // pwm0 register
#define TOSHIBA_LED_PWM1 0x02 // pwm1 register
#define TOSHIBA_LED_PWM2 0x03 // pwm2 register
#define TOSHIBA_LED_ENABLE 0x04 // enable register
int SITL::ToshibaLED::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
void SITL::ToshibaLED::update(const class Aircraft &aircraft)
{
if (data->nmsgs != 1) {
// this is really just because it is unexpected from the
// ArduPilot code, rather than being incorrect from a
// simulated device perspective.
AP_HAL::panic("Reading from Toshiba LED?!");
if (last_print_pwm0 == get_register(ToshibaLEDDevReg::PWM0) &&
last_print_pwm1 == get_register(ToshibaLEDDevReg::PWM1) &&
last_print_pwm2 == get_register(ToshibaLEDDevReg::PWM2) &&
last_print_enable == get_register(ToshibaLEDDevReg::ENABLE)) {
return;
}
const struct I2C::i2c_msg &msg = data->msgs[0];
const uint8_t reg = msg.buf[0];
const uint8_t val = msg.buf[1];
switch(reg) {
case TOSHIBA_LED_PWM0:
// ::fprintf(stderr, "ToshibaLED: pwm0=%u %u %u\n", msg.buf[1], msg.buf[2], msg.buf[3]);
_pwm0 = val;
break;
case TOSHIBA_LED_PWM1:
// ::fprintf(stderr, "ToshibaLED: pwm1=%u\n", val);
_pwm1 = val;
break;
case TOSHIBA_LED_PWM2:
// ::fprintf(stderr, "ToshibaLED: pwm2=%u\n", val);
_pwm2 = val;
break;
case TOSHIBA_LED_ENABLE:
if (val != 0x03) {
AP_HAL::panic("Unexpected enable value (%u)", val);
}
// ::fprintf(stderr, "ToshibaLED: enabling\n");
_enabled = true;
break;
default:
AP_HAL::panic("Unexpected register (%u)", reg);
}
// kill(0, SIGTRAP);
return -1;
last_print_pwm0 = get_register(ToshibaLEDDevReg::PWM0);
last_print_pwm1 = get_register(ToshibaLEDDevReg::PWM1);
last_print_pwm2 = get_register(ToshibaLEDDevReg::PWM2);
last_print_enable = get_register(ToshibaLEDDevReg::ENABLE);
// gcs().send_text(MAV_SEVERITY_INFO, "SIM_ToshibaLED: PWM0=%u PWM1=%u PWM2=%u ENABLE=%u", last_print_pwm0, last_print_pwm1, last_print_pwm2, last_print_enable);
}

35
libraries/SITL/SIM_ToshibaLED.h
View File

@ -2,18 +2,35 @@
namespace SITL {
class ToshibaLED : public I2CDevice
class ToshibaLEDDevReg : public I2CRegEnum {
public:
static constexpr uint8_t PWM0 = 0x01;
static constexpr uint8_t PWM1 = 0x02;
static constexpr uint8_t PWM2 = 0x03;
static constexpr uint8_t ENABLE = 0x04;
};
class ToshibaLED : public I2CDevice, protected I2CRegisters_8Bit
{
public:
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
// void update(const struct sitl_input input) override;
void init() override {
add_register("PWM0", ToshibaLEDDevReg::PWM0, O_WRONLY);
add_register("PWM1", ToshibaLEDDevReg::PWM1, O_WRONLY);
add_register("PWM2", ToshibaLEDDevReg::PWM2, O_WRONLY);
add_register("ENABLE", ToshibaLEDDevReg::ENABLE, O_WRONLY);
}
void update(const class Aircraft &aircraft) override;
int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
return I2CRegisters_8Bit::rdwr(data);
}
private:
bool _enabled;
bool _pwm0; // FIXME: just an array of register values?!
bool _pwm1;
bool _pwm2;
bool _pwm3;
uint32_t last_internal_clock_update_ms;
uint8_t last_print_pwm0;
uint8_t last_print_pwm1;
uint8_t last_print_pwm2;
uint8_t last_print_enable;
};
} // namespace SITL