From 2a124fd99898166637e6bea3e3123aa6bfe176ef Mon Sep 17 00:00:00 2001 From: Vilius Date: Thu, 15 Aug 2024 12:29:02 +0300 Subject: [PATCH] Add Bosch BMM350 magnetometer (#23362) * 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 --- src/drivers/drv_sensor.h | 2 + src/drivers/magnetometer/bosch/CMakeLists.txt | 1 + .../magnetometer/bosch/bmm350/BMM350.cpp | 773 ++++++++++++++++++ .../magnetometer/bosch/bmm350/BMM350.hpp | 210 +++++ .../bosch/bmm350/Bosch_BMM350_registers.hpp | 159 ++++ .../magnetometer/bosch/bmm350/CMakeLists.txt | 48 ++ src/drivers/magnetometer/bosch/bmm350/Kconfig | 5 + .../magnetometer/bosch/bmm350/bmm350_main.cpp | 89 ++ .../magnetometer/bosch/bmm350/module.yaml | 44 + 9 files changed, 1331 insertions(+) create mode 100755 src/drivers/magnetometer/bosch/bmm350/BMM350.cpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/BMM350.hpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt create mode 100644 src/drivers/magnetometer/bosch/bmm350/Kconfig create mode 100644 src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp create mode 100644 src/drivers/magnetometer/bosch/bmm350/module.yaml diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 378c92b6736c..53eecce4be71 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -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 */ diff --git a/src/drivers/magnetometer/bosch/CMakeLists.txt b/src/drivers/magnetometer/bosch/CMakeLists.txt index b814e7224fb1..d7e848ec86a4 100644 --- a/src/drivers/magnetometer/bosch/CMakeLists.txt +++ b/src/drivers/magnetometer/bosch/CMakeLists.txt @@ -32,3 +32,4 @@ ############################################################################ add_subdirectory(bmm150) +add_subdirectory(bmm350) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp new file mode 100755 index 000000000000..e4e8e19f9c5e --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -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(¶m_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(_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(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(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(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; +} diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp new file mode 100644 index 000000000000..4ba7f535dbd9 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp @@ -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 +#include +#include +#include +#include +#include +#include +#include + +using namespace Bosch_BMM350; +using namespace time_literals; + +class BMM350 : public device::I2C, public I2CSPIDriver, 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) _param_bmm350_odr, + (ParamInt) _param_bmm350_avg, + (ParamInt) _param_bmm350_drive + ) + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; +}; diff --git a/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp new file mode 100644 index 000000000000..dc391f8824aa --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp @@ -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 + +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 diff --git a/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt b/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt new file mode 100644 index 000000000000..ca253077287e --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/CMakeLists.txt @@ -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 + ) diff --git a/src/drivers/magnetometer/bosch/bmm350/Kconfig b/src/drivers/magnetometer/bosch/bmm350/Kconfig new file mode 100644 index 000000000000..834a2c694780 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_BOSCH_BMM350 + bool "bmm350" + default n + ---help--- + Enable support for bosch bmm350 diff --git a/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp b/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp new file mode 100644 index 000000000000..c707d7957b6b --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/bmm350_main.cpp @@ -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 +#include + +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; +} diff --git a/src/drivers/magnetometer/bosch/bmm350/module.yaml b/src/drivers/magnetometer/bosch/bmm350/module.yaml new file mode 100644 index 000000000000..140c5c321a8a --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/module.yaml @@ -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]