diff --git a/libraries/AP_Compass/AP_Compass.cpp b/libraries/AP_Compass/AP_Compass.cpp
index da8e2a1313..355b2bf9f8 100644
--- a/libraries/AP_Compass/AP_Compass.cpp
+++ b/libraries/AP_Compass/AP_Compass.cpp
@@ -21,6 +21,7 @@
#include "AP_Compass_AK8963.h"
#include "AP_Compass_Backend.h"
#include "AP_Compass_BMM150.h"
+#include "AP_Compass_BMM350.h"
#include "AP_Compass_HMC5843.h"
#include "AP_Compass_IST8308.h"
#include "AP_Compass_IST8310.h"
@@ -516,7 +517,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
// @Param: DISBLMSK
// @DisplayName: Compass disable driver type mask
// @Description: This is a bitmask of driver types to disable. If a driver type is set in this mask then that driver will not try to find a sensor at startup
- // @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,11:DroneCAN,12:QMC5883,14:MAG3110,15:IST8308,16:RM3100,17:MSP,18:ExternalAHRS
+ // @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,11:DroneCAN,12:QMC5883,14:MAG3110,15:IST8308,16:RM3100,17:MSP,18:ExternalAHRS,21:BMM350
// @User: Advanced
AP_GROUPINFO("DISBLMSK", 33, Compass, _driver_type_mask, 0),
@@ -1314,6 +1315,23 @@ void Compass::_probe_external_i2c_compasses(void)
}
}
#endif // AP_COMPASS_BMM150_ENABLED
+
+#if AP_COMPASS_BMM350_ENABLED
+ // BMM350 on I2C
+ FOREACH_I2C_EXTERNAL(i) {
+ for (uint8_t addr=BMM350_I2C_ADDR_MIN; addr <= BMM350_I2C_ADDR_MAX; addr++) {
+ ADD_BACKEND(DRIVER_BMM350,
+ AP_Compass_BMM350::probe(GET_I2C_DEVICE(i, addr), true, ROTATION_NONE));
+ }
+ }
+#if !defined(HAL_SKIP_AUTO_INTERNAL_I2C_PROBE)
+ FOREACH_I2C_INTERNAL(i) {
+ for (uint8_t addr=BMM350_I2C_ADDR_MIN; addr <= BMM350_I2C_ADDR_MAX; addr++) {
+ ADD_BACKEND(DRIVER_BMM350, AP_Compass_BMM350::probe(GET_I2C_DEVICE(i, addr), all_external, ROTATION_NONE));
+ }
+ }
+#endif
+#endif // AP_COMPASS_BMM350_ENABLED
}
/*
diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index fe1a66ad7e..6498c1d31c 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/libraries/AP_Compass/AP_Compass.h
@@ -496,6 +496,9 @@ private:
#if AP_COMPASS_QMC5883P_ENABLED
DRIVER_QMC5883P =20,
#endif
+#if AP_COMPASS_BMM350_ENABLED
+ DRIVER_BMM350 =21,
+#endif
};
bool _driver_enabled(enum DriverType driver_type);
diff --git a/libraries/AP_Compass/AP_Compass_BMM350.cpp b/libraries/AP_Compass/AP_Compass_BMM350.cpp
new file mode 100644
index 0000000000..d5f83419b8
--- /dev/null
+++ b/libraries/AP_Compass/AP_Compass_BMM350.cpp
@@ -0,0 +1,492 @@
+/*
+ * This file 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 file 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 .
+ */
+#include "AP_Compass_BMM350.h"
+
+#if AP_COMPASS_BMM350_ENABLED
+
+#include
+
+#include
+
+#include
+#include
+
+#define BMM350_REG_CHIP_ID 0x00
+#define BMM350_REG_PMU_CMD_AGGR_SET 0x04
+#define BMM350_REG_PMU_CMD_AXIS_EN 0x05
+#define BMM350_REG_PMU_CMD 0x06
+#define BMM350_REG_PMU_CMD_STATUS_0 0x07
+#define BMM350_REG_INT_CTRL 0x2E
+#define BMM350_REG_MAG_X_XLSB 0x31
+#define BMM350_REG_OTP_CMD 0x50
+#define BMM350_REG_OTP_DATA_MSB 0x52
+#define BMM350_REG_OTP_DATA_LSB 0x53
+#define BMM350_REG_OTP_STATUS 0x55
+#define BMM350_REG_CMD 0x7E
+
+// OTP(one-time programmable memory)
+#define BMM350_OTP_CMD_DIR_READ (0x01<<5U)
+#define BMM350_OTP_CMD_PWR_OFF_OTP (0x04<<5U)
+
+#define BMM350_OTP_STATUS_ERROR_MASK 0xE0
+#define BMM350_OTP_STATUS_CMD_DONE 0x01
+
+#define BMM350_TEMP_OFF_SENS 0x0D
+#define BMM350_MAG_OFFSET_X 0x0E
+#define BMM350_MAG_OFFSET_Y 0x0F
+#define BMM350_MAG_OFFSET_Z 0x10
+#define BMM350_MAG_SENS_X 0x10
+#define BMM350_MAG_SENS_Y 0x11
+#define BMM350_MAG_SENS_Z 0x11
+#define BMM350_MAG_TCO_X 0x12
+#define BMM350_MAG_TCO_Y 0x13
+#define BMM350_MAG_TCO_Z 0x14
+#define BMM350_MAG_TCS_X 0x12
+#define BMM350_MAG_TCS_Y 0x13
+#define BMM350_MAG_TCS_Z 0x14
+#define BMM350_MAG_DUT_T_0 0x18
+#define BMM350_CROSS_X_Y 0x15
+#define BMM350_CROSS_Y_X 0x15
+#define BMM350_CROSS_Z_X 0x16
+#define BMM350_CROSS_Z_Y 0x16
+#define BMM350_SENS_CORR_Y 0.01f
+#define BMM350_TCS_CORR_Z 0.0001f
+
+#define BMM350_CMD_SOFTRESET 0xB6
+#define BMM350_INT_MODE_PULSED (0<<0U)
+#define BMM350_INT_POL_ACTIVE_HIGH (1<<1U)
+#define BMM350_INT_OD_PUSHPULL (1<<2U)
+#define BMM350_INT_OUTPUT_DISABLE (0<<3U)
+#define BMM350_INT_DRDY_EN (1<<7U)
+
+// Averaging performance
+#define BMM350_AVERAGING_4 (0x02 << 4U)
+#define BMM350_AVERAGING_8 (0x03 << 4U)
+
+// Output data rate
+#define BMM350_ODR_100HZ 0x04
+#define BMM350_ODR_50HZ 0x05
+
+// Power modes
+#define BMM350_PMU_CMD_SUSPEND_MODE 0x00
+#define BMM350_PMU_CMD_NORMAL_MODE 0x01
+#define BMM350_PMU_CMD_UPD_OAE 0x02
+#define BMM350_PMU_CMD_FGR 0x05
+#define BMM350_PMU_CMD_BR 0x07
+
+// OTP data length
+#define BMM350_OTP_DATA_LENGTH 32U
+
+// Chip ID of BMM350
+#define BMM350_CHIP_ID 0x33
+
+#define BMM350_XY_SENSITIVE 14.55f
+#define BMM350_Z_SENSITIVE 9.0f
+#define BMM350_TEMP_SENSITIVE 0.00204f
+#define BMM350_XY_INA_GAIN 19.46f
+#define BMM350_Z_INA_GAIN 31.0f
+#define BMM350_ADC_GAIN (1.0f / 1.5f)
+#define BMM350_LUT_GAIN 0.714607238769531f
+#define BMM350_POWER ((float)(1000000.0 / 1048576.0))
+
+#define BMM350_XY_SCALE (BMM350_POWER / (BMM350_XY_SENSITIVE * BMM350_XY_INA_GAIN * BMM350_ADC_GAIN * BMM350_LUT_GAIN))
+#define BMM350_Z_SCALE (BMM350_POWER / (BMM350_Z_SENSITIVE * BMM350_Z_INA_GAIN * BMM350_ADC_GAIN * BMM350_LUT_GAIN))
+#define BMM350_TEMP_SCALE (1.0f / (BMM350_TEMP_SENSITIVE * BMM350_ADC_GAIN * BMM350_LUT_GAIN * 1048576))
+
+extern const AP_HAL::HAL &hal;
+
+AP_Compass_Backend *AP_Compass_BMM350::probe(AP_HAL::OwnPtr dev,
+ bool force_external,
+ enum Rotation rotation)
+{
+ if (!dev) {
+ return nullptr;
+ }
+ AP_Compass_BMM350 *sensor = NEW_NOTHROW AP_Compass_BMM350(std::move(dev), force_external, rotation);
+ if (!sensor || !sensor->init()) {
+ delete sensor;
+ return nullptr;
+ }
+
+ return sensor;
+}
+
+AP_Compass_BMM350::AP_Compass_BMM350(AP_HAL::OwnPtr dev,
+ bool force_external,
+ enum Rotation rotation)
+ : _dev(std::move(dev))
+ , _force_external(force_external)
+ , _rotation(rotation)
+{
+}
+
+/**
+ * @brief Read out OTP(one-time programmable memory) data of sensor which is the compensation coefficients
+ * @see https://github.com/boschsensortec/BMM350-SensorAPI
+ */
+bool AP_Compass_BMM350::read_otp_data()
+{
+ uint16_t otp_data[BMM350_OTP_DATA_LENGTH];
+
+ for (uint8_t index = 0; index < BMM350_OTP_DATA_LENGTH; index++) {
+ // Set OTP address
+ if (!_dev->write_register(BMM350_REG_OTP_CMD, (BMM350_OTP_CMD_DIR_READ | index))) {
+ return false;
+ }
+
+ // Wait for OTP status be ready
+ int8_t tries = 3; // Try polling 3 times
+ while (tries--)
+ {
+ uint8_t status;
+ hal.scheduler->delay_microseconds(300);
+ // Read OTP status
+ if (!read_bytes(BMM350_REG_OTP_STATUS, &status, 1) || ((status & BMM350_OTP_STATUS_ERROR_MASK) != 0)) {
+ return false;
+ }
+ if (status & BMM350_OTP_STATUS_CMD_DONE) {
+ break;
+ }
+ }
+
+ if (tries == -1) {
+ return false;
+ }
+
+ // Read OTP data
+ be16_t reg_data;
+ if (!read_bytes(BMM350_REG_OTP_DATA_MSB, (uint8_t *)®_data, 2)) {
+ return false;
+ }
+
+ otp_data[index] = be16toh(reg_data);
+ }
+
+ // Update magnetometer offset and sensitivity data.
+ // 12-bit unsigned integer to be left-aligned in a 16-bit integer
+ _mag_comp.offset_coef.x = float(int16_t((otp_data[BMM350_MAG_OFFSET_X] & 0x0FFF) << 4) >> 4);
+ _mag_comp.offset_coef.y = float(int16_t((((otp_data[BMM350_MAG_OFFSET_X] & 0xF000) >> 4) +
+ (otp_data[BMM350_MAG_OFFSET_Y] & 0x00FF)) << 4) >> 4);
+ _mag_comp.offset_coef.z = float(int16_t(((otp_data[BMM350_MAG_OFFSET_Y] & 0x0F00) +
+ (otp_data[BMM350_MAG_OFFSET_Z] & 0x00FF)) << 4) >> 4);
+ _mag_comp.offset_coef.temp = float(int8_t(otp_data[BMM350_TEMP_OFF_SENS])) * (1.0f / 5.0f);
+
+ _mag_comp.sensit_coef.x = float(int8_t((otp_data[BMM350_MAG_SENS_X] & 0xFF00) >> 8)) * (1.0f / 256.0f);
+ _mag_comp.sensit_coef.y = float(int8_t(otp_data[BMM350_MAG_SENS_Y])) * (1.0f / 256.0f) + BMM350_SENS_CORR_Y;
+ _mag_comp.sensit_coef.z = float(int8_t((otp_data[BMM350_MAG_SENS_Z] & 0xFF00) >> 8)) * (1.0f / 256.0f);
+ _mag_comp.sensit_coef.temp = float(int8_t((otp_data[BMM350_TEMP_OFF_SENS] & 0xFF00) >> 8)) * (1.0f / 512.0f);
+
+ _mag_comp.tco.x = float(int8_t(otp_data[BMM350_MAG_TCO_X])) * (1.0f / 32.0f);
+ _mag_comp.tco.y = float(int8_t(otp_data[BMM350_MAG_TCO_Y])) * (1.0f / 32.0f);
+ _mag_comp.tco.z = float(int8_t(otp_data[BMM350_MAG_TCO_Z])) * (1.0f / 32.0f);
+
+ _mag_comp.tcs.x = float(int8_t((otp_data[BMM350_MAG_TCS_X] & 0xFF00) >> 8)) * (1.0f / 16384.0f);
+ _mag_comp.tcs.y = float(int8_t((otp_data[BMM350_MAG_TCS_Y] & 0xFF00) >> 8)) * (1.0f / 16384.0f);
+ _mag_comp.tcs.z = float(int8_t((otp_data[BMM350_MAG_TCS_Z] & 0xFF00) >> 8)) * (1.0f / 16384.0f) - BMM350_TCS_CORR_Z;
+
+ _mag_comp.t0_reading = float(int16_t(otp_data[BMM350_MAG_DUT_T_0])) * (1.0f / 512.0f) + 23.0f;
+
+ _mag_comp.cross_axis.cross_x_y = float(int8_t(otp_data[BMM350_CROSS_X_Y])) * (1.0f / 800.0f);
+ _mag_comp.cross_axis.cross_y_x = float(int8_t((otp_data[BMM350_CROSS_Y_X] & 0xFF00) >> 8)) * (1.0f / 800.0f);
+ _mag_comp.cross_axis.cross_z_x = float(int8_t(otp_data[BMM350_CROSS_Z_X])) * (1.0f / 800.0f);
+ _mag_comp.cross_axis.cross_z_y = float(int8_t((otp_data[BMM350_CROSS_Z_Y] & 0xFF00) >> 8)) * (1.0f / 800.0f);
+
+ return true;
+}
+
+/**
+ * @brief Wait PMU_CMD register operation completed. Need to specify which command just sent
+ */
+bool AP_Compass_BMM350::wait_pmu_cmd_ready(const uint8_t cmd, const uint32_t timeout)
+{
+ const uint32_t start_tick = AP_HAL::millis();
+
+ do {
+ hal.scheduler->delay(1);
+ uint8_t status;
+ if (!read_bytes(BMM350_REG_PMU_CMD_STATUS_0, &status, 1)) {
+ return false;
+ }
+ if (((status & 0x01) == 0x00) && (((status & 0xE0) >> 5) == cmd)) {
+ return true;
+ }
+ } while ((AP_HAL::millis() - start_tick) < timeout);
+
+ return false;
+}
+
+/**
+ * @brief Reset bit of magnetic register and wait for change to normal mode
+ */
+bool AP_Compass_BMM350::mag_reset_and_wait()
+{
+ uint8_t data;
+
+ // Get PMU command status 0 data
+ if (!read_bytes(BMM350_REG_PMU_CMD_STATUS_0, &data, 1)) {
+ return false;
+ }
+
+ // Check whether the power mode is Normal
+ if (data & 0x08) {
+ // Set PMU command to suspend mode
+ if (!_dev->write_register(BMM350_REG_PMU_CMD, BMM350_PMU_CMD_SUSPEND_MODE)) {
+ return false;
+ }
+ wait_pmu_cmd_ready(BMM350_PMU_CMD_SUSPEND_MODE, 6);
+ }
+
+ // Set BR(bit reset) to PMU_CMD register
+ // In offical example, it wait for 14ms. But it may not be enough, so we wait an extra 5ms
+ if (!_dev->write_register(BMM350_REG_PMU_CMD, BMM350_PMU_CMD_BR) ||
+ !wait_pmu_cmd_ready(BMM350_PMU_CMD_BR, 19)) {
+ return false;
+ }
+
+ // Set FGR(flux-guide reset) to PMU_CMD register
+ // 18ms got from offical example, we wait an extra 5ms
+ if (!_dev->write_register(BMM350_REG_PMU_CMD, BMM350_PMU_CMD_FGR) ||
+ !wait_pmu_cmd_ready(BMM350_PMU_CMD_FGR, 23)) {
+ return false;
+ }
+
+ // Switch to normal mode
+ if (!set_power_mode(POWER_MODE_NORMAL)) {
+ return false;
+ }
+
+ return true;
+}
+
+/**
+ * @brief Switch sensor power mode
+ */
+bool AP_Compass_BMM350::set_power_mode(const enum power_mode mode)
+{
+ uint8_t pmu_cmd;
+
+ // Get PMU register data as current mode
+ if (!read_bytes(BMM350_REG_PMU_CMD, &pmu_cmd, 1)) {
+ return false;
+ }
+
+ if (pmu_cmd == BMM350_PMU_CMD_NORMAL_MODE || pmu_cmd == BMM350_PMU_CMD_UPD_OAE) {
+ // Set PMU command to suspend mode
+ if (!_dev->write_register(BMM350_REG_PMU_CMD, POWER_MODE_SUSPEND)) {
+ return false;
+ }
+ // Wait for sensor switch to suspend mode
+ // Wait for maximum 6ms, it got from the official example, not explained in datasheet
+ wait_pmu_cmd_ready(POWER_MODE_SUSPEND, 6);
+ }
+
+ // Set PMU command to target mode
+ if (!_dev->write_register(BMM350_REG_PMU_CMD, mode)) {
+ return false;
+ }
+
+ // Wait for mode change. When switch from suspend mode to normal mode, we wait for at most 38ms.
+ // It got from official example too
+ wait_pmu_cmd_ready(mode, 38);
+
+ return true;
+}
+
+/**
+ * @brief Read bytes from sensor
+ */
+bool AP_Compass_BMM350::read_bytes(const uint8_t reg, uint8_t *out, const uint16_t read_len)
+{
+ uint8_t data[read_len + 2];
+
+ if (!_dev->read_registers(reg, data, read_len + 2)) {
+ return false;
+ }
+
+ memcpy(out, &data[2], read_len);
+
+ return true;
+}
+
+bool AP_Compass_BMM350::init()
+{
+ _dev->get_semaphore()->take_blocking();
+
+ // 10 retries for init
+ _dev->set_retries(10);
+
+ // Use checked registers to cope with bus errors
+ _dev->setup_checked_registers(4);
+
+ int8_t boot_retries = 5;
+ while (boot_retries--) {
+ // Soft reset
+ if (!_dev->write_register(BMM350_REG_CMD, BMM350_CMD_SOFTRESET)) {
+ continue;
+ }
+ hal.scheduler->delay(24); // Wait 24ms for soft reset complete, it got from offical example
+
+ // Read and verify chip ID
+ uint8_t chip_id;
+ if (!read_bytes(BMM350_REG_CHIP_ID, &chip_id, 1)) {
+ continue;
+ }
+ if (chip_id == BMM350_CHIP_ID) {
+ break;
+ }
+ }
+
+ if (boot_retries == -1) {
+ goto err;
+ }
+
+ // Read out OTP data
+ if (!read_otp_data()) {
+ goto err;
+ }
+
+ // Power off OTP
+ if (!_dev->write_register(BMM350_REG_OTP_CMD, BMM350_OTP_CMD_PWR_OFF_OTP)) {
+ goto err;
+ }
+
+ // Magnetic reset
+ if (!mag_reset_and_wait()) {
+ goto err;
+ }
+
+ // Configure interrupt settings and enable DRDY
+ // Set INT mode as PULSED | active_high polarity | PUSH_PULL | unmap | DRDY interrupt
+ if (!_dev->write_register(BMM350_REG_INT_CTRL, (BMM350_INT_MODE_PULSED |
+ BMM350_INT_POL_ACTIVE_HIGH |
+ BMM350_INT_OD_PUSHPULL |
+ BMM350_INT_OUTPUT_DISABLE |
+ BMM350_INT_DRDY_EN))) {
+ goto err;
+ }
+
+ // Setup ODR and performance. 100Hz ODR and 4 average for lownoise
+ if (!_dev->write_register(BMM350_REG_PMU_CMD_AGGR_SET, (BMM350_AVERAGING_4 | BMM350_ODR_100HZ))) {
+ goto err;
+ }
+
+ // Update ODR and avg parameter
+ if (!_dev->write_register(BMM350_REG_PMU_CMD, BMM350_PMU_CMD_UPD_OAE)) {
+ goto err;
+ }
+ // Wait at most 20ms for update ODR and avg paramter
+ wait_pmu_cmd_ready(BMM350_PMU_CMD_UPD_OAE, 20);
+
+ // Enable measurement of 3 axis
+ if (!_dev->write_register(BMM350_REG_PMU_CMD_AXIS_EN, 0x07)) {
+ goto err;
+ }
+
+ // Switch power mode to normal mode
+ if (!set_power_mode(POWER_MODE_NORMAL)) {
+ goto err;
+ }
+
+ // Lower retries for run
+ _dev->set_retries(3);
+
+ _dev->get_semaphore()->give();
+
+ /* Register the compass instance in the frontend */
+ _dev->set_device_type(DEVTYPE_BMM350);
+ if (!register_compass(_dev->get_bus_id(), _compass_instance)) {
+ return false;
+ }
+ set_dev_id(_compass_instance, _dev->get_bus_id());
+
+ // printf("BMM350: Found at address 0x%x as compass %u\n", _dev->get_bus_address(), _compass_instance);
+
+ set_rotation(_compass_instance, _rotation);
+
+ if (_force_external) {
+ set_external(_compass_instance, true);
+ }
+
+ // Call timer() at 100Hz
+ _dev->register_periodic_callback(1000000U/100U, FUNCTOR_BIND_MEMBER(&AP_Compass_BMM350::timer, void));
+
+ return true;
+
+err:
+ _dev->get_semaphore()->give();
+ return false;
+}
+
+void AP_Compass_BMM350::timer()
+{
+ struct PACKED {
+ uint8_t magx[3];
+ uint8_t magy[3];
+ uint8_t magz[3];
+ uint8_t temp[3];
+ } data;
+
+ // Read out measurement data
+ if (!read_bytes(BMM350_REG_MAG_X_XLSB, (uint8_t *)&data, sizeof(data))) {
+ return;
+ }
+
+ // Converts 24-bit raw data to signed integer value
+ const int32_t magx_raw = int32_t(((uint32_t)data.magx[0] << 8) + ((uint32_t)data.magx[1] << 16) + ((uint32_t)data.magx[2] << 24)) >> 8;
+ const int32_t magy_raw = int32_t(((uint32_t)data.magy[0] << 8) + ((uint32_t)data.magy[1] << 16) + ((uint32_t)data.magy[2] << 24)) >> 8;
+ const int32_t magz_raw = int32_t(((uint32_t)data.magz[0] << 8) + ((uint32_t)data.magz[1] << 16) + ((uint32_t)data.magz[2] << 24)) >> 8;
+ const int32_t temp_raw = int32_t(((uint32_t)data.temp[0] << 8) + ((uint32_t)data.temp[1] << 16) + ((uint32_t)data.temp[2] << 24)) >> 8;
+
+ // Convert mag lsb to uT and temp lsb to degC
+ float magx = (float)magx_raw * BMM350_XY_SCALE;
+ float magy = (float)magy_raw * BMM350_XY_SCALE;
+ float magz = (float)magz_raw * BMM350_Z_SCALE;
+ float temp = (float)temp_raw * BMM350_TEMP_SCALE;
+
+ if (temp > 0.0f) {
+ temp -= 25.49f;
+ } else if (temp < 0.0f) {
+ temp += 25.49f;
+ }
+
+ // Apply compensation
+ temp = ((1 + _mag_comp.sensit_coef.temp) * temp) + _mag_comp.offset_coef.temp;
+ // Compensate raw magnetic data
+ magx = ((1 + _mag_comp.sensit_coef.x) * magx) + _mag_comp.offset_coef.x + (_mag_comp.tco.x * (temp - _mag_comp.t0_reading));
+ magx /= 1 + _mag_comp.tcs.x * (temp - _mag_comp.t0_reading);
+
+ magy = ((1 + _mag_comp.sensit_coef.y) * magy) + _mag_comp.offset_coef.y + (_mag_comp.tco.y * (temp - _mag_comp.t0_reading));
+ magy /= 1 + _mag_comp.tcs.y * (temp - _mag_comp.t0_reading);
+
+ magz = ((1 + _mag_comp.sensit_coef.z) * magz) + _mag_comp.offset_coef.z + (_mag_comp.tco.z * (temp - _mag_comp.t0_reading));
+ magz /= 1 + _mag_comp.tcs.z * (temp - _mag_comp.t0_reading);
+
+ const float cr_ax_comp_x = (magx - _mag_comp.cross_axis.cross_x_y * magy) / (1 - _mag_comp.cross_axis.cross_y_x * _mag_comp.cross_axis.cross_x_y);
+ const float cr_ax_comp_y = (magy - _mag_comp.cross_axis.cross_y_x * magx) / (1 - _mag_comp.cross_axis.cross_y_x * _mag_comp.cross_axis.cross_x_y);
+ const float cr_ax_comp_z = (magz + (magx * (_mag_comp.cross_axis.cross_y_x * _mag_comp.cross_axis.cross_z_y - _mag_comp.cross_axis.cross_z_x) -
+ magy * (_mag_comp.cross_axis.cross_z_y - _mag_comp.cross_axis.cross_x_y * _mag_comp.cross_axis.cross_z_x)) /
+ (1 - _mag_comp.cross_axis.cross_y_x * _mag_comp.cross_axis.cross_x_y));
+
+ // Store in field vector and convert uT to milligauss
+ Vector3f field { cr_ax_comp_x * 10.0f, cr_ax_comp_y * 10.0f, cr_ax_comp_z * 10.0f };
+ accumulate_sample(field, _compass_instance);
+}
+
+void AP_Compass_BMM350::read()
+{
+ drain_accumulated_samples(_compass_instance);
+}
+
+#endif // AP_COMPASS_BMM350_ENABLED
diff --git a/libraries/AP_Compass/AP_Compass_BMM350.h b/libraries/AP_Compass/AP_Compass_BMM350.h
new file mode 100644
index 0000000000..4cf5fdf555
--- /dev/null
+++ b/libraries/AP_Compass/AP_Compass_BMM350.h
@@ -0,0 +1,112 @@
+/*
+ * This file 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 file 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 .
+ */
+
+#pragma once
+
+#include "AP_Compass_config.h"
+
+#if AP_COMPASS_BMM350_ENABLED
+
+#include
+#include
+#include
+#include
+
+#include "AP_Compass.h"
+#include "AP_Compass_Backend.h"
+
+#define BMM350_I2C_ADDR_MIN 0x14
+#define BMM350_I2C_ADDR_MAX 0x17
+
+
+class AP_Compass_BMM350 : public AP_Compass_Backend
+{
+public:
+ static AP_Compass_Backend *probe(AP_HAL::OwnPtr dev,
+ bool force_external,
+ enum Rotation rotation);
+
+ void read() override;
+
+ static constexpr const char *name = "BMM350";
+
+private:
+ AP_Compass_BMM350(AP_HAL::OwnPtr dev,
+ bool force_external,
+ enum Rotation rotation);
+
+ AP_HAL::OwnPtr _dev;
+
+ /**
+ * @brief BMM350 offset/sensitivity coefficient structure
+ */
+ struct vector4f
+ {
+ float x; // x axis
+ float y; // y axis
+ float z; // z axis
+ float temp; // Temperature
+ };
+
+ /**
+ * @brief BMM350 magnetometer cross axis compensation structure
+ */
+ struct cross_axis
+ {
+ float cross_x_y;
+ float cross_y_x;
+ float cross_z_x;
+ float cross_z_y;
+ };
+
+ /**
+ * @brief BMM350 magnetometer compensate structure
+ */
+ struct mag_compensate
+ {
+ struct vector4f offset_coef; // Offset coefficient
+ struct vector4f sensit_coef; // Sensitivity coefficient
+ Vector3f tco; // Temperature coefficient of the offset
+ Vector3f tcs; // Temperature coefficient of the sensitivity
+ float t0_reading; // Initialize T0_reading parameter
+ struct cross_axis cross_axis; // Cross axis compensation
+ };
+
+ enum power_mode
+ {
+ POWER_MODE_SUSPEND = 0,
+ POWER_MODE_NORMAL = 1,
+ POWER_MODE_FORCED = 3,
+ POWER_MODE_FORCED_FAST = 4
+ };
+
+ /**
+ * Device periodic callback to read data from the sensor.
+ */
+ bool init();
+ void timer();
+ bool read_otp_data();
+ bool wait_pmu_cmd_ready(const uint8_t cmd, const uint32_t timeout);
+ bool mag_reset_and_wait();
+ bool set_power_mode(const enum power_mode mode);
+ bool read_bytes(const uint8_t reg, uint8_t *out, const uint16_t read_len);
+
+ uint8_t _compass_instance;
+ bool _force_external;
+ enum Rotation _rotation;
+ struct mag_compensate _mag_comp; // Structure for mag compensate
+};
+
+#endif // AP_COMPASS_BMM350_ENABLED
diff --git a/libraries/AP_Compass/AP_Compass_Backend.h b/libraries/AP_Compass/AP_Compass_Backend.h
index 79bfe1f66f..68c0959ccc 100644
--- a/libraries/AP_Compass/AP_Compass_Backend.h
+++ b/libraries/AP_Compass/AP_Compass_Backend.h
@@ -74,6 +74,7 @@ public:
DEVTYPE_AK09918 = 0x14,
DEVTYPE_AK09915 = 0x15,
DEVTYPE_QMC5883P = 0x16,
+ DEVTYPE_BMM350 = 0x17,
};
#if AP_COMPASS_MSP_ENABLED
diff --git a/libraries/AP_Compass/AP_Compass_config.h b/libraries/AP_Compass/AP_Compass_config.h
index c6c236db17..9d135be9dd 100644
--- a/libraries/AP_Compass/AP_Compass_config.h
+++ b/libraries/AP_Compass/AP_Compass_config.h
@@ -76,6 +76,10 @@
#define AP_COMPASS_BMM150_DETECT_BACKENDS_ENABLED AP_COMPASS_BMM150_ENABLED
#endif
+#ifndef AP_COMPASS_BMM350_ENABLED
+#define AP_COMPASS_BMM350_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
+#endif
+
#ifndef AP_COMPASS_HMC5843_ENABLED
#define AP_COMPASS_HMC5843_ENABLED AP_COMPASS_I2C_BACKEND_DEFAULT_ENABLED
#endif