mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 07:47:35 +08:00
Memsic MMC5983MA magnetometer driver
This commit is contained in:
Jacob Dahl
@@ -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
|
||||
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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"
|
||||
|
||||
@@ -0,0 +1,3 @@
|
||||
menu "Memsic"
|
||||
rsource "*/Kconfig"
|
||||
endmenu
|
||||
@@ -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
|
||||
)
|
||||
@@ -0,0 +1,5 @@
|
||||
menuconfig DRIVERS_MAGNETOMETER_MEMSIC_MMC5983MA
|
||||
bool "mmc5983ma"
|
||||
default n
|
||||
---help---
|
||||
Enable support for mmc5983ma
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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 <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
|
||||
// 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<MMC5983MA>
|
||||
{
|
||||
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;
|
||||
};
|
||||
@@ -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 <drivers/device/i2c.h>
|
||||
|
||||
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);
|
||||
}
|
||||
@@ -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 <px4_platform_common/module.h>
|
||||
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user