fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Add Bosch BMM350 magnetometer (#23362)

Browse Source 
* Add Bosch BMM350 magnetometer

* BMM350 replace info messages with debug messages

* BMM350 update measurement interval

* BMM350 fix offsets, update based on review

* BMM350 Update default parameters to 50Hz

* Update OTP Word LSB check

* BMM350 fix styles and formatting

* BMM350 update error checks
This commit is contained in:
Vilius 2024-08-15 12:29:02 +03:00 committed by GitHub
parent af0129dab7
commit 2a124fd998
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
9 changed files with 1331 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/drv_sensor.h
View File

@ -242,6 +242,8 @@
#define DRV_INS_DEVTYPE_VN300 0xE3
#define DRV_DIFF_PRESS_DEVTYPE_ASP5033 0xE4
#define DRV_MAG_DEVTYPE_BMM350 0xE5
#define DRV_DEVTYPE_UNUSED 0xff
#endif /* _DRV_SENSOR_H */

1
src/drivers/magnetometer/bosch/CMakeLists.txt
View File

@ -32,3 +32,4 @@
############################################################################
add_subdirectory(bmm150)
add_subdirectory(bmm350)

773
src/drivers/magnetometer/bosch/bmm350/BMM350.cpp Executable file
View File

@ -0,0 +1,773 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMM350.hpp"
using namespace time_literals;
BMM350::BMM350(const I2CSPIDriverConfig &config) :
I2C(config),
I2CSPIDriver(config),
ModuleParams(nullptr),
_px4_mag(get_device_id(), config.rotation)
{
}
BMM350::~BMM350()
{
perf_free(_reset_perf);
perf_free(_bad_read_perf);
perf_free(_self_test_failed_perf);
}
int BMM350::init()
{
ModuleParams::updateParams();
ParametersUpdate(true);
int ret = I2C::init();
if (ret != PX4_OK) {
DEVICE_DEBUG("I2C::init failed (%i)", ret);
return ret;
}
return Reset() ? 0 : -1;
}
bool BMM350::Reset()
{
RegisterWrite(Register::CMD, SOFT_RESET);
_state = STATE::RESET;
ScheduleClear();
ScheduleDelayed(1_ms);
return true;
}
void BMM350::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_reset_perf);
perf_print_counter(_bad_read_perf);
perf_print_counter(_self_test_failed_perf);
}
void BMM350::ParametersUpdate(bool force)
{
if (_parameter_update_sub.updated() || force) {
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams();
UpdateMagParams();
}
}
void BMM350::UpdateMagParams()
{
uint8_t odr = GetODR(_param_bmm350_odr.get());
uint8_t avg = GetAVG(_param_bmm350_avg.get());
_mag_odr_mode = odr;
_mag_avg_mode = avg;
_mag_pad_drive = static_cast<uint8_t>(_param_bmm350_drive.get());
PX4_DEBUG("Set params odr = %d, avg = %d, drive = %d", _mag_odr_mode, _mag_avg_mode, _mag_pad_drive);
}
uint8_t BMM350::GetODR(int value)
{
switch (value) {
case 0: return ODR_400HZ;
case 1: return ODR_200HZ;
case 2: return ODR_100HZ;
case 3: return ODR_50HZ;
case 4: return ODR_25HZ;
case 5: return ODR_12_5HZ;
case 6: return ODR_6_25HZ;
case 7: return ODR_3_125HZ;
case 8: return ODR_1_5625HZ;
default: return ODR_200HZ;
}
}
hrt_abstime BMM350::OdrToUs(uint8_t odr)
{
switch (odr) {
case ODR_400HZ:
return 2500_us;
case ODR_200HZ:
return 5000_us;
case ODR_100HZ:
return 10000_us;
case ODR_50HZ:
return 20000_us;
case ODR_25HZ:
return 40000_us;
case ODR_12_5HZ:
return 80000_us;
case ODR_6_25HZ:
return 160000_us;
case ODR_3_125HZ:
return 320000_us;
case ODR_1_5625HZ:
return 640000_us;
default:
return 5000_us;
}
}
uint8_t BMM350::GetAVG(int value)
{
switch (value) {
case 0: return AVG_NO_AVG;
case 1: return AVG_2;
case 2: return AVG_4;
case 3: return AVG_8;
default: return AVG_2;
}
}
int BMM350::probe()
{
for (int i = 0; i < 3; i++) {
uint8_t chip_id;
if (PX4_OK == RegisterRead(Register::CHIP_ID, &chip_id)) {
PX4_DEBUG("CHIP_ID: 0x%02hhX", chip_id);
if (chip_id == chip_identification_number) {
return PX4_OK;
}
}
}
return PX4_ERROR;
}
void BMM350::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
int ret = PX4_OK;
ParametersUpdate();
switch (_state) {
case STATE::RESET: {
if (RegisterWrite(Register::CMD, SOFT_RESET) == PX4_OK) {
_reset_timestamp = now;
_state = STATE::WAIT_FOR_RESET;
perf_count(_reset_perf);
}
ScheduleDelayed(10_ms);
}
break;
case STATE::WAIT_FOR_RESET: {
ret = probe();
if (ret == PX4_OK) {
_state = STATE::RESET;
uint8_t status_0;
ret = RegisterRead(Register::PMU_STATUS_0, &status_0);
if (ret == PX4_OK && (status_0 & PWR_NORMAL) != 0) {
ret = PX4_ERROR;
}
if (ret == PX4_OK) {
ret = UpdateMagOffsets();
}
if (ret == PX4_OK) {
PX4_DEBUG("Going to FGR");
_state = STATE::FGR;
}
ScheduleDelayed(10_ms);
} else {
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 10 ms");
ScheduleDelayed(10_ms);
}
}
}
break;
case STATE::FGR: {
_state = STATE::RESET;
uint8_t odr_reg_data = (ODR_100HZ & 0xf);
odr_reg_data = ((odr_reg_data & ~(0x30)) | ((AVG_2 << 0x4) & 0x30));
ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data);
if (ret == PX4_OK) {
ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, EN_XYZ);
}
if (ret == PX4_OK) {
ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FGR);
}
if (ret == PX4_OK) {
PX4_DEBUG("Going to BR");
_state = STATE::BR;
}
ScheduleDelayed(30_ms);
}
break;
case STATE::BR: {
uint8_t status_0;
_state = STATE::RESET;
ret = RegisterRead(Register::PMU_STATUS_0, &status_0);
if (ret == PX4_OK && PMU_CMD_STATUS_0_RES(status_0) == PMU_STATUS_FGR) {
ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_BR_FAST);
if (ret == PX4_OK) {
PX4_DEBUG("Going to after reset");
_state = STATE::AFTER_RESET;
}
}
ScheduleDelayed(4_ms);
}
break;
case STATE::AFTER_RESET: {
uint8_t status_0;
_state = STATE::RESET;
ret = RegisterRead(Register::PMU_STATUS_0, &status_0);
if (ret == PX4_OK && PMU_CMD_STATUS_0_RES(status_0) == PMU_STATUS_BR_FAST) {
_state = STATE::MEASURE_FORCED;
_self_test_state = SELF_TEST_STATE::INIT;
PX4_DEBUG("Going to fast FM");
}
ScheduleNow();
}
break;
case STATE::MEASURE_FORCED: {
ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM);
if (ret == PX4_OK) {
_state = STATE::SELF_TEST_CHECK;
} else {
_state = STATE::RESET;
}
ScheduleDelayed(OdrToUs(_mag_odr_mode));
break;
}
case STATE::SELF_TEST_CHECK: {
float xyzt[4];
_state = STATE::RESET;
if (ReadOutRawData(xyzt) != PX4_OK) {
perf_count(_self_test_failed_perf);
ScheduleNow();
break;
}
switch (_self_test_state) {
case SELF_TEST_STATE::INIT:
memcpy(_initial_self_test_values, xyzt, sizeof(_initial_self_test_values));
if (RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_X) == PX4_OK) {
_self_test_state = SELF_TEST_STATE::POS_X;
_state = STATE::MEASURE_FORCED;
}
break;
case SELF_TEST_STATE::POS_X:
if (xyzt[0] - _initial_self_test_values[0] >= 130.0f &&
RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_X) == PX4_OK) {
_self_test_state = SELF_TEST_STATE::NEG_X;
_state = STATE::MEASURE_FORCED;
}
break;
case SELF_TEST_STATE::NEG_X:
if (xyzt[0] - _initial_self_test_values[0] <= -130.0f &&
RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_Y) == PX4_OK) {
_self_test_state = SELF_TEST_STATE::POS_Y;
_state = STATE::MEASURE_FORCED;
}
break;
case SELF_TEST_STATE::POS_Y:
if (xyzt[1] - _initial_self_test_values[1] >= 130.0f &&
RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_Y) == PX4_OK) {
_self_test_state = SELF_TEST_STATE::NEG_Y;
_state = STATE::MEASURE_FORCED;
}
break;
case SELF_TEST_STATE::NEG_Y:
if (xyzt[1] - _initial_self_test_values[1] <= -130.0f &&
RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_DISABLE) == PX4_OK) {
PX4_DEBUG("Self test good, going to configure");
_state = STATE::CONFIGURE;
}
break;
}
if (_state == STATE::RESET) {
PX4_DEBUG("Self test failed");
perf_count(_self_test_failed_perf);
}
ScheduleDelayed(1_ms);
}
break;
case STATE::CONFIGURE:
if (Configure() == PX4_OK) {
_state = STATE::READ;
PX4_DEBUG("Configure went fine");
ScheduleOnInterval(OdrToUs(_mag_odr_mode), 50_ms);
} else {
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(100_ms);
}
break;
case STATE::READ: {
// -- start get_compensated_mag_xyz_temp_data
float out_data[4] = {0.0f};
float dut_offset_coeff[3], dut_sensit_coeff[3], dut_tcos[3], dut_tcss[3];
float cr_ax_comp_x, cr_ax_comp_y, cr_ax_comp_z;
ret = ReadOutRawData(out_data);
if (ret == PX4_OK) {
// Apply compensation to temperature reading
out_data[3] = (1 + _mag_comp_vals.dut_sensit_coef.t_sens) * out_data[3] +
_mag_comp_vals.dut_offset_coef.t_offs;
// Store magnetic compensation structure to an array
dut_offset_coeff[0] = _mag_comp_vals.dut_offset_coef.offset_x;
dut_offset_coeff[1] = _mag_comp_vals.dut_offset_coef.offset_y;
dut_offset_coeff[2] = _mag_comp_vals.dut_offset_coef.offset_z;
dut_sensit_coeff[0] = _mag_comp_vals.dut_sensit_coef.sens_x;
dut_sensit_coeff[1] = _mag_comp_vals.dut_sensit_coef.sens_y;
dut_sensit_coeff[2] = _mag_comp_vals.dut_sensit_coef.sens_z;
dut_tcos[0] = _mag_comp_vals.dut_tco.tco_x;
dut_tcos[1] = _mag_comp_vals.dut_tco.tco_y;
dut_tcos[2] = _mag_comp_vals.dut_tco.tco_z;
dut_tcss[0] = _mag_comp_vals.dut_tcs.tcs_x;
dut_tcss[1] = _mag_comp_vals.dut_tcs.tcs_y;
dut_tcss[2] = _mag_comp_vals.dut_tcs.tcs_z;
for (int idx = 0; idx < 3; idx++) {
out_data[idx] *= 1 + dut_sensit_coeff[idx];
out_data[idx] += dut_offset_coeff[idx];
out_data[idx] += dut_tcos[idx] * (out_data[3] - _mag_comp_vals.dut_t0);
out_data[idx] /= 1 + dut_tcss[idx] * (out_data[3] - _mag_comp_vals.dut_t0);
}
cr_ax_comp_x = (out_data[0] - _mag_comp_vals.cross_axis.cross_x_y * out_data[1]) /
(1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y);
cr_ax_comp_y = (out_data[1] - _mag_comp_vals.cross_axis.cross_y_x * out_data[0]) /
(1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y);
cr_ax_comp_z =
(out_data[2] +
(out_data[0] *
(_mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_z_y -
_mag_comp_vals.cross_axis.cross_z_x) -
out_data[1] *
(_mag_comp_vals.cross_axis.cross_z_y - _mag_comp_vals.cross_axis.cross_x_y *
_mag_comp_vals.cross_axis.cross_z_x)) /
(1 - _mag_comp_vals.cross_axis.cross_y_x * _mag_comp_vals.cross_axis.cross_x_y));
out_data[0] = cr_ax_comp_x;
out_data[1] = cr_ax_comp_y;
out_data[2] = cr_ax_comp_z;
_px4_mag.set_error_count(perf_event_count(_bad_read_perf) + perf_event_count(_self_test_failed_perf));
_px4_mag.set_temperature(out_data[3]);
_px4_mag.update(now, out_data[0], out_data[1], out_data[2]);
} else {
perf_count(_bad_read_perf);
}
}
break;
}
}
int BMM350::Configure()
{
PX4_DEBUG("Configuring");
uint8_t readData = 0;
int ret;
// Set pad drive
ret = RegisterWrite(Register::PAD_CTRL, (_mag_pad_drive & 0x7));
if (ret != PX4_OK) {
PX4_DEBUG("Couldn't set pad drive, defaults to 7");
return ret;
}
// Set PMU data aggregation
uint8_t odr = _mag_odr_mode;
uint8_t avg = _mag_avg_mode;
if (odr == ODR_400HZ && avg >= AVG_2) {
avg = AVG_NO_AVG;
} else if (odr == ODR_200HZ && avg >= AVG_4) {
avg = AVG_2;
} else if (odr == ODR_100HZ && avg >= AVG_8) {
avg = AVG_4;
}
uint8_t odr_reg_data = (odr & 0xf);
odr_reg_data = ((odr_reg_data & ~(0x30)) | ((avg << 0x4) & 0x30));
ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data);
if (ret != PX4_OK) {
PX4_DEBUG("Cannot set PMU AGG REG");
return ret;
}
ret = RegisterRead(Register::PMU_CMD_AGGR_SET, &readData);
if (ret != PX4_OK || readData != odr_reg_data) {
PX4_DEBUG("Couldn't check PMU AGGR REG");
return ret;
}
odr_reg_data = PMU_CMD_UPDATE_OAE;
ret = RegisterWrite(Register::PMU_CMD, odr_reg_data);
if (ret != PX4_OK) {
PX4_DEBUG("Couldn't write PMU CMD REG");
return ret;
}
ret = RegisterRead(Register::PMU_CMD, &readData);
if (ret != PX4_OK || readData != odr_reg_data) {
PX4_DEBUG("Couldn't check PMU CMD REG");
return ret;
}
// Enable AXIS
uint8_t axis_data = EN_X | EN_Y | EN_Z;
// PMU_CMD_AXIS_EN
ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, axis_data);
if (ret != PX4_OK) {
PX4_DEBUG("Couldn't write AXIS data");
return ret;
}
ret = RegisterRead(Register::PMU_CMD_AXIS_EN, &readData);
if (ret != PX4_OK || readData != axis_data) {
PX4_DEBUG("Couldn't cross check the set AXIS");
return ret;
}
ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_NM);
if (ret != PX4_OK) {
PX4_DEBUG("Failed to start mag in normal mode");
return ret;
}
// microTesla -> Gauss
_px4_mag.set_scale(0.01f);
return ret;
}
int32_t BMM350::FixSign(uint32_t inval, int8_t num_bits)
{
int32_t power = 1 << (num_bits - 1); // Calculate 2^(num_bits - 1)
int32_t retval = static_cast<int32_t>(inval);
if (retval >= power) {
retval -= (power << 1); // Equivalent to retval = retval - (power * 2)
}
return retval;
}
int BMM350::ReadOutRawData(float *out_data)
{
if (out_data == NULL) {
return -1;
}
float temp = 0.0;
struct BMM350::raw_mag_data raw_data = {0};
// --- Start read_uncomp_mag_temp_data
uint8_t mag_data[14] = {0};
uint32_t raw_mag_x, raw_mag_y, raw_mag_z, raw_temp;
uint8_t cmd = static_cast<uint8_t>(Register::DATAX_XLSB);
uint8_t res = transfer(&cmd, 1, (uint8_t *)&mag_data, sizeof(mag_data));
if (res != PX4_OK) {
return -1;
}
raw_mag_x = mag_data[2] + ((uint32_t)mag_data[3] << 8) + ((uint32_t)mag_data[4] << 16);
raw_mag_y = mag_data[5] + ((uint32_t)mag_data[6] << 8) + ((uint32_t)mag_data[7] << 16);
raw_mag_z = mag_data[8] + ((uint32_t)mag_data[9] << 8) + ((uint32_t)mag_data[10] << 16);
raw_temp = mag_data[11] + ((uint32_t)mag_data[12] << 8) + ((uint32_t)mag_data[13] << 16);
raw_data.raw_x = FixSign(raw_mag_x, 24);
raw_data.raw_y = FixSign(raw_mag_y, 24);
raw_data.raw_z = FixSign(raw_mag_z, 24);
raw_data.raw_t = FixSign(raw_temp, 24);
// --- End read_uncomp_mag_temp_data
// --- Start read_out_raw_data
out_data[0] = (float)raw_data.raw_x * lsb_to_utc_degc[0];
out_data[1] = (float)raw_data.raw_y * lsb_to_utc_degc[1];
out_data[2] = (float)raw_data.raw_z * lsb_to_utc_degc[2];
out_data[3] = (float)raw_data.raw_t * lsb_to_utc_degc[3];
// Adjust temperature
if (out_data[3] > 0.0f) {
temp = (float)(out_data[3] - (1 * 25.49f));
} else if (out_data[3] < 0.0f) {
temp = (float)(out_data[3] - (-1 * 25.49f));
} else {
temp = (float)(out_data[3]);
}
out_data[3] = temp;
return res;
}
int BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb)
{
uint8_t otp_cmd = OTP_DIR_READ | (addr & OTP_WORD_MSK);
int ret = RegisterWrite(Register::OTP_CMD, otp_cmd);
uint8_t otp_status = 0;
if (ret == PX4_OK) {
do {
ret = RegisterRead(Register::OTP_STATUS, &otp_status);
if (ret != PX4_OK) {
return PX4_ERROR;
}
} while (!(otp_status & 0x01));
uint8_t msb = 0, lsb = 0;
ret = RegisterRead(Register::OTP_DATA_MSB, &msb);
if (ret == PX4_OK) {
ret = RegisterRead(Register::OTP_DATA_LSB, &lsb);
if (ret == PX4_OK) {
*lsb_msb = ((msb << 8) | lsb) & 0xffff;
}
}
}
return ret;
}
int BMM350::UpdateMagOffsets()
{
PX4_DEBUG("DUMPING OTP");
uint16_t otp_data[32] = {0};
for (int idx = 0; idx < 32; idx++) {
if (ReadOTPWord(idx, &otp_data[idx]) != PX4_OK) {
return PX4_ERROR;
}
PX4_DEBUG("i: %i, val = %i", idx, otp_data[idx]);
}
if (RegisterWrite(Register::OTP_CMD, PWR_OFF_OTP) != PX4_OK) {
return PX4_ERROR;
}
PX4_DEBUG("var_id: %i", (otp_data[30] & 0x7f00) >> 9);
PX4_DEBUG("UPDATING OFFSETS");
uint16_t off_x_lsb_msb, off_y_lsb_msb, off_z_lsb_msb, t_off;
uint8_t sens_x, sens_y, sens_z, t_sens;
uint8_t tco_x, tco_y, tco_z;
uint8_t tcs_x, tcs_y, tcs_z;
uint8_t cross_x_y, cross_y_x, cross_z_x, cross_z_y;
off_x_lsb_msb = otp_data[0x0E] & 0x0FFF;
off_y_lsb_msb = ((otp_data[0x0E] & 0xF000) >> 4) +
(otp_data[0x0F] & 0x00FF);
off_z_lsb_msb = (otp_data[0x0F] & 0x0F00) +
(otp_data[0x10] & 0x00FF);
t_off = otp_data[0x0D] & 0x00FF;
_mag_comp_vals.dut_offset_coef.offset_x = FixSign(off_x_lsb_msb, 12);
_mag_comp_vals.dut_offset_coef.offset_y = FixSign(off_y_lsb_msb, 12);
_mag_comp_vals.dut_offset_coef.offset_z = FixSign(off_z_lsb_msb, 12);
_mag_comp_vals.dut_offset_coef.t_offs = FixSign(t_off, 8) / 5.0f;
sens_x = (otp_data[0x10] & 0xFF00) >> 8;
sens_y = (otp_data[0x11] & 0x00FF);
sens_z = (otp_data[0x11] & 0xFF00) >> 8;
t_sens = (otp_data[0x0D] & 0xFF00) >> 8;
_mag_comp_vals.dut_sensit_coef.sens_x = FixSign(sens_x, 8) / 256.0f;
_mag_comp_vals.dut_sensit_coef.sens_y = (FixSign(sens_y, 8) / 256.0f) + 0.01f;
_mag_comp_vals.dut_sensit_coef.sens_z = FixSign(sens_z, 8) / 256.0f;
_mag_comp_vals.dut_sensit_coef.t_sens = FixSign(t_sens, 8) / 512.0f;
tco_x = (otp_data[0x12] & 0x00FF);
tco_y = (otp_data[0x13] & 0x00FF);
tco_z = (otp_data[0x14] & 0x00FF);
_mag_comp_vals.dut_tco.tco_x = FixSign(tco_x, 8) / 32.0f;
_mag_comp_vals.dut_tco.tco_y = FixSign(tco_y, 8) / 32.0f;
_mag_comp_vals.dut_tco.tco_z = FixSign(tco_z, 8) / 32.0f;
tcs_x = (otp_data[0x12] & 0xFF00) >> 8;
tcs_y = (otp_data[0x13] & 0xFF00) >> 8;
tcs_z = (otp_data[0x14] & 0xFF00) >> 8;
_mag_comp_vals.dut_tcs.tcs_x = FixSign(tcs_x, 8) / 16384.0f;
_mag_comp_vals.dut_tcs.tcs_y = FixSign(tcs_y, 8) / 16384.0f;
_mag_comp_vals.dut_tcs.tcs_z = (FixSign(tcs_z, 8) / 16384.0f) - 0.0001f;
_mag_comp_vals.dut_t0 = (FixSign(otp_data[0x18], 16) / 512.0f) + 23.0f;
cross_x_y = (otp_data[0x15] & 0x00FF);
cross_y_x = (otp_data[0x15] & 0xFF00) >> 8;
cross_z_x = (otp_data[0x16] & 0x00FF);
cross_z_y = (otp_data[0x16] & 0xFF00) >> 8;
_mag_comp_vals.cross_axis.cross_x_y = FixSign(cross_x_y, 8) / 800.0f;
_mag_comp_vals.cross_axis.cross_y_x = FixSign(cross_y_x, 8) / 800.0f;
_mag_comp_vals.cross_axis.cross_z_x = FixSign(cross_z_x, 8) / 800.0f;
_mag_comp_vals.cross_axis.cross_z_y = FixSign(cross_z_y, 8) / 800.0f;
return PX4_OK;
}
int BMM350::RegisterRead(Register reg, uint8_t *value)
{
const uint8_t cmd = static_cast<uint8_t>(reg);
uint8_t buffer[3] = {0};
int ret = transfer(&cmd, 1, buffer, 3);
if (ret != PX4_OK) {
PX4_DEBUG("register read 0x%02hhX failed, ret = %d", cmd, ret);
} else {
*value = buffer[2];
}
return ret;
}
int BMM350::RegisterWrite(Register reg, uint8_t value)
{
uint8_t buffer[2] {(uint8_t)reg, value};
int ret = transfer(buffer, sizeof(buffer), nullptr, 0);
if (ret != PX4_OK) {
PX4_DEBUG("register write 0x%02hhX failed, ret = %d", (uint8_t)reg, ret);
}
return ret;
}

