mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-14 05:13:34 +08:00
drivers/magnetometer: new ST IIS2MDC Magnetometer driver
This commit is contained in:
Jacob Dahl
@@ -64,6 +64,7 @@
|
||||
#define DRV_MAG_DEVTYPE_IST8308 0x0B
|
||||
#define DRV_MAG_DEVTYPE_LIS2MDL 0x0C
|
||||
#define DRV_MAG_DEVTYPE_MMC5983MA 0x0D
|
||||
#define DRV_MAG_DEVTYPE_IIS2MDC 0x0E
|
||||
|
||||
#define DRV_IMU_DEVTYPE_LSM303D 0x11
|
||||
|
||||
|
||||
@@ -41,4 +41,5 @@ add_subdirectory(lis3mdl)
|
||||
add_subdirectory(lsm303agr)
|
||||
add_subdirectory(memsic)
|
||||
add_subdirectory(rm3100)
|
||||
add_subdirectory(st)
|
||||
add_subdirectory(vtrantech)
|
||||
|
||||
@@ -14,6 +14,7 @@ menu "Magnetometer"
|
||||
select DRIVERS_MAGNETOMETER_RM3100
|
||||
select DRIVERS_MAGNETOMETER_VTRANTECH_VCM1193L
|
||||
select DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA
|
||||
select DRIVERS_MAGNETOMETER_ST_IIS2MDC
|
||||
---help---
|
||||
Enable default set of magnetometer drivers
|
||||
rsource "*/Kconfig"
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
menu "ST"
|
||||
rsource "*/Kconfig"
|
||||
endmenu
|
||||
@@ -0,0 +1,45 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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__st__iis2mdc
|
||||
MAIN iis2mdc
|
||||
COMPILE_FLAGS
|
||||
# -DDEBUG_BUILD
|
||||
SRCS
|
||||
iis2mdc_i2c.cpp
|
||||
iis2mdc_main.cpp
|
||||
iis2mdc.cpp
|
||||
DEPENDS
|
||||
drivers_magnetometer
|
||||
px4_work_queue
|
||||
)
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_MAGNETOMETER_ST_IIS2MDC
|
||||
bool "iis2mdc"
|
||||
default n
|
||||
---help---
|
||||
Enable support for iis2mdc
|
||||
@@ -0,0 +1,137 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "iis2mdc.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
IIS2MDC::IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config) :
|
||||
I2CSPIDriver(config),
|
||||
_interface(interface),
|
||||
_px4_mag(interface->get_device_id(), config.rotation),
|
||||
_sample_count(perf_alloc(PC_COUNT, "iis2mdc_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "iis2mdc_comms_errors"))
|
||||
{}
|
||||
|
||||
IIS2MDC::~IIS2MDC()
|
||||
{
|
||||
perf_free(_sample_count);
|
||||
perf_free(_comms_errors);
|
||||
delete _interface;
|
||||
}
|
||||
|
||||
int IIS2MDC::init()
|
||||
{
|
||||
if (hrt_absolute_time() < 20_ms) {
|
||||
px4_usleep(20_ms); // ~10ms power-on time
|
||||
}
|
||||
|
||||
write_register(IIS2MDC_ADDR_CFG_REG_A, MD_CONTINUOUS | ODR_100 | COMP_TEMP_EN);
|
||||
write_register(IIS2MDC_ADDR_CFG_REG_B, OFF_CANC);
|
||||
write_register(IIS2MDC_ADDR_CFG_REG_C, BDU);
|
||||
|
||||
_px4_mag.set_scale(100.f / 65535.f); // +/- 50 Gauss, 16bit
|
||||
|
||||
ScheduleDelayed(20_ms);
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void IIS2MDC::RunImpl()
|
||||
{
|
||||
uint8_t status = read_register(IIS2MDC_ADDR_STATUS_REG);
|
||||
|
||||
if (status & IIS2MDC_STATUS_REG_READY) {
|
||||
SensorData data = {};
|
||||
|
||||
if (read_register_block(&data) == PX4_OK) {
|
||||
int16_t x = int16_t((data.xout1 << 8) | data.xout0);
|
||||
int16_t y = int16_t((data.yout1 << 8) | data.yout0);
|
||||
int16_t z = -int16_t((data.zout1 << 8) | data.zout0);
|
||||
int16_t t = int16_t((data.tout1 << 8) | data.tout0);
|
||||
// 16 bits twos complement with a sensitivity of 8 LSB/°C. Typically, the output zero level corresponds to 25 °C.
|
||||
_px4_mag.set_temperature(float(t) / 8.f + 25.f);
|
||||
_px4_mag.update(hrt_absolute_time(), x, y, z);
|
||||
_px4_mag.set_error_count(perf_event_count(_comms_errors));
|
||||
perf_count(_sample_count);
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("read failed");
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("not ready: %u", status);
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
|
||||
ScheduleDelayed(10_ms);
|
||||
}
|
||||
|
||||
uint8_t IIS2MDC::read_register_block(SensorData *data)
|
||||
{
|
||||
uint8_t reg = IIS2MDC_ADDR_OUTX_L_REG;
|
||||
|
||||
if (_interface->read(reg, data, sizeof(SensorData)) != PX4_OK) {
|
||||
perf_count(_comms_errors);
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
uint8_t IIS2MDC::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 IIS2MDC::write_register(uint8_t reg, uint8_t value)
|
||||
{
|
||||
if (_interface->write(reg, &value, sizeof(value)) != PX4_OK) {
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
}
|
||||
|
||||
void IIS2MDC::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
perf_print_counter(_sample_count);
|
||||
perf_print_counter(_comms_errors);
|
||||
}
|
||||
@@ -0,0 +1,95 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
|
||||
// IIS2MDC Registers
|
||||
#define IIS2MDC_ADDR_CFG_REG_A 0x60
|
||||
#define IIS2MDC_ADDR_CFG_REG_B 0x61
|
||||
#define IIS2MDC_ADDR_CFG_REG_C 0x62
|
||||
#define IIS2MDC_ADDR_STATUS_REG 0x67
|
||||
#define IIS2MDC_ADDR_OUTX_L_REG 0x68
|
||||
#define IIS2MDC_ADDR_WHO_AM_I 0x4F
|
||||
|
||||
// IIS2MDC Definitions
|
||||
#define IIS2MDC_WHO_AM_I 0b01000000
|
||||
#define IIS2MDC_STATUS_REG_READY 0b00001111
|
||||
// CFG_REG_A
|
||||
#define COMP_TEMP_EN (1 << 7)
|
||||
#define MD_CONTINUOUS (0 << 0)
|
||||
#define ODR_100 ((1 << 3) | (1 << 2))
|
||||
// CFG_REG_B
|
||||
#define OFF_CANC (1 << 1)
|
||||
// CFG_REG_C
|
||||
#define BDU (1 << 4)
|
||||
|
||||
extern device::Device *IIS2MDC_I2C_interface(const I2CSPIDriverConfig &config);
|
||||
|
||||
class IIS2MDC : public I2CSPIDriver<IIS2MDC>
|
||||
{
|
||||
public:
|
||||
IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config);
|
||||
virtual ~IIS2MDC();
|
||||
|
||||
struct SensorData {
|
||||
uint8_t xout0;
|
||||
uint8_t xout1;
|
||||
uint8_t yout0;
|
||||
uint8_t yout1;
|
||||
uint8_t zout0;
|
||||
uint8_t zout1;
|
||||
uint8_t tout0;
|
||||
uint8_t tout1;
|
||||
};
|
||||
|
||||
static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
|
||||
static void print_usage();
|
||||
|
||||
int init();
|
||||
void print_status() override;
|
||||
|
||||
void RunImpl();
|
||||
|
||||
private:
|
||||
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;
|
||||
perf_counter_t _sample_count;
|
||||
perf_counter_t _comms_errors;
|
||||
};
|
||||
@@ -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 "iis2mdc.h"
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
class IIS2MDC_I2C : public device::I2C
|
||||
{
|
||||
public:
|
||||
IIS2MDC_I2C(const I2CSPIDriverConfig &config);
|
||||
virtual ~IIS2MDC_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();
|
||||
};
|
||||
|
||||
IIS2MDC_I2C::IIS2MDC_I2C(const I2CSPIDriverConfig &config) :
|
||||
I2C(config)
|
||||
{
|
||||
}
|
||||
|
||||
int IIS2MDC_I2C::probe()
|
||||
{
|
||||
uint8_t data = 0;
|
||||
|
||||
if (read(IIS2MDC_ADDR_WHO_AM_I, &data, 1)) {
|
||||
DEVICE_DEBUG("read_reg fail");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
if (data != IIS2MDC_WHO_AM_I) {
|
||||
DEVICE_DEBUG("IIS2MDC bad ID: %02x", data);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
_retries = 1;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int IIS2MDC_I2C::read(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t cmd = address;
|
||||
return transfer(&cmd, 1, (uint8_t *)data, count);
|
||||
}
|
||||
|
||||
int IIS2MDC_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 *IIS2MDC_I2C_interface(const I2CSPIDriverConfig &config)
|
||||
{
|
||||
return new IIS2MDC_I2C(config);
|
||||
}
|
||||
@@ -0,0 +1,116 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 "iis2mdc.h"
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
I2CSPIDriverBase *IIS2MDC::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
|
||||
{
|
||||
device::Device *interface = IIS2MDC_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%lx)", config.bus, config.spi_devid);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
IIS2MDC *dev = new IIS2MDC(interface, config);
|
||||
|
||||
if (dev == nullptr) {
|
||||
delete interface;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (OK != dev->init()) {
|
||||
delete dev;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return dev;
|
||||
}
|
||||
|
||||
void IIS2MDC::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("iis2mdc", "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_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
extern "C" int iis2mdc_main(int argc, char *argv[])
|
||||
{
|
||||
using ThisDriver = IIS2MDC;
|
||||
int ch;
|
||||
BusCLIArguments cli{true, false};
|
||||
cli.i2c_address = 0x1E;
|
||||
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_IIS2MDC);
|
||||
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user