SITL: make ToshibaLED an 8-bit register device

libraries/SITL/SIM_ToshibaLED.cpp

@@ -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?!");
-    }
-    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);
+    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;
     }
-    // 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);
 }

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