210
src/drivers/magnetometer/bosch/bmm350/BMM350.hpp Normal file
View File

@ -0,0 +1,210 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file BMM350.hpp
*
* Driver for the Bosch BMM350 connected via I2C.
*
*/
#pragma once
#include "Bosch_BMM350_registers.hpp"
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module_params.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/SubscriptionInterval.hpp>
using namespace Bosch_BMM350;
using namespace time_literals;
class BMM350 : public device::I2C, public I2CSPIDriver<BMM350>, public ModuleParams
{
public:
BMM350(const I2CSPIDriverConfig &config);
~BMM350() override;
static void print_usage();
void RunImpl();
int init() override;
void print_status() override;
private:
struct mag_temp_data {
float x;
float y;
float z;
float temp;
};
struct raw_mag_data {
int32_t raw_x;
int32_t raw_y;
int32_t raw_z;
int32_t raw_t;
};
struct register_config_t {
Register reg;
uint8_t set_bits{0};
uint8_t clear_bits{0};
};
struct dut_offset_coef {
float t_offs;
float offset_x;
float offset_y;
float offset_z;
};
struct dut_sensit_coef {
float t_sens;
float sens_x;
float sens_y;
float sens_z;
};
struct dut_tco {
float tco_x;
float tco_y;
float tco_z;
};
struct dut_tcs {
float tcs_x;
float tcs_y;
float tcs_z;
};
struct cross_axis {
float cross_x_y;
float cross_y_x;
float cross_z_x;
float cross_z_y;
};
struct mag_compensate_vals {
struct dut_offset_coef dut_offset_coef;
struct dut_sensit_coef dut_sensit_coef;
struct dut_tco dut_tco;
struct dut_tcs dut_tcs;
float dut_t0;
struct cross_axis cross_axis;
};
int probe() override;
bool Reset();
int Configure();
int RegisterRead(Register reg, uint8_t *value);
int RegisterWrite(Register reg, uint8_t value);
int8_t CompensateAxisAndTemp();
int ReadOutRawData(float *out_data);
int ReadOTPWord(uint8_t addr, uint16_t *lsb_msb);
int32_t FixSign(uint32_t inval, int8_t num_bits);
int UpdateMagOffsets();
void ParametersUpdate(bool force = false);
void UpdateMagParams();
uint8_t GetODR(int value);
hrt_abstime OdrToUs(uint8_t value);
uint8_t GetAVG(int value);
PX4Magnetometer _px4_mag;
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME ": reset")};
perf_counter_t _bad_read_perf{perf_alloc(PC_COUNT, MODULE_NAME ": bad read")};
perf_counter_t _self_test_failed_perf{perf_alloc(PC_COUNT, MODULE_NAME ": self test failed")};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
mag_compensate_vals _mag_comp_vals{0};
float _initial_self_test_values[4];
uint8_t _mag_odr_mode = ODR_200HZ;
uint8_t _mag_avg_mode = AVG_2;
uint8_t _mag_pad_drive = 7;
static constexpr float BXY_SENS = 14.55f;
static constexpr float BZ_SENS = 9.0f;
static constexpr float TEMP_SENS = 0.00204f;
static constexpr float INA_XY_GAIN_TRT = 19.46f;
static constexpr float INA_Z_GAIN_TRT = 31.0f;
static constexpr float ADC_GAIN = 1 / 1.5f;
static constexpr float LUT_GAIN = 0.714607238769531f;
static constexpr float POWER = 1000000.0f / 1048576.0f;
float lsb_to_utc_degc[4] = {
(POWER / (BXY_SENS *INA_XY_GAIN_TRT *ADC_GAIN * LUT_GAIN)),
(POWER / (BXY_SENS *INA_XY_GAIN_TRT *ADC_GAIN * LUT_GAIN)),
(POWER / (BZ_SENS *INA_Z_GAIN_TRT *ADC_GAIN * LUT_GAIN)),
1 / (TEMP_SENS *ADC_GAIN *LUT_GAIN * 1048576)
};
enum class STATE : uint8_t {
RESET,
WAIT_FOR_RESET,
FGR,
BR,
AFTER_RESET,
MEASURE_FORCED,
SELF_TEST_CHECK,
CONFIGURE,
READ,
} _state{STATE::RESET};
enum class SELF_TEST_STATE : uint8_t {
INIT,
POS_X,
NEG_X,
POS_Y,
NEG_Y
} _self_test_state{SELF_TEST_STATE::INIT};
DEFINE_PARAMETERS(
(ParamInt<px4::params::BMM350_ODR>) _param_bmm350_odr,
(ParamInt<px4::params::BMM350_AVG>) _param_bmm350_avg,
(ParamInt<px4::params::BMM350_DRIVE>) _param_bmm350_drive
)
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
};

