fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 05:21:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

fxas21002c move to PX4Gyroscope and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2019-05-31 18:20:32 -04:00
parent c8ea198a78
commit b57dff8594
2 changed files with 65 additions and 510 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/nxp/fmuk66-v3/init/rc.board_sensors
+1 -1
View File
@@ -21,4 +21,4 @@ mpl3115a2 -I start
fxos8701cq start -R 0 fxos8701cq start -R 0
# Internal SPI (gyro) # Internal SPI (gyro)
fxas21002c start -R 0 fxas21002c start
src/drivers/imu/fxas21002c/fxas21002c.cpp
+64 -509
View File
@@ -37,51 +37,20 @@
* connected via SPI * connected via SPI
*/ */
#include <drivers/device/spi.h>
#include <drivers/drv_hrt.h>
#include <lib/conversion/rotation.h>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <perf/perf_counter.h>
#include <px4_config.h> #include <px4_config.h>
#include <px4_defines.h> #include <px4_defines.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <px4_log.h>
#include <perf/perf_counter.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <drivers/drv_gyro.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/integrator.h>
#include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#include <px4_getopt.h> #include <px4_getopt.h>
#include <systemlib/err.h>
#include <px4_work_queue/ScheduledWorkItem.hpp> #include <px4_work_queue/ScheduledWorkItem.hpp>
/* SPI protocol address bits */ /* SPI protocol address bits */
#define DIR_READ(a) ((a) | (1 << 7)) #define DIR_READ(a) ((a) | (1 << 7))
#define DIR_WRITE(a) ((a) & 0x7f) #define DIR_WRITE(a) ((a) & 0x7f)
#define swap16(w) __builtin_bswap16((w)) #define swap16(w) __builtin_bswap16((w))
#define FXAS21002C_DEVICE_PATH_GYRO "/dev/fxas21002c_gyro"
#define FXAS21002C_DEVICE_PATH_GYRO_EXT "/dev/fxas21002c_gyro_ext"
#define FXAS21002C_STATUS 0x00 #define FXAS21002C_STATUS 0x00
#define FXAS21002C_OUT_X_MSB 0x01 #define FXAS21002C_OUT_X_MSB 0x01
@@ -210,8 +179,6 @@
#define FXAS21002C_TEMP_OFFSET_CELSIUS 40 #define FXAS21002C_TEMP_OFFSET_CELSIUS 40
#define FXAS21002C_DEFAULT_ONCHIP_FILTER_FREQ 64 // ODR dependant #define FXAS21002C_DEFAULT_ONCHIP_FILTER_FREQ 64 // ODR dependant
#define FXAS21002C_MAX_OFFSET 0.45f /**< max offset: 25 degrees/s */
/* /*
we set the timer interrupt to run a bit faster than the desired we set the timer interrupt to run a bit faster than the desired
sample rate and then throw away duplicates using the data ready bit. sample rate and then throw away duplicates using the data ready bit.
@@ -221,19 +188,18 @@
*/ */
#define FXAS21002C_TIMER_REDUCTION 250 #define FXAS21002C_TIMER_REDUCTION 250
using namespace time_literals;
extern "C" { __EXPORT int fxas21002c_main(int argc, char *argv[]); } extern "C" { __EXPORT int fxas21002c_main(int argc, char *argv[]); }
class FXAS21002C : public device::SPI, public px4::ScheduledWorkItem class FXAS21002C : public device::SPI, public px4::ScheduledWorkItem
{ {
public: public:
FXAS21002C(int bus, const char *path, uint32_t device, enum Rotation rotation); FXAS21002C(int bus, uint32_t device, enum Rotation rotation);
virtual ~FXAS21002C(); virtual ~FXAS21002C();
virtual int init(); 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. * Diagnostics - print some basic information about the driver.
*/ */
@@ -254,48 +220,35 @@ protected:
private: private:
unsigned _call_interval; PX4Gyroscope _px4_gyro;
ringbuffer::RingBuffer *_reports; unsigned _current_rate{800};
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
orb_advert_t _gyro_topic;
int _orb_class_instance;
int _class_instance;
unsigned _current_rate;
unsigned _orientation;
float _last_temperature;
unsigned _read;
unsigned _read{0};
perf_counter_t _sample_perf; perf_counter_t _sample_perf;
perf_counter_t _errors; perf_counter_t _errors;
perf_counter_t _bad_registers; perf_counter_t _bad_registers;
perf_counter_t _duplicates; perf_counter_t _duplicates;
uint8_t _register_wait; uint8_t _register_wait{0};
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
Integrator _gyro_int;
enum Rotation _rotation;
/* this is used to support runtime checking of key /* this is used to support runtime checking of key
*configuration registers to detect SPI bus errors and sensor *configuration registers to detect SPI bus errors and sensor
* reset * reset
*/ */
#define FXAS21002C_NUM_CHECKED_REGISTERS 6 static constexpr int FXAS21002C_NUM_CHECKED_REGISTERS{6};
static const uint8_t _checked_registers[FXAS21002C_NUM_CHECKED_REGISTERS]; static constexpr uint8_t _checked_registers[FXAS21002C_NUM_CHECKED_REGISTERS] {
uint8_t _checked_values[FXAS21002C_NUM_CHECKED_REGISTERS]; FXAS21002C_WHO_AM_I,
uint8_t _checked_next; FXAS21002C_F_SETUP,
FXAS21002C_CTRL_REG0,
FXAS21002C_CTRL_REG1,
FXAS21002C_CTRL_REG2,
FXAS21002C_CTRL_REG3,
};
uint8_t _checked_values[FXAS21002C_NUM_CHECKED_REGISTERS] {};
uint8_t _checked_next{0};
/** /**
* Start automatic measurement. * Start automatic measurement.
@@ -314,11 +267,6 @@ private:
*/ */
void reset(); void reset();
/**
* disable I2C on the chip
*/
void disable_i2c();
/** /**
* Put the chip In stand by * Put the chip In stand by
*/ */
@@ -391,88 +339,28 @@ private:
*/ */
int set_samplerate(unsigned frequency); int set_samplerate(unsigned frequency);
/**
* Set the lowpass filter of the driver
*
* @param samplerate The current samplerate
* @param frequency The cutoff frequency for the lowpass filter
*/
void set_sw_lowpass_filter(float samplerate, float bandwidth);
/* /*
set onchip low pass filter frequency set onchip low pass filter frequency
*/ */
void set_onchip_lowpass_filter(int frequency_hz); void set_onchip_lowpass_filter(int frequency_hz);
/**
* Self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/* this class cannot be copied */
FXAS21002C(const FXAS21002C &);
FXAS21002C operator=(const FXAS21002C &);
}; };
/* /*
list of registers that will be checked in check_registers(). Note list of registers that will be checked in check_registers(). Note
that ADDR_WHO_AM_I must be first in the list. that ADDR_WHO_AM_I must be first in the list.
*/ */
const uint8_t FXAS21002C::_checked_registers[FXAS21002C_NUM_CHECKED_REGISTERS] = { constexpr uint8_t FXAS21002C::_checked_registers[];
FXAS21002C_WHO_AM_I,
FXAS21002C_F_SETUP,
FXAS21002C_CTRL_REG0,
FXAS21002C_CTRL_REG1,
FXAS21002C_CTRL_REG2,
FXAS21002C_CTRL_REG3,
};
FXAS21002C::FXAS21002C(int bus, uint32_t device, enum Rotation rotation) :
FXAS21002C::FXAS21002C(int bus, const char *path, uint32_t device, enum Rotation rotation) : SPI("FXAS21002C", nullptr, bus, device, SPIDEV_MODE0, 2 * 1000 * 1000),
SPI("FXAS21002C", path, bus, device, SPIDEV_MODE0, 2 * 1000 * 1000),
ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())), ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())),
_call_interval(0), _px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation),
_reports(nullptr),
_gyro_scale{},
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_gyro_topic(nullptr),
_orb_class_instance(-1),
_class_instance(-1),
_current_rate(800),
_orientation(0),
_last_temperature(0.0f),
_read(0),
_sample_perf(perf_alloc(PC_ELAPSED, "fxas21002c_acc_read")), _sample_perf(perf_alloc(PC_ELAPSED, "fxas21002c_acc_read")),
_errors(perf_alloc(PC_COUNT, "fxas21002c_err")), _errors(perf_alloc(PC_COUNT, "fxas21002c_err")),
_bad_registers(perf_alloc(PC_COUNT, "fxas21002c_bad_reg")), _bad_registers(perf_alloc(PC_COUNT, "fxas21002c_bad_reg")),
_duplicates(perf_alloc(PC_COUNT, "fxas21002c_acc_dupe")), _duplicates(perf_alloc(PC_COUNT, "fxas21002c_acc_dupe"))
_register_wait(0),
_gyro_filter_x(FXAS21002C_DEFAULT_RATE, FXAS21002C_DEFAULT_FILTER_FREQ),
_gyro_filter_y(FXAS21002C_DEFAULT_RATE, FXAS21002C_DEFAULT_FILTER_FREQ),
_gyro_filter_z(FXAS21002C_DEFAULT_RATE, FXAS21002C_DEFAULT_FILTER_FREQ),
_gyro_int(1000000 / FXAS21002C_MAX_OUTPUT_RATE, true),
_rotation(rotation),
_checked_values{},
_checked_next(0)
{ {
// enable debug() calls _px4_gyro.set_device_type(DRV_GYR_DEVTYPE_FXAS2100C);
//_debug_enabled = false;
_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_FXAS2100C;
// default scale factors
_gyro_scale.x_offset = 0.0f;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0.0f;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0.0f;
_gyro_scale.z_scale = 1.0f;
} }
FXAS21002C::~FXAS21002C() FXAS21002C::~FXAS21002C()
@@ -480,16 +368,6 @@ FXAS21002C::~FXAS21002C()
/* make sure we are truly inactive */ /* make sure we are truly inactive */
stop(); stop();
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
if (_class_instance != -1) {
unregister_class_devname(GYRO_BASE_DEVICE_PATH, _class_instance);
}
/* delete the perf counter */ /* delete the perf counter */
perf_free(_sample_perf); perf_free(_sample_perf);
perf_free(_errors); perf_free(_errors);
@@ -506,47 +384,13 @@ FXAS21002C::init()
return PX4_ERROR; return PX4_ERROR;
} }
param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
float gyro_cut = FXAS21002C_DEFAULT_FILTER_FREQ;
if (gyro_cut_ph != PARAM_INVALID && param_get(gyro_cut_ph, &gyro_cut) == PX4_OK) {
set_sw_lowpass_filter(FXAS21002C_DEFAULT_RATE, gyro_cut);
} else {
PX4_ERR("IMU_GYRO_CUTOFF param invalid");
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s));
if (_reports == nullptr) {
return PX4_ERROR;
}
reset(); reset();
/* fill report structures */ start();
measure();
_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
sensor_gyro_s grp;
_reports->get(&grp);
/* measurement will have generated a report, publish */
_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_orb_class_instance, (external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_gyro_topic == nullptr) {
PX4_ERR("ADVERT ERR");
return PX4_ERROR;
}
return PX4_OK; return PX4_OK;
} }
void void
FXAS21002C::reset() FXAS21002C::reset()
{ {
@@ -556,7 +400,6 @@ FXAS21002C::reset()
* [4-2]: DR[2-0]=000 for 200Hz ODR * [4-2]: DR[2-0]=000 for 200Hz ODR
* [1-0]: Active=0, Ready=0 for Standby mode * [1-0]: Active=0, Ready=0 for Standby mode
*/ */
write_reg(FXAS21002C_CTRL_REG1, 0); write_reg(FXAS21002C_CTRL_REG1, 0);
/* write 0000 0000 = 0x00 to CTRL_REG0 to configure range and filters /* write 0000 0000 = 0x00 to CTRL_REG0 to configure range and filters
@@ -566,18 +409,16 @@ FXAS21002C::reset()
* [2]: HPF_EN=0 disable HPF * [2]: HPF_EN=0 disable HPF
* [1-0]: FS[1-0]=00 for 1600dps (TBD CHANGE TO 2000dps when final trimmed parts available) * [1-0]: FS[1-0]=00 for 1600dps (TBD CHANGE TO 2000dps when final trimmed parts available)
*/ */
write_checked_reg(FXAS21002C_CTRL_REG0, CTRL_REG0_BW_LOW | CTRL_REG0_FS_2000_DPS); write_checked_reg(FXAS21002C_CTRL_REG0, CTRL_REG0_BW_LOW | CTRL_REG0_FS_2000_DPS);
/* write CTRL_REG1 to configure 800Hz ODR and enter Active mode */ /* write CTRL_REG1 to configure 800Hz ODR and enter Active mode */
write_checked_reg(FXAS21002C_CTRL_REG1, CTRL_REG1_DR_800HZ | CTRL_REG1_ACTIVE); write_checked_reg(FXAS21002C_CTRL_REG1, CTRL_REG1_DR_800HZ | CTRL_REG1_ACTIVE);
/* Set the default */ /* Set the default */
set_samplerate(FXAS21002C_DEFAULT_RATE);
set_samplerate(0);
set_range(FXAS21002C_DEFAULT_RANGE_DPS); set_range(FXAS21002C_DEFAULT_RANGE_DPS);
set_onchip_lowpass_filter(FXAS21002C_DEFAULT_ONCHIP_FILTER_FREQ); set_onchip_lowpass_filter(FXAS21002C_DEFAULT_ONCHIP_FILTER_FREQ);
_read = 0; _read = 0;
} }
@@ -595,122 +436,6 @@ FXAS21002C::probe()
return -EIO; return -EIO;
} }
ssize_t
FXAS21002C::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(sensor_gyro_s);
sensor_gyro_s *gbuf = reinterpret_cast<sensor_gyro_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_call_interval > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the measurement code while we are doing this;
* we are careful to avoid racing with it.
*/
while (count--) {
if (_reports->get(gbuf)) {
ret += sizeof(*gbuf);
gbuf++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement */
_reports->flush();
measure();
/* measurement will have generated a report, copy it out */
if (_reports->get(gbuf)) {
ret = sizeof(*gbuf);
}
return ret;
}
int
FXAS21002C::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, FXAS21002C_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned interval = 1000000 / arg;
/* check against maximum sane rate */
if (interval < 1000) {
return -EINVAL;
}
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call_interval = interval;
/* adjust filters */
float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f / interval;
set_sw_lowpass_filter(sample_rate, cutoff_freq_hz);
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case SENSORIOCRESET:
reset();
return OK;
case GYROIOCSSCALE:
/* copy scale in */
memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale));
return OK;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
int
FXAS21002C::self_test()
{
if (_read == 0) {
return 1;
}
return 0;
}
uint8_t uint8_t
FXAS21002C::read_reg(unsigned reg) FXAS21002C::read_reg(unsigned reg)
{ {
@@ -750,9 +475,7 @@ FXAS21002C::write_checked_reg(unsigned reg, uint8_t value)
void void
FXAS21002C::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) FXAS21002C::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{ {
uint8_t val; uint8_t val = read_reg(reg);
val = read_reg(reg);
val &= ~clearbits; val &= ~clearbits;
val |= setbits; val |= setbits;
write_checked_reg(reg, val); write_checked_reg(reg, val);
@@ -763,29 +486,28 @@ FXAS21002C::set_range(unsigned max_dps)
{ {
uint8_t bits = CTRL_REG0_FS_250_DPS; uint8_t bits = CTRL_REG0_FS_250_DPS;
float new_range_scale_dps_digit; float new_range_scale_dps_digit;
float new_range;
if (max_dps == 0) { if (max_dps == 0) {
max_dps = 2000; max_dps = 2000;
} }
if (max_dps <= 250) { if (max_dps <= 250) {
new_range = 250; //new_range = 250;
new_range_scale_dps_digit = 7.8125e-3f; new_range_scale_dps_digit = 7.8125e-3f;
bits = CTRL_REG0_FS_250_DPS; bits = CTRL_REG0_FS_250_DPS;
} else if (max_dps <= 500) { } else if (max_dps <= 500) {
new_range = 500; //new_range = 500;
new_range_scale_dps_digit = 15.625e-3f; new_range_scale_dps_digit = 15.625e-3f;
bits = CTRL_REG0_FS_500_DPS; bits = CTRL_REG0_FS_500_DPS;
} else if (max_dps <= 1000) { } else if (max_dps <= 1000) {
new_range = 1000; //new_range = 1000;
new_range_scale_dps_digit = 31.25e-3f; new_range_scale_dps_digit = 31.25e-3f;
bits = CTRL_REG0_FS_1000_DPS; bits = CTRL_REG0_FS_1000_DPS;
} else if (max_dps <= 2000) { } else if (max_dps <= 2000) {
new_range = 2000; //new_range = 2000;
new_range_scale_dps_digit = 62.5e-3f; new_range_scale_dps_digit = 62.5e-3f;
bits = CTRL_REG0_FS_2000_DPS; bits = CTRL_REG0_FS_2000_DPS;
@@ -794,15 +516,17 @@ FXAS21002C::set_range(unsigned max_dps)
} }
set_standby(_current_rate, true); set_standby(_current_rate, true);
_gyro_range_rad_s = new_range / 180.0f * M_PI_F;
_gyro_range_scale = new_range_scale_dps_digit / 180.0f * M_PI_F; _px4_gyro.set_scale(new_range_scale_dps_digit / 180.0f * M_PI_F);
modify_reg(FXAS21002C_CTRL_REG0, CTRL_REG0_FS_MASK, bits); modify_reg(FXAS21002C_CTRL_REG0, CTRL_REG0_FS_MASK, bits);
set_standby(_current_rate, false); set_standby(_current_rate, false);
return OK; return OK;
} }
void FXAS21002C::set_standby(int rate, bool standby_true) void
FXAS21002C::set_standby(int rate, bool standby_true)
{ {
uint8_t c = 0; uint8_t c = 0;
uint8_t s = 0; uint8_t s = 0;
@@ -869,10 +593,14 @@ FXAS21002C::set_samplerate(unsigned frequency)
set_standby(last_rate, true); set_standby(last_rate, true);
modify_reg(FXAS21002C_CTRL_REG1, CTRL_REG1_DR_MASK, bits); modify_reg(FXAS21002C_CTRL_REG1, CTRL_REG1_DR_MASK, bits);
set_standby(_current_rate, false); set_standby(_current_rate, false);
_px4_gyro.set_sample_rate(_current_rate);
return OK; return OK;
} }
void FXAS21002C::set_onchip_lowpass_filter(int frequency_hz) void
FXAS21002C::set_onchip_lowpass_filter(int frequency_hz)
{ {
int high = 256 / (800 / _current_rate); int high = 256 / (800 / _current_rate);
int med = high / 2 ; int med = high / 2 ;
@@ -907,37 +635,20 @@ void FXAS21002C::set_onchip_lowpass_filter(int frequency_hz)
set_standby(_current_rate, false); set_standby(_current_rate, false);
} }
void
FXAS21002C::set_sw_lowpass_filter(float samplerate, float bandwidth)
{
_gyro_filter_x.set_cutoff_frequency(samplerate, bandwidth);
_gyro_filter_y.set_cutoff_frequency(samplerate, bandwidth);
_gyro_filter_z.set_cutoff_frequency(samplerate, bandwidth);
}
void void
FXAS21002C::start() FXAS21002C::start()
{ {
/* make sure we are stopped first */ /* make sure we are stopped first */
stop(); stop();
/* reset the report ring */
_reports->flush();
/* start polling at the specified rate */ /* start polling at the specified rate */
ScheduleOnInterval(_call_interval - FXAS21002C_TIMER_REDUCTION, 10000); ScheduleOnInterval((1_s / FXAS21002C_DEFAULT_RATE) - FXAS21002C_TIMER_REDUCTION, 10000);
} }
void void
FXAS21002C::stop() FXAS21002C::stop()
{ {
ScheduleClear(); ScheduleClear();
/* reset internal states */
/* discard unread data in the buffers */
_reports->flush();
} }
void void
@@ -989,7 +700,7 @@ FXAS21002C::measure()
int16_t x; int16_t x;
int16_t y; int16_t y;
int16_t z; int16_t z;
} raw_gyro_report; } raw_gyro_report{};
#pragma pack(pop) #pragma pack(pop)
sensor_gyro_s gyro_report; sensor_gyro_s gyro_report;
@@ -1008,8 +719,8 @@ FXAS21002C::measure()
} }
/* fetch data from the sensor */ /* fetch data from the sensor */
memset(&raw_gyro_report, 0, sizeof(raw_gyro_report));
raw_gyro_report.cmd = DIR_READ(FXAS21002C_STATUS); raw_gyro_report.cmd = DIR_READ(FXAS21002C_STATUS);
const hrt_abstime timestamp_sample = hrt_absolute_time();
transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report)); transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report));
if (!(raw_gyro_report.status & DR_STATUS_ZYXDR)) { if (!(raw_gyro_report.status & DR_STATUS_ZYXDR)) {
@@ -1024,79 +735,22 @@ FXAS21002C::measure()
* compensated (factory trim values applied) when the device is operating in the Active * compensated (factory trim values applied) when the device is operating in the Active
* mode and actively measuring the angular rate. * mode and actively measuring the angular rate.
*/ */
if ((_read % _current_rate) == 0) { if ((_read % _current_rate) == 0) {
_last_temperature = read_reg(FXAS21002C_TEMP) * 1.0f; const float temperature = read_reg(FXAS21002C_TEMP) * 1.0f;
gyro_report.temperature = _last_temperature; _px4_gyro.set_temperature(temperature);
} }
/*
* 1) Scale raw value to SI units using scaling from datasheet.
* 2) Subtract static offset (in SI units)
* 3) Scale the statically calibrated values with a linear
* dynamically obtained factor
*
* Note: the static sensor offset is the number the sensor outputs
* at a nominally 'zero' input. Therefore the offset has to
* be subtracted.
*
* Example: A gyro outputs a value of 74 at zero angular rate
* the offset is 74 from the origin and subtracting
* 74 from all measurements centers them around zero.
*/
gyro_report.timestamp = hrt_absolute_time();
// report the error count as the number of bad // report the error count as the number of bad
// register reads. This allows the higher level // register reads. This allows the higher level
// code to decide if it should use this sensor based on // code to decide if it should use this sensor based on
// whether it has had failures // whether it has had failures
gyro_report.error_count = perf_event_count(_bad_registers); _px4_gyro.set_error_count(perf_event_count(_bad_registers));
gyro_report.x_raw = swap16(raw_gyro_report.x); gyro_report.x_raw = swap16(raw_gyro_report.x);
gyro_report.y_raw = swap16(raw_gyro_report.y); gyro_report.y_raw = swap16(raw_gyro_report.y);
gyro_report.z_raw = swap16(raw_gyro_report.z); gyro_report.z_raw = swap16(raw_gyro_report.z);
float xraw_f = gyro_report.x_raw; _px4_gyro.update(timestamp_sample, gyro_report.x_raw, gyro_report.y_raw, gyro_report.z_raw);
float yraw_f = gyro_report.y_raw;
float zraw_f = gyro_report.z_raw;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
gyro_report.x = _gyro_filter_x.apply(x_in_new);
gyro_report.y = _gyro_filter_y.apply(y_in_new);
gyro_report.z = _gyro_filter_z.apply(z_in_new);
matrix::Vector3f gval(x_in_new, y_in_new, z_in_new);
matrix::Vector3f gval_integrated;
bool gyro_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
gyro_report.x_integral = gval_integrated(0);
gyro_report.y_integral = gval_integrated(1);
gyro_report.z_integral = gval_integrated(2);
gyro_report.scaling = _gyro_range_scale;
/* return device ID */
gyro_report.device_id = _device_id.devid;
_reports->force(&gyro_report);
/* notify anyone waiting for data */
if (gyro_notify) {
poll_notify(POLLIN);
if (!(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
}
}
_read++; _read++;
@@ -1104,7 +758,6 @@ FXAS21002C::measure()
perf_end(_sample_perf); perf_end(_sample_perf);
} }
void void
FXAS21002C::print_info() FXAS21002C::print_info()
{ {
@@ -1113,7 +766,6 @@ FXAS21002C::print_info()
perf_print_counter(_errors); perf_print_counter(_errors);
perf_print_counter(_bad_registers); perf_print_counter(_bad_registers);
perf_print_counter(_duplicates); perf_print_counter(_duplicates);
_reports->print_info("report queue");
::printf("checked_next: %u\n", _checked_next); ::printf("checked_next: %u\n", _checked_next);
for (uint8_t i = 0; i < FXAS21002C_NUM_CHECKED_REGISTERS; i++) { for (uint8_t i = 0; i < FXAS21002C_NUM_CHECKED_REGISTERS; i++) {
@@ -1127,7 +779,7 @@ FXAS21002C::print_info()
} }
} }
::printf("temperature: %.2f\n", (double)_last_temperature); _px4_gyro.print_status();
} }
void void
@@ -1182,8 +834,6 @@ namespace fxas21002c
FXAS21002C *g_dev; FXAS21002C *g_dev;
void start(bool external_bus, enum Rotation rotation); void start(bool external_bus, enum Rotation rotation);
void test();
void reset();
void info(); void info();
void regdump(); void regdump();
void usage(); void usage();
@@ -1198,8 +848,6 @@ void test_error();
void void
start(bool external_bus, enum Rotation rotation) start(bool external_bus, enum Rotation rotation)
{ {
int fd;
if (g_dev != nullptr) { if (g_dev != nullptr) {
PX4_INFO("already started"); PX4_INFO("already started");
exit(0); exit(0);
@@ -1208,14 +856,14 @@ start(bool external_bus, enum Rotation rotation)
/* create the driver */ /* create the driver */
if (external_bus) { if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_GYRO) #if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_GYRO)
g_dev = new FXAS21002C(PX4_SPI_BUS_EXT, FXAS21002C_DEVICE_PATH_GYRO, PX4_SPIDEV_EXT_GYRO, rotation); g_dev = new FXAS21002C(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_GYRO, rotation);
#else #else
PX4_ERR("External SPI not available"); PX4_ERR("External SPI not available");
exit(0); exit(0);
#endif #endif
} else { } else {
g_dev = new FXAS21002C(PX4_SPI_BUS_SENSORS, FXAS21002C_DEVICE_PATH_GYRO, PX4_SPIDEV_GYRO, rotation); g_dev = new FXAS21002C(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_GYRO, rotation);
} }
if (g_dev == nullptr) { if (g_dev == nullptr) {
@@ -1227,19 +875,6 @@ start(bool external_bus, enum Rotation rotation)
goto fail; goto fail;
} }
/* set the poll rate to default, starts automatic data collection */
fd = open(FXAS21002C_DEVICE_PATH_GYRO, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
close(fd);
exit(0); exit(0);
fail: fail:
@@ -1251,72 +886,6 @@ fail:
errx(1, "driver start failed"); 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 fd_gyro = -1;
sensor_gyro_s g_report{};
ssize_t sz;
/* get the driver */
fd_gyro = open(FXAS21002C_DEVICE_PATH_GYRO, O_RDONLY);
if (fd_gyro < 0) {
err(1, "%s open failed", FXAS21002C_DEVICE_PATH_GYRO);
}
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
if (sz != sizeof(g_report)) {
err(1, "immediate gyro read failed");
}
print_message(g_report);
if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "reset to default polling");
}
close(fd_gyro);
/* XXX add poll-rate tests here too */
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = open(FXAS21002C_DEVICE_PATH_GYRO, O_RDONLY);
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);
exit(0);
}
/** /**
* Print a little info about the driver. * Print a little info about the driver.
*/ */
@@ -1370,11 +939,10 @@ test_error()
void void
usage() usage()
{ {
PX4_INFO("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'"); PX4_INFO("missing command: try 'start', 'info', 'testerror' or 'regdump'");
PX4_INFO("options:"); PX4_INFO("options:");
PX4_INFO(" -X (external bus)"); PX4_INFO(" -X (external bus)");
PX4_INFO(" -R rotation"); PX4_INFO(" -R rotation");
PX4_INFO(" -a range in ga 2,4,8");
} }
} // namespace } // namespace
@@ -1383,13 +951,13 @@ int
fxas21002c_main(int argc, char *argv[]) fxas21002c_main(int argc, char *argv[])
{ {
bool external_bus = false; bool external_bus = false;
int ch;
enum Rotation rotation = ROTATION_NONE; enum Rotation rotation = ROTATION_NONE;
int ch = 0;
int myoptind = 1; int myoptind = 1;
const char *myoptarg = NULL; const char *myoptarg = NULL;
while ((ch = px4_getopt(argc, argv, "XR:a:", &myoptind, &myoptarg)) != EOF) { while ((ch = px4_getopt(argc, argv, "XR:", &myoptind, &myoptarg)) != EOF) {
switch (ch) { switch (ch) {
case 'X': case 'X':
external_bus = true; external_bus = true;
@@ -1401,7 +969,7 @@ fxas21002c_main(int argc, char *argv[])
default: default:
fxas21002c::usage(); fxas21002c::usage();
exit(0); return 0;
} }
} }
@@ -1415,20 +983,6 @@ fxas21002c_main(int argc, char *argv[])
fxas21002c::start(external_bus, rotation); fxas21002c::start(external_bus, rotation);
} }
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
fxas21002c::test();
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
fxas21002c::reset();
}
/* /*
* Print driver information. * Print driver information.
*/ */
@@ -1450,5 +1004,6 @@ fxas21002c_main(int argc, char *argv[])
fxas21002c::test_error(); fxas21002c::test_error();
} }
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); PX4_WARN("unrecognized command, try 'start', 'info', 'testerror' or 'regdump'");
return -1;
} }