diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 2ed1652814..22eff58ba6 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -234,6 +234,7 @@ then lis3mdl -X -q -a 0x1c start fi qmc5883l -X -q start + qmc5883p -X -q start rm3100 -X -q start # start last (wait for possible icm20948 passthrough mode) diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 4560ed1f58..87184924bc 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -65,6 +65,8 @@ #define DRV_MAG_DEVTYPE_MMC5983MA 0x0D #define DRV_MAG_DEVTYPE_IIS2MDC 0x0E +#define DRV_MAG_DEVTYPE_QMC5883P 0x0F + #define DRV_IMU_DEVTYPE_LSM303D 0x11 #define DRV_IMU_DEVTYPE_SIM 0x14 diff --git a/src/drivers/magnetometer/CMakeLists.txt b/src/drivers/magnetometer/CMakeLists.txt index da1dac554e..2bee08e030 100644 --- a/src/drivers/magnetometer/CMakeLists.txt +++ b/src/drivers/magnetometer/CMakeLists.txt @@ -35,6 +35,7 @@ add_subdirectory(akm) add_subdirectory(bosch) add_subdirectory(hmc5883) add_subdirectory(qmc5883l) +add_subdirectory(qmc5883p) add_subdirectory(isentek) add_subdirectory(lis3mdl) add_subdirectory(lsm303agr) diff --git a/src/drivers/magnetometer/Kconfig b/src/drivers/magnetometer/Kconfig index f86b807a96..34bb902a83 100644 --- a/src/drivers/magnetometer/Kconfig +++ b/src/drivers/magnetometer/Kconfig @@ -7,6 +7,7 @@ menu "Magnetometer" select DRIVERS_MAGNETOMETER_BOSCH_BMM150 select DRIVERS_MAGNETOMETER_HMC5883 select DRIVERS_MAGNETOMETER_QMC5883L + select DRIVERS_MAGNETOMETER_QMC5883P select DRIVERS_MAGNETOMETER_ISENTEK_IST8308 select DRIVERS_MAGNETOMETER_ISENTEK_IST8310 select DRIVERS_MAGNETOMETER_LIS3MDL diff --git a/src/drivers/magnetometer/qmc5883p/CMakeLists.txt b/src/drivers/magnetometer/qmc5883p/CMakeLists.txt new file mode 100644 index 0000000000..8db530afe6 --- /dev/null +++ b/src/drivers/magnetometer/qmc5883p/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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__qmc5883p + MAIN qmc5883p + COMPILE_FLAGS + SRCS + QMC5883P.cpp + QMC5883P.hpp + qmc5883p_main.cpp + QST_QMC5883P_registers.hpp + DEPENDS + drivers_magnetometer + px4_work_queue + ) diff --git a/src/drivers/magnetometer/qmc5883p/Kconfig b/src/drivers/magnetometer/qmc5883p/Kconfig new file mode 100644 index 0000000000..0179e34029 --- /dev/null +++ b/src/drivers/magnetometer/qmc5883p/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_MAGNETOMETER_QMC5883P + bool "qmc5883p" + default n + ---help--- + Enable support for qmc5883p diff --git a/src/drivers/magnetometer/qmc5883p/QMC5883P.cpp b/src/drivers/magnetometer/qmc5883p/QMC5883P.cpp new file mode 100644 index 0000000000..b0d9693b19 --- /dev/null +++ b/src/drivers/magnetometer/qmc5883p/QMC5883P.cpp @@ -0,0 +1,308 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "QMC5883P.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +QMC5883P::QMC5883P(const I2CSPIDriverConfig &config) : + I2C(config), + I2CSPIDriver(config), + _px4_mag(get_device_id(), config.rotation) +{ +} + +QMC5883P::~QMC5883P() +{ + perf_free(_reset_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); +} + +int QMC5883P::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool QMC5883P::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void QMC5883P::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); +} + +int QMC5883P::probe() +{ + _retries = 1; + + for (int i = 0; i < 3; i++) { + // first read 0x0 once + const uint8_t cmd = 0; + uint8_t buffer{}; + + if (transfer(&cmd, 1, &buffer, 1) == PX4_OK) { + const uint8_t CHIP_ID = RegisterRead(Register::CHIP_ID); + + if (CHIP_ID == Chip_ID) { + return PX4_OK; + } + } + } + + return PX4_ERROR; +} + +void QMC5883P::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: { + // CNTL2: Software Reset + RegisterWrite(Register::CNTL2, CNTL2_BIT::SOFT_RST); + _reset_timestamp = now; + _failure_count = 0; + _state = STATE::WAIT_FOR_RESET; + perf_count(_reset_perf); + ScheduleDelayed(100_ms); // POR Completion Time + break; + } + + case STATE::WAIT_FOR_RESET: { + // SOFT_RST: This bit is automatically reset to zero after POR routine + if ((RegisterRead(Register::CHIP_ID) == Chip_ID) + && ((RegisterRead(Register::CNTL2) & CNTL2_BIT::SOFT_RST) == 0)) { + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1_s) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(10_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + } + + case STATE::CONFIGURE: + if (Configure()) { + // if configure succeeded then start reading every 20 ms (50 Hz) + _state = STATE::READ; + ScheduleOnInterval(20_ms, 20_ms); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1_s) { + 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 X_LSB; + uint8_t X_MSB; + uint8_t Y_LSB; + uint8_t Y_MSB; + uint8_t Z_LSB; + uint8_t Z_MSB; + uint8_t N_LSB; + uint8_t N_MSB; + uint8_t STATUS; + } buffer{}; + + bool success = false; + uint8_t cmd = static_cast(Register::X_LSB); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) { + // process data if successful transfer, no overflow + if ((buffer.STATUS & STATUS_BIT::OVL) == 0) { + int16_t x = combine(buffer.X_MSB, buffer.X_LSB); + int16_t y = combine(buffer.Y_MSB, buffer.Y_LSB); + int16_t z = combine(buffer.Z_MSB, buffer.Z_LSB); + + if (buffer.STATUS & STATUS_BIT::DRDY) { + _prev_data[0] = x; + _prev_data[1] = y; + _prev_data[2] = z; + + // Sensor orientation + // Forward X := -Y + // Right Y := -X + // Down Z := -Z + int16_t new_x = (y == INT16_MIN) ? INT16_MAX : -y; + int16_t new_y = (x == INT16_MIN) ? INT16_MAX : -x; + int16_t new_z = (z == INT16_MIN) ? INT16_MAX : -z; + + _px4_mag.update(now, new_x, new_y, new_z); + 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 QMC5883P::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); + ScheduleDelayed(10_ms); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + _px4_mag.set_scale(1.f / 12000.f); // 12000 LSB/Gauss (Field Range = ±2G) + + return success; +} + +bool QMC5883P::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_INFO("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_INFO("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 QMC5883P::RegisterRead(Register reg) +{ + const uint8_t cmd = static_cast(reg); + uint8_t buffer{}; + transfer(&cmd, 1, &buffer, 1); + return buffer; +} + +void QMC5883P::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t buffer[2] { (uint8_t)reg, value }; + transfer(buffer, sizeof(buffer), nullptr, 0); +} + +void QMC5883P::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/qmc5883p/QMC5883P.hpp b/src/drivers/magnetometer/qmc5883p/QMC5883P.hpp new file mode 100644 index 0000000000..3ad532d0fb --- /dev/null +++ b/src/drivers/magnetometer/qmc5883p/QMC5883P.hpp @@ -0,0 +1,113 @@ +/**************************************************************************** + * + * 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 QMC5883P.hpp + * + * Driver for the QMC5883P connected via I2C. + * + * @author Amovlab Lv Guofei + */ + +#pragma once + +#include "QST_QMC5883P_registers.hpp" + +#include +#include +#include +#include +#include + +using namespace QST_QMC5883P; + +class QMC5883P : public device::I2C, public I2CSPIDriver +{ +public: + QMC5883P(const I2CSPIDriverConfig &config); + ~QMC5883P() override; + + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + // 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); + + 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")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + int _failure_count{0}; + + int16_t _prev_data[3] {}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + READ + } _state{STATE::RESET}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{2}; + register_config_t _register_cfg[size_register_cfg] { + // Register | Set bits, Clear bits + { Register::CNTL1, CNTL1_BIT::MODE_CONTINUOUS | CNTL1_BIT::OSR1_8 | CNTL1_BIT::ODR_50HZ, CNTL1_BIT::OSR2_8}, + { Register::CNTL2, CNTL2_BIT::RNG_2G, CNTL2_BIT::SOFT_RST | CNTL2_BIT::SELF_TEST}, + }; +}; diff --git a/src/drivers/magnetometer/qmc5883p/QST_QMC5883P_registers.hpp b/src/drivers/magnetometer/qmc5883p/QST_QMC5883P_registers.hpp new file mode 100644 index 0000000000..57fffa6579 --- /dev/null +++ b/src/drivers/magnetometer/qmc5883p/QST_QMC5883P_registers.hpp @@ -0,0 +1,114 @@ +/**************************************************************************** + * + * 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 QST_QMC5883P_registers.hpp + * + * QST QMC5883P registers. + * + * @author Amovlab Lv Guofei + * + */ +#pragma once + +#include + +namespace QST_QMC5883P +{ + +// 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); + +// default value +static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface +static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b0101100; // default I2C address + +static constexpr uint8_t Chip_ID = 0x80; + +//static constexpr uint8_t XYZ_SIGN_CONFIG = 0x29; // XYZ symbol configuration value +enum class Register : uint8_t { +// Register addr + X_LSB = 0x01, // Data Output X LSB Register XOUT[7:0] + X_MSB = 0x02, // Data Output X MSB Register XOUT[15:8] + Y_LSB = 0x03, // Data Output Y LSB Register YOUT[7:0] + Y_MSB = 0x04, // Data Output Y MSB Register YOUT[15:8] + Z_LSB = 0x05, // Data Output Z LSB Register ZOUT[7:0] + Z_MSB = 0x06, // Data Output Z MSB Register ZOUT[15:8] + N_LSB = 0X07, // No Use + N_MSB = 0X08, // No Use + STATUS = 0x09, // Status Register 1 + + CNTL1 = 0x0A, // Control Register 1 + CNTL2 = 0x0B, // Control Register 2 + + CHIP_ID = 0x00, +}; + +// STATUS +enum STATUS_BIT : uint8_t { + DRDY = Bit0, // 0: no new data, 1: new data is ready + OVL = Bit1, // 0: no data overflow occurs, 1: data overflow occurs +}; + +// CNTL1 +enum CNTL1_BIT : uint8_t { + // OSR2[7:6] + OSR2_8 = Bit7 | Bit6, // 00 + // OSR1[5:4] + OSR1_8 = Bit5 | Bit4, // 11 + // ODR[3:2] + ODR_50HZ = Bit2, // 01 + // MODE[1:0] + MODE_CONTINUOUS = Bit1 | Bit0, // 11 +}; + +// CNTL2 +enum CNTL2_BIT : uint8_t { + // RNG[3:2] + RNG_2G = Bit3 | Bit2, // 11 + + // SELF_TEST[6] + SELF_TEST = Bit6, //1: self_test enable, auto clear after the data is updated + + // SOFT_RST[7] + SOFT_RST = 0, //1: Soft reset, restore default value of all registers, 0: no reset +}; + +} // namespace QMC5883P diff --git a/src/drivers/magnetometer/qmc5883p/qmc5883p_main.cpp b/src/drivers/magnetometer/qmc5883p/qmc5883p_main.cpp new file mode 100644 index 0000000000..e9e1d262bf --- /dev/null +++ b/src/drivers/magnetometer/qmc5883p/qmc5883p_main.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2025 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 "QMC5883P.hpp" + +#include +#include + +void QMC5883P::print_usage() +{ + PRINT_MODULE_USAGE_NAME("qmc5883p", "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(0x2C); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +extern "C" int qmc5883p_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = QMC5883P; + 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_QMC5883P); + + 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; +}