From 41b0f83c0cc34a9c7ed32aa5dabf9dafe70094f6 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 6 Feb 2021 12:50:23 -0500 Subject: [PATCH] drivers/magnetometer: Bosch BMM150 rewrite - same pattern as other new drivers (state machine, checked register mechanism, no sleeps, etc) - self test has been rolled into regular startup sequence - still I2C only, but will be expanded in the future - configure to high accuracy preset --- boards/uvify/core/default.cmake | 2 +- src/drivers/magnetometer/CMakeLists.txt | 2 +- src/drivers/magnetometer/bmm150/bmm150.cpp | 719 ------------------ src/drivers/magnetometer/bmm150/bmm150.hpp | 338 -------- src/drivers/magnetometer/bosch/CMakeLists.txt | 34 + .../magnetometer/bosch/bmm150/BMM150.cpp | 468 ++++++++++++ .../magnetometer/bosch/bmm150/BMM150.hpp | 137 ++++ .../bosch/bmm150/Bosch_BMM150_registers.hpp | 125 +++ .../{ => bosch}/bmm150/CMakeLists.txt | 10 +- .../magnetometer/bosch/bmm150/bmm150_main.cpp | 106 +++ 10 files changed, 879 insertions(+), 1062 deletions(-) delete mode 100644 src/drivers/magnetometer/bmm150/bmm150.cpp delete mode 100644 src/drivers/magnetometer/bmm150/bmm150.hpp create mode 100644 src/drivers/magnetometer/bosch/CMakeLists.txt create mode 100644 src/drivers/magnetometer/bosch/bmm150/BMM150.cpp create mode 100644 src/drivers/magnetometer/bosch/bmm150/BMM150.hpp create mode 100644 src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp rename src/drivers/magnetometer/{ => bosch}/bmm150/CMakeLists.txt (90%) create mode 100644 src/drivers/magnetometer/bosch/bmm150/bmm150_main.cpp diff --git a/boards/uvify/core/default.cmake b/boards/uvify/core/default.cmake index 4755c6d31e..4c7663d9b1 100644 --- a/boards/uvify/core/default.cmake +++ b/boards/uvify/core/default.cmake @@ -28,7 +28,7 @@ px4_add_board( imu/invensense/mpu9250 irlock lights/rgbled_ncp5623c - magnetometer/bmm150 + magnetometer/bosch/bmm150 magnetometer/lis3mdl magnetometer/isentek/ist8310 optical_flow # all available optical flow drivers diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index 8fd6620994..84bbbe8924 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ add_subdirectory(akm) -add_subdirectory(bmm150) +add_subdirectory(bosch) add_subdirectory(hmc5883) add_subdirectory(qmc5883l) add_subdirectory(isentek) diff --git a/src/drivers/magnetometer/bmm150/bmm150.cpp b/src/drivers/magnetometer/bmm150/bmm150.cpp deleted file mode 100644 index e2c4acd3d4..0000000000 --- a/src/drivers/magnetometer/bmm150/bmm150.cpp +++ /dev/null @@ -1,719 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 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 bmm150.cpp - * Driver for the Bosch BMM 150 MEMS magnetometer connected via I2C. - */ - -#include "bmm150.hpp" -#include -#include - -BMM150::BMM150(I2CSPIBusOption bus_option, const int bus, int bus_frequency, enum Rotation rotation) : - I2C(DRV_MAG_DEVTYPE_BMM150, MODULE_NAME, bus, BMM150_SLAVE_ADDRESS, bus_frequency), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), - _px4_mag(get_device_id(), rotation), - _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), - _bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad transfers")), - _good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good transfers")), - _measure_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": measure")), - _comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms errors")), - _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": duplicates")) -{ - _px4_mag.set_external(external()); - - // default range scale from uT to gauss - _px4_mag.set_scale(0.01f); -} - -BMM150::~BMM150() -{ - /* delete the perf counter */ - perf_free(_sample_perf); - perf_free(_bad_transfers); - perf_free(_good_transfers); - perf_free(_measure_perf); - perf_free(_comms_errors); - perf_free(_duplicates); -} - -int BMM150::init() -{ - int ret = I2C::init(); - - /* if probe/setup failed, bail now */ - if (ret != OK) { - DEVICE_DEBUG("I2C setup failed"); - return ret; - } - - /* Bring the device to sleep mode */ - modify_reg(BMM150_POWER_CTRL_REG, 1, 1); - - px4_usleep(10000); - - /* check id*/ - if (read_reg(BMM150_CHIP_ID_REG) != BMM150_CHIP_ID) { - DEVICE_DEBUG("id of magnetometer is not: 0x%02x", BMM150_CHIP_ID); - return -EIO; - } - - if (reset() != OK) { - goto out; - } - - init_trim_registers(); - - if (measure()) { - return -EIO; - } - - px4_usleep(10000); - - if (collect()) { - return -EIO; - } - - _call_interval = 1000000 / BMM150_MAX_DATA_RATE; - start(); - -out: - return ret; -} - -int BMM150::probe() -{ - /* During I2C Initialization, sensor is in suspend mode - * and chip Id will return 0x00, hence returning OK. After - * I2C initialization, sensor is brought to sleep mode - * and Chip Id is verified. In sleep mode, we can read - * chip id. */ - - /* @Note: Please read BMM150 Datasheet for more details */ - return OK; -} - -void BMM150::start() -{ - /* reset the report ring and state machine */ - _collect_phase = false; - - /* schedule a cycle to start things */ - ScheduleNow(); -} - -void BMM150::RunImpl() -{ - if (_collect_phase) { - collect(); - unsigned wait_gap = _call_interval - BMM150_CONVERSION_INTERVAL; - - if (wait_gap != 0) { - // need to wait some time before new measurement - ScheduleDelayed(wait_gap); - - return; - } - } - - measure(); - - /* schedule a fresh cycle call when the measurement is done */ - ScheduleDelayed(BMM150_CONVERSION_INTERVAL); -} - -int BMM150::measure() -{ - _collect_phase = true; - - perf_begin(_measure_perf); - - /* start measure */ - int ret = set_power_mode(BMM150_FORCED_MODE); - - if (ret != OK) { - perf_count(_comms_errors); - perf_cancel(_measure_perf); - return -EIO; - } - - perf_end(_measure_perf); - - return OK; -} - -int BMM150::collect() -{ - _collect_phase = false; - - uint8_t mag_data[8]; - uint16_t resistance, lsb, msb, msblsb; - - int16_t x_raw{0}; - int16_t y_raw{0}; - int16_t z_raw{0}; - - float x{0.f}; - float y{0.f}; - float z{0.f}; - - /* start collecting data */ - perf_begin(_sample_perf); - - /* Read Magnetometer data*/ - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - if (OK != get_data(BMM150_DATA_X_LSB_REG, mag_data, sizeof(mag_data))) { - return -EIO; - } - - /* Array holding the mag XYZ and R data - mag_data[0] - X LSB - mag_data[1] - X MSB - mag_data[2] - Y LSB - mag_data[3] - Y MSB - mag_data[4] - Z LSB - mag_data[5] - Z MSB - mag_data[6] - R LSB - mag_data[7] - R MSB - */ - - /* Extract X axis data */ - lsb = ((mag_data[0] & 0xF8)); - msb = ((uint16_t)(mag_data[1]) << 8); - msblsb = (msb | lsb); - x_raw = (int16_t)msblsb / 8; - - - /* Extract Y axis data */ - lsb = ((mag_data[2] & 0xF8)); - msb = (((uint16_t)(mag_data[3])) << 8); - msblsb = (msb | lsb); - y_raw = (int16_t)msblsb / 8; - - /* Extract Z axis data */ - lsb = ((mag_data[4] & 0xFE)); - msb = (((uint16_t)(mag_data[5])) << 8); - msblsb = (msb | lsb); - z_raw = (int16_t)msblsb / 2; - - /* Extract Resistance data */ - lsb = ((mag_data[6] & 0xFC)); - msb = (((uint16_t)mag_data[7]) << 8); - msblsb = (msb | lsb); - resistance = (uint16_t)msblsb / 4; - - /* Check whether data is new or old */ - if (!(mag_data[6] & 0x01)) { - perf_end(_sample_perf); - perf_count(_duplicates); - return -EIO; - } - - if (x_raw == 0 && - y_raw == 0 && - z_raw == 0 && - resistance == 0) { - - // all zero data - probably a I2C bus error - perf_count(_comms_errors); - perf_count(_bad_transfers); - perf_end(_sample_perf); - return -EIO; - } - - perf_count(_good_transfers); - - /* Compensation for X axis */ - if (x_raw != BMM150_FLIP_OVERFLOW_ADCVAL) { - /* no overflow */ - if ((resistance != 0) && (_dig_xyz1 != 0)) { - /* this is not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/master/bmm150.c */ - x = ((_dig_xyz1 * 16384.0f / resistance) - 16384.0f); - x = (((x_raw * ((((_dig_xy2 * ((float)(x * x / 268435456.0f)) + x * _dig_xy1 / - 16384.0f)) + 256.0f) * (_dig_x2 + 160.0f))) / 8192.0f) + (_dig_x1 * 8.0f)) / 16.0f; - - } else { - x = BMM150_OVERFLOW_OUTPUT_FLOAT; - } - - } else { - x = BMM150_OVERFLOW_OUTPUT_FLOAT; - } - - /* Compensation for Y axis */ - if (y_raw != BMM150_FLIP_OVERFLOW_ADCVAL) { - /* no overflow */ - if ((resistance != 0) && (_dig_xyz1 != 0)) { - - y = ((((float)_dig_xyz1) * 16384.0f / resistance) - 16384.0f); - y = (((y_raw * ((((_dig_xy2 * (y * y / 268435456.0f) + y * _dig_xy1 / 16384.0f)) + - 256.0f) * (_dig_y2 + 160.0f))) / 8192.0f) + (_dig_y1 * 8.0f)) / 16.0f; - - - } else { - y = BMM150_OVERFLOW_OUTPUT_FLOAT; - } - - } else { - /* overflow, set output to 0.0f */ - y = BMM150_OVERFLOW_OUTPUT_FLOAT; - } - - - /* Compensation for Z axis */ - if (z_raw != BMM150_HALL_OVERFLOW_ADCVAL) { - /* no overflow */ - if ((_dig_z2 != 0) && (_dig_z1 != 0) && (_dig_xyz1 != 0) && (resistance != 0)) { - z = ((((z_raw - _dig_z4) * 131072.0f) - (_dig_z3 * (resistance - _dig_xyz1))) / (( - _dig_z2 + _dig_z1 * resistance / 32768.0f) * 4.0f)) / 16.0f; - } - - } else { - /* overflow, set output to 0.0f */ - z = BMM150_OVERFLOW_OUTPUT_FLOAT; - } - - // report the error count as the number of bad transfers. - // This allows the higher level code to decide if it - // should use this sensor based on whether it has had failures - _px4_mag.set_error_count(perf_event_count(_bad_transfers)); - - _px4_mag.update(timestamp_sample, x, y, z); - - _last_raw_x = x_raw; - _last_raw_y = y_raw; - _last_raw_z = z_raw; - _last_resistance = resistance; - - perf_end(_sample_perf); - return OK; -} - -int BMM150::reset() -{ - int ret = OK; - - /* Soft-reset */ - modify_reg(BMM150_POWER_CTRL_REG, BMM150_SOFT_RESET_MASK, BMM150_SOFT_RESET_VALUE); - - px4_usleep(5000); - - /* Enable Magnetometer in normal mode */ - ret += set_power_mode(BMM150_DEFAULT_POWER_MODE); - - px4_usleep(1000); - - /* Set the data rate to default */ - ret += set_data_rate(BMM150_DEFAULT_ODR); - - /* set the preset mode as regular*/ - ret += set_presetmode(BMM150_PRESETMODE_REGULAR); - - return OK; -} - -uint8_t BMM150::read_reg(uint8_t reg) -{ - const uint8_t cmd = reg; - uint8_t ret{0}; - transfer(&cmd, 1, &ret, 1); - return ret; -} - -int BMM150::write_reg(uint8_t reg, uint8_t value) -{ - const uint8_t cmd[2] = { reg, value}; - return transfer(cmd, 2, nullptr, 0); -} - -int BMM150::get_data(uint8_t reg, uint8_t *data, unsigned len) -{ - const uint8_t cmd = reg; - return transfer(&cmd, 1, data, len); -} - -void BMM150::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); -} - -int BMM150::set_power_mode(uint8_t power_mode) -{ - uint8_t setbits = 0; - uint8_t clearbits = BMM150_POWER_MASK; - - switch (power_mode) { - case BMM150_NORMAL_MODE: - _power = 0; - break; - - case BMM150_FORCED_MODE: - _power = 1; - break; - - case BMM150_SLEEP_MODE: - _power = 3; - break; - - default: - return -EINVAL; - } - - setbits |= power_mode; - modify_reg(BMM150_CTRL_REG, clearbits, setbits); - - return OK; -} - -int BMM150::set_data_rate(uint8_t data_rate) -{ - uint8_t setbits = 0; - uint8_t clearbits = BMM150_OUTPUT_DATA_RATE_MASK; - - switch (data_rate) { - case BMM150_DATA_RATE_10HZ: - _output_data_rate = 10; - break; - - case BMM150_DATA_RATE_02HZ: - _output_data_rate = 2; - break; - - case BMM150_DATA_RATE_06HZ: - _output_data_rate = 6; - break; - - case BMM150_DATA_RATE_08HZ: - _output_data_rate = 8; - break; - - case BMM150_DATA_RATE_15HZ: - _output_data_rate = 15; - break; - - case BMM150_DATA_RATE_20HZ: - _output_data_rate = 20; - break; - - case BMM150_DATA_RATE_25HZ: - _output_data_rate = 25; - break; - - case BMM150_DATA_RATE_30HZ: - _output_data_rate = 30; - break; - - default: - return -EINVAL; - } - - setbits |= data_rate; - modify_reg(BMM150_CTRL_REG, clearbits, setbits); - - return OK; -} - -int BMM150::init_trim_registers() -{ - int ret = OK; - uint8_t data[2] = {0}; - uint16_t msb, lsb, msblsb; - - _dig_x1 = read_reg(BMM150_DIG_X1); - _dig_y1 = read_reg(BMM150_DIG_Y1); - _dig_x2 = read_reg(BMM150_DIG_X2); - _dig_y2 = read_reg(BMM150_DIG_Y2); - _dig_xy1 = read_reg(BMM150_DIG_XY1); - _dig_xy2 = read_reg(BMM150_DIG_XY2); - - ret += get_data(BMM150_DIG_Z1_LSB, data, 2); - lsb = data[0]; - msb = ((uint16_t)data[1]) << 8; - msblsb = (msb | lsb); - _dig_z1 = (uint16_t)msblsb; - - ret += get_data(BMM150_DIG_Z2_LSB, data, 2); - lsb = data[0]; - msb = ((uint16_t)data[1]) << 8; - msblsb = (msb | lsb); - _dig_z2 = (int16_t)msblsb; - - ret += get_data(BMM150_DIG_Z3_LSB, data, 2); - lsb = data[0]; - msb = ((uint16_t)data[1]) << 8; - msblsb = (msb | lsb); - _dig_z3 = (int16_t)msblsb; - - ret += get_data(BMM150_DIG_Z4_LSB, data, 2); - lsb = data[0]; - msb = ((uint16_t)data[1]) << 8; - msblsb = (msb | lsb); - _dig_z4 = (int16_t)msblsb; - - ret += get_data(BMM150_DIG_XYZ1_LSB, data, 2); - lsb = data[0]; - msb = ((uint16_t)(data[1] & 0x7F) << 8); - msblsb = (msb | lsb); - _dig_xyz1 = (uint16_t)msblsb; - - return ret; -} - -int BMM150::set_rep_xy(uint8_t rep_xy) -{ - uint8_t cmd[2] = {BMM150_XY_REP_CTRL_REG, rep_xy}; - return transfer((const uint8_t *)cmd, sizeof(cmd), nullptr, 0); -} - -int BMM150::set_rep_z(uint8_t rep_z) -{ - uint8_t cmd[2] = {BMM150_Z_REP_CTRL_REG, rep_z}; - return transfer((const uint8_t *)cmd, sizeof(cmd), nullptr, 0); -} - -int BMM150::set_presetmode(uint8_t presetmode) -{ - int ret = OK; - uint8_t data_rate, rep_xy, rep_z; - - if (presetmode == 1) { - data_rate = BMM150_LOWPOWER_DR; - rep_xy = BMM150_LOWPOWER_REPXY; - rep_z = BMM150_LOWPOWER_REPZ; - - } else if (presetmode == 2) { - data_rate = BMM150_REGULAR_DR; - rep_xy = BMM150_REGULAR_REPXY; - rep_z = BMM150_REGULAR_REPZ; - - } else if (presetmode == 3) { - data_rate = BMM150_HIGHACCURACY_DR; - rep_xy = BMM150_HIGHACCURACY_REPXY; - rep_z = BMM150_HIGHACCURACY_REPZ; - - } else if (presetmode == 4) { - data_rate = BMM150_ENHANCED_DR; - rep_xy = BMM150_ENHANCED_REPXY; - rep_z = BMM150_ENHANCED_REPZ; - - } else { - return -EINVAL; - } - - ret = set_data_rate(data_rate); - - ret += set_rep_xy(rep_xy); - ret += set_rep_z(rep_z); - - return ret; -} - -void BMM150::print_status() -{ - I2CSPIDriverBase::print_status(); - perf_print_counter(_sample_perf); - perf_print_counter(_bad_transfers); - perf_print_counter(_good_transfers); -} - -int BMM150::self_test() -{ - /* normal self-test */ - set_power_mode(BMM150_SLEEP_MODE); - - modify_reg(BMM150_CTRL_REG, 0, 1); - int i; - - for (i = 0; i < 100; ++i) { - px4_usleep(1000); - uint8_t ctrl_reg = read_reg(BMM150_CTRL_REG); - - if ((ctrl_reg & 1) == 0) { - break; - } - } - - if (i == 100) { - PX4_ERR("timeout"); - return -1; - } - - uint8_t results[3]; - results[0] = read_reg(BMM150_DATA_X_LSB_REG); - results[1] = read_reg(BMM150_DATA_Y_LSB_REG); - results[2] = read_reg(BMM150_DATA_Z_LSB_REG); - bool failed = false; - - for (i = 0; i < 3; ++i) { - if ((results[i] & 1) == 0) { - PX4_ERR("Self-test failed for axis %i", i); - failed = true; - } - } - - if (failed) { - return -1; - } - - PX4_INFO("Success"); - return reset(); -} - -void BMM150::print_registers() -{ - printf("BMM150 registers\n"); - - uint8_t reg = BMM150_CHIP_ID_REG; - uint8_t v = read_reg(reg); - printf("Chip Id: %02x:%02x\n", (unsigned)reg, (unsigned)v); - - reg = BMM150_INT_SETT_CTRL_REG; - v = read_reg(reg); - printf("Int sett Ctrl reg: %02x:%02x\n", (unsigned)reg, (unsigned)v); - - reg = BMM150_AXES_EN_CTRL_REG; - v = read_reg(reg); - printf("Axes En Ctrl reg: %02x:%02x\n", (unsigned)reg, (unsigned)v); - - printf("Trim data: %i %i, %i %i, %i %i %i %i, %i %i, %i\n", - _dig_x1, _dig_y1, _dig_x2, _dig_y2, _dig_z1, _dig_z2, _dig_z3, _dig_z4, _dig_xy1, _dig_xy2, _dig_xyz1); - - printf("Latest raw data: x=%i y=%i z=%i resistance=%i\n", - _last_raw_x, _last_raw_y, _last_raw_z, _last_resistance); -} - -void BMM150::print_usage() -{ - PRINT_MODULE_USAGE_NAME("bmm150", "driver"); - PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); - PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); - PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); - PRINT_MODULE_USAGE_COMMAND("reset"); - PRINT_MODULE_USAGE_COMMAND("regdump"); - PRINT_MODULE_USAGE_COMMAND("selftest"); - PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); -} - -void BMM150::custom_method(const BusCLIArguments &cli) -{ - switch (cli.custom1) { - case 0: reset(); - break; - - case 1: print_registers(); - break; - - case 2: self_test(); - break; - } -} - -I2CSPIDriverBase *BMM150::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) -{ - BMM150 *interface = new BMM150(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); - - if (interface == nullptr) { - PX4_ERR("alloc failed"); - return nullptr; - } - - if (interface->init() != OK) { - delete interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); - return nullptr; - } - - return interface; -} - -extern "C" __EXPORT int bmm150_main(int argc, char *argv[]) -{ - int ch; - using ThisDriver = BMM150; - BusCLIArguments cli{true, false}; - cli.default_i2c_frequency = BMM150_BUS_SPEED; - - 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_BMM150); - - 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); - } - - if (!strcmp(verb, "reset")) { - cli.custom1 = 0; - return ThisDriver::module_custom_method(cli, iterator); - } - - if (!strcmp(verb, "regdump")) { - cli.custom1 = 1; - return ThisDriver::module_custom_method(cli, iterator); - } - - if (!strcmp(verb, "selftest")) { - cli.custom1 = 2; - return ThisDriver::module_custom_method(cli, iterator); - } - - ThisDriver::print_usage(); - return -1; -} diff --git a/src/drivers/magnetometer/bmm150/bmm150.hpp b/src/drivers/magnetometer/bmm150/bmm150.hpp deleted file mode 100644 index 44aeb4ed29..0000000000 --- a/src/drivers/magnetometer/bmm150/bmm150.hpp +++ /dev/null @@ -1,338 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include - -#define BMM150_SLAVE_ADDRESS 0x10 - -#define BMM150_BUS_SPEED 1000*100 - -/* Chip Identification number */ -#define BMM150_CHIP_ID 0x32 - -/* Chip Id Register */ -#define BMM150_CHIP_ID_REG 0x40 - -/* Data Registers */ -#define BMM150_DATA_X_LSB_REG 0x42 -#define BMM150_DATA_X_MSB_REG 0x43 -#define BMM150_DATA_Y_LSB_REG 0x44 -#define BMM150_DATA_Y_MSB_REG 0x45 -#define BMM150_DATA_Z_LSB_REG 0x46 -#define BMM150_DATA_Z_MSB_REG 0x47 -#define BMM150_R_LSB 0x48 -#define BMM150_R_MSB 0x49 - -/* Interrupt status registers */ -#define BMM150_INT_STATUS_REG 0x4A - -/* Control Registers */ -#define BMM150_POWER_CTRL_REG 0x4B -#define BMM150_CTRL_REG 0x4C -#define BMM150_INT_SETT_CTRL_REG 0x4D -#define BMM150_AXES_EN_CTRL_REG 0x4E -#define BMM150_LOW_THRES_SETT_REG 0x4F -#define BMM150_HIGH_THERS_SETT_REG 0x50 - -/* Repetitions control registers */ -#define BMM150_XY_REP_CTRL_REG 0x51 -#define BMM150_Z_REP_CTRL_REG 0x52 - -/* Preset mode definitions */ -#define BMM150_PRESETMODE_LOWPOWER 1 -#define BMM150_PRESETMODE_REGULAR 2 -#define BMM150_PRESETMODE_HIGHACCURACY 3 -#define BMM150_PRESETMODE_ENHANCED 4 - - -/* Data rate value definitions */ -#define BMM150_DATA_RATE_10HZ 0x00 -#define BMM150_DATA_RATE_02HZ 0x08 -#define BMM150_DATA_RATE_06HZ 0x10 -#define BMM150_DATA_RATE_08HZ 0x18 -#define BMM150_DATA_RATE_15HZ 0x20 -#define BMM150_DATA_RATE_20HZ 0x28 -#define BMM150_DATA_RATE_25HZ 0x30 -#define BMM150_DATA_RATE_30HZ 0x38 - -/* Advance self-test settings Definitions */ -#define BMM150_ADV_ST_OFF 0x00 -#define BMM150_ADV_ST_NEG 0x80 -#define BMM150_ADV_ST_POS 0xC0 - - -/* Interrupt settings and axes enable bits definitions */ -#define BMM150_CHANNEL_X_ENABLE 0x08 -#define BMM150_CHANNEL_Y_ENABLE 0x10 - - -/*Overflow Definitions */ -/* compensated output value returned if sensor had overflow */ -#define BMM150_OVERFLOW_OUTPUT -32768 -#define BMM150_OVERFLOW_OUTPUT_S32 ((int32_t)(-2147483647-1)) -#define BMM150_OVERFLOW_OUTPUT_FLOAT 0.0f -#define BMM150_FLIP_OVERFLOW_ADCVAL -4096 -#define BMM150_HALL_OVERFLOW_ADCVAL -16384 - - -/* Preset modes - Repetitions-XY Rates */ -#define BMM150_LOWPOWER_REPXY 1 -#define BMM150_REGULAR_REPXY 10 -#define BMM150_HIGHACCURACY_REPXY 23 -#define BMM150_ENHANCED_REPXY 7 - -/* Preset modes - Repetitions-Z Rates */ -#define BMM150_LOWPOWER_REPZ 2 -#define BMM150_REGULAR_REPZ 30 -#define BMM150_HIGHACCURACY_REPZ 82 -#define BMM150_ENHANCED_REPZ 26 - -/* Preset modes - Data rates */ -#define BMM150_LOWPOWER_DR BMM150_DATA_RATE_30HZ -#define BMM150_REGULAR_DR BMM150_DATA_RATE_30HZ -#define BMM150_HIGHACCURACY_DR BMM150_DATA_RATE_20HZ -#define BMM150_ENHANCED_DR BMM150_DATA_RATE_10HZ - - -/* Power modes value definitions */ -#define BMM150_NORMAL_MODE 0x00 -#define BMM150_FORCED_MODE 0x02 -#define BMM150_SLEEP_MODE 0x06 - -/* Default power mode */ -#define BMM150_DEFAULT_POWER_MODE BMM150_NORMAL_MODE - -/* Default output data rate */ -#define BMM150_DEFAULT_ODR BMM150_DATA_RATE_30HZ - -/* Maximum output data rate */ -#define BMM150_MAX_DATA_RATE 30 - -/* Default BMM150_INT_SETT_CTRL_REG Value */ -#define BMM150_DEFAULT_INT_SETT 0x3F - -/* Trim Extended Registers */ -#define BMM150_DIG_X1 0x5D -#define BMM150_DIG_Y1 0x5E -#define BMM150_DIG_Z4_LSB 0x62 -#define BMM150_DIG_Z4_MSB 0x63 -#define BMM150_DIG_X2 0x64 -#define BMM150_DIG_Y2 0x65 -#define BMM150_DIG_Z2_LSB 0x68 -#define BMM150_DIG_Z2_MSB 0x69 -#define BMM150_DIG_Z1_LSB 0x6A -#define BMM150_DIG_Z1_MSB 0x6B -#define BMM150_DIG_XYZ1_LSB 0x6C -#define BMM150_DIG_XYZ1_MSB 0x6D -#define BMM150_DIG_Z3_LSB 0x6E -#define BMM150_DIG_Z3_MSB 0x6F -#define BMM150_DIG_XY2 0x70 -#define BMM150_DIG_XY1 0x71 - - -/* Mask definitions for power mode */ -#define BMM150_POWER_MASK 0x06 - -/* Mask definitions for data rate */ -#define BMM150_OUTPUT_DATA_RATE_MASK 0x38 - -#define BMM150_SOFT_RESET_VALUE 0x82 - -/* Mask definitions for Soft-Reset */ -#define BMM150_SOFT_RESET_MASK 0x82 - -/* This value is set based on Max output data rate value */ -#define BMM150_CONVERSION_INTERVAL (1000000 / BMM150_MAX_DATA_RATE) /* microseconds */ - -struct bmm150_data { - int16_t x; - int16_t y; - int16_t z; -}; - -class BMM150 : public device::I2C, public I2CSPIDriver -{ -public: - BMM150(I2CSPIBusOption bus_option, const int bus, int bus_frequency, enum Rotation rotation); - virtual ~BMM150(); - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - int init() override; - - void print_status() override; - - void print_registers(); - - void RunImpl(); - - void custom_method(const BusCLIArguments &cli) override; - -private: - int probe() override; - - PX4Magnetometer _px4_mag; - - /* altitude conversion calibration */ - unsigned _call_interval{0}; - - bool _collect_phase{false}; - - uint8_t _power{BMM150_DEFAULT_POWER_MODE}; - uint8_t _output_data_rate{BMM150_DEFAULT_POWER_MODE}; - - int8_t _dig_x1{0};/**< trim x1 data */ - int8_t _dig_y1{0};/**< trim y1 data */ - - int8_t _dig_x2{0};/**< trim x2 data */ - int8_t _dig_y2{0};/**< trim y2 data */ - - uint16_t _dig_z1{0};/**< trim z1 data */ - int16_t _dig_z2{0};/**< trim z2 data */ - int16_t _dig_z3{0};/**< trim z3 data */ - int16_t _dig_z4{0};/**< trim z4 data */ - - uint8_t _dig_xy1{0};/**< trim xy1 data */ - int8_t _dig_xy2{0};/**< trim xy2 data */ - - uint16_t _dig_xyz1{0};/**< trim xyz1 data */ - - int16_t _last_raw_x{0}; - int16_t _last_raw_y{0}; - int16_t _last_raw_z{0}; - uint16_t _last_resistance{0}; - - perf_counter_t _sample_perf; - perf_counter_t _bad_transfers; - perf_counter_t _good_transfers; - perf_counter_t _measure_perf; - perf_counter_t _comms_errors; - perf_counter_t _duplicates; - - int init_trim_registers(); - - /** - * Start automatic measurement. - */ - void start(); - - int measure(); //start measure - int collect(); //get results and publish - - /** - * Read the specified number of bytes from BMM150. - * - * @param reg The register to read. - * @param data Pointer to buffer for bytes read. - * @param len Number of bytes to read - * @return OK if the transfer was successful, -errno otherwise. - */ - int get_data(uint8_t reg, uint8_t *data, unsigned len); - - /** - * Resets the chip. - */ - int reset(); - - /** - * Measurement self test - * - * @return 0 on success, 1 on failure - */ - int self_test(); - - /** - * Read a register from the BMM150 - * - * @param reg The register to read. - * @return The value that was read. - */ - uint8_t read_reg(uint8_t reg); - - /** - * Write a register in the BMM150 - * - * @param reg The register to write. - * @param value The new value to write. - * @return OK if the transfer was successful, -errno otherwise. - */ - int write_reg(uint8_t reg, uint8_t value); - - /** - * Modify a register in the BMM150 - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /* - set the power mode of BMM150. - */ - int set_power_mode(uint8_t power); - - /* - Set the data rate of BMM150. - */ - int set_data_rate(uint8_t data_rate); - - /* - Set the XY-repetitions - */ - int set_rep_xy(uint8_t rep_xy); - - /* - Set the Z- repetitions number - */ - int set_rep_z(uint8_t rep_z); - - /* - Set the preset modes for BMM150 sensor.The preset mode setting is - depend on Data Rate, XY and Z repetitions - */ - int set_presetmode(uint8_t presetmode); - -}; diff --git a/src/drivers/magnetometer/bosch/CMakeLists.txt b/src/drivers/magnetometer/bosch/CMakeLists.txt new file mode 100644 index 0000000000..b814e7224f --- /dev/null +++ b/src/drivers/magnetometer/bosch/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# 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. +# +############################################################################ + +add_subdirectory(bmm150) diff --git a/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp b/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp new file mode 100644 index 0000000000..214b57ac0f --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm150/BMM150.cpp @@ -0,0 +1,468 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "BMM150.hpp" + +using namespace time_literals; + +BMM150::BMM150(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation) : + I2C(DRV_MAG_DEVTYPE_BMM150, MODULE_NAME, bus, I2C_ADDRESS_DEFAULT, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _px4_mag(get_device_id(), rotation) +{ + _px4_mag.set_external(external()); +} + +BMM150::~BMM150() +{ + perf_free(_reset_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_overflow_perf); +} + +int BMM150::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool BMM150::Reset() +{ + RegisterWrite(Register::POWER_CONTROL, 0); + _state = STATE::RESET; + ScheduleClear(); + ScheduleDelayed(1_ms); + return true; +} + +void BMM150::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_overflow_perf); +} + +int BMM150::probe() +{ + const uint8_t POWER_CONTROL = RegisterRead(Register::POWER_CONTROL); + const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID); + + PX4_DEBUG("POWER_CONTROL: 0x%02hhX, CHIP_ID: 0x%02hhX", POWER_CONTROL, CHIP_ID); + + // either power control bit is set and chip ID can be read, or both registers are 0x00 + if ((POWER_CONTROL & POWER_CONTROL_BIT::PowerControl) && (CHIP_ID == chip_identification_number)) { + return PX4_OK; + + } else if ((POWER_CONTROL == 0) && (CHIP_ID == 0)) { + return PX4_OK; + } + + return PX4_ERROR; +} + +float BMM150::compensate_x(int16_t mag_data_x, uint16_t data_rhall) +{ + float retval = 0; + + // Processing compensation equations + // not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1624-L1633 as of 2020-09-25 + float process_comp_x0 = (((float)_trim_data.dig_xyz1) * 16384.f / data_rhall); + retval = (process_comp_x0 - 16384.f); + float process_comp_x1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.f); + float process_comp_x2 = process_comp_x1 + retval * ((float)_trim_data.dig_xy1) / 16384.f; + float process_comp_x3 = ((float)_trim_data.dig_x2) + 160.f; + float process_comp_x4 = mag_data_x * ((process_comp_x2 + 256.f) * process_comp_x3); + retval = ((process_comp_x4 / 8192.f) + (((float)_trim_data.dig_x1) * 8.f)) / 16.f; + + return retval; +} + +float BMM150::compensate_y(int16_t mag_data_y, uint16_t data_rhall) +{ + float retval = 0; + + // Processing compensation equations + // not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1660-L1667 as of 2020-09-25 + float process_comp_y0 = ((float)_trim_data.dig_xyz1) * 16384.f / data_rhall; + retval = process_comp_y0 - 16384.f; + float process_comp_y1 = ((float)_trim_data.dig_xy2) * (retval * retval / 268435456.f); + float process_comp_y2 = process_comp_y1 + retval * ((float)_trim_data.dig_xy1) / 16384.f; + float process_comp_y3 = ((float)_trim_data.dig_y2) + 160.0f; + float process_comp_y4 = mag_data_y * (((process_comp_y2) + 256.f) * process_comp_y3); + retval = ((process_comp_y4 / 8192.f) + (((float)_trim_data.dig_y1) * 8.f)) / 16.f; + + return retval; +} + +float BMM150::compensate_z(int16_t mag_data_z, uint16_t data_rhall) +{ + float retval = 0; + + // Processing compensation equations + // not documented, but derived from https://github.com/BoschSensortec/BMM150-Sensor-API/blob/a20641f216057f0c54de115fe81b57368e119c01/bmm150.c#L1696-L1703 as of 2020-09-25 + float process_comp_z0 = ((float)mag_data_z) - ((float)_trim_data.dig_z4); + float process_comp_z1 = ((float)data_rhall) - ((float)_trim_data.dig_xyz1); + float process_comp_z2 = (((float)_trim_data.dig_z3) * process_comp_z1); + float process_comp_z3 = ((float)_trim_data.dig_z1) * ((float)data_rhall) / 32768.f; + float process_comp_z4 = ((float)_trim_data.dig_z2) + process_comp_z3; + float process_comp_z5 = (process_comp_z0 * 131072.f) - process_comp_z2; + retval = (process_comp_z5 / ((process_comp_z4) * 4.f)) / 16.f; + + return retval; +} + +static constexpr int16_t combine_xy_int13(const uint8_t msb, const uint8_t lsb) +{ + int16_t x = ((msb << 8) | lsb); + return x / 8; // arithmetic shift by 3 (13 bit signed integer) +} + +static constexpr int16_t combine_z_int15(const uint8_t msb, const uint8_t lsb) +{ + int16_t z = ((msb << 8) | lsb); + return z / 2; // arithmetic shift by 1 (15 bit signed integer) +} + +static constexpr uint16_t combine_rhall_uint14(const uint8_t msb, const uint8_t lsb) +{ + uint16_t rhall = ((msb << 8) | lsb); + return (rhall >> 2) & 0x3FFF; // 14 bit unsigned integer +} + +void BMM150::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // POWER_CONTROL: soft reset + RegisterWrite(Register::POWER_CONTROL, POWER_CONTROL_BIT::SoftReset | POWER_CONTROL_BIT::PowerControl); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + ScheduleDelayed(3_ms); // 3.0 ms start-up time from suspend to sleep + break; + + case STATE::WAIT_FOR_RESET: + + // Soft reset always brings the device into sleep mode (power off -> suspend -> sleep) + if ((RegisterRead(Register::CHIP_ID) == chip_identification_number) + && (RegisterRead(Register::POWER_CONTROL) == POWER_CONTROL_BIT::PowerControl) + && (RegisterRead(Register::OP_MODE) == OP_MODE_BIT::Opmode_Sleep)) { + + // if reset succeeded then start self test + RegisterSetBits(Register::OP_MODE, OP_MODE_BIT::Self_Test); + + _state = STATE::SELF_TEST_CHECK; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + 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: { + // After performing self test OpMode "Self test" bit is set to 0 + const bool opmode_self_test_cleared = ((RegisterRead(Register::OP_MODE) & OP_MODE_BIT::Self_Test) == 0); + + // When self-test is successful, the corresponding self-test result bits are set + // “X-Self-Test” register 0x42 bit0 + // “Y-Self-Test” register 0x44 bit0 + // “Z-Self-Test” register 0x46 bit0 + const bool x_success = RegisterRead(Register::DATAX_LSB) & Bit0; + const bool y_success = RegisterRead(Register::DATAY_LSB) & Bit0; + const bool z_success = RegisterRead(Register::DATAZ_LSB) & Bit0; + + if (opmode_self_test_cleared && (!x_success || !y_success || !z_success)) { + PX4_DEBUG("self test failed, resetting"); + perf_count(_self_test_failed_perf); + _state = STATE::RESET; + ScheduleDelayed(1000_ms); + + } else { + _state = STATE::READ_TRIM; + ScheduleDelayed(1_ms); + } + } + + break; + + case STATE::READ_TRIM: { + // Trim register value is read + uint8_t cmd = static_cast(Register::DIG_X1); + uint8_t trim_x1y1[2] {}; + + if (transfer(&cmd, 1, trim_x1y1, 2) == PX4_OK) { + cmd = static_cast(Register::DIG_Z4_LSB); + uint8_t trim_xyz_data[4] {}; + + if (transfer(&cmd, 1, trim_xyz_data, 4) == PX4_OK) { + cmd = static_cast(Register::DIG_Z2_LSB); + uint8_t trim_xy1xy2[10] {}; + + if (transfer(&cmd, 1, trim_xy1xy2, 10) == PX4_OK) { + _trim_data.dig_x1 = (int8_t)trim_x1y1[0]; + _trim_data.dig_y1 = (int8_t)trim_x1y1[1]; + + _trim_data.dig_x2 = (int8_t)trim_xyz_data[2]; + _trim_data.dig_y2 = (int8_t)trim_xyz_data[3]; + + uint16_t temp_msb; + temp_msb = ((uint16_t)trim_xy1xy2[3]) << 8; + _trim_data.dig_z1 = (uint16_t)(temp_msb | trim_xy1xy2[2]); + + temp_msb = ((uint16_t)trim_xy1xy2[1]) << 8; + _trim_data.dig_z2 = (int16_t)(temp_msb | trim_xy1xy2[0]); + + temp_msb = ((uint16_t)trim_xy1xy2[7]) << 8; + _trim_data.dig_z3 = (int16_t)(temp_msb | trim_xy1xy2[6]); + + temp_msb = ((uint16_t)trim_xyz_data[1]) << 8; + _trim_data.dig_z4 = (int16_t)(temp_msb | trim_xyz_data[0]); + + _trim_data.dig_xy1 = trim_xy1xy2[9]; + _trim_data.dig_xy2 = (int8_t)trim_xy1xy2[8]; + + temp_msb = ((uint16_t)(trim_xy1xy2[5] & 0x7F)) << 8; + _trim_data.dig_xyz1 = (uint16_t)(temp_msb | trim_xy1xy2[4]); + + if ((_trim_data.dig_xyz1 != 0) && (_trim_data.dig_z2 != 0) && (_trim_data.dig_z1 != 0)) { + _state = STATE::CONFIGURE; + ScheduleDelayed(1_ms); + return; + } + } + } + } + + // reset if reading trim failed + PX4_DEBUG("reading trim failed, resetting"); + perf_count(_bad_register_perf); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading every 50 ms (20 Hz) + _state = STATE::READ; + ScheduleOnInterval(50_ms, 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: { + struct TransferBuffer { + uint8_t DATAX_LSB; + uint8_t DATAX_MSB; + uint8_t DATAY_LSB; + uint8_t DATAY_MSB; + uint8_t DATAZ_LSB; + uint8_t DATAZ_MSB; + uint8_t RHALL_LSB; + uint8_t RHALL_MSB; + uint8_t STATUS; + } buffer{}; + + bool success = false; + // 0x42 to 0x4A with a burst read. + uint8_t cmd = static_cast(Register::DATAX_LSB); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + + int16_t x = combine_xy_int13(buffer.DATAX_MSB, buffer.DATAX_LSB); + int16_t y = combine_xy_int13(buffer.DATAY_MSB, buffer.DATAY_LSB); + int16_t z = combine_z_int15(buffer.DATAZ_MSB, buffer.DATAZ_LSB); + uint16_t rhall = combine_rhall_uint14(buffer.RHALL_MSB, buffer.RHALL_LSB); + + const bool data_ready = buffer.RHALL_LSB & Bit0; + + if (data_ready && (rhall != 0)) { + if ((buffer.STATUS & STATUS_BIT::Overflow) || + (x == OVERFLOW_XYAXES) || (y == OVERFLOW_XYAXES) || (z == OVERFLOW_ZAXIS)) { + // overflow ADC value, record error, but don't publish + perf_count(_overflow_perf); + + } else { + _px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)); + _px4_mag.update(now, compensate_x(x, rhall), compensate_y(y, rhall), compensate_z(z, rhall)); + + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + + } else { + perf_count(_bad_transfer_perf); + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + } + } + + break; + } +} + +bool BMM150::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + // microTesla -> Gauss + _px4_mag.set_scale(0.01f); + + return success; +} + +bool BMM150::RegisterCheck(const register_config_t ®_cfg) +{ + bool success = true; + + const uint8_t reg_value = RegisterRead(reg_cfg.reg); + + if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); + success = false; + } + + if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { + PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); + success = false; + } + + return success; +} + +uint8_t BMM150::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer{}; + int ret = transfer(&cmd, 1, &buffer, 1); + + if (ret != PX4_OK) { + PX4_DEBUG("register read 0x%02hhX failed, ret = %d", cmd, ret); + return -1; + } + + return buffer; +} + +void BMM150::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); + } +} + +void BMM150::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} diff --git a/src/drivers/magnetometer/bosch/bmm150/BMM150.hpp b/src/drivers/magnetometer/bosch/bmm150/BMM150.hpp new file mode 100644 index 0000000000..97ed6cf159 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm150/BMM150.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file BMM150.hpp + * + * Driver for the Bosch BMM150 connected via I2C. + * + */ + +#pragma once + +#include "Bosch_BMM150_registers.hpp" + +#include +#include +#include +#include +#include + +using namespace Bosch_BMM150; + +class BMM150 : public device::I2C, public I2CSPIDriver +{ +public: + BMM150(I2CSPIBusOption bus_option, int bus, int bus_frequency, enum Rotation rotation = ROTATION_NONE); + ~BMM150() override; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + + struct trim_registers { + int8_t dig_x1; // trim x1 data + int8_t dig_y1; // trim y1 data + int8_t dig_x2; // trim x2 data + int8_t dig_y2; // trim y2 data + uint16_t dig_z1; // trim z1 data + int16_t dig_z2; // trim z2 data + int16_t dig_z3; // trim z3 data + int16_t dig_z4; // trim z4 data + uint8_t dig_xy1; // trim xy1 data + int8_t dig_xy2; // trim xy2 data + uint16_t dig_xyz1; // trim xyz1 data + } _trim_data{}; + + // Sensor Configuration + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + bool Reset(); + bool Configure(); + bool RegisterCheck(const register_config_t ®_cfg); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }; + void RegisterSetBits(Register reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); }; + + // obtain the compensated magnetometer data in micro-tesla. + float compensate_x(int16_t mag_data_x, uint16_t data_rhall); + float compensate_y(int16_t mag_data_y, uint16_t data_rhall); + float compensate_z(int16_t mag_data_z, uint16_t data_rhall); + + PX4Magnetometer _px4_mag; + + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; + perf_counter_t _overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": overflow")}; + 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}; + int _failure_count{0}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + SELF_TEST_CHECK, + READ_TRIM, + CONFIGURE, + READ, + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{4}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::POWER_CONTROL, POWER_CONTROL_BIT::PowerControl, POWER_CONTROL_BIT::SoftReset }, + { Register::OP_MODE, OP_MODE_BIT::ODR_20Hz, OP_MODE_BIT::Opmode_Sleep | OP_MODE_BIT::Self_Test }, + { Register::REPXY, REPXY_BIT::XY_HA_SET, REPXY_BIT::XY_HA_CLEAR }, + { Register::REPZ, REPZ_BIT::Z_HA_SET, REPZ_BIT::Z_HA_CLEAR }, + }; +}; diff --git a/src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp b/src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp new file mode 100644 index 0000000000..a3e89fa837 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm150/Bosch_BMM150_registers.hpp @@ -0,0 +1,125 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * @file Bosch_BMM150_registers.hpp + * + * Bosch BMM150 registers. + * + */ + +#pragma once + +#include + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +namespace Bosch_BMM150 +{ +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x10; + +static constexpr uint8_t chip_identification_number = 0x32; + +static constexpr int16_t OVERFLOW_XYAXES = -4096; +static constexpr int16_t OVERFLOW_ZAXIS = -16384; + +enum class Register : uint8_t { + CHIP_ID = 0x40, // magnetometer chip identification number + + DATAX_LSB = 0x42, // 5-bit LSB of x-axis magnetic field data + DATAX_MSB = 0x43, // 8-bit MSB of x-axis magnetic field data + DATAY_LSB = 0x44, // 5-bit LSB of y-axis magnetic field data + DATAY_MSB = 0x45, // 8-bit MSB of y-axis magnetic field data + DATAZ_LSB = 0x46, // 7-bit LSB of z-axis magnetic field data + DATAZ_MSB = 0x47, // 8-bit MSB of z-axis magnetic field data + RHALL_LSB = 0x48, // 6-bit LSB of hall resistance + RHALL_MSB = 0x49, // 8-bit MSB of hall resistance + STATUS = 0x4A, // Status register + POWER_CONTROL = 0x4B, // power control, soft reset and interface SPI mode selection + OP_MODE = 0x4C, // operation mode, output data rate and self-test + + REPXY = 0x51, // number of repetitions for x/y-axis + REPZ = 0x52, // number of repetitions for z-axis + + DIG_X1 = 0x5D, + + DIG_Z4_LSB = 0x62, + + DIG_Z2_LSB = 0x68, +}; + +// POWER_CONTROL +enum POWER_CONTROL_BIT : uint8_t { + SoftReset = Bit7 | Bit1, // soft reset trigger bits. + PowerControl = Bit0, // When set to “0”, suspend mode is selected +}; + +// OP_MODE +enum OP_MODE_BIT : uint8_t { + // 5:3 Data rate control + ODR_20Hz = Bit5 | Bit3, // ODR 20 Hz + + // 2:1 Operation mode control + Opmode_Sleep = Bit2 | Bit1, // Sleep mode + Self_Test = Bit0, +}; + +// STATUS +enum STATUS_BIT : uint8_t { + Overflow = Bit6, // one or more axes exceeded maximum range of the device +}; + +// REPXY +enum REPXY_BIT : uint8_t { + // high accurary preset nXY = 47, REPXY = 0x17 = 0b0001'0111 + XY_HA_SET = Bit4 | Bit2 | Bit1 | Bit0, + XY_HA_CLEAR = Bit7 | Bit6 | Bit5 | Bit3, +}; + +// REPZ +enum REPZ_BIT : uint8_t { + // high accurary preset nZ = 83, REPZ = 0x52 = 0b0101'0010 + Z_HA_SET = Bit6 | Bit4 | Bit1, + Z_HA_CLEAR = Bit7 | Bit5 | Bit3 | Bit2 | Bit0, +}; + +} // namespace Bosch_BMM150 diff --git a/src/drivers/magnetometer/bmm150/CMakeLists.txt b/src/drivers/magnetometer/bosch/bmm150/CMakeLists.txt similarity index 90% rename from src/drivers/magnetometer/bmm150/CMakeLists.txt rename to src/drivers/magnetometer/bosch/bmm150/CMakeLists.txt index a75f046ea3..536d810f28 100644 --- a/src/drivers/magnetometer/bmm150/CMakeLists.txt +++ b/src/drivers/magnetometer/bosch/bmm150/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# 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 @@ -30,12 +30,16 @@ # POSSIBILITY OF SUCH DAMAGE. # ############################################################################ + px4_add_module( - MODULE drivers__bmm150 + MODULE drivers__magnetometer__bosch__bmm150 MAIN bmm150 COMPILE_FLAGS SRCS - bmm150.cpp + BMM150.cpp + BMM150.hpp + bmm150_main.cpp + Bosch_BMM150_registers.hpp DEPENDS drivers_magnetometer px4_work_queue diff --git a/src/drivers/magnetometer/bosch/bmm150/bmm150_main.cpp b/src/drivers/magnetometer/bosch/bmm150/bmm150_main.cpp new file mode 100644 index 0000000000..00050213b4 --- /dev/null +++ b/src/drivers/magnetometer/bosch/bmm150/bmm150_main.cpp @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +#include "BMM150.hpp" + +#include +#include + +I2CSPIDriverBase *BMM150::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + BMM150 *instance = new BMM150(iterator.configuredBusOption(), iterator.bus(), cli.bus_frequency, cli.rotation); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (instance->init() != PX4_OK) { + delete instance; + PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + return nullptr; + } + + return instance; +} + +void BMM150::print_usage() +{ + PRINT_MODULE_USAGE_NAME("bmm150", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int bmm150_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = BMM150; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + + 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_BMM150); + + 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; +}