SITL: simulated i2c support

libraries/SITL/SIM_Aircraft.cpp

@@ -849,6 +849,11 @@ void Aircraft::update_external_payload(const struct sitl_input &input)
         external_payload_mass += sprayer->payload_mass();
     }
 
+    // update i2c
+    if (i2c) {
+        i2c->update(*this);
+    }
+
     // update buzzer
     if (buzzer && buzzer->is_enabled()) {
         buzzer->update(input);

libraries/SITL/SIM_Aircraft.h

@@ -29,6 +29,7 @@
 #include "SIM_Parachute.h"
 #include "SIM_Precland.h"
 #include "SIM_RichenPower.h"
+#include "SIM_I2C.h"
 #include "SIM_Buzzer.h"
 #include <Filter/Filter.h>
 
@@ -144,6 +145,7 @@ public:
     void set_gripper_servo(Gripper_Servo *_gripper) { gripper = _gripper; }
     void set_gripper_epm(Gripper_EPM *_gripper_epm) { gripper_epm = _gripper_epm; }
     void set_precland(SIM_Precland *_precland);
+    void set_i2c(class I2C *_i2c) { i2c = _i2c; }
 
 protected:
     SITL *sitl;
@@ -302,6 +304,7 @@ private:
     Parachute *parachute;
     RichenPower *richenpower;
     SIM_Precland *precland;
+    class I2C *i2c;
 };
 
 } // namespace SITL

libraries/SITL/SIM_I2C.cpp

@@ -0,0 +1,93 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulated i2c buses and devices
+*/
+
+
+#include <GCS_MAVLink/GCS.h>
+#include <SITL/SITL.h>
+
+#include "SIM_I2C.h"
+#include "SIM_ToshibaLED.h"
+
+#include <signal.h>
+
+using namespace SITL;
+
+enum class IOCtlType {
+    RDWR = 0,
+};
+
+class IgnoredI2CDevice : public I2CDevice
+{
+public:
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override {
+        return -1;
+    }
+};
+static IgnoredI2CDevice ignored;
+
+static ToshibaLED toshibaled;
+
+struct i2c_device_at_address {
+    uint8_t bus;
+    uint8_t addr;
+    I2CDevice &device;
+} i2c_devices[] {
+    { 1, 0x55, toshibaled },
+    { 1, 0x38, ignored }, // NCP5623
+    { 1, 0x39, ignored }, // NCP5623C
+    { 1, 0x40, ignored }, // KellerLD
+    { 1, 0x76, ignored }, // MS56XX
+};
+
+void I2C::update(const class Aircraft &aircraft)
+{
+    for (auto daa : i2c_devices) {
+        daa.device.update(aircraft);
+    }
+}
+
+int I2C::ioctl_rdwr(i2c_rdwr_ioctl_data *data)
+{
+    int ret = 0;
+    for (uint8_t i=0; i<data->nmsgs; i++) {
+        const i2c_msg &msg = data->msgs[i];
+        const uint8_t addr = msg.addr;
+        bool handled = false;
+        for (auto dev_at_address : i2c_devices) {
+            // where's the bus check?!
+            if (dev_at_address.addr == addr) {
+                ret = dev_at_address.device.rdwr(data);
+                handled = true;
+            }
+        }
+        if (!handled) {
+            ::fprintf(stderr, "Unhandled i2c message: addr=0x%x flags=%u len=%u\n", msg.addr, msg.flags, msg.len);
+            return -1;  // ?!
+        }
+    }
+    return ret;
+}
+
+int I2C::ioctl(uint8_t ioctl_type, void *data)
+{
+    switch ((IOCtlType) ioctl_type) {
+    case IOCtlType::RDWR:
+        return ioctl_rdwr((i2c_rdwr_ioctl_data*)data);
+    }
+    return -1;
+}

libraries/SITL/SIM_I2C.h

@@ -0,0 +1,57 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  Simulated i2c buses and devices
+*/
+
+#pragma once
+
+#include <AP_Param/AP_Param.h>
+
+#include "stdint.h"
+
+namespace SITL {
+
+class I2C {
+public:
+    I2C() {}
+
+    // update i2c state
+    void update(const class Aircraft &aircraft);
+
+    int ioctl(uint8_t ioctl_type, void *data);
+
+    // the following must be identical to AP_HAL_SITL/I2CDevice.h
+#define I2C_M_RD 1
+#define I2C_RDWR 0
+    struct i2c_msg {
+        uint8_t addr;
+        uint8_t flags;
+        uint8_t *buf;
+        uint16_t len; // FIXME: what type should this be?
+    };
+    struct i2c_rdwr_ioctl_data {
+        i2c_msg *msgs;
+        uint8_t nmsgs;
+    };
+    // end "the following"
+
+private:
+    int ioctl_rdwr(i2c_rdwr_ioctl_data *data);
+
+};
+
+}

libraries/SITL/SIM_I2CDevice.h

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "SIM_I2C.h"
+
+#include <SITL/SIM_Aircraft.h>
+
+namespace SITL {
+
+class I2CDevice {
+public:
+    virtual void update(const class Aircraft &aircraft) { }
+
+    virtual int rdwr(I2C::i2c_rdwr_ioctl_data *&data) = 0;
+};
+
+} // namespace SITL

libraries/SITL/SIM_ToshibaLED.cpp

@@ -0,0 +1,46 @@
+#include "SIM_ToshibaLED.h"
+
+#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)
+{
+    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);
+    }
+    // kill(0, SIGTRAP);
+    return -1;
+}

libraries/SITL/SIM_ToshibaLED.h

@@ -0,0 +1,19 @@
+#include "SIM_I2CDevice.h"
+
+namespace SITL {
+
+class ToshibaLED : public I2CDevice
+{
+public:
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
+    // void update(const struct sitl_input input) override;
+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;
+};
+
+} // namespace SITL

libraries/SITL/SITL.h

@@ -12,6 +12,7 @@
 #include "SIM_Buzzer.h"
 #include "SIM_Gripper_EPM.h"
 #include "SIM_Gripper_Servo.h"
+#include "SIM_I2C.h"
 #include "SIM_Parachute.h"
 #include "SIM_Precland.h"
 #include "SIM_Sprayer.h"
@@ -350,6 +351,10 @@ public:
     // convert a set of roll rates from body frame to earth frame
     static Vector3f convert_earth_frame(const Matrix3f &dcm, const Vector3f &gyro);
 
+    int i2c_ioctl(uint8_t i2c_operation, void *data) {
+        return i2c_sim.ioctl(i2c_operation, data);
+    }
+
     Sprayer sprayer_sim;
 
     // simulated ship takeoffs
@@ -360,6 +365,7 @@ public:
 
     Parachute parachute_sim;
     Buzzer buzzer_sim;
+    I2C i2c_sim;
     ToneAlarm tonealarm_sim;
     SIM_Precland precland_sim;
     RichenPower richenpower_sim;