fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

fxos8701cq cleanup and move to PX4Accelerometer/PX4Magnetomer

Browse Source
This commit is contained in:
Daniel Agar 2019-08-28 23:17:56 -04:00
parent ee9f65b38b
commit 9c6d4e28ea
4 changed files with 152 additions and 1622 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/drv_sensor.h
View File

@ -101,7 +101,7 @@
#define DRV_DIFF_PRESS_DEVTYPE_SDP33 0x50
#define DRV_BARO_DEVTYPE_MPL3115A2 0x51
#define DRV_ACC_DEVTYPE_FXOS8701C 0x52
#define DRV_MAG_DEVTYPE_FXOS8701C 0x53
#define DRV_GYR_DEVTYPE_FXAS2100C 0x54
#define DRV_ACC_DEVTYPE_ADIS16448 0x55
#define DRV_MAG_DEVTYPE_ADIS16448 0x56

1222
src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp
View File

File diff suppressed because it is too large Load Diff

242
src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp
View File

@ -32,26 +32,21 @@
****************************************************************************/
/**
* @file fxos8701cq.cpp
* @file FXOS8701CQ.hpp
* Driver for the NXP FXOS8701CQ 6-axis sensor with integrated linear accelerometer and
* magnetometer connected via SPI.
*/
#include <drivers/device/integrator.h>
#include <drivers/device/ringbuffer.h>
#pragma once
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <ecl/geo/geo.h>
#include <lib/conversion/rotation.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <perf/perf_counter.h>
#include <px4_getopt.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <systemlib/err.h>
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
# include <drivers/drv_mag.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#endif
/* SPI protocol address bits */
@ -60,11 +55,6 @@
#define ADDR_7(a) ((a) & (1 << 7))
#define swap16(w) __builtin_bswap16((w))
#define swap16RightJustify14(w) (((int16_t)swap16(w)) >> 2)
#define FXOS8701C_DEVICE_PATH_ACCEL "/dev/fxos8701cq_accel"
#define FXOS8701C_DEVICE_PATH_ACCEL_EXT "/dev/fxos8701cq_accel_ext"
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
# define FXOS8701C_DEVICE_PATH_MAG "/dev/fxos8701cq_mag"
#endif
#define FXOS8701CQ_DR_STATUS 0x00
# define DR_STATUS_ZYXDR (1 << 3)
@ -112,12 +102,6 @@
/* default values for this device */
#define FXOS8701C_ACCEL_DEFAULT_RANGE_G 8
#define FXOS8701C_ACCEL_DEFAULT_RATE 400 /* ODR is 400 in Hybird mode (accel + mag) */
#define FXOS8701C_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ 50
#define FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define FXOS8701C_ACCEL_MAX_OUTPUT_RATE 280
#define FXOS8701C_MAG_DEFAULT_RANGE_GA 12 /* It is fixed at 12 G */
#define FXOS8701C_MAG_DEFAULT_RATE 100
/*
we set the timer interrupt to run a bit faster than the desired
@ -127,154 +111,33 @@
*/
#define FXOS8701C_TIMER_REDUCTION 240
extern "C" { __EXPORT int fxos8701cq_main(int argc, char *argv[]); }
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
class FXOS8701CQ_mag;
#endif
class FXOS8701CQ : public device::SPI, public px4::ScheduledWorkItem
{
public:
FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation);
FXOS8701CQ(int bus, uint32_t device, enum Rotation rotation);
virtual ~FXOS8701CQ();
virtual int init();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
/**
* Diagnostics - print some basic information about the driver.
*/
void print_info();
/**
* dump register values
*/
void print_registers();
/**
* deliberately trigger an error
*/
void test_error();
protected:
virtual int probe();
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
friend class FXOS8701CQ_mag;
virtual ssize_t mag_read(struct file *filp, char *buffer, size_t buflen);
virtual int mag_ioctl(struct file *filp, int cmd, unsigned long arg);
#endif
private:
void Run() override;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
FXOS8701CQ_mag *_mag;
unsigned _call_mag_interval;
ringbuffer::RingBuffer *_mag_reports;
struct mag_calibration_s _mag_scale;
unsigned _mag_range_ga;
float _mag_range_scale;
unsigned _mag_samplerate;
unsigned _mag_read;
perf_counter_t _mag_sample_perf;
int16_t _last_raw_mag_x;
int16_t _last_raw_mag_y;
int16_t _last_raw_mag_z;
hrt_abstime _mag_last_measure{0};
#endif
unsigned _call_accel_interval;
ringbuffer::RingBuffer *_accel_reports;
struct accel_calibration_s _accel_scale;
unsigned _accel_range_m_s2;
float _accel_range_scale;
unsigned _accel_samplerate;
unsigned _accel_onchip_filter_bandwith;
orb_advert_t _accel_topic;
int _accel_orb_class_instance;
int _accel_class_instance;
unsigned _accel_read;
perf_counter_t _accel_sample_perf;
perf_counter_t _bad_registers;
perf_counter_t _bad_values;
perf_counter_t _accel_duplicates;
uint8_t _register_wait;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
Integrator _accel_int;
enum Rotation _rotation;
// values used to
float _last_accel[3];
uint8_t _constant_accel_count;
// last temperature value
float _last_temperature;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define FXOS8701C_NUM_CHECKED_REGISTERS 5
static const uint8_t _checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[FXOS8701C_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
/**
* Start automatic measurement.
*/
void start();
/**
* Stop automatic measurement.
*/
void stop();
/**
* Reset chip.
*
* Resets the chip and measurements ranges, but not scale and offset.
*/
void reset();
/**
* disable I2C on the chip
*/
void disable_i2c();
/**
* check key registers for correct values
*/
void check_registers(void);
/**
* Fetch accel measurements from the sensor and update the report ring.
*/
void measure();
/**
* Fetch mag measurements from the sensor and update the report ring.
*/
void mag_measure();
void check_registers();
/**
* Read a register from the FXOS8701C
@ -329,24 +192,6 @@ private:
*/
int mag_set_range(unsigned max_g);
/**
* Set the FXOS8701C on-chip anti-alias filter bandwith.
*
* @param bandwidth The anti-alias filter bandwidth in Hz
* Zero selects the highest bandwidth
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth);
/**
* Set the driver lowpass filter bandwidth.
*
* @param bandwidth The anti-alias filter bandwidth in Hz
* Zero selects the highest bandwidth
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int accel_set_driver_lowpass_filter(float samplerate, float bandwidth);
/**
* Set the FXOS8701C internal accel and mag sampling frequency.
*
@ -357,53 +202,30 @@ private:
*/
int accel_set_samplerate(unsigned frequency);
/**
* Set the FXOS8701CQ internal mag sampling frequency.
*
* @param frequency The mag reporting frequency is set to not less than
* this value. (sampling is all way the same as accel
* Zero selects the maximum rate supported.
* @return OK if the value can be supported.
*/
int mag_set_samplerate(unsigned frequency);
/* this class cannot be copied */
FXOS8701CQ(const FXOS8701CQ &);
FXOS8701CQ operator=(const FXOS8701CQ &);
};
PX4Accelerometer _px4_accel;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/**
* Helper class implementing the mag driver node.
*/
class FXOS8701CQ_mag : public device::CDev
{
public:
FXOS8701CQ_mag(FXOS8701CQ *parent);
~FXOS8701CQ_mag();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class FXOS8701CQ;
void parent_poll_notify();
private:
FXOS8701CQ *_parent;
orb_advert_t _mag_topic;
int _mag_orb_class_instance;
int _mag_class_instance;
void measure();
/* this class does not allow copying due to ptr data members */
FXOS8701CQ_mag(const FXOS8701CQ_mag &);
FXOS8701CQ_mag operator=(const FXOS8701CQ_mag &);
};
PX4Magnetometer _px4_mag;
hrt_abstime _mag_last_measure{0};
perf_counter_t _mag_sample_perf;
#endif
unsigned _accel_samplerate{FXOS8701C_ACCEL_DEFAULT_RATE};
perf_counter_t _accel_sample_perf;
perf_counter_t _accel_sample_interval_perf;
perf_counter_t _bad_registers;
perf_counter_t _accel_duplicates;
uint8_t _register_wait{0};
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
static constexpr int FXOS8701C_NUM_CHECKED_REGISTERS{5};
static const uint8_t _checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[FXOS8701C_NUM_CHECKED_REGISTERS] {};
uint8_t _checked_next{0};
};

308
src/drivers/imu/fxos8701cq/fxos8701cq_main.cpp
View File

@ -39,22 +39,22 @@
#include "FXOS8701CQ.hpp"
#include <px4_getopt.h>
/**
* Local functions in support of the shell command.
*/
namespace fxos8701cq
{
FXOS8701CQ *g_dev;
FXOS8701CQ *g_dev{nullptr};
void start(bool external_bus, enum Rotation rotation);
void test();
void reset();
void info();
void stop();
void regdump();
void usage();
void test_error();
int start(bool external_bus, enum Rotation rotation);
int info();
int stop();
int regdump();
int usage();
int test_error();
/**
* Start the driver.
@ -62,27 +62,25 @@ void test_error();
* This function call only returns once the driver is
* up and running or failed to detect the sensor.
*/
void
int
start(bool external_bus, enum Rotation rotation)
{
int fd;
if (g_dev != nullptr) {
PX4_INFO("already started");
exit(0);
return 0;
}
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_ACCEL_MAG)
g_dev = new FXOS8701CQ(PX4_SPI_BUS_EXT, FXOS8701C_DEVICE_PATH_ACCEL, PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
g_dev = new FXOS8701CQ(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
#else
PX4_ERR("External SPI not available");
exit(0);
return 0;
#endif
} else {
g_dev = new FXOS8701CQ(PX4_SPI_BUS_SENSORS, FXOS8701C_DEVICE_PATH_ACCEL, PX4_SPIDEV_ACCEL_MAG, rotation);
g_dev = new FXOS8701CQ(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_ACCEL_MAG, rotation);
}
if (g_dev == nullptr) {
@ -94,34 +92,7 @@ start(bool external_bus, enum Rotation rotation)
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(FXOS8701C_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
int fd_mag;
fd_mag = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY);
/* don't fail if open cannot be opened */
if (0 <= fd_mag) {
if (ioctl(fd_mag, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
}
close(fd_mag);
#endif
close(fd);
exit(0);
return PX4_OK;
fail:
if (g_dev != nullptr) {
@ -129,236 +100,89 @@ fail:
g_dev = nullptr;
}
errx(1, "driver start failed");
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test()
{
int rv = 1;
int fd_accel = -1;
sensor_accel_s accel_report;
ssize_t sz;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
int fd_mag = -1;
int ret;
struct mag_report m_report;
#endif
/* get the driver */
fd_accel = open(FXOS8701C_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd_accel < 0) {
PX4_ERR("%s open failed", FXOS8701C_DEVICE_PATH_ACCEL);
goto exit_none;
}
/* do a simple demand read */
sz = read(fd_accel, &accel_report, sizeof(sensor_accel_s));
if (sz != sizeof(sensor_accel_s)) {
PX4_ERR("immediate read failed");
goto exit_with_accel;
}
PX4_INFO("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x);
PX4_INFO("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y);
PX4_INFO("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z);
PX4_INFO("accel x: \t%d\traw", (int)accel_report.x_raw);
PX4_INFO("accel y: \t%d\traw", (int)accel_report.y_raw);
PX4_INFO("accel z: \t%d\traw", (int)accel_report.z_raw);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/* get the driver */
fd_mag = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY);
if (fd_mag < 0) {
PX4_ERR("%s open failed", FXOS8701C_DEVICE_PATH_MAG);
goto exit_with_accel;
}
/* check if mag is onboard or external */
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) {
PX4_ERR("failed to get if mag is onboard or external");
goto exit_with_mag_accel;
}
PX4_INFO("mag device active: %s", ret ? "external" : "onboard");
/* do a simple demand read */
sz = read(fd_mag, &m_report, sizeof(m_report));
if (sz != sizeof(m_report)) {
PX4_ERR("immediate read failed");
goto exit_with_mag_accel;
}
PX4_INFO("mag x: \t% 9.5f\tga", (double)m_report.x);
PX4_INFO("mag y: \t% 9.5f\tga", (double)m_report.y);
PX4_INFO("mag z: \t% 9.5f\tga", (double)m_report.z);
PX4_INFO("mag x: \t%d\traw", (int)m_report.x_raw);
PX4_INFO("mag y: \t%d\traw", (int)m_report.y_raw);
PX4_INFO("mag z: \t%d\traw", (int)m_report.z_raw);
#endif
/* reset to default polling */
if (ioctl(fd_accel, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("reset to default polling");
} else {
rv = 0;
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
exit_with_mag_accel:
close(fd_mag);
#endif
exit_with_accel:
close(fd_accel);
reset();
if (rv == 0) {
PX4_INFO("PASS");
}
exit_none:
exit(rv);
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(FXOS8701C_DEVICE_PATH_ACCEL, O_RDONLY);
int rv = 1;
if (fd < 0) {
PX4_ERR("Open failed\n");
exit(1);
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
PX4_ERR("driver reset failed");
exit(1);
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("accel pollrate reset failed");
exit(1);
}
close(fd);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
fd = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY);
if (fd < 0) {
PX4_ERR("mag could not be opened, external mag might be used");
} else {
/* no need to reset the mag as well, the reset() is the same */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
PX4_ERR("mag pollrate reset failed");
} else {
rv = 0;
}
}
close(fd);
#endif
exit(rv);
PX4_ERR("driver start failed");
return PX4_ERROR;
}
/**
* Print a little info about the driver.
*/
void
int
info()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
exit(1);
return 1;
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
return 0;
}
void
int
stop()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
exit(1);
return 1;
}
delete g_dev;
g_dev = nullptr;
exit(0);
return 0;
}
/**
* dump registers from device
*/
void
int
regdump()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
exit(1);
return 1;
}
printf("regdump @ %p\n", g_dev);
g_dev->print_registers();
exit(0);
return 0;
}
/**
* trigger an error
*/
void
int
test_error()
{
if (g_dev == nullptr) {
PX4_ERR("driver not running\n");
exit(1);
return 1;
}
g_dev->test_error();
exit(0);
return 0;
}
void
int
usage()
{
PX4_INFO("missing command: try 'start', 'info', 'stop', 'test', 'reset', 'testerror' or 'regdump'");
PX4_INFO("missing command: try 'start', 'info', 'stop', 'testerror' or 'regdump'");
PX4_INFO("options:");
PX4_INFO(" -X (external bus)");
PX4_INFO(" -R rotation");
return 0;
}
} // namespace
int
fxos8701cq_main(int argc, char *argv[])
extern "C" { __EXPORT int fxos8701cq_main(int argc, char *argv[]); }
int fxos8701cq_main(int argc, char *argv[])
{
bool external_bus = false;
int ch;
@ -385,52 +209,22 @@ fxos8701cq_main(int argc, char *argv[])
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
fxos8701cq::start(external_bus, rotation);
return fxos8701cq::start(external_bus, rotation);
} else if (!strcmp(verb, "stop")) {
return fxos8701cq::stop();
} else if (!strcmp(verb, "info")) {
return fxos8701cq::info();
} else if (!strcmp(verb, "regdump")) {
return fxos8701cq::regdump();
} else if (!strcmp(verb, "testerror")) {
return fxos8701cq::test_error();
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
fxos8701cq::test();
}
if (!strcmp(verb, "stop")) {
fxos8701cq::stop();
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
fxos8701cq::reset();
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info")) {
fxos8701cq::info();
}
/*
* dump device registers
*/
if (!strcmp(verb, "regdump")) {
fxos8701cq::regdump();
}
/*
* trigger an error
*/
if (!strcmp(verb, "testerror")) {
fxos8701cq::test_error();
}
errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset', 'info', 'testerror' or 'regdump'");
PX4_ERR("unrecognized command, try 'start', 'stop', 'info', 'testerror' or 'regdump'");
return PX4_ERROR;
}