SITL: add support for simulated QMC5883L compass

libraries/SITL/SIM_I2C.cpp

@@ -36,6 +36,7 @@
 #include "SIM_LM2755.h"
 #include "SIM_MS5525.h"
 #include "SIM_MS5611.h"
+#include "SIM_QMC5883L.h"
 
 #include <signal.h>
 
@@ -81,6 +82,9 @@ static LM2755 lm2755;
 static IS31FL3195 is31fl3195;
 #define SIM_IS31FL3195_ADDR 0x54
 #endif
+#if AP_SIM_COMPASS_QMC5883L_ENABLED
+static QMC5883L qmc5883l;
+#endif
 
 struct i2c_device_at_address {
     uint8_t bus;
@@ -116,6 +120,9 @@ struct i2c_device_at_address {
     { 2, 0x40, tsys03 },
 #endif
     { 2, 0x77, ms5611 },        // MS5611: BARO_PROBE_EXT = 2
+#if AP_SIM_COMPASS_QMC5883L_ENABLED
+    { 2, 0x0D, qmc5883l },
+#endif
 };
 
 void I2C::init()

libraries/SITL/SIM_QMC5883L.cpp

@@ -0,0 +1,128 @@
+#include "SIM_config.h"
+
+#if AP_SIM_COMPASS_QMC5883L_ENABLED
+
+#include "SIM_QMC5883L.h"
+
+// FIXME: read the datasheet :-)
+
+#include <SITL/SITL.h>
+#include <SITL/SIM_Aircraft.h>
+
+#include <stdio.h>
+
+#define QMC5883L_REG_CONF1 0x09
+#define QMC5883L_REG_CONF2 0x0A
+
+#define QMC5883L_MODE_STANDBY 0x00
+#define QMC5883L_MODE_CONTINUOUS 0x01
+
+#define QMC5883L_ODR_100HZ (0x02 << 2)
+
+#define QMC5883L_OSR_512 (0x00 << 6)
+
+#define QMC5883L_RNG_8G (0x01 << 4)
+
+SITL::QMC5883L::QMC5883L()
+{
+    writable_registers.set(0);
+    writable_registers.set(0x0b);
+    writable_registers.set(0x20);
+    writable_registers.set(0x21);
+    writable_registers.set(QMC5883L_REG_CONF1);
+    writable_registers.set(QMC5883L_REG_CONF2);
+
+    reset();
+}
+
+void SITL::QMC5883L::reset()
+{
+    memset(registers.byte, 0, ARRAY_SIZE(registers.byte));
+    registers.byname.ZEROX_ZEROC = 0x01;
+    registers.byname.REG_STATUS = 0x0;
+    registers.byname.REG_ID = 0xFF;
+}
+
+
+int SITL::QMC5883L::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
+{
+    if (data->nmsgs == 2) {
+        if (data->msgs[0].flags != 0) {
+            AP_HAL::panic("Unexpected flags");
+        }
+        if (data->msgs[1].flags != I2C_M_RD) {
+            AP_HAL::panic("Unexpected flags");
+        }
+        const uint8_t reg_addr = data->msgs[0].buf[0];
+        for (uint8_t i=0; i<data->msgs[1].len; i++) {
+            const uint8_t register_value = registers.byte[reg_addr+i];
+            data->msgs[1].buf[i] = register_value;
+
+            // FIXME: is this really how the status data-ready bit works?
+            if (reg_addr == 0x05) {  // that's the last data register...
+                registers.byname.REG_STATUS &= ~0x04;
+            }
+        }
+        return 0;
+    }
+
+    if (data->nmsgs == 1) {
+        if (data->msgs[0].flags != 0) {
+            AP_HAL::panic("Unexpected flags");
+        }
+        const uint8_t reg_addr = data->msgs[0].buf[0];
+        if (!writable_registers.get(reg_addr)) {
+            AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
+        }
+        const uint8_t register_value = data->msgs[0].buf[1];
+        ::fprintf(stderr, "Setting register (0x%x) to 0x%x\n", reg_addr, register_value);
+        registers.byte[reg_addr] = register_value;
+        return 0;
+    }
+
+    return -1;
+};
+
+void SITL::QMC5883L::update(const Aircraft &aircraft)
+{
+
+    // FIXME: swipe stuff from AP_Compass_SITL here.
+
+    // FIXME: somehow decide to use the simulated compass offsets etc
+    // from SITL
+
+    Vector3f f = AP::sitl()->state.bodyMagField;
+
+    // Vector3<int16_t> field
+    // int16_t str_x = 123;
+    // int16_t str_y = -56;
+    // int16_t str_z = 1;
+
+    f.rotate_inverse(ROTATION_ROLL_180_YAW_270);
+
+    f.x = -f.x;
+    f.z = -f.z;
+
+    f.x *= 3;
+    f.y *= 3;
+    f.z *= 3;
+
+    Vector3<int16_t> k {
+        k.x = htole16((int16_t)f.x),
+        k.y = htole16((int16_t)f.y),
+        k.z = htole16((int16_t)f.z)
+    };
+
+    if (registers.byname.REG_CONF1 & QMC5883L_MODE_CONTINUOUS) {
+        // FIXME: clock according to configuration here
+        registers.byname.REG_STATUS |= 0x04;
+        registers.byname.DATA_OUTPUT_X_low = k.x & 0xFF;
+        registers.byname.DATA_OUTPUT_X_high = k.x >> 8;
+        registers.byname.DATA_OUTPUT_Y_low = k.y & 0xFF;
+        registers.byname.DATA_OUTPUT_Y_high = k.y >> 8;
+        registers.byname.DATA_OUTPUT_Z_low = k.z & 0xFF;
+        registers.byname.DATA_OUTPUT_Z_high = k.z >> 8;
+    }
+}
+
+#endif  // AP_SIM_COMPASS_QMC5883L_ENABLED

libraries/SITL/SIM_QMC5883L.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include "SIM_config.h"
+
+#if AP_SIM_COMPASS_QMC5883L_ENABLED
+
+#include "SIM_I2CDevice.h"
+
+namespace SITL {
+
+class QMC5883L : public I2CDevice
+{
+public:
+
+    QMC5883L();
+
+    void update(const class Aircraft &aircraft) override;
+
+    int rdwr(I2C::i2c_rdwr_ioctl_data *&data) override;
+
+private:
+
+    union Registers {
+        uint8_t byte[256];
+        struct PACKED ByName {
+            uint8_t DATA_OUTPUT_X_low;
+            uint8_t DATA_OUTPUT_X_high;
+            uint8_t DATA_OUTPUT_Y_low;
+            uint8_t DATA_OUTPUT_Y_high;
+            uint8_t DATA_OUTPUT_Z_low;
+            uint8_t DATA_OUTPUT_Z_high;
+            uint8_t REG_STATUS;
+            uint8_t unused1[2]; // unused
+            uint8_t REG_CONF1;
+            uint8_t REG_CONF2;
+            uint8_t ZEROX_ZEROB; // magic register number from driver
+            uint8_t ZEROX_ZEROC; // magic register from driver should always be 0x01
+            uint8_t REG_ID;
+            uint8_t unused2[242];
+        } byname;
+    } registers;
+
+    // 256 1-byte registers:
+    assert_storage_size<Registers::ByName, 256> assert_storage_size_registers_reg;
+
+    Bitmask<256> writable_registers;
+
+    void reset();
+
+    uint32_t cmd_take_reading_received_ms;
+};
+
+} // namespace SITL
+
+#endif  // AP_SIM_COMPASS_QMC5883L_ENABLED

libraries/SITL/SIM_config.h

@@ -78,3 +78,13 @@
 #ifndef AP_SIM_GPS_UBLOX_ENABLED
 #define AP_SIM_GPS_UBLOX_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED
 #endif
+
+
+// simulated compass support; currently only in SITL, not SimOnHW:
+#ifndef AP_SIM_COMPASS_BACKEND_DEFAULT_ENABLED
+#define AP_SIM_COMPASS_BACKEND_DEFAULT_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
+#endif
+
+#ifndef AP_SIM_COMPASS_QMC5883L_ENABLED
+#define AP_SIM_COMPASS_QMC5883L_ENABLED AP_SIM_COMPASS_BACKEND_DEFAULT_ENABLED
+#endif