diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 549b7313c1..fc6265ba4b 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -63,6 +63,7 @@ #define DRV_MAG_DEVTYPE_IST8308 0x0B #define DRV_MAG_DEVTYPE_LIS2MDL 0x0C +#define DRV_MAG_DEVTYPE_MMC5983MA 0x0D #define DRV_IMU_DEVTYPE_LSM303D 0x11 diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index 7255574717..6eb5bbe492 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -39,5 +39,6 @@ add_subdirectory(isentek) add_subdirectory(lis2mdl) add_subdirectory(lis3mdl) add_subdirectory(lsm303agr) +add_subdirectory(memsic) add_subdirectory(rm3100) add_subdirectory(vtrantech) diff --git a/src/drivers/magnetometer/Kconfig b/src/drivers/magnetometer/Kconfig index 1285915080..99bb90f54e 100644 --- a/src/drivers/magnetometer/Kconfig +++ b/src/drivers/magnetometer/Kconfig @@ -14,6 +14,7 @@ menu "Magnetometer" select DRIVERS_MAGNETOMETER_LSM303AGR select DRIVERS_MAGNETOMETER_RM3100 select DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L + select DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA ---help--- Enable default set of magnetometer drivers rsource "*/Kconfig" diff --git a/src/drivers/magnetometer/memsic/Kconfig b/src/drivers/magnetometer/memsic/Kconfig new file mode 100644 index 0000000000..22e5aa4175 --- /dev/null +++ b/src/drivers/magnetometer/memsic/Kconfig @@ -0,0 +1,3 @@ +menu "Memsic" + rsource "*/Kconfig" +endmenu diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/CMakeLists.txt b/src/drivers/magnetometer/memsic/mmc5983ma/CMakeLists.txt new file mode 100644 index 0000000000..3bba9619d7 --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 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. +# +############################################################################ +px4_add_module( + MODULE drivers__magnetometer__memsic__mmc5983ma + MAIN mmc5983ma + COMPILE_FLAGS + SRCS + mmc5983ma_i2c.cpp + mmc5983ma_main.cpp + mmc5983ma.cpp + DEPENDS + drivers_magnetometer + px4_work_queue + ) diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/Kconfig b/src/drivers/magnetometer/memsic/mmc5983ma/Kconfig new file mode 100644 index 0000000000..f3d1f31457 --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA + bool "mmc5983ma" + default n + ---help--- + Enable support for mmc5983ma diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.cpp b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.cpp new file mode 100644 index 0000000000..11ec0ea7de --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.cpp @@ -0,0 +1,215 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#include "mmc5983ma.h" + +using namespace time_literals; + +MMC5983MA::MMC5983MA(device::Device *interface, const I2CSPIDriverConfig &config) : + I2CSPIDriver(config), + _interface(interface), + _px4_mag(interface->get_device_id(), config.rotation), + _sample_count(perf_alloc(PC_COUNT, "mmc5983ma_read")), + _comms_errors(perf_alloc(PC_COUNT, "mmc5983ma_comms_errors")) +{} + +MMC5983MA::~MMC5983MA() +{ + perf_free(_sample_count); + perf_free(_comms_errors); + delete _interface; +} + +int MMC5983MA::init() +{ + // Start with a reset of the chip + write_register(MMC5983MA_ADDR_CTRL_REG2, MMC5983MA_CTRL_REG1_SW_RESET); + px4_usleep(20_ms); + + // Set measurement BW to 200HZ + write_register(MMC5983MA_ADDR_CTRL_REG1, MMC5983MA_CTRL_REG1_BW_200HZ); + + ScheduleNow(); + + return PX4_OK; +} + +void MMC5983MA::RunImpl() +{ + // The measure/collect loop uses the set/reset functionality of the chip to eliminate temperature related bias + // by reversing the polarity of the sensing element and taking the difference of the two measurements to eliminate + // the offset and then dividing by 2 to arrive at the true value of the field measurement. + // + // The measurement will contain not only the sensors response to the external magnetic field, H, but also the Offset. + // + // Output1 = +H + Offset + // Output2 = -H + Offset + // Measurment = (Output1 - Output2) / 2 + // + // Please refer to Page 18 of the datasheet + // https://www.memsic.com/Public/Uploads/uploadfile/files/20220119/MMC5983MADatasheetRevA.pdf + + switch (_state) { + case State::Measure: { + + uint8_t set_reset_flag = _sample_index == 0 ? MMC5983MA_CTRL_REG0_SET : MMC5983MA_CTRL_REG0_RESET; + write_register(MMC5983MA_ADDR_CTRL_REG0, MMC5983MA_CTRL_REG0_TM_M | set_reset_flag); + + _collect_retries = 0; + _state = State::Collect; + + // 200Hz BW is 4ms measurement time + ScheduleDelayed(5_ms); + return; + } + + case State::Collect: { + + uint8_t status = read_register(MMC5983MA_ADDR_STATUS_REG); + + if (status & MMC5983MA_STATUS_REG_MEAS_M_DONE) { + SensorData data = {}; + + if (read_register_block(&data) != PX4_OK) { + PX4_DEBUG("read failed"); + perf_count(_comms_errors); + _state = State::Measure; + _sample_index = 0; + ScheduleDelayed(100_ms); + return; + } + + // Measurement available + _measurements[_sample_index] = data; + _sample_index++; + + if (_sample_index > 1) { + publish_data(); + _sample_index = 0; + perf_count(_sample_count); + } + + _state = State::Measure; + + // Immediately schedule next measurement + ScheduleNow(); + return; + + } else { + PX4_DEBUG("not ready"); + perf_count(_comms_errors); + _collect_retries++; + _state = _collect_retries > 3 ? State::Measure : State::Collect; + ScheduleDelayed(5_ms); + return; + } + } + } // end switch/case +} + +void MMC5983MA::publish_data() +{ + uint32_t xraw_1 = (_measurements[0].xout0 << 10) | (_measurements[0].xout1 << 2) | (( + _measurements[0].xyzout2 & 0b11000000) >> 6); + uint32_t yraw_1 = (_measurements[0].yout0 << 10) | (_measurements[0].yout1 << 2) | (( + _measurements[0].xyzout2 & 0b00110000) >> 4); + uint32_t zraw_1 = (_measurements[0].zout0 << 10) | (_measurements[0].zout1 << 2) | (( + _measurements[0].xyzout2 & 0b00001100) >> 2); + + uint32_t xraw_2 = (_measurements[1].xout0 << 10) | (_measurements[1].xout1 << 2) | (( + _measurements[1].xyzout2 & 0b11000000) >> 6); + uint32_t yraw_2 = (_measurements[1].yout0 << 10) | (_measurements[1].yout1 << 2) | (( + _measurements[1].xyzout2 & 0b00110000) >> 4); + uint32_t zraw_2 = (_measurements[1].zout0 << 10) | (_measurements[1].zout1 << 2) | (( + _measurements[1].xyzout2 & 0b00001100) >> 2); + + // NOTE: Temperature conversions did not work + // float trawf = float(_measurements[0].tout + _measurements[1].tout) / 2.f; + // float temp_c = trawf * 0.8f - 75.f; + // _px4_mag.set_temperature(temp_c); + + // +/- 8 Gauss full scale range + // 18-bit mode scaling factor: 0.0625 mG/LSB + float x1 = -8.f + (float(xraw_1) * 0.0625f) / 1e3f; + float x2 = -8.f + (float(xraw_2) * 0.0625f) / 1e3f; + float y1 = -8.f + (float(yraw_1) * 0.0625f) / 1e3f; + float y2 = -8.f + (float(yraw_2) * 0.0625f) / 1e3f; + float z1 = -8.f + (float(zraw_1) * 0.0625f) / 1e3f; + float z2 = -8.f + (float(zraw_2) * 0.0625f) / 1e3f; + + // Remove the offset from the measurements (SET/RESET) + float x = (x1 - x2) / 2.f; + float y = -1.f * (y1 - y2) / 2.f; // Y axis is inverted to convert from LH to RH + float z = (z1 - z2) / 2.f; + + _px4_mag.update(hrt_absolute_time(), x, y, z); + _px4_mag.set_error_count(perf_event_count(_comms_errors)); +} + +uint8_t MMC5983MA::read_register_block(SensorData *data) +{ + uint8_t reg = MMC5983MA_ADDR_XOUT_0; + + if (_interface->read(reg, data, sizeof(SensorData)) != PX4_OK) { + perf_count(_comms_errors); + + return PX4_ERROR; + } + + return PX4_OK; +} + +uint8_t MMC5983MA::read_register(uint8_t reg) +{ + uint8_t value = 0; + + if (_interface->read(reg, &value, sizeof(value)) != PX4_OK) { + perf_count(_comms_errors); + } + + return value; +} + +void MMC5983MA::write_register(uint8_t reg, uint8_t value) +{ + if (_interface->write(reg, &value, sizeof(value)) != PX4_OK) { + perf_count(_comms_errors); + } +} + +void MMC5983MA::print_status() +{ + I2CSPIDriverBase::print_status(); + perf_print_counter(_sample_count); + perf_print_counter(_comms_errors); +} diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.h b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.h new file mode 100644 index 0000000000..afa526dd1a --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma.h @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include + +// MMC5983MA Registers +#define MMC5983MA_ADDR_XOUT_0 0x00 +#define MMC5983MA_ADDR_STATUS_REG 0x08 +#define MMC5983MA_ADDR_CTRL_REG0 0x09 +#define MMC5983MA_ADDR_CTRL_REG1 0x0A +#define MMC5983MA_ADDR_CTRL_REG2 0x0B +#define MMC5983MA_ADDR_PRODUCT_ID 0x2F +// MMC5983MA Definitions +#define MMC5983MA_PRODUCT_ID 0x30 +#define MMC5983MA_STATUS_REG_MEAS_M_DONE (1 << 0) +#define MMC5983MA_CTRL_REG0_TM_M (1 << 0) +#define MMC5983MA_CTRL_REG0_SET (1 << 3) +#define MMC5983MA_CTRL_REG0_RESET (1 << 4) + +#define MMC5983MA_CTRL_REG1_BW_200HZ (0b00000001) +#define MMC5983MA_CTRL_REG1_SW_RESET (1 << 7) + +extern device::Device *MMC5983MA_I2C_interface(const I2CSPIDriverConfig &config); + +class MMC5983MA : public I2CSPIDriver +{ +public: + MMC5983MA(device::Device *interface, const I2CSPIDriverConfig &config); + virtual ~MMC5983MA(); + + struct SensorData { + uint8_t xout0; + uint8_t xout1; + uint8_t yout0; + uint8_t yout1; + uint8_t zout0; + uint8_t zout1; + uint8_t xyzout2; + uint8_t tout; + }; + + enum class State { + Measure, + Collect, + }; + + static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance); + static void print_usage(); + + int init(); + void print_status() override; + + void RunImpl(); + +private: + void publish_data(); + + // Read data + uint8_t read_register_block(SensorData *data); + uint8_t read_register(uint8_t reg); + void write_register(uint8_t reg, uint8_t value); + + device::Device *_interface; + PX4Magnetometer _px4_mag; + State _state = State::Measure; + int _sample_index = 0; + int _collect_retries = 0; + SensorData _measurements[2]; + perf_counter_t _sample_count; + perf_counter_t _comms_errors; +}; diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_i2c.cpp b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_i2c.cpp new file mode 100644 index 0000000000..294fcb27af --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_i2c.cpp @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#include "mmc5983ma.h" +#include + +class MMC5983MA_I2C : public device::I2C +{ +public: + MMC5983MA_I2C(const I2CSPIDriverConfig &config); + virtual ~MMC5983MA_I2C() = default; + + virtual int read(unsigned address, void *data, unsigned count) override; + virtual int write(unsigned address, void *data, unsigned count) override; + +protected: + virtual int probe(); +}; + +MMC5983MA_I2C::MMC5983MA_I2C(const I2CSPIDriverConfig &config) : + I2C(config) +{ +} + +int MMC5983MA_I2C::probe() +{ + uint8_t data = 0; + + if (read(MMC5983MA_ADDR_PRODUCT_ID, &data, 1)) { + DEVICE_DEBUG("read_reg fail"); + return -EIO; + } + + if (data != MMC5983MA_PRODUCT_ID) { + DEVICE_DEBUG("MMC5983MA bad ID: %02x", data); + return -EIO; + } + + _retries = 1; + + return OK; +} + +int MMC5983MA_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +int MMC5983MA_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +device::Device *MMC5983MA_I2C_interface(const I2CSPIDriverConfig &config) +{ + return new MMC5983MA_I2C(config); +} diff --git a/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_main.cpp b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_main.cpp new file mode 100644 index 0000000000..b8d6e81e34 --- /dev/null +++ b/src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_main.cpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (c) 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. + * + ****************************************************************************/ + +#include "mmc5983ma.h" +#include + +I2CSPIDriverBase *MMC5983MA::instantiate(const I2CSPIDriverConfig &config, int runtime_instance) +{ + device::Device *interface = MMC5983MA_I2C_interface(config); + + 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)", config.bus, config.spi_devid); + return nullptr; + } + + MMC5983MA *dev = new MMC5983MA(interface, config); + + if (dev == nullptr) { + delete interface; + return nullptr; + } + + if (OK != dev->init()) { + delete dev; + return nullptr; + } + + return dev; +} + +void MMC5983MA::print_usage() +{ + PRINT_MODULE_USAGE_NAME("mmc5983ma", "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(0x30); + PRINT_MODULE_USAGE_COMMAND("reset"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int mmc5983ma_main(int argc, char *argv[]) +{ + using ThisDriver = MMC5983MA; + int ch; + BusCLIArguments cli{true, false}; + cli.i2c_address = 0x30; + cli.default_i2c_frequency = 400000; + + 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_MMC5983MA); + + 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; +}