From dec618645db6166c3a7e80a6c2777049ee0a9758 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 19 Aug 2020 16:15:55 +1000 Subject: [PATCH] SITL: add support for simulated QMC5883L compass --- libraries/SITL/SIM_I2C.cpp | 7 ++ libraries/SITL/SIM_QMC5883L.cpp | 128 ++++++++++++++++++++++++++++++++ libraries/SITL/SIM_QMC5883L.h | 55 ++++++++++++++ libraries/SITL/SIM_config.h | 10 +++ 4 files changed, 200 insertions(+) create mode 100644 libraries/SITL/SIM_QMC5883L.cpp create mode 100644 libraries/SITL/SIM_QMC5883L.h diff --git a/libraries/SITL/SIM_I2C.cpp b/libraries/SITL/SIM_I2C.cpp index 51450870d7..196301942d 100644 --- a/libraries/SITL/SIM_I2C.cpp +++ b/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 @@ -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() diff --git a/libraries/SITL/SIM_QMC5883L.cpp b/libraries/SITL/SIM_QMC5883L.cpp new file mode 100644 index 0000000000..e9e93813a3 --- /dev/null +++ b/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 +#include + +#include + +#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; imsgs[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 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 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 diff --git a/libraries/SITL/SIM_QMC5883L.h b/libraries/SITL/SIM_QMC5883L.h new file mode 100644 index 0000000000..10aa171965 --- /dev/null +++ b/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 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 diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 4eb427c0ae..aab57e099d 100644 --- a/libraries/SITL/SIM_config.h +++ b/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