mirror of https://github.com/ArduPilot/ardupilot
AP_Compass: Add in BMM350 Driver
This commit is contained in:
cuav-chen2
Peter Barker
parent
8f4f20dc32
commit
89a4571c58
|
|
@ -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
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
#include "AP_Compass_BMM350.h"
|
||||
|
||||
#if AP_COMPASS_BMM350_ENABLED
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include <utility>
|
||||
|
||||
#include <AP_HAL/utility/sparse-endian.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#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<AP_HAL::Device> 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<AP_HAL::Device> 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
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "AP_Compass_config.h"
|
||||
|
||||
#if AP_COMPASS_BMM350_ENABLED
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
|
||||
#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<AP_HAL::Device> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation);
|
||||
|
||||
void read() override;
|
||||
|
||||
static constexpr const char *name = "BMM350";
|
||||
|
||||
private:
|
||||
AP_Compass_BMM350(AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
bool force_external,
|
||||
enum Rotation rotation);
|
||||
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _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
|
||||
|
|
@ -74,6 +74,7 @@ public:
|
|||
DEVTYPE_AK09918 = 0x14,
|
||||
DEVTYPE_AK09915 = 0x15,
|
||||
DEVTYPE_QMC5883P = 0x16,
|
||||
DEVTYPE_BMM350 = 0x17,
|
||||
};
|
||||
|
||||
#if AP_COMPASS_MSP_ENABLED
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Reference in New Issue