159
src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp Normal file
View File

@ -0,0 +1,159 @@
/****************************************************************************
*
* Copyright (c) 2020-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Bosch_BMM350_registers.hpp
*
* Bosch BMM350 registers.
*
*/
#pragma once
#include <cstdint>
namespace Bosch_BMM350
{
#define ENABLE_X_AXIS(axis_data) (axis_data = (axis_data & ~(0x01)) | (1 & 0x01))
#define ENABLE_Y_AXIS(axis_data) (axis_data = ((axis_data & ~(0x02)) | ((1 << 0x1) & 0x02)))
#define ENABLE_Z_AXIS(axis_data) (axis_data = ((axis_data & ~(0x04)) | ((1 << 0x2) & 0x04)))
static constexpr uint32_t I2C_SPEED = 400 * 1000;
static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x14;
static constexpr uint8_t chip_identification_number = 0x33;
enum class Register : uint8_t {
CHIP_ID = 0x00,
PAD_CTRL = 0x03,
PMU_CMD_AGGR_SET = 0x04,
PMU_CMD_AXIS_EN = 0x05,
PMU_CMD = 0x06,
PMU_STATUS_0 = 0x07,
PMU_STATUS_1 = 0x08,
I2C_WDT_SET = 0x0a,
DATAX_XLSB = 0x31,
OTP_CMD = 0x50,
OTP_DATA_MSB = 0x52,
OTP_DATA_LSB = 0x53,
OTP_STATUS = 0x55,
TMR_SELF_TEST_USER = 0x60,
CMD = 0x7e
};
enum CONTROL_CMD : uint8_t {
SOFT_RESET = 0xb6,
EN_XYZ = 0x7
};
enum PMU_CONTROL_CMD : uint8_t {
PMU_CMD_SUSPEND = 0x00,
PMU_CMD_NM = 0x01,
PMU_CMD_UPDATE_OAE = 0x02,
PMU_CMD_FM = 0x03,
PMU_CMD_FAST_FM = 0x04,
PMU_CMD_FGR = 0x05,
PMU_CMD_FAST_FGR = 0x06,
PMU_CMD_BR = 0x07,
PMU_CMD_BR_FAST = 0x08,
PMU_CMD_NM_TC = 0x09
};
static inline uint8_t PMU_CMD_STATUS_0_RES(uint8_t val)
{
return (val >> 5) & 0x7;
};
enum PMU_STATUS_0_BIT : uint8_t {
PMU_BUSY = (1 << 0),
ODR_OVWR = (1 << 1),
AVG_OVWR = (1 << 2),
PWR_NORMAL = (1 << 3),
ILLEGAL_CMD = (1 << 4)
};
enum PMU_STATUS_VALUE {
PMU_STATUS_SUS = 0x0,
PMU_STATUS_NM = 0x1,
PMU_STATUS_UPDATE_OAE = 0x2,
PMU_STATUS_FM = 0x3,
PMU_STATUS_FM_FAST = 0x4,
PMU_STATUS_FGR = 0x5,
PMU_STATUS_FGR_FAST = 0x6,
PMU_STATUS_BR = 0x7,
PMU_STATUS_BR_FAST = 0x7,
};
enum ODR_CONTROL_CMD : uint8_t {
ODR_400HZ = 0x2,
ODR_200HZ = 0x3,
ODR_100HZ = 0x4,
ODR_50HZ = 0x5,
ODR_25HZ = 0x6,
ODR_12_5HZ = 0x7,
ODR_6_25HZ = 0x8,
ODR_3_125HZ = 0x9,
ODR_1_5625HZ = 0xa
};
enum AVG_CONTROL_CMD : uint8_t {
AVG_NO_AVG = 0x0,
AVG_2 = 0x1,
AVG_4 = 0x2,
AVG_8 = 0x3
};
enum ENABLE_AXIS_BIT : uint8_t {
EN_X = (1 << 0),
EN_Y = (1 << 1),
EN_Z = (1 << 2)
};
enum OTP_CONTROL_CMD : uint8_t {
PWR_OFF_OTP = 0x80,
OTP_DIR_READ = 0x20,
OTP_WORD_MSK = 0x1F,
};
enum SELF_TEST_CMD : uint8_t {
SELF_TEST_DISABLE = 0x00,
SELF_TEST_POS_X = 0x0d,
SELF_TEST_NEG_X = 0x0b,
SELF_TEST_POS_Y = 0x15,
SELF_TEST_NEG_Y = 0x13,
};
} // namespace Bosch_BMM350

48
src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt Normal file
View File

@ -0,0 +1,48 @@
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__magnetometer__bosch__bmm350
MAIN bmm350
COMPILE_FLAGS
SRCS
BMM350.cpp
BMM350.hpp
bmm350_main.cpp
Bosch_BMM350_registers.hpp
DEPENDS
drivers_magnetometer
px4_work_queue
MODULE_CONFIG
module.yaml
)

5
src/drivers/magnetometer/bosch/bmm350/Kconfig Normal file
View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_MAGNETOMETER_BOSCH_BMM350
bool "bmm350"
default n
---help---
Enable support for bosch bmm350

89
src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp Normal file
View File

@ -0,0 +1,89 @@
/****************************************************************************
*
* Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "BMM350.hpp"
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
void BMM350::print_usage()
{
PRINT_MODULE_USAGE_NAME("bmm350", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x14);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
extern "C" int bmm350_main(int argc, char *argv[])
{
int ch;
using ThisDriver = BMM350;
BusCLIArguments cli{true, false};
cli.default_i2c_frequency = I2C_SPEED;
cli.i2c_address = I2C_ADDRESS_DEFAULT;
while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optArg());
break;
}
}
const char *verb = cli.optArg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_MAG_DEVTYPE_BMM350);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}

44
src/drivers/magnetometer/bosch/bmm350/module.yaml Normal file
View File

@ -0,0 +1,44 @@
module_name: BMM350
parameters:
- group: Magnetometer
definitions:
BMM350_ODR:
description:
short: BMM350 ODR rate
long: |
Defines which ODR rate to use during data polling.
type: enum
values:
0: 400 Hz
1: 200 Hz
2: 100 Hz
3: 50 Hz
4: 25 Hz
5: 12.5 Hz
6: 6.25 Hz
7: 3.125 Hz
8: 1.5625 Hz
reboot_required: true
default: [3]
BMM350_AVG:
description:
short: BMM350 data averaging
long: |
Defines which averaging mode to use during data polling.
type: enum
values:
0: No averaging
1: 2 sample averaging
2: 4 sample averaging
3: 8 sample averaging
reboot_required: true
default: [1]
BMM350_DRIVE:
description:
short: BMM350 pad drive strength setting
long: |
This setting helps avoid signal problems like overshoot or undershoot.
type: int32
min: 0
max: 7
default: [7]