diff --git a/libraries/SITL/SIM_ToshibaLED.cpp b/libraries/SITL/SIM_ToshibaLED.cpp index c1f2828baa..f9ec017f3a 100644 --- a/libraries/SITL/SIM_ToshibaLED.cpp +++ b/libraries/SITL/SIM_ToshibaLED.cpp @@ -2,45 +2,18 @@ #include -#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); } diff --git a/libraries/SITL/SIM_ToshibaLED.h b/libraries/SITL/SIM_ToshibaLED.h index c18ee22d12..200a137751 100644 --- a/libraries/SITL/SIM_ToshibaLED.h +++ b/libraries/SITL/SIM_ToshibaLED.h @@ -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