mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-06 04:40:35 +08:00
drivers: add lis2mdl mag driver
This commit is contained in:
Beat Küng
@@ -63,6 +63,7 @@
|
||||
#define DRV_MAG_DEVTYPE_AK09916 0x09
|
||||
#define DRV_IMU_DEVTYPE_ICM20948 0x0A
|
||||
#define DRV_MAG_DEVTYPE_IST8308 0x0B
|
||||
#define DRV_MAG_DEVTYPE_LIS2MDL 0x0C
|
||||
|
||||
#define DRV_IMU_DEVTYPE_LSM303D 0x11
|
||||
#define DRV_ACC_DEVTYPE_BMA180 0x12
|
||||
@@ -160,6 +161,7 @@
|
||||
#define DRV_MAG_DEVTYPE_UAVCAN 0x88
|
||||
#define DRV_DIST_DEVTYPE_UAVCAN 0x89
|
||||
|
||||
|
||||
#define DRV_DEVTYPE_UNUSED 0xff
|
||||
|
||||
#endif /* _DRV_SENSOR_H */
|
||||
|
||||
@@ -0,0 +1,41 @@
|
||||
############################################################################
|
||||
#
|
||||
# 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__lis2mdl
|
||||
MAIN lis2mdl
|
||||
SRCS
|
||||
lis2mdl_i2c.cpp
|
||||
lis2mdl_spi.cpp
|
||||
lis2mdl_main.cpp
|
||||
lis2mdl.cpp
|
||||
)
|
||||
@@ -0,0 +1,189 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 lis2mdl.cpp
|
||||
*
|
||||
* Driver for the LIS2MDL magnetometer connected via I2C or SPI.
|
||||
*
|
||||
* Based on the LIS2MDL driver.
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/time.h>
|
||||
#include "lis2mdl.h"
|
||||
|
||||
LIS2MDL::LIS2MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus) :
|
||||
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus),
|
||||
_px4_mag(interface->get_device_id(), interface->external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT, rotation),
|
||||
_interface(interface),
|
||||
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")),
|
||||
_conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")),
|
||||
_range_errors(perf_alloc(PC_COUNT, MODULE_NAME": range_errors")),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
|
||||
_measure_interval(0)
|
||||
{
|
||||
_px4_mag.set_external(_interface->external());
|
||||
_px4_mag.set_scale(0.0015f); /* 49.152f / (2^15) */
|
||||
}
|
||||
|
||||
LIS2MDL::~LIS2MDL()
|
||||
{
|
||||
// free perf counters
|
||||
perf_free(_sample_perf);
|
||||
perf_free(_comms_errors);
|
||||
perf_free(_range_errors);
|
||||
perf_free(_conf_errors);
|
||||
}
|
||||
|
||||
int
|
||||
LIS2MDL::measure()
|
||||
{
|
||||
struct {
|
||||
uint8_t status;
|
||||
uint8_t x[2];
|
||||
uint8_t y[2];
|
||||
uint8_t z[2];
|
||||
} lis_report;
|
||||
|
||||
struct {
|
||||
int16_t x;
|
||||
int16_t y;
|
||||
int16_t z;
|
||||
int16_t t;
|
||||
} report;
|
||||
|
||||
uint8_t buf_rx[2] = {};
|
||||
|
||||
_px4_mag.set_error_count(perf_event_count(_comms_errors));
|
||||
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
int ret = _interface->read(ADDR_STATUS_REG, (uint8_t *)&lis_report, sizeof(lis_report));
|
||||
|
||||
/**
|
||||
* Silicon Bug: the X axis will be read instead of the temperature registers if you do a sequential read through XYZ.
|
||||
* The temperature registers must be addressed directly.
|
||||
*/
|
||||
ret = _interface->read(ADDR_OUT_T_L, (uint8_t *)&buf_rx, sizeof(buf_rx));
|
||||
|
||||
if (ret != OK) {
|
||||
perf_end(_sample_perf);
|
||||
perf_count(_comms_errors);
|
||||
PX4_WARN("Register read error.");
|
||||
return ret;
|
||||
}
|
||||
|
||||
perf_end(_sample_perf);
|
||||
|
||||
if ((lis_report.status & (1 << 3)) == 0) { // check data ready
|
||||
return 0;
|
||||
}
|
||||
|
||||
report.x = (int16_t)((lis_report.x[1] << 8) | lis_report.x[0]);
|
||||
report.y = (int16_t)((lis_report.y[1] << 8) | lis_report.y[0]);
|
||||
report.z = (int16_t)((lis_report.z[1] << 8) | lis_report.z[0]);
|
||||
|
||||
report.t = (int16_t)((buf_rx[1] << 8) | buf_rx[0]);
|
||||
|
||||
float temperature = report.t;
|
||||
_px4_mag.set_temperature(25.0f + (temperature / 8.0f));
|
||||
|
||||
/* swap x and y axis */
|
||||
_px4_mag.update(timestamp_sample, report.y, report.x, report.z);
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void
|
||||
LIS2MDL::RunImpl()
|
||||
{
|
||||
/* _measure_interval == 0 is used as _task_should_exit */
|
||||
if (_measure_interval == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (measure() != OK) {
|
||||
PX4_DEBUG("measure error");
|
||||
}
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
ScheduleDelayed(LIS2MDL_CONVERSION_INTERVAL);
|
||||
}
|
||||
|
||||
int
|
||||
LIS2MDL::init()
|
||||
{
|
||||
|
||||
int ret = write_reg(ADDR_CFG_REG_A, CFG_REG_A_ODR | CFG_REG_A_MD | CFG_REG_A_TEMP_COMP_EN);
|
||||
ret = write_reg(ADDR_CFG_REG_B, 0);
|
||||
ret = write_reg(ADDR_CFG_REG_C, CFG_REG_C_BDU);
|
||||
|
||||
_measure_interval = LIS2MDL_CONVERSION_INTERVAL;
|
||||
start();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
LIS2MDL::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
PX4_INFO("poll interval: %u", _measure_interval);
|
||||
_px4_mag.print_status();
|
||||
}
|
||||
|
||||
void
|
||||
LIS2MDL::start()
|
||||
{
|
||||
/* schedule a cycle to start things */
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
int
|
||||
LIS2MDL::read_reg(uint8_t reg, uint8_t &val)
|
||||
{
|
||||
uint8_t buf = val;
|
||||
int ret = _interface->read(reg, &buf, 1);
|
||||
val = buf;
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
LIS2MDL::write_reg(uint8_t reg, uint8_t val)
|
||||
{
|
||||
uint8_t buf = val;
|
||||
return _interface->write(reg, &buf, 1);
|
||||
}
|
||||
@@ -0,0 +1,154 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 lis2mdl.h
|
||||
*
|
||||
* Shared defines for the LIS2MDL driver.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
|
||||
/**
|
||||
* LIS2MDL internal constants and data structures.
|
||||
*/
|
||||
|
||||
/* Max measurement rate is 50Hz */
|
||||
#define LIS2MDL_CONVERSION_INTERVAL (1000000 / 50)
|
||||
|
||||
#define ADDR_WHO_AM_I 0x4f
|
||||
#define ID_WHO_AM_I 0x40
|
||||
|
||||
#define ADDR_CFG_REG_A 0x60
|
||||
#define ADDR_CFG_REG_B 0x61
|
||||
#define ADDR_CFG_REG_C 0x62
|
||||
#define ADDR_INT_CTRL_REG 0x63
|
||||
|
||||
#define ADDR_STATUS_REG 0x67
|
||||
#define ADDR_OUT_X_L 0x68
|
||||
#define ADDR_OUT_X_H 0x69
|
||||
#define ADDR_OUT_Y_L 0x6a
|
||||
#define ADDR_OUT_Y_H 0x6b
|
||||
#define ADDR_OUT_Z_L 0x6c
|
||||
#define ADDR_OUT_Z_H 0x6d
|
||||
#define ADDR_OUT_T_L 0x6e
|
||||
#define ADDR_OUT_T_H 0x6f
|
||||
|
||||
#define CFG_REG_A_TEMP_COMP_EN (1 << 7)
|
||||
#define CFG_REG_A_ODR (2 << 2) /* 50Hz (100Hz creates spikes randomly) */
|
||||
#define CFG_REG_A_MD (0 << 0) /* continuous mode */
|
||||
|
||||
#define CFG_REG_B_LPF (1 << 0) /* LPF */
|
||||
|
||||
#define CFG_REG_C_BDU (1 << 4) /* avoids reading of incorrect data due to async reads */
|
||||
|
||||
/* interface factories */
|
||||
extern device::Device *LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
|
||||
extern device::Device *LIS2MDL_I2C_interface(int bus, int bus_frequency);
|
||||
|
||||
|
||||
class LIS2MDL : public I2CSPIDriver<LIS2MDL>
|
||||
{
|
||||
public:
|
||||
LIS2MDL(device::Device *interface, enum Rotation rotation, I2CSPIBusOption bus_option, int bus);
|
||||
virtual ~LIS2MDL();
|
||||
|
||||
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance);
|
||||
static void print_usage();
|
||||
|
||||
virtual int init();
|
||||
|
||||
void print_status() override;
|
||||
|
||||
/**
|
||||
* Configures the device with default register values.
|
||||
*/
|
||||
int set_default_register_values();
|
||||
|
||||
void RunImpl();
|
||||
|
||||
private:
|
||||
PX4Magnetometer _px4_mag;
|
||||
|
||||
device::Device *_interface;
|
||||
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _conf_errors;
|
||||
perf_counter_t _range_errors;
|
||||
perf_counter_t _sample_perf;
|
||||
|
||||
unsigned int _measure_interval;
|
||||
|
||||
/**
|
||||
* Issue a measurement command & publish data.
|
||||
*
|
||||
* @return OK if the measurement command was successful.
|
||||
*/
|
||||
int measure();
|
||||
|
||||
/**
|
||||
* @brief Initialises the automatic measurement state machine and start it.
|
||||
*
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start();
|
||||
|
||||
/**
|
||||
* @brief Reads a register.
|
||||
*
|
||||
* @param reg The register to read.
|
||||
* @param val The value read.
|
||||
* @return OK on read success.
|
||||
*/
|
||||
int read_reg(uint8_t reg, uint8_t &val);
|
||||
|
||||
/**
|
||||
* @brief Writes a register.
|
||||
*
|
||||
* @param reg The register to write.
|
||||
* @param val The value to write.
|
||||
* @return OK on write success.
|
||||
*/
|
||||
int write_reg(uint8_t reg, uint8_t val);
|
||||
|
||||
};
|
||||
@@ -0,0 +1,130 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 lis2mdl_i2c.cpp
|
||||
*
|
||||
* I2C interface for LIS2MDL
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#include "board_config.h"
|
||||
#include "lis2mdl.h"
|
||||
|
||||
#define LIS2MDLL_ADDRESS 0x1e
|
||||
|
||||
class LIS2MDL_I2C : public device::I2C
|
||||
{
|
||||
public:
|
||||
LIS2MDL_I2C(int bus, int bus_frequency);
|
||||
virtual ~LIS2MDL_I2C() = default;
|
||||
|
||||
virtual int read(unsigned address, void *data, unsigned count);
|
||||
virtual int write(unsigned address, void *data, unsigned count);
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
|
||||
};
|
||||
|
||||
device::Device *
|
||||
LIS2MDL_I2C_interface(int bus, int bus_frequency);
|
||||
|
||||
device::Device *
|
||||
LIS2MDL_I2C_interface(int bus, int bus_frequency)
|
||||
{
|
||||
return new LIS2MDL_I2C(bus, bus_frequency);
|
||||
}
|
||||
|
||||
LIS2MDL_I2C::LIS2MDL_I2C(int bus, int bus_frequency) :
|
||||
I2C(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_I2C", bus, LIS2MDLL_ADDRESS, bus_frequency)
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
LIS2MDL_I2C::probe()
|
||||
{
|
||||
uint8_t data = 0;
|
||||
|
||||
_retries = 10;
|
||||
|
||||
if (read(ADDR_WHO_AM_I, &data, 1)) {
|
||||
DEVICE_DEBUG("read_reg fail");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
_retries = 2;
|
||||
|
||||
if (data != ID_WHO_AM_I) {
|
||||
DEVICE_DEBUG("LIS2MDL bad ID: %02x", data);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
LIS2MDL_I2C::read(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t cmd = address;
|
||||
return transfer(&cmd, 1, (uint8_t *)data, count);
|
||||
}
|
||||
|
||||
int
|
||||
LIS2MDL_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);
|
||||
}
|
||||
@@ -0,0 +1,131 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 lis2mdl_main.cpp
|
||||
*
|
||||
* Driver for the LIS2MDL magnetometer connected via I2C or SPI.
|
||||
*/
|
||||
|
||||
#include "lis2mdl.h"
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
I2CSPIDriverBase *LIS2MDL::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance)
|
||||
{
|
||||
device::Device *interface = nullptr;
|
||||
|
||||
if (iterator.busType() == BOARD_I2C_BUS) {
|
||||
interface = LIS2MDL_I2C_interface(iterator.bus(), cli.bus_frequency);
|
||||
|
||||
} else if (iterator.busType() == BOARD_SPI_BUS) {
|
||||
interface = LIS2MDL_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
LIS2MDL *dev = new LIS2MDL(interface, cli.rotation, iterator.configuredBusOption(), iterator.bus());
|
||||
|
||||
if (dev == nullptr) {
|
||||
delete interface;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (OK != dev->init()) {
|
||||
delete dev;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return dev;
|
||||
}
|
||||
|
||||
void LIS2MDL::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("lis2mdl", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
extern "C" int lis2mdl_main(int argc, char *argv[])
|
||||
{
|
||||
using ThisDriver = LIS2MDL;
|
||||
int ch;
|
||||
BusCLIArguments cli{true, true};
|
||||
cli.default_i2c_frequency = 400000;
|
||||
cli.default_spi_frequency = 11 * 1000 * 1000;
|
||||
|
||||
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_LIS2MDL);
|
||||
|
||||
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;
|
||||
}
|
||||
@@ -0,0 +1,144 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 lis2mdl_spi.cpp
|
||||
*
|
||||
* SPI interface for LIS2MDL
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <assert.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
#include <sys/types.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_device.h>
|
||||
|
||||
#include "board_config.h"
|
||||
#include "lis2mdl.h"
|
||||
|
||||
/* SPI protocol address bits */
|
||||
#define DIR_READ (1<<7)
|
||||
#define DIR_WRITE (0<<7)
|
||||
#define ADDR_INCREMENT (1<<6)
|
||||
|
||||
class LIS2MDL_SPI : public device::SPI
|
||||
{
|
||||
public:
|
||||
LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
|
||||
virtual ~LIS2MDL_SPI() = default;
|
||||
|
||||
virtual int init();
|
||||
virtual int read(unsigned address, void *data, unsigned count);
|
||||
virtual int write(unsigned address, void *data, unsigned count);
|
||||
};
|
||||
|
||||
device::Device *
|
||||
LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
|
||||
|
||||
device::Device *
|
||||
LIS2MDL_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
|
||||
{
|
||||
return new LIS2MDL_SPI(bus, devid, bus_frequency, spi_mode);
|
||||
}
|
||||
|
||||
LIS2MDL_SPI::LIS2MDL_SPI(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode) :
|
||||
SPI(DRV_MAG_DEVTYPE_LIS2MDL, "LIS2MDL_SPI", bus, devid, spi_mode, bus_frequency)
|
||||
{
|
||||
}
|
||||
|
||||
int
|
||||
LIS2MDL_SPI::init()
|
||||
{
|
||||
int ret;
|
||||
|
||||
ret = SPI::init();
|
||||
|
||||
if (ret != OK) {
|
||||
DEVICE_DEBUG("SPI init failed");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
// read WHO_AM_I value
|
||||
uint8_t data = 0;
|
||||
|
||||
if (read(ADDR_WHO_AM_I, &data, 1)) {
|
||||
DEVICE_DEBUG("LIS2MDL read_reg fail");
|
||||
}
|
||||
|
||||
if (data != ID_WHO_AM_I) {
|
||||
DEVICE_DEBUG("LIS2MDL bad ID: %02x", data);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int
|
||||
LIS2MDL_SPI::read(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
if (sizeof(buf) < (count + 1)) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
buf[0] = address | DIR_READ | ADDR_INCREMENT;
|
||||
|
||||
int ret = transfer(&buf[0], &buf[0], count + 1);
|
||||
memcpy(data, &buf[1], count);
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
LIS2MDL_SPI::write(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
if (sizeof(buf) < (count + 1)) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
buf[0] = address | DIR_WRITE;
|
||||
memcpy(&buf[1], data, count);
|
||||
|
||||
return transfer(&buf[0], &buf[0], count + 1);
|
||||
}
|
||||
Reference in New Issue
Block a user