fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 11:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

fxos8701cq: Fully remove the mag to facilitate calibration (#10499)

Browse Source 
The previous removal of the mag was not complete. It just disabled publishing. This commit conditionally removes all the the mag functionality.
This commit is contained in:
David Sidrane
2018-09-17 13:07:46 -04:00
committed by Daniel Agar
parent e17f0802cc
commit c1df899b6f
1 changed files with 108 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/fxos8701cq/fxos8701cq.cpp
+108 -39
View File
@@ -67,7 +67,9 @@
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_mag.h>
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
# include <drivers/drv_mag.h>
#endif
#include <drivers/device/ringbuffer.h>
#include <drivers/device/integrator.h>
@@ -84,7 +86,9 @@
#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"
#define FXOS8701C_DEVICE_PATH_MAG "/dev/fxos8701cq_mag"
#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)
@@ -150,7 +154,9 @@
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
{
@@ -181,23 +187,37 @@ public:
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:
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
FXOS8701CQ_mag *_mag;
struct hrt_call _mag_call;
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;
#endif
struct hrt_call _accel_call;
struct hrt_call _mag_call;
unsigned _call_accel_interval;
unsigned _call_mag_interval;
ringbuffer::RingBuffer *_accel_reports;
ringbuffer::RingBuffer *_mag_reports;
struct accel_calibration_s _accel_scale;
unsigned _accel_range_m_s2;
@@ -205,20 +225,14 @@ private:
unsigned _accel_samplerate;
unsigned _accel_onchip_filter_bandwith;
struct mag_calibration_s _mag_scale;
unsigned _mag_range_ga;
float _mag_range_scale;
unsigned _mag_samplerate;
orb_advert_t _accel_topic;
int _accel_orb_class_instance;
int _accel_class_instance;
unsigned _accel_read;
unsigned _mag_read;
perf_counter_t _accel_sample_perf;
perf_counter_t _mag_sample_perf;
perf_counter_t _bad_registers;
perf_counter_t _bad_values;
perf_counter_t _accel_duplicates;
@@ -239,9 +253,6 @@ private:
// last temperature value
float _last_temperature;
int16_t _last_raw_mag_x;
int16_t _last_raw_mag_y;
int16_t _last_raw_mag_z;
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
@@ -416,6 +427,7 @@ const uint8_t FXOS8701CQ::_checked_registers[FXOS8701C_NUM_CHECKED_REGISTERS] =
FXOS8701CQ_M_CTRL_REG2,
};
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/**
* Helper class implementing the mag driver node.
*/
@@ -449,34 +461,39 @@ private:
FXOS8701CQ_mag(const FXOS8701CQ_mag &);
FXOS8701CQ_mag operator=(const FXOS8701CQ_mag &);
};
#endif
FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation rotation) :
SPI("FXOS8701CQ", path, bus, device, SPIDEV_MODE0,
1 * 1000 * 1000),
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
_mag(new FXOS8701CQ_mag(this)),
_accel_call{},
_mag_call{},
_call_accel_interval(0),
_call_mag_interval(0),
_accel_reports(nullptr),
_mag_reports(nullptr),
_mag_scale{},
_mag_range_ga(0.0f),
_mag_range_scale(0.0f),
_mag_samplerate(0),
_mag_read(0),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "fxos8701cq_mag_read")),
_last_raw_mag_x(0),
_last_raw_mag_y(0),
_last_raw_mag_z(0),
#endif
_accel_call {},
_call_accel_interval(0),
_accel_reports(nullptr),
_accel_scale{},
_accel_range_m_s2(0.0f),
_accel_range_scale(0.0f),
_accel_samplerate(0),
_accel_onchip_filter_bandwith(0),
_mag_scale{},
_mag_range_ga(0.0f),
_mag_range_scale(0.0f),
_mag_samplerate(0),
_accel_topic(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_accel_read(0),
_mag_read(0),
_accel_sample_perf(perf_alloc(PC_ELAPSED, "fxos8701cq_acc_read")),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "fxos8701cq_mag_read")),
_bad_registers(perf_alloc(PC_COUNT, "fxos8701cq_bad_reg")),
_bad_values(perf_alloc(PC_COUNT, "fxos8701cq_bad_val")),
_accel_duplicates(perf_alloc(PC_COUNT, "fxos8701cq_acc_dupe")),
@@ -488,9 +505,6 @@ FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation
_rotation(rotation),
_constant_accel_count(0),
_last_temperature(0),
_last_raw_mag_x(0),
_last_raw_mag_y(0),
_last_raw_mag_z(0),
_checked_next(0)
{
// enable debug() calls
@@ -498,10 +512,11 @@ FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_FXOS8701C;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/* Prime _mag with parents devid. */
_mag->_device_id.devid = _device_id.devid;
_mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_FXOS8701C;
#endif
// default scale factors
_accel_scale.x_offset = 0.0f;
@@ -511,12 +526,14 @@ FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation
_accel_scale.z_offset = 0.0f;
_accel_scale.z_scale = 1.0f;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
_mag_scale.x_offset = 0.0f;
_mag_scale.x_scale = 1.0f;
_mag_scale.y_offset = 0.0f;
_mag_scale.y_scale = 1.0f;
_mag_scale.z_offset = 0.0f;
_mag_scale.z_scale = 1.0f;
#endif
}
FXOS8701CQ::~FXOS8701CQ()
@@ -529,19 +546,25 @@ FXOS8701CQ::~FXOS8701CQ()
delete _accel_reports;
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
if (_mag_reports != nullptr) {
delete _mag_reports;
}
#endif
if (_accel_class_instance != -1) {
unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance);
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
delete _mag;
perf_free(_mag_sample_perf);
#endif
/* delete the perf counter */
perf_free(_accel_sample_perf);
perf_free(_mag_sample_perf);
perf_free(_bad_registers);
perf_free(_bad_values);
perf_free(_accel_duplicates);
@@ -555,22 +578,25 @@ FXOS8701CQ::init()
/* do SPI init (and probe) first */
if (SPI::init() != OK) {
PX4_ERR("SPI init failed");
return PX4_ERROR;
return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr) {
return PX4_ERROR;
return ret;
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr) {
return PX4_ERROR;
return ret;
}
#endif
// set software low pass filter for controllers
param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
float accel_cut = FXOS8701C_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
@@ -586,6 +612,7 @@ FXOS8701CQ::init()
reset();
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/* do CDev init for the mag device node */
ret = _mag->init();
@@ -594,9 +621,12 @@ FXOS8701CQ::init()
return PX4_ERROR;
}
#endif
/* fill report structures */
measure();
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/* advertise sensor topic, measure manually to initialize valid report */
struct mag_report mrp;
_mag_reports->get(&mrp);
@@ -610,6 +640,7 @@ FXOS8701CQ::init()
return PX4_ERROR;
}
#endif
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
@@ -657,14 +688,17 @@ FXOS8701CQ::reset()
// operate in conjunction with this on-chip filter
accel_set_onchip_lowpass_filter_bandwidth(FXOS8701C_ACCEL_DEFAULT_ONCHIP_FILTER_FREQ);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
mag_set_range(FXOS8701C_MAG_DEFAULT_RANGE_GA);
#endif
/* enable set it To Standby mode at 800 Hz which becomes 400 Hz due to hybird mode */
write_checked_reg(FXOS8701CQ_CTRL_REG1, CTRL_REG1_DR(0) | CTRL_REG1_ACTIVE);
_accel_read = 0;
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
_mag_read = 0;
#endif
}
int
@@ -721,6 +755,7 @@ FXOS8701CQ::read(struct file *filp, char *buffer, size_t buflen)
return ret;
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
ssize_t
FXOS8701CQ::mag_read(struct file *filp, char *buffer, size_t buflen)
{
@@ -761,6 +796,7 @@ FXOS8701CQ::mag_read(struct file *filp, char *buffer, size_t buflen)
return ret;
}
#endif
int
FXOS8701CQ::ioctl(struct file *filp, int cmd, unsigned long arg)
@@ -890,6 +926,7 @@ FXOS8701CQ::ioctl(struct file *filp, int cmd, unsigned long arg)
}
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
int
FXOS8701CQ::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
{
@@ -1001,6 +1038,7 @@ FXOS8701CQ::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
return SPI::ioctl(filp, cmd, arg);
}
}
#endif
uint8_t
FXOS8701CQ::read_reg(unsigned reg)
@@ -1087,6 +1125,7 @@ FXOS8701CQ::accel_set_range(unsigned max_g)
return OK;
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
int
FXOS8701CQ::mag_set_range(unsigned max_ga)
{
@@ -1095,6 +1134,7 @@ FXOS8701CQ::mag_set_range(unsigned max_ga)
return OK;
}
#endif
int
FXOS8701CQ::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
@@ -1155,6 +1195,7 @@ FXOS8701CQ::accel_set_samplerate(unsigned frequency)
return OK;
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
int
FXOS8701CQ::mag_set_samplerate(unsigned frequency)
{
@@ -1183,7 +1224,7 @@ FXOS8701CQ::mag_set_samplerate(unsigned frequency)
return OK;
}
#endif
void
FXOS8701CQ::start()
@@ -1193,7 +1234,9 @@ FXOS8701CQ::start()
/* reset the report ring */
_accel_reports->flush();
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
_mag_reports->flush();
#endif
/* start polling at the specified rate */
hrt_call_every(&_accel_call,
@@ -1209,14 +1252,17 @@ void
FXOS8701CQ::stop()
{
hrt_cancel(&_accel_call);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
hrt_cancel(&_mag_call);
#endif
/* reset internal states */
memset(_last_accel, 0, sizeof(_last_accel));
/* discard unread data in the buffers */
_accel_reports->flush();
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
_mag_reports->flush();
#endif
}
void
@@ -1352,11 +1398,13 @@ FXOS8701CQ::measure()
accel_report.y_raw = swap16RightJustify14(raw_accel_mag_report.y);
accel_report.z_raw = swap16RightJustify14(raw_accel_mag_report.z);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
/* Save off the Mag readings todo: revist integrating theses */
_last_raw_mag_x = swap16(raw_accel_mag_report.mx);
_last_raw_mag_y = swap16(raw_accel_mag_report.my);
_last_raw_mag_z = swap16(raw_accel_mag_report.mz);
#endif
float xraw_f = accel_report.x_raw;
float yraw_f = accel_report.y_raw;
@@ -1436,6 +1484,7 @@ FXOS8701CQ::measure()
perf_end(_accel_sample_perf);
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
void
FXOS8701CQ::mag_measure()
{
@@ -1499,19 +1548,26 @@ FXOS8701CQ::mag_measure()
/* stop the perf counter */
perf_end(_mag_sample_perf);
}
#endif
void
FXOS8701CQ::print_info()
{
printf("accel reads: %u\n", _accel_read);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
printf("mag reads: %u\n", _mag_read);
#endif
perf_print_counter(_accel_sample_perf);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
perf_print_counter(_mag_sample_perf);
#endif
perf_print_counter(_bad_registers);
perf_print_counter(_bad_values);
perf_print_counter(_accel_duplicates);
_accel_reports->print_info("accel reports");
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
_mag_reports->print_info("mag reports");
#endif
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i = 0; i < FXOS8701C_NUM_CHECKED_REGISTERS; i++) {
@@ -1560,6 +1616,7 @@ FXOS8701CQ::test_error()
write_reg(FXOS8701CQ_CTRL_REG1, 0);
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
FXOS8701CQ_mag::FXOS8701CQ_mag(FXOS8701CQ *parent) :
CDev("FXOS8701C_mag", FXOS8701C_DEVICE_PATH_MAG),
_parent(parent),
@@ -1626,6 +1683,7 @@ FXOS8701CQ_mag::measure_trampoline(void *arg)
{
_parent->mag_measure_trampoline(arg);
}
#endif
/**
* Local functions in support of the shell command.
@@ -1653,7 +1711,7 @@ void test_error();
void
start(bool external_bus, enum Rotation rotation, unsigned range)
{
int fd, fd_mag;
int fd;
if (g_dev != nullptr) {
PX4_INFO("already started");
@@ -1697,6 +1755,8 @@ start(bool external_bus, enum Rotation rotation, unsigned range)
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 */
@@ -1706,8 +1766,10 @@ start(bool external_bus, enum Rotation rotation, unsigned range)
}
}
close(fd);
close(fd_mag);
#endif
close(fd);
exit(0);
fail:
@@ -1732,10 +1794,11 @@ test()
int fd_accel = -1;
struct accel_report accel_report;
ssize_t sz;
int ret;
#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);
@@ -1761,6 +1824,7 @@ test()
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);
@@ -1791,6 +1855,7 @@ test()
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) {
@@ -1800,8 +1865,10 @@ test()
rv = 0;
}
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
exit_with_mag_accel:
close(fd_mag);
#endif
exit_with_accel:
@@ -1843,6 +1910,7 @@ reset()
close(fd);
#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
fd = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY);
if (fd < 0) {
@@ -1859,6 +1927,7 @@ reset()
}
close(fd);
#endif
exit(rv);
}