From f6fb6598419cebccfc1f8cfc66f34d6644a89e3e Mon Sep 17 00:00:00 2001 From: Viliuks Date: Thu, 4 Jul 2024 00:43:16 +0300 Subject: [PATCH 1/8] Add Bosch BMM350 magnetometer --- src/drivers/drv_sensor.h | 2 + src/drivers/magnetometer/bosch/CMakeLists.txt | 1 + .../magnetometer/bosch/bmm350/BMM350.cpp | 658 ++++++++++++++++++ .../magnetometer/bosch/bmm350/BMM350.hpp | 180 +++++ .../bosch/bmm350/Bosch_BMM350_registers.hpp | 122 ++++ .../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 | 45 ++ 9 files changed, 1150 insertions(+) create mode 100644 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 100644 index 000000000000..e0c660a172bf --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -0,0 +1,658 @@ +/**************************************************************************** + * + * 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_INFO("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; + } +} + +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++) { + const uint8_t CMD = RegisterRead(Register::CMD); + const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID); + + PX4_INFO("CMD: 0x%02hhX, CHIP_ID: 0x%02hhX", CMD, CHIP_ID); + + if (CHIP_ID == chip_identification_number) { + PX4_INFO("Found chip"); + return PX4_OK; + + } else if (CHIP_ID == 0 && CMD == 0) { + PX4_INFO("Suspended, but found"); + return PX4_OK; + } + } + + return PX4_ERROR; +} + +void BMM350::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + ParametersUpdate(); + + switch (_state) { + case STATE::RESET: { + RegisterWrite(Register::CMD, SOFT_RESET); + _reset_timestamp = now; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + ScheduleDelayed(10_ms); + } + break; + + case STATE::WAIT_FOR_RESET: { + uint8_t chipId; + + if ((chipId = RegisterRead(Register::CHIP_ID)) == chip_identification_number) { + UpdateMagOffsets(); + RegisterWrite(Register::OTP_CMD, PWR_OFF_OTP); + PX4_INFO("After reset chip id = %i", chipId); + _state = STATE::AFTER_RESET; + ScheduleDelayed(10_ms); + + } else { + _state = STATE::RESET; + ScheduleDelayed(30_ms); + } + } + break; + + case STATE::AFTER_RESET: + uint8_t chipId; + + if (((chipId = RegisterRead(Register::CHIP_ID)) == chip_identification_number)) { + + // Prep self test + RegisterWrite(Register::PMU_CMD, PMU_CMD_SUSPEND); + px4_usleep(30000); + uint8_t odr_reg_data = (ODR_100HZ & 0xf); + odr_reg_data = ((odr_reg_data & ~(0x30)) | ((AVG_2 << 0x4) & 0x30)); + RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + RegisterWrite(Register::PMU_CMD_AXIS_EN, 0x07); + RegisterWrite(Register::PMU_CMD, PMU_CMD_FGR); + px4_usleep(30000); + RegisterWrite(Register::PMU_CMD, PMU_CMD_BR_FAST); + px4_usleep(4000); + RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); + + PX4_INFO("Chip id found going to self test id= %i", chipId); + _state = STATE::SELF_TEST_CHECK; + 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::SELF_TEST_CHECK: { + + float out_ust[4] = {0.0f}; + + float out_ustxh = 0.0f, out_ustxl = 0.0f, out_ustyh = 0.0f, out_ustyl = 0.0f, out_ust_x = 0.0f, out_ust_y = 0.0f; + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_X); + px4_usleep(1000); + RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); + px4_usleep(6000); + ReadOutRawData(out_ust); + out_ustxh = out_ust[0]; + + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_X); + px4_usleep(1000); + RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); + px4_usleep(6000); + ReadOutRawData(out_ust); + out_ustxl = out_ust[0]; + + + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_Y); + px4_usleep(1000); + RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); + px4_usleep(6000); + ReadOutRawData(out_ust); + out_ustyh = out_ust[1]; + + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_Y); + px4_usleep(1000); + RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); + px4_usleep(6000); + ReadOutRawData(out_ust); + out_ustyl = out_ust[1]; + + out_ust_x = out_ustxh - out_ustxl; + out_ust_y = out_ustyh - out_ustyl; + + PX4_INFO("outustxh = %.5f, outustxl = %.5f", static_cast(out_ustxh), static_cast(out_ustxl)); + PX4_INFO("outustyh = %.5f, outustyl = %.5f", static_cast(out_ustyh), static_cast(out_ustyl)); + PX4_INFO("out_ust_x = %.5f, out_ust_y = %.5f", static_cast(out_ust_x), static_cast(out_ust_y)); + + // Datasheet 5.1.6 + if (out_ust_x >= 130 && out_ust_y >= 130) { + PX4_INFO("Running to configure"); + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else if (perf_event_count(_self_test_failed_perf) >= 5) { + PX4_INFO("Failed after 5 attempts, procceed still"); + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + perf_count(_self_test_failed_perf); + _state = STATE::RESET; + ScheduleDelayed(1_s); + } + } + + break; + + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading every 50 ms (20 Hz) + _state = STATE::READ; + PX4_INFO("Configure went fine"); + ScheduleOnInterval(50_ms, 50_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_INFO("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_INFO("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::READ: { + // -- start get_compensated_mag_xyz_temp_data + int8_t res = 0; + uint8_t idx; + 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; + + res = ReadOutRawData(out_data); + + if (res == 0) { + // 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 (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.update(now, cr_ax_comp_x, cr_ax_comp_y, cr_ax_comp_z); + + } else { + perf_count(_bad_read_perf); + } + } + + break; + } +} + +bool BMM350::Configure() +{ + PX4_INFO("Configuring"); + bool success = true; + uint8_t readData = 0; + + // Set pad drive + RegisterWrite(Register::PAD_CTRL, (_mag_pad_drive & 0x7)); + // 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)); + + RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + + if ((readData = RegisterRead(Register::PMU_CMD_AGGR_SET)) != odr_reg_data) { + PX4_INFO("Couldn't set PMU AGGR REG"); + success = false; + } + + odr_reg_data = PMU_CMD_UPDATE_OAE; + RegisterWrite(Register::PMU_CMD, odr_reg_data); + + if ((readData = RegisterRead(Register::PMU_CMD)) != odr_reg_data) { + PX4_INFO("Couldn't set PMU CMD REG"); + success = false; + } + + // Enable AXIS + uint8_t axis_data = (1 & 0x01); + axis_data = ((axis_data & ~(0x02)) | ((1 << 0x1) & 0x02)); + axis_data = ((axis_data & ~(0x04)) | ((1 << 0x2) & 0x04)); // evaluates to 0x07 + + // PMU_CMD_AXIS_EN + RegisterWrite(Register::PMU_CMD_AXIS_EN, axis_data); + + if ((readData = RegisterRead(Register::PMU_CMD_AXIS_EN)) != axis_data) { + PX4_INFO("Couldnt set AXIS"); + success = false; + } + + RegisterWrite(Register::PMU_CMD, PMU_CMD_NM); + + // microTesla -> Gauss + _px4_mag.set_scale(0.01f); + + return success; +} + +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; +} + +int8_t BMM350::ReadOutRawData(float *out_data) +{ + if (out_data == NULL) { + return -1; + } + + float temp = 0.0; + struct BMM350::raw_mag_data raw_data = {0}; + float lsb_to_ut_degc[4]; + + // --- Start read_uncomp_mag_temp_data + uint8_t mag_data[14] = {0}; + uint8_t raw_reg[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 *)&raw_reg, sizeof(raw_reg)); + + if (res != PX4_OK) { + return -1; + } + + // Throwaway first two bytes + memcpy(mag_data, raw_reg + 2, sizeof(mag_data) - 2); + + raw_mag_x = mag_data[0] + ((uint32_t)mag_data[1] << 8) + ((uint32_t)mag_data[2] << 16); + raw_mag_y = mag_data[3] + ((uint32_t)mag_data[4] << 8) + ((uint32_t)mag_data[5] << 16); + raw_mag_z = mag_data[6] + ((uint32_t)mag_data[7] << 8) + ((uint32_t)mag_data[8] << 16); + raw_temp = mag_data[9] + ((uint32_t)mag_data[10] << 8) + ((uint32_t)mag_data[11] << 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 update_dafault_coefiecients + float bxy_sens, bz_sens, temp_sens, ina_xy_gain_trgt, ina_z_gain_trgt, adc_gain, lut_gain; + float power; + + bxy_sens = 14.55f; + bz_sens = 9.0f; + temp_sens = 0.00204f; + + ina_xy_gain_trgt = 19.46f; + + ina_z_gain_trgt = 31.0; + + adc_gain = 1 / 1.5f; + lut_gain = 0.714607238769531f; + + power = (float)(1000000.0 / 1048576.0); + + lsb_to_ut_degc[0] = (power / (bxy_sens * ina_xy_gain_trgt * adc_gain * lut_gain)); + lsb_to_ut_degc[1] = (power / (bxy_sens * ina_xy_gain_trgt * adc_gain * lut_gain)); + lsb_to_ut_degc[2] = (power / (bz_sens * ina_z_gain_trgt * adc_gain * lut_gain)); + lsb_to_ut_degc[3] = 1 / (temp_sens * adc_gain * lut_gain * 1048576); + // --- End update_default_coeficients + + // --- Start read_out_raw_data + out_data[0] = (float)raw_data.raw_x * lsb_to_ut_degc[0]; + out_data[1] = (float)raw_data.raw_y * lsb_to_ut_degc[1]; + out_data[2] = (float)raw_data.raw_z * lsb_to_ut_degc[2]; + out_data[3] = (float)raw_data.raw_t * lsb_to_ut_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; +} + +int8_t BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb) +{ + if (lsb_msb == NULL) { + return -1; + } + + uint8_t otp_cmd = OTP_DIR_READ | (addr & OTP_WORD_MSK); + RegisterWrite(Register::OTP_CMD, otp_cmd); + uint8_t otp_status = 0; + + do { + px4_usleep(300); + otp_status = RegisterRead(Register::OTP_STATUS); + } while (!(otp_status & 0x01)); + + uint8_t msb = RegisterRead(Register::OTP_DATA_MSB); + uint8_t lsb = RegisterRead(Register::OTP_DATA_LSB); + *lsb_msb = ((msb << 8) | lsb) & 0xffff; + return 1; +} + +void BMM350::UpdateMagOffsets() +{ + PX4_INFO("DUMPING OTP"); + uint16_t otp_word = 0; + uint16_t otp_data[32] = {0}; + + for (int idx = 0; idx < 32; idx++) { + ReadOTPWord(idx, &otp_word); + otp_data[idx] = otp_word; + PX4_DEBUG("i: %i, val = %i", idx, otp_data[idx]); + } + + PX4_INFO("var_id: %i", (otp_data[30] & 0x7f00) >> 9); + + PX4_INFO("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; +} + +uint8_t BMM350::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer[3] = {0}; + int ret = transfer(&cmd, 1, buffer, 3); + + if (ret != PX4_OK) { + PX4_INFO("register read 0x%02hhX failed, ret = %d", cmd, ret); + return -1; + } + + return buffer[2]; +} + +void 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_INFO("register write 0x%02hhX failed, ret = %d", (uint8_t)reg, 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..c09d046c2796 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp @@ -0,0 +1,180 @@ +/**************************************************************************** + * + * 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(); + bool Configure(); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + + int8_t CompensateAxisAndTemp(); + int8_t ReadOutRawData(float *out_data); + int8_t ReadOTPWord(uint8_t addr, uint16_t *lsb_msb); + int32_t FixSign(uint32_t inval, int8_t num_bits); + + void UpdateMagOffsets(); + void ParametersUpdate(bool force = false); + void UpdateMagParams(); + uint8_t GetODR(int 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}; + + uint8_t _mag_odr_mode = ODR_200HZ; + uint8_t _mag_avg_mode = AVG_2; + uint8_t _mag_pad_drive = 7; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + AFTER_RESET, + SELF_TEST_CHECK, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + 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..d24b546ca370 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp @@ -0,0 +1,122 @@ +/**************************************************************************** + * + * 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 +{ +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, + + I2C_WDT_SET = 0x0a, + + DATAX_XLSB = 0x31, + + OTP_CMD = 0x50, + OTP_DATA_LSB = 0x52, + OTP_DATA_MSB = 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 +}; + +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 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..b7f4dbd01e6c --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm350/module.yaml @@ -0,0 +1,45 @@ +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: [1] + 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] + From adaa7ba5871e621f002381d823612a1bea79fd67 Mon Sep 17 00:00:00 2001 From: Viliuks Date: Thu, 4 Jul 2024 21:16:21 +0300 Subject: [PATCH 2/8] BMM350 replace info messages with debug messages --- .../magnetometer/bosch/bmm350/BMM350.cpp | 46 +++++++++---------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp index e0c660a172bf..3ee68e7a6277 100644 --- a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -100,7 +100,7 @@ void BMM350::UpdateMagParams() _mag_odr_mode = odr; _mag_avg_mode = avg; _mag_pad_drive = static_cast(_param_bmm350_drive.get()); - PX4_INFO("Set params odr = %d, avg = %d, drive = %d", _mag_odr_mode, _mag_avg_mode, _mag_pad_drive); + 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) @@ -150,14 +150,14 @@ int BMM350::probe() const uint8_t CMD = RegisterRead(Register::CMD); const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID); - PX4_INFO("CMD: 0x%02hhX, CHIP_ID: 0x%02hhX", CMD, CHIP_ID); + PX4_DEBUG("CMD: 0x%02hhX, CHIP_ID: 0x%02hhX", CMD, CHIP_ID); if (CHIP_ID == chip_identification_number) { - PX4_INFO("Found chip"); + PX4_DEBUG("Found chip"); return PX4_OK; } else if (CHIP_ID == 0 && CMD == 0) { - PX4_INFO("Suspended, but found"); + PX4_DEBUG("Suspended, but found"); return PX4_OK; } } @@ -186,7 +186,7 @@ void BMM350::RunImpl() if ((chipId = RegisterRead(Register::CHIP_ID)) == chip_identification_number) { UpdateMagOffsets(); RegisterWrite(Register::OTP_CMD, PWR_OFF_OTP); - PX4_INFO("After reset chip id = %i", chipId); + PX4_DEBUG("After reset chip id = %i", chipId); _state = STATE::AFTER_RESET; ScheduleDelayed(10_ms); @@ -215,7 +215,7 @@ void BMM350::RunImpl() px4_usleep(4000); RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); - PX4_INFO("Chip id found going to self test id= %i", chipId); + PX4_DEBUG("Chip id found going to self test id= %i", chipId); _state = STATE::SELF_TEST_CHECK; ScheduleDelayed(10_ms); @@ -270,18 +270,18 @@ void BMM350::RunImpl() out_ust_x = out_ustxh - out_ustxl; out_ust_y = out_ustyh - out_ustyl; - PX4_INFO("outustxh = %.5f, outustxl = %.5f", static_cast(out_ustxh), static_cast(out_ustxl)); - PX4_INFO("outustyh = %.5f, outustyl = %.5f", static_cast(out_ustyh), static_cast(out_ustyl)); - PX4_INFO("out_ust_x = %.5f, out_ust_y = %.5f", static_cast(out_ust_x), static_cast(out_ust_y)); + PX4_DEBUG("outustxh = %.5f, outustxl = %.5f", static_cast(out_ustxh), static_cast(out_ustxl)); + PX4_DEBUG("outustyh = %.5f, outustyl = %.5f", static_cast(out_ustyh), static_cast(out_ustyl)); + PX4_DEBUG("out_ust_x = %.5f, out_ust_y = %.5f", static_cast(out_ust_x), static_cast(out_ust_y)); // Datasheet 5.1.6 if (out_ust_x >= 130 && out_ust_y >= 130) { - PX4_INFO("Running to configure"); + PX4_DEBUG("Running to configure"); _state = STATE::CONFIGURE; ScheduleDelayed(10_ms); } else if (perf_event_count(_self_test_failed_perf) >= 5) { - PX4_INFO("Failed after 5 attempts, procceed still"); + PX4_DEBUG("Failed after 5 attempts, procceed still"); _state = STATE::CONFIGURE; ScheduleDelayed(10_ms); @@ -299,17 +299,17 @@ void BMM350::RunImpl() if (Configure()) { // if configure succeeded then start reading every 50 ms (20 Hz) _state = STATE::READ; - PX4_INFO("Configure went fine"); + PX4_DEBUG("Configure went fine"); ScheduleOnInterval(50_ms, 50_ms); } else { // CONFIGURE not complete if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { - PX4_INFO("Configure failed, resetting"); + PX4_DEBUG("Configure failed, resetting"); _state = STATE::RESET; } else { - PX4_INFO("Configure failed, retrying"); + PX4_DEBUG("Configure failed, retrying"); } ScheduleDelayed(100_ms); @@ -387,7 +387,7 @@ void BMM350::RunImpl() bool BMM350::Configure() { - PX4_INFO("Configuring"); + PX4_DEBUG("Configuring"); bool success = true; uint8_t readData = 0; @@ -413,7 +413,7 @@ bool BMM350::Configure() RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); if ((readData = RegisterRead(Register::PMU_CMD_AGGR_SET)) != odr_reg_data) { - PX4_INFO("Couldn't set PMU AGGR REG"); + PX4_DEBUG("Couldn't set PMU AGGR REG"); success = false; } @@ -421,7 +421,7 @@ bool BMM350::Configure() RegisterWrite(Register::PMU_CMD, odr_reg_data); if ((readData = RegisterRead(Register::PMU_CMD)) != odr_reg_data) { - PX4_INFO("Couldn't set PMU CMD REG"); + PX4_DEBUG("Couldn't set PMU CMD REG"); success = false; } @@ -434,7 +434,7 @@ bool BMM350::Configure() RegisterWrite(Register::PMU_CMD_AXIS_EN, axis_data); if ((readData = RegisterRead(Register::PMU_CMD_AXIS_EN)) != axis_data) { - PX4_INFO("Couldnt set AXIS"); + PX4_DEBUG("Couldnt set AXIS"); success = false; } @@ -563,7 +563,7 @@ int8_t BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb) void BMM350::UpdateMagOffsets() { - PX4_INFO("DUMPING OTP"); + PX4_DEBUG("DUMPING OTP"); uint16_t otp_word = 0; uint16_t otp_data[32] = {0}; @@ -573,9 +573,9 @@ void BMM350::UpdateMagOffsets() PX4_DEBUG("i: %i, val = %i", idx, otp_data[idx]); } - PX4_INFO("var_id: %i", (otp_data[30] & 0x7f00) >> 9); + PX4_DEBUG("var_id: %i", (otp_data[30] & 0x7f00) >> 9); - PX4_INFO("UPDATING OFFSETS"); + 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; @@ -640,7 +640,7 @@ uint8_t BMM350::RegisterRead(Register reg) int ret = transfer(&cmd, 1, buffer, 3); if (ret != PX4_OK) { - PX4_INFO("register read 0x%02hhX failed, ret = %d", cmd, ret); + PX4_DEBUG("register read 0x%02hhX failed, ret = %d", cmd, ret); return -1; } @@ -653,6 +653,6 @@ void BMM350::RegisterWrite(Register reg, uint8_t value) int ret = transfer(buffer, sizeof(buffer), nullptr, 0); if (ret != PX4_OK) { - PX4_INFO("register write 0x%02hhX failed, ret = %d", (uint8_t)reg, ret); + PX4_DEBUG("register write 0x%02hhX failed, ret = %d", (uint8_t)reg, ret); } } From 3874f7255e0822a749fa17e6b89725e45c355458 Mon Sep 17 00:00:00 2001 From: Viliuks Date: Fri, 5 Jul 2024 13:54:42 +0300 Subject: [PATCH 3/8] BMM350 update measurement interval --- .../magnetometer/bosch/bmm350/BMM350.cpp | 31 +++++++++++++++++-- .../magnetometer/bosch/bmm350/BMM350.hpp | 1 + .../bosch/bmm350/Bosch_BMM350_registers.hpp | 14 +++++++++ 3 files changed, 43 insertions(+), 3 deletions(-) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp index 3ee68e7a6277..866b152b3a4d 100644 --- a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -128,6 +128,31 @@ uint8_t BMM350::GetODR(int value) } } +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) { @@ -208,7 +233,7 @@ void BMM350::RunImpl() uint8_t odr_reg_data = (ODR_100HZ & 0xf); odr_reg_data = ((odr_reg_data & ~(0x30)) | ((AVG_2 << 0x4) & 0x30)); RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); - RegisterWrite(Register::PMU_CMD_AXIS_EN, 0x07); + RegisterWrite(Register::PMU_CMD_AXIS_EN, EN_XYZ); RegisterWrite(Register::PMU_CMD, PMU_CMD_FGR); px4_usleep(30000); RegisterWrite(Register::PMU_CMD, PMU_CMD_BR_FAST); @@ -297,10 +322,9 @@ void BMM350::RunImpl() case STATE::CONFIGURE: if (Configure()) { - // if configure succeeded then start reading every 50 ms (20 Hz) _state = STATE::READ; PX4_DEBUG("Configure went fine"); - ScheduleOnInterval(50_ms, 50_ms); + ScheduleOnInterval(OdrToUs(_mag_odr_mode), 50_ms); } else { // CONFIGURE not complete @@ -374,6 +398,7 @@ void BMM350::RunImpl() 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, cr_ax_comp_x, cr_ax_comp_y, cr_ax_comp_z); } else { diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp index c09d046c2796..acb0308859f7 100644 --- a/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp @@ -145,6 +145,7 @@ class BMM350 : public device::I2C, public I2CSPIDriver, public ModulePar 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; diff --git a/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp index d24b546ca370..7bcee6820c35 100644 --- a/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp +++ b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp @@ -55,6 +55,8 @@ enum class Register : uint8_t { 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, @@ -87,6 +89,18 @@ enum PMU_CONTROL_CMD : uint8_t { 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 ODR_CONTROL_CMD : uint8_t { ODR_400HZ = 0x2, ODR_200HZ = 0x3, From 31547afd81a5d67d3a2eb60dc06241493e31130a Mon Sep 17 00:00:00 2001 From: Viliuks Date: Thu, 11 Jul 2024 15:21:01 +0300 Subject: [PATCH 4/8] BMM350 fix offsets, update based on review --- .../magnetometer/bosch/bmm350/BMM350.cpp | 487 ++++++++++-------- .../magnetometer/bosch/bmm350/BMM350.hpp | 41 +- .../bosch/bmm350/Bosch_BMM350_registers.hpp | 29 +- 3 files changed, 338 insertions(+), 219 deletions(-) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp index 866b152b3a4d..a1ea0a463865 100644 --- a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -128,29 +128,39 @@ uint8_t BMM350::GetODR(int value) } } -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; - } +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) @@ -172,18 +182,14 @@ uint8_t BMM350::GetAVG(int value) int BMM350::probe() { for (int i = 0; i < 3; i++) { - const uint8_t CMD = RegisterRead(Register::CMD); - const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID); + uint8_t chip_id; - PX4_DEBUG("CMD: 0x%02hhX, CHIP_ID: 0x%02hhX", CMD, 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) { - PX4_DEBUG("Found chip"); - return PX4_OK; - - } else if (CHIP_ID == 0 && CMD == 0) { - PX4_DEBUG("Suspended, but found"); - return PX4_OK; + if (chip_id == chip_identification_number) { + return PX4_OK; + } } } @@ -197,131 +203,186 @@ void BMM350::RunImpl() switch (_state) { case STATE::RESET: { - RegisterWrite(Register::CMD, SOFT_RESET); - _reset_timestamp = now; - _state = STATE::WAIT_FOR_RESET; - perf_count(_reset_perf); + 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: { - uint8_t chipId; + int ret = probe(); + + if (ret == PX4_OK) { + uint8_t status_0; + ret = RegisterRead(Register::PMU_STATUS_0, &status_0); - if ((chipId = RegisterRead(Register::CHIP_ID)) == chip_identification_number) { - UpdateMagOffsets(); - RegisterWrite(Register::OTP_CMD, PWR_OFF_OTP); - PX4_DEBUG("After reset chip id = %i", chipId); - _state = STATE::AFTER_RESET; - ScheduleDelayed(10_ms); + if (ret == PX4_OK && (status_0 & PWR_NORMAL) != 0) { + ret = PX4_ERROR; + } + ret = UpdateMagOffsets(); + if (ret == PX4_OK) { + PX4_DEBUG("Going to FGR"); + _state = STATE::FGR; + ScheduleDelayed(10_ms); + + } else { + _state = STATE::RESET; + ScheduleDelayed(10_ms); + } } else { - _state = STATE::RESET; - ScheduleDelayed(30_ms); + 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::AFTER_RESET: - uint8_t chipId; - - if (((chipId = RegisterRead(Register::CHIP_ID)) == chip_identification_number)) { - - // Prep self test - RegisterWrite(Register::PMU_CMD, PMU_CMD_SUSPEND); - px4_usleep(30000); + case STATE::FGR: { + int ret; uint8_t odr_reg_data = (ODR_100HZ & 0xf); odr_reg_data = ((odr_reg_data & ~(0x30)) | ((AVG_2 << 0x4) & 0x30)); - RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); - RegisterWrite(Register::PMU_CMD_AXIS_EN, EN_XYZ); - RegisterWrite(Register::PMU_CMD, PMU_CMD_FGR); - px4_usleep(30000); - RegisterWrite(Register::PMU_CMD, PMU_CMD_BR_FAST); - px4_usleep(4000); - RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); - - PX4_DEBUG("Chip id found going to self test id= %i", chipId); - _state = STATE::SELF_TEST_CHECK; - ScheduleDelayed(10_ms); + ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, EN_XYZ); + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FGR); - } else { - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { - PX4_DEBUG("Reset failed, retrying"); - _state = STATE::RESET; - ScheduleDelayed(100_ms); + if (ret == PX4_OK) { + PX4_DEBUG("Going to BR"); + _state = STATE::BR; } else { - PX4_DEBUG("Reset not complete, check again in 10 ms"); - ScheduleDelayed(10_ms); + _state = STATE::RESET; + } + + ScheduleDelayed(30_ms); + } + break; + + case STATE::BR: { + uint8_t status_0; + _state = STATE::RESET; + int 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; + int 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::SELF_TEST_CHECK: { + case STATE::MEASURE_FORCED: { + int ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); - float out_ust[4] = {0.0f}; - - float out_ustxh = 0.0f, out_ustxl = 0.0f, out_ustyh = 0.0f, out_ustyl = 0.0f, out_ust_x = 0.0f, out_ust_y = 0.0f; - RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_X); - px4_usleep(1000); - RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); - px4_usleep(6000); - ReadOutRawData(out_ust); - out_ustxh = out_ust[0]; - - RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_X); - px4_usleep(1000); - RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); - px4_usleep(6000); - ReadOutRawData(out_ust); - out_ustxl = out_ust[0]; - - - RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_Y); - px4_usleep(1000); - RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); - px4_usleep(6000); - ReadOutRawData(out_ust); - out_ustyh = out_ust[1]; - - RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_NEG_Y); - px4_usleep(1000); - RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); - px4_usleep(6000); - ReadOutRawData(out_ust); - out_ustyl = out_ust[1]; - - out_ust_x = out_ustxh - out_ustxl; - out_ust_y = out_ustyh - out_ustyl; - - PX4_DEBUG("outustxh = %.5f, outustxl = %.5f", static_cast(out_ustxh), static_cast(out_ustxl)); - PX4_DEBUG("outustyh = %.5f, outustyl = %.5f", static_cast(out_ustyh), static_cast(out_ustyl)); - PX4_DEBUG("out_ust_x = %.5f, out_ust_y = %.5f", static_cast(out_ust_x), static_cast(out_ust_y)); - - // Datasheet 5.1.6 - if (out_ust_x >= 130 && out_ust_y >= 130) { - PX4_DEBUG("Running to configure"); - _state = STATE::CONFIGURE; - ScheduleDelayed(10_ms); - - } else if (perf_event_count(_self_test_failed_perf) >= 5) { - PX4_DEBUG("Failed after 5 attempts, procceed still"); - _state = STATE::CONFIGURE; - ScheduleDelayed(10_ms); + if (ret == PX4_OK) { + _state = STATE::SELF_TEST_CHECK; } else { - perf_count(_self_test_failed_perf); _state = STATE::RESET; - ScheduleDelayed(1_s); } + + 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(); + } + + switch (_self_test_state) { + case SELF_TEST_STATE::INIT: + memcpy(_initial_self_test_values, xyzt, sizeof(_initial_self_test_values)); + RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_X); + _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); + _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); + _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); + _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_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()) { + if (Configure() == PX4_OK) { _state = STATE::READ; PX4_DEBUG("Configure went fine"); ScheduleOnInterval(OdrToUs(_mag_odr_mode), 50_ms); @@ -344,14 +405,13 @@ void BMM350::RunImpl() case STATE::READ: { // -- start get_compensated_mag_xyz_temp_data int8_t res = 0; - uint8_t idx; 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; res = ReadOutRawData(out_data); - if (res == 0) { + if (res == 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; @@ -373,7 +433,7 @@ void BMM350::RunImpl() dut_tcss[1] = _mag_comp_vals.dut_tcs.tcs_y; dut_tcss[2] = _mag_comp_vals.dut_tcs.tcs_z; - for (idx = 0; idx < 3; idx++) { + 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); @@ -399,7 +459,7 @@ void BMM350::RunImpl() 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, cr_ax_comp_x, cr_ax_comp_y, cr_ax_comp_z); + _px4_mag.update(now, out_data[0], out_data[1], out_data[2]); } else { perf_count(_bad_read_perf); @@ -410,14 +470,19 @@ void BMM350::RunImpl() } } -bool BMM350::Configure() +int BMM350::Configure() { PX4_DEBUG("Configuring"); - bool success = true; uint8_t readData = 0; + int ret; // Set pad drive - RegisterWrite(Register::PAD_CTRL, (_mag_pad_drive & 0x7)); + ret = RegisterWrite(Register::PAD_CTRL, (_mag_pad_drive & 0x7)); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't set pad drive, defaults to 7"); + } + // Set PMU data aggregation uint8_t odr = _mag_odr_mode; uint8_t avg = _mag_avg_mode; @@ -435,40 +500,56 @@ bool BMM350::Configure() uint8_t odr_reg_data = (odr & 0xf); odr_reg_data = ((odr_reg_data & ~(0x30)) | ((avg << 0x4) & 0x30)); - RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); + ret = RegisterWrite(Register::PMU_CMD_AGGR_SET, odr_reg_data); - if ((readData = RegisterRead(Register::PMU_CMD_AGGR_SET)) != odr_reg_data) { - PX4_DEBUG("Couldn't set PMU AGGR REG"); - success = false; + if (ret != PX4_OK) { + PX4_DEBUG("Cannot set PMU AGG REG"); + } + + ret = RegisterRead(Register::PMU_CMD_AGGR_SET, &readData); + + if (ret != PX4_OK || readData != odr_reg_data) { + PX4_DEBUG("Couldn't check PMU AGGR REG"); } odr_reg_data = PMU_CMD_UPDATE_OAE; - RegisterWrite(Register::PMU_CMD, odr_reg_data); + ret = RegisterWrite(Register::PMU_CMD, odr_reg_data); + + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't write PMU CMD REG"); + } - if ((readData = RegisterRead(Register::PMU_CMD)) != odr_reg_data) { - PX4_DEBUG("Couldn't set PMU CMD REG"); - success = false; + ret = RegisterRead(Register::PMU_CMD, &readData); + + if (ret != PX4_OK || readData != odr_reg_data) { + PX4_DEBUG("Couldn't check PMU CMD REG"); } // Enable AXIS - uint8_t axis_data = (1 & 0x01); - axis_data = ((axis_data & ~(0x02)) | ((1 << 0x1) & 0x02)); - axis_data = ((axis_data & ~(0x04)) | ((1 << 0x2) & 0x04)); // evaluates to 0x07 - + uint8_t axis_data = EN_X | EN_Y | EN_Z; // PMU_CMD_AXIS_EN - RegisterWrite(Register::PMU_CMD_AXIS_EN, axis_data); + ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, axis_data); - if ((readData = RegisterRead(Register::PMU_CMD_AXIS_EN)) != axis_data) { - PX4_DEBUG("Couldnt set AXIS"); - success = false; + if (ret != PX4_OK) { + PX4_DEBUG("Couldn't write AXIS data"); } - RegisterWrite(Register::PMU_CMD, PMU_CMD_NM); + ret = RegisterRead(Register::PMU_CMD_AXIS_EN, &readData); + + if (ret != PX4_OK || readData != axis_data) { + PX4_DEBUG("Couldn't cross check the set AXIS"); + } + + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_NM); + + if (ret != PX4_OK) { + PX4_DEBUG("Failed to start mag in normal mode"); + } // microTesla -> Gauss _px4_mag.set_scale(0.01f); - return success; + return ret; } int32_t BMM350::FixSign(uint32_t inval, int8_t num_bits) @@ -483,7 +564,7 @@ int32_t BMM350::FixSign(uint32_t inval, int8_t num_bits) return retval; } -int8_t BMM350::ReadOutRawData(float *out_data) +int BMM350::ReadOutRawData(float *out_data) { if (out_data == NULL) { return -1; @@ -491,28 +572,23 @@ int8_t BMM350::ReadOutRawData(float *out_data) float temp = 0.0; struct BMM350::raw_mag_data raw_data = {0}; - float lsb_to_ut_degc[4]; // --- Start read_uncomp_mag_temp_data uint8_t mag_data[14] = {0}; - uint8_t raw_reg[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 *)&raw_reg, sizeof(raw_reg)); + uint8_t res = transfer(&cmd, 1, (uint8_t *)&mag_data, sizeof(mag_data)); if (res != PX4_OK) { return -1; } - // Throwaway first two bytes - memcpy(mag_data, raw_reg + 2, sizeof(mag_data) - 2); - - raw_mag_x = mag_data[0] + ((uint32_t)mag_data[1] << 8) + ((uint32_t)mag_data[2] << 16); - raw_mag_y = mag_data[3] + ((uint32_t)mag_data[4] << 8) + ((uint32_t)mag_data[5] << 16); - raw_mag_z = mag_data[6] + ((uint32_t)mag_data[7] << 8) + ((uint32_t)mag_data[8] << 16); - raw_temp = mag_data[9] + ((uint32_t)mag_data[10] << 8) + ((uint32_t)mag_data[11] << 16); + 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); @@ -520,34 +596,11 @@ int8_t BMM350::ReadOutRawData(float *out_data) raw_data.raw_t = FixSign(raw_temp, 24); // --- End read_uncomp_mag_temp_data - // --- Start update_dafault_coefiecients - float bxy_sens, bz_sens, temp_sens, ina_xy_gain_trgt, ina_z_gain_trgt, adc_gain, lut_gain; - float power; - - bxy_sens = 14.55f; - bz_sens = 9.0f; - temp_sens = 0.00204f; - - ina_xy_gain_trgt = 19.46f; - - ina_z_gain_trgt = 31.0; - - adc_gain = 1 / 1.5f; - lut_gain = 0.714607238769531f; - - power = (float)(1000000.0 / 1048576.0); - - lsb_to_ut_degc[0] = (power / (bxy_sens * ina_xy_gain_trgt * adc_gain * lut_gain)); - lsb_to_ut_degc[1] = (power / (bxy_sens * ina_xy_gain_trgt * adc_gain * lut_gain)); - lsb_to_ut_degc[2] = (power / (bz_sens * ina_z_gain_trgt * adc_gain * lut_gain)); - lsb_to_ut_degc[3] = 1 / (temp_sens * adc_gain * lut_gain * 1048576); - // --- End update_default_coeficients - // --- Start read_out_raw_data - out_data[0] = (float)raw_data.raw_x * lsb_to_ut_degc[0]; - out_data[1] = (float)raw_data.raw_y * lsb_to_ut_degc[1]; - out_data[2] = (float)raw_data.raw_z * lsb_to_ut_degc[2]; - out_data[3] = (float)raw_data.raw_t * lsb_to_ut_degc[3]; + 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) { @@ -565,39 +618,48 @@ int8_t BMM350::ReadOutRawData(float *out_data) return res; } -int8_t BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb) +int BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb) { - if (lsb_msb == NULL) { - return -1; - } - uint8_t otp_cmd = OTP_DIR_READ | (addr & OTP_WORD_MSK); - RegisterWrite(Register::OTP_CMD, otp_cmd); + int ret = RegisterWrite(Register::OTP_CMD, otp_cmd); uint8_t otp_status = 0; - do { - px4_usleep(300); - otp_status = RegisterRead(Register::OTP_STATUS); - } while (!(otp_status & 0x01)); + 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, lsb; + ret = RegisterRead(Register::OTP_DATA_MSB, &msb); + + if (ret == PX4_OK) { + ret = RegisterRead(Register::OTP_DATA_LSB, &lsb); + *lsb_msb = ((msb << 8) | lsb) & 0xffff; + } + } - uint8_t msb = RegisterRead(Register::OTP_DATA_MSB); - uint8_t lsb = RegisterRead(Register::OTP_DATA_LSB); - *lsb_msb = ((msb << 8) | lsb) & 0xffff; - return 1; + return ret; } -void BMM350::UpdateMagOffsets() +int BMM350::UpdateMagOffsets() { PX4_DEBUG("DUMPING OTP"); - uint16_t otp_word = 0; uint16_t otp_data[32] = {0}; for (int idx = 0; idx < 32; idx++) { - ReadOTPWord(idx, &otp_word); - otp_data[idx] = otp_word; + 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"); @@ -656,9 +718,10 @@ void BMM350::UpdateMagOffsets() _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; } -uint8_t BMM350::RegisterRead(Register reg) +int BMM350::RegisterRead(Register reg, uint8_t *value) { const uint8_t cmd = static_cast(reg); uint8_t buffer[3] = {0}; @@ -666,13 +729,15 @@ uint8_t BMM350::RegisterRead(Register reg) if (ret != PX4_OK) { PX4_DEBUG("register read 0x%02hhX failed, ret = %d", cmd, ret); - return -1; + + } else { + *value = buffer[2]; } - return buffer[2]; + return ret; } -void BMM350::RegisterWrite(Register reg, uint8_t value) +int BMM350::RegisterWrite(Register reg, uint8_t value) { uint8_t buffer[2] {(uint8_t)reg, value}; int ret = transfer(buffer, sizeof(buffer), nullptr, 0); @@ -680,4 +745,6 @@ void BMM350::RegisterWrite(Register reg, uint8_t value) 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 index acb0308859f7..4ba7f535dbd9 100644 --- a/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.hpp @@ -131,17 +131,17 @@ class BMM350 : public device::I2C, public I2CSPIDriver, public ModulePar int probe() override; bool Reset(); - bool Configure(); + int Configure(); - uint8_t RegisterRead(Register reg); - void RegisterWrite(Register reg, uint8_t value); + int RegisterRead(Register reg, uint8_t *value); + int RegisterWrite(Register reg, uint8_t value); int8_t CompensateAxisAndTemp(); - int8_t ReadOutRawData(float *out_data); - int8_t ReadOTPWord(uint8_t addr, uint16_t *lsb_msb); + int ReadOutRawData(float *out_data); + int ReadOTPWord(uint8_t addr, uint16_t *lsb_msb); int32_t FixSign(uint32_t inval, int8_t num_bits); - void UpdateMagOffsets(); + int UpdateMagOffsets(); void ParametersUpdate(bool force = false); void UpdateMagParams(); uint8_t GetODR(int value); @@ -159,19 +159,48 @@ class BMM350 : public device::I2C, public I2CSPIDriver, public ModulePar 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, diff --git a/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp index 7bcee6820c35..dc391f8824aa 100644 --- a/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp +++ b/src/drivers/magnetometer/bosch/bmm350/Bosch_BMM350_registers.hpp @@ -44,6 +44,10 @@ 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; @@ -63,8 +67,8 @@ enum class Register : uint8_t { DATAX_XLSB = 0x31, OTP_CMD = 0x50, - OTP_DATA_LSB = 0x52, - OTP_DATA_MSB = 0x53, + OTP_DATA_MSB = 0x52, + OTP_DATA_LSB = 0x53, OTP_STATUS = 0x55, TMR_SELF_TEST_USER = 0x60, @@ -89,7 +93,8 @@ enum PMU_CONTROL_CMD : uint8_t { PMU_CMD_NM_TC = 0x09 }; -static inline uint8_t PMU_CMD_STATUS_0_RES(uint8_t val){ +static inline uint8_t PMU_CMD_STATUS_0_RES(uint8_t val) +{ return (val >> 5) & 0x7; }; @@ -101,6 +106,18 @@ enum PMU_STATUS_0_BIT : uint8_t { 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, @@ -120,6 +137,12 @@ enum AVG_CONTROL_CMD : uint8_t { 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, From 15471cfe54f4ef72285380badf45541cffaebd3b Mon Sep 17 00:00:00 2001 From: Viliuks Date: Thu, 11 Jul 2024 15:56:57 +0300 Subject: [PATCH 5/8] BMM350 Update default parameters to 50Hz --- src/drivers/magnetometer/bosch/bmm350/BMM350.cpp | 6 +++++- src/drivers/magnetometer/bosch/bmm350/module.yaml | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp index a1ea0a463865..dbd5c714c6c1 100644 --- a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -223,7 +223,9 @@ void BMM350::RunImpl() if (ret == PX4_OK && (status_0 & PWR_NORMAL) != 0) { ret = PX4_ERROR; } + ret = UpdateMagOffsets(); + if (ret == PX4_OK) { PX4_DEBUG("Going to FGR"); _state = STATE::FGR; @@ -654,12 +656,14 @@ int BMM350::UpdateMagOffsets() 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){ + 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"); diff --git a/src/drivers/magnetometer/bosch/bmm350/module.yaml b/src/drivers/magnetometer/bosch/bmm350/module.yaml index b7f4dbd01e6c..33dcbe3bfa9c 100644 --- a/src/drivers/magnetometer/bosch/bmm350/module.yaml +++ b/src/drivers/magnetometer/bosch/bmm350/module.yaml @@ -19,7 +19,7 @@ parameters: 7: 3.125 Hz 8: 1.5625 Hz reboot_required: true - default: [1] + default: [3] BMM350_AVG: description: short: BMM350 data averaging From 398a1399aeaf76ae596ad8c6f57d47129c2b70cc Mon Sep 17 00:00:00 2001 From: Viliuks Date: Fri, 9 Aug 2024 14:43:33 +0300 Subject: [PATCH 6/8] Update OTP Word LSB check --- src/drivers/magnetometer/bosch/bmm350/BMM350.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) mode change 100644 => 100755 src/drivers/magnetometer/bosch/bmm350/BMM350.cpp diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp old mode 100644 new mode 100755 index dbd5c714c6c1..2b8c792d40f9 --- a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -635,12 +635,14 @@ int BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb) } } while (!(otp_status & 0x01)); - uint8_t msb, lsb; + uint8_t msb = 0, lsb = 0; ret = RegisterRead(Register::OTP_DATA_MSB, &msb); if (ret == PX4_OK) { ret = RegisterRead(Register::OTP_DATA_LSB, &lsb); - *lsb_msb = ((msb << 8) | lsb) & 0xffff; + if(ret == PX4_OK){ + *lsb_msb = ((msb << 8) | lsb) & 0xffff; + } } } From 59da25e36e05911802a2793b2a06eb1a255918c1 Mon Sep 17 00:00:00 2001 From: Viliuks Date: Fri, 9 Aug 2024 15:13:25 +0300 Subject: [PATCH 7/8] BMM350 fix styles and formatting --- src/drivers/magnetometer/bosch/bmm350/BMM350.cpp | 3 ++- src/drivers/magnetometer/bosch/bmm350/module.yaml | 1 - 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp index 2b8c792d40f9..3e062c15e99f 100755 --- a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -640,7 +640,8 @@ int BMM350::ReadOTPWord(uint8_t addr, uint16_t *lsb_msb) if (ret == PX4_OK) { ret = RegisterRead(Register::OTP_DATA_LSB, &lsb); - if(ret == PX4_OK){ + + if (ret == PX4_OK) { *lsb_msb = ((msb << 8) | lsb) & 0xffff; } } diff --git a/src/drivers/magnetometer/bosch/bmm350/module.yaml b/src/drivers/magnetometer/bosch/bmm350/module.yaml index 33dcbe3bfa9c..140c5c321a8a 100644 --- a/src/drivers/magnetometer/bosch/bmm350/module.yaml +++ b/src/drivers/magnetometer/bosch/bmm350/module.yaml @@ -42,4 +42,3 @@ parameters: min: 0 max: 7 default: [7] - From cbbd986df65917672f1db9d90b16c3ccd243770d Mon Sep 17 00:00:00 2001 From: Viliuks Date: Wed, 14 Aug 2024 15:45:53 +0300 Subject: [PATCH 8/8] BMM350 update error checks --- .../magnetometer/bosch/bmm350/BMM350.cpp | 74 +++++++++++-------- 1 file changed, 45 insertions(+), 29 deletions(-) diff --git a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp index 3e062c15e99f..e4e8e19f9c5e 100755 --- a/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp +++ b/src/drivers/magnetometer/bosch/bmm350/BMM350.cpp @@ -199,6 +199,7 @@ int BMM350::probe() void BMM350::RunImpl() { const hrt_abstime now = hrt_absolute_time(); + int ret = PX4_OK; ParametersUpdate(); switch (_state) { @@ -214,9 +215,10 @@ void BMM350::RunImpl() break; case STATE::WAIT_FOR_RESET: { - int ret = probe(); + ret = probe(); if (ret == PX4_OK) { + _state = STATE::RESET; uint8_t status_0; ret = RegisterRead(Register::PMU_STATUS_0, &status_0); @@ -224,18 +226,18 @@ void BMM350::RunImpl() ret = PX4_ERROR; } - ret = UpdateMagOffsets(); + if (ret == PX4_OK) { + ret = UpdateMagOffsets(); + } if (ret == PX4_OK) { PX4_DEBUG("Going to FGR"); _state = STATE::FGR; - ScheduleDelayed(10_ms); - } else { - _state = STATE::RESET; - ScheduleDelayed(10_ms); } + ScheduleDelayed(10_ms); + } else { if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); @@ -251,19 +253,22 @@ void BMM350::RunImpl() break; case STATE::FGR: { - int ret; + _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); - ret = RegisterWrite(Register::PMU_CMD_AXIS_EN, EN_XYZ); - ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FGR); + + 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; - - } else { - _state = STATE::RESET; } ScheduleDelayed(30_ms); @@ -273,7 +278,7 @@ void BMM350::RunImpl() case STATE::BR: { uint8_t status_0; _state = STATE::RESET; - int ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + 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); @@ -291,7 +296,7 @@ void BMM350::RunImpl() case STATE::AFTER_RESET: { uint8_t status_0; _state = STATE::RESET; - int ret = RegisterRead(Register::PMU_STATUS_0, &status_0); + 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; @@ -304,7 +309,7 @@ void BMM350::RunImpl() break; case STATE::MEASURE_FORCED: { - int ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); + ret = RegisterWrite(Register::PMU_CMD, PMU_CMD_FAST_FM); if (ret == PX4_OK) { _state = STATE::SELF_TEST_CHECK; @@ -325,19 +330,23 @@ void BMM350::RunImpl() 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)); - RegisterWrite(Register::TMR_SELF_TEST_USER, SELF_TEST_POS_X); - _self_test_state = SELF_TEST_STATE::POS_X; - _state = STATE::MEASURE_FORCED; + + 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); + 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; } @@ -345,8 +354,8 @@ void BMM350::RunImpl() 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); + 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; } @@ -354,8 +363,8 @@ void BMM350::RunImpl() 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); + 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; } @@ -363,8 +372,8 @@ void BMM350::RunImpl() 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); + 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; } @@ -406,14 +415,13 @@ void BMM350::RunImpl() case STATE::READ: { // -- start get_compensated_mag_xyz_temp_data - int8_t res = 0; 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; - res = ReadOutRawData(out_data); + ret = ReadOutRawData(out_data); - if (res == PX4_OK) { + 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; @@ -483,6 +491,7 @@ int BMM350::Configure() if (ret != PX4_OK) { PX4_DEBUG("Couldn't set pad drive, defaults to 7"); + return ret; } // Set PMU data aggregation @@ -506,12 +515,14 @@ int BMM350::Configure() 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; @@ -519,12 +530,14 @@ int BMM350::Configure() 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 @@ -534,18 +547,21 @@ int BMM350::Configure() 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