fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 09:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Analog Devices ADIS16497 move to PX4Accelerometer/PX4Gyroscope and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2019-07-01 22:47:31 -04:00
committed by GitHub
parent cdadb393b7
commit 9450496eb4
10 changed files with 139 additions and 639 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/adis16497/ADIS16497.cpp
+91 -287
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2019 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
@@ -32,7 +32,6 @@
****************************************************************************/
#include "ADIS16497.hpp"
#include "ADIS16497_gyro.hpp"
#include <px4_config.h>
#include <ecl/geo/geo.h>
@@ -41,7 +40,7 @@
#define DIR_WRITE 0x80
// ADIS16497 registers
static constexpr uint8_t PAGE_ID = 0x0; // Page identifier
static constexpr uint8_t PAGE_ID = 0x0; // Page identifier
// Page 0x00
static constexpr uint8_t SYS_E_FLAG = 0x08; // Output, system error flags
@@ -53,145 +52,70 @@ static constexpr uint8_t PROD_ID = 0x7E; // Output, product identification
static constexpr uint8_t GLOB_CMD = 0x02; // Control, global commands
static constexpr uint8_t FNCTIO_CTRL = 0x06; // Control, I/O pins, functional definitions
static constexpr uint8_t GPIO_CTRL = 0x08; // Control, I/O pins, general-purpose
static constexpr uint8_t CONFIG = 0x0A; // Control, clock and miscellaneous corrections
static constexpr uint8_t DEC_RATE = 0x0C; // Control, output sample rate decimation
static constexpr uint8_t CONFIG = 0x0A; // Control, clock and miscellaneous corrections
static constexpr uint8_t DEC_RATE = 0x0C; // Control, output sample rate decimation
static constexpr uint8_t NULL_CNFG = 0x0E; // Control, automatic bias correction configuration
static constexpr uint8_t SYNC_SCALE = 0x10; // Control, automatic bias correction configuration
static constexpr uint8_t RANG_MDL = 0x12; // Measurement range (model-specific) Identifier TODO use this
static constexpr uint8_t FILTR_BNK_0 = 0x16; // Filter selection
static constexpr uint8_t FILTR_BNK_1 = 0x18; // Filter selection
static constexpr uint16_t PROD_ID_ADIS16497 = 0x4071; // ADIS16497 Identification, device number
// Stall time between SPI transfers
static constexpr uint8_t T_STALL = 2;
static constexpr uint32_t ADIS16497_DEFAULT_RATE = 1000;
using namespace time_literals;
ADIS16497::ADIS16497(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation) :
SPI("ADIS16497", path_accel, bus, device, SPIDEV_MODE3, 5000000),
ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) :
SPI("ADIS16497", nullptr, bus, device, SPIDEV_MODE3, 5000000),
ScheduledWorkItem(px4::device_bus_to_wq(get_device_id())),
_gyro(new ADIS16497_gyro(this, path_gyro)),
_sample_perf(perf_alloc(PC_ELAPSED, "ADIS16497_read")),
_sample_interval_perf(perf_alloc(PC_INTERVAL, "ADIS16497_read_int")),
_bad_transfers(perf_alloc(PC_COUNT, "ADIS16497_bad_transfers")),
_rotation(rotation)
_px4_accel(get_device_id(), ORB_PRIO_MAX, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_MAX, rotation),
_sample_interval_perf(perf_alloc(PC_INTERVAL, "adis16497: read interval")),
_sample_perf(perf_alloc(PC_ELAPSED, "adis16497: read")),
_bad_transfers(perf_alloc(PC_COUNT, "adis16497: bad transfers"))
{
#ifdef GPIO_SPI1_RESET_ADIS16497
// Configure hardware reset line
px4_arch_configgpio(GPIO_SPI1_RESET_ADIS16497);
#endif /* GPIO_SPI1_RESET_ADIS16497 */
#endif // GPIO_SPI1_RESET_ADIS16497
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ADIS16497;
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_ADIS16497);
_px4_accel.set_sample_rate(ADIS16497_DEFAULT_RATE);
_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
_gyro->_device_id.devid = _device_id.devid;
_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_ADIS16497;
// Software low pass filter for controllers
const param_t accel_cut_ph = param_find("IMU_ACCEL_CUTOFF");
float accel_cut = ADIS16497_ACCEL_DEFAULT_DRIVER_FILTER_FREQ;
if (accel_cut_ph != PARAM_INVALID && param_get(accel_cut_ph, &accel_cut) == PX4_OK) {
_accel_filter.set_cutoff_frequency(_sample_rate, accel_cut);
} else {
PX4_ERR("IMU_ACCEL_CUTOFF param invalid");
}
const param_t gyro_cut_ph = param_find("IMU_GYRO_CUTOFF");
float gyro_cut = ADIS16497_GYRO_DEFAULT_DRIVER_FILTER_FREQ;
if (gyro_cut_ph != PARAM_INVALID && param_get(gyro_cut_ph, &gyro_cut) == PX4_OK) {
_gyro_filter.set_cutoff_frequency(_sample_rate, gyro_cut);
} else {
PX4_ERR("IMU_GYRO_CUTOFF param invalid");
}
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16497);
_px4_gyro.set_sample_rate(ADIS16497_DEFAULT_RATE);
_px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB
}
ADIS16497::~ADIS16497()
{
/* make sure we are truly inactive */
// make sure we are truly inactive
stop();
/* delete the gyro subdriver */
delete _gyro;
if (_accel_class_instance != -1) {
unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance);
}
/* delete the perf counter */
perf_free(_sample_perf);
// delete the perf counters
perf_free(_sample_interval_perf);
perf_free(_sample_perf);
perf_free(_bad_transfers);
}
int
ADIS16497::init()
{
if (hrt_absolute_time() < 250_ms) {
// Power-on startup time (if needed)
up_mdelay(250);
}
/* do SPI init (and probe) first */
// do SPI init (and probe) first
if (SPI::init() != OK) {
/* if probe/setup failed, bail now */
DEVICE_DEBUG("SPI setup failed");
// if probe/setup failed, bail now
PX4_DEBUG("SPI setup failed");
return PX4_ERROR;
}
/* Initialize offsets and scales */
_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;
start();
_accel_scale.x_offset = 0.0f;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0.0f;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0.0f;
_accel_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
int ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
/* fetch an initial set of measurements for advertisement */
measure();
/* advertise sensor topic, measure manually to initialize valid report */
sensor_accel_s arp = {};
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, &_accel_orb_class_instance, ORB_PRIO_MAX);
if (_accel_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
}
sensor_gyro_s grp = {};
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_gyro->_gyro_orb_class_instance, ORB_PRIO_MAX);
if (_gyro->_gyro_topic == nullptr) {
PX4_ERR("ADVERT FAIL");
}
if (ret == PX4_OK) {
start();
}
return ret;
return PX4_OK;
}
int ADIS16497::reset()
@@ -201,22 +125,19 @@ int ADIS16497::reset()
px4_arch_gpiowrite(GPIO_SPI1_RESET_ADIS16497, 0);
// The RST line must be in a low state for at least 10 μs to ensure a proper reset initiation and recovery.
up_udelay(10);
usleep(10_us);
px4_arch_gpiowrite(GPIO_SPI1_RESET_ADIS16497, 1);
// Reset recovery time
up_mdelay(250);
#else
// Software reset (global command bit 7)
uint8_t value[2] = {};
uint8_t value[2] {};
value[0] = (1 << 7);
write_reg16(PAGE_ID, 0x03);
write_reg16(GLOB_CMD, (uint16_t)value[0]);
#endif // GPIO_SPI1_RESET_ADIS16497
// Reset recovery time
up_mdelay(210);
#endif /* GPIO_SPI1_RESET_ADIS16497 */
usleep(210_ms);
// Switch to configuration page
write_reg16(PAGE_ID, 0x03);
@@ -226,7 +147,7 @@ int ADIS16497::reset()
write_reg16(FNCTIO_CTRL, FNCTIO_CTRL_DEFAULT);
up_udelay(340);
usleep(340_us);
const uint16_t fnctio_ctrl = read_reg16(FNCTIO_CTRL);
@@ -240,7 +161,7 @@ int ADIS16497::reset()
write_reg16(CONFIG, CONFIG_DEFAULT);
up_udelay(45);
usleep(45_us);
const uint16_t config = read_reg16(CONFIG);
@@ -254,7 +175,7 @@ int ADIS16497::reset()
write_reg16(DEC_RATE, DEC_RATE_DEFAULT);
up_udelay(340);
usleep(340_us);
const uint16_t dec_rate = read_reg16(DEC_RATE);
@@ -268,7 +189,7 @@ int ADIS16497::reset()
write_reg16(NULL_CNFG, NULL_CNFG_DEFAULT);
up_udelay(71);
usleep(71_us);
const uint16_t null_cnfg = read_reg16(NULL_CNFG);
@@ -284,7 +205,7 @@ int ADIS16497::reset()
write_reg16(FILTR_BNK_0, FILTR_BNK_0_SETUP);
write_reg16(FILTR_BNK_1, FILTR_BNK_1_SETUP);
up_udelay(65);
usleep(65_us);
const uint16_t filtr_bnk_0 = read_reg16(FILTR_BNK_0);
@@ -308,7 +229,7 @@ int ADIS16497::reset()
write_reg16(GLOB_CMD, (uint16_t)value[0]);
// save Recovery Time
up_mdelay(1125);
usleep(1125_ms);
*/
return OK;
@@ -317,35 +238,30 @@ int ADIS16497::reset()
int
ADIS16497::probe()
{
DEVICE_DEBUG("probe");
reset();
// read product id (5 attempts)
for (int i = 0; i < 5; i++) {
if (reset() != PX4_OK) {
continue;
}
// Switch to output page
write_reg16(PAGE_ID, 0x00);
// Check if the device is an ADIS16497
static constexpr uint16_t PROD_ID_ADIS16497 = 0x4071;
uint16_t product_id = read_reg16(PROD_ID);
if (product_id == PROD_ID_ADIS16497) {
PX4_DEBUG("PRODUCT: %X", product_id);
if (self_test_sensor()) {
// TODO check model here and set measurement ranges
if (self_test()) {
return PX4_OK;
} else {
PX4_ERR("probe attempt %d: self test failed, resetting", i);
reset();
}
} else {
PX4_ERR("probe attempt %d: read product id failed, resetting", i);
reset();
}
}
@@ -353,17 +269,17 @@ ADIS16497::probe()
}
bool
ADIS16497::self_test_sensor()
ADIS16497::self_test()
{
// Switch to configuration page
write_reg16(PAGE_ID, 0x03);
// Self test (globa l command bit 1)
uint8_t value[2] = {};
uint8_t value[2] {};
value[0] = (1 << 1);
write_reg16(GLOB_CMD, (uint16_t)value[0]);
up_mdelay(20); // Self test time
usleep(20_ms); // Self test time
// Switch to output page
write_reg16(PAGE_ID, 0x0);
@@ -387,50 +303,10 @@ ADIS16497::self_test_sensor()
return true;
}
int
ADIS16497::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCRESET:
return reset();
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
memcpy(&_accel_scale, s, sizeof(_accel_scale));
return OK;
}
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
int
ADIS16497::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
/* these are shared with the accel side */
case SENSORIOCRESET:
return ioctl(filp, cmd, arg);
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);
}
}
uint16_t
ADIS16497::read_reg16(uint8_t reg)
{
uint16_t cmd[1];
uint16_t cmd[1] {};
cmd[0] = ((reg | DIR_READ) << 8) & 0xff00;
transferhword(cmd, nullptr, 1);
@@ -444,7 +320,7 @@ ADIS16497::read_reg16(uint8_t reg)
void
ADIS16497::write_reg(uint8_t reg, uint8_t val)
{
uint8_t cmd[2];
uint8_t cmd[2] {};
cmd[0] = reg | 0x8;
cmd[1] = val;
transfer(cmd, cmd, sizeof(cmd));
@@ -453,7 +329,7 @@ ADIS16497::write_reg(uint8_t reg, uint8_t val)
void
ADIS16497::write_reg16(uint8_t reg, uint16_t value)
{
uint16_t cmd[2];
uint16_t cmd[2] {};
cmd[0] = ((reg | DIR_WRITE) << 8) | (0x00ff & value);
cmd[1] = (((reg + 0x1) | DIR_WRITE) << 8) | ((0xff00 & value) >> 8);
@@ -474,8 +350,8 @@ ADIS16497::start()
// Make sure we are stopped first
stop();
// Start polling at the specified rate
ScheduleOnInterval(_call_interval, 1000);
// start polling at the specified rate
ScheduleOnInterval((1_s / ADIS16497_DEFAULT_RATE), 10000);
#endif
}
@@ -495,7 +371,7 @@ ADIS16497::data_ready_interrupt(int irq, void *context, void *arg)
{
ADIS16497 *dev = reinterpret_cast<ADIS16497 *>(arg);
/* make another measurement */
// make another measurement
dev->ScheduleNow();
return PX4_OK;
@@ -504,24 +380,24 @@ ADIS16497::data_ready_interrupt(int irq, void *context, void *arg)
void
ADIS16497::Run()
{
/* make another measurement */
// make another measurement
measure();
}
int
ADIS16497::measure()
{
perf_begin(_sample_perf);
perf_count(_sample_interval_perf);
perf_begin(_sample_perf);
// Fetch the full set of measurements from the ADIS16497 in one pass (burst read).
ADISReport adis_report {};
ADISReport adis_report{};
adis_report.cmd = ((BURST_CMD | DIR_READ) << 8) & 0xff00;
// ADIS16497 burst report should be 320 bits
static_assert(sizeof(adis_report) == (320 / 8), "ADIS16497 report not 320 bits");
const hrt_abstime t = hrt_absolute_time();
const hrt_abstime timestamp_sample = hrt_absolute_time();
if (OK != transferhword((uint16_t *)&adis_report, ((uint16_t *)&adis_report), sizeof(adis_report) / sizeof(uint16_t))) {
perf_count(_bad_transfers);
@@ -534,19 +410,21 @@ ADIS16497::measure()
static constexpr uint16_t BURST_ID_DEFAULT = 0xA5A5;
if (adis_report.BURST_ID != BURST_ID_DEFAULT) {
PX4_ERR("BURST_ID: %#X", adis_report.BURST_ID);
PX4_DEBUG("BURST_ID: %#X", adis_report.BURST_ID);
perf_count(_bad_transfers);
perf_end(_sample_perf);
return -EIO;
}
// Check all Status/Error Flag Indicators
if (adis_report.SYS_E_FLAG != 0) {
PX4_ERR("SYS_E_FLAG: %#X", adis_report.SYS_E_FLAG);
PX4_DEBUG("SYS_E_FLAG: %#X", adis_report.SYS_E_FLAG);
perf_count(_bad_transfers);
perf_end(_sample_perf);
return -EIO;
}
@@ -554,123 +432,49 @@ ADIS16497::measure()
uint32_t checksum_calc = crc32((uint16_t *)&adis_report.SYS_E_FLAG, 15);
if (checksum != checksum_calc) {
PX4_ERR("CHECKSUM: %#X vs. calculated: %#X", checksum, checksum_calc);
PX4_DEBUG("CHECKSUM: %#X vs. calculated: %#X", checksum, checksum_calc);
perf_count(_bad_transfers);
perf_end(_sample_perf);
return -EIO;
}
// TODO check data counter here to see if we're missing samples/getting repeated samples
const uint64_t error_count = perf_event_count(_bad_transfers);
_px4_accel.set_error_count(error_count);
_px4_gyro.set_error_count(error_count);
publish_accel(t, adis_report);
publish_gyro(t, adis_report);
const float temperature = (int16_t(adis_report.TEMP_OUT) * 0.0125f) + 25.0f; // 1 LSB = 0.0125°C, 0x0000 at 25°C
_px4_accel.set_temperature(temperature);
_px4_gyro.set_temperature(temperature);
// TODO check data counter here to see if we're missing samples/getting repeated samples
{
float xraw_f = (int32_t(adis_report.X_ACCEL_OUT) << 16 | adis_report.X_ACCEL_LOW) / 65536.0f;
float yraw_f = (int32_t(adis_report.Y_ACCEL_OUT) << 16 | adis_report.Y_ACCEL_LOW) / 65536.0f;
float zraw_f = (int32_t(adis_report.Z_ACCEL_OUT) << 16 | adis_report.Z_ACCEL_LOW) / 65536.0f;
_px4_accel.update(timestamp_sample, xraw_f, yraw_f, zraw_f);
}
{
float xraw_f = (int32_t(adis_report.X_GYRO_OUT) << 16 | adis_report.X_GYRO_LOW) / 65536.0f;
float yraw_f = (int32_t(adis_report.Y_GYRO_OUT) << 16 | adis_report.Y_GYRO_LOW) / 65536.0f;
float zraw_f = (int32_t(adis_report.Z_GYRO_OUT) << 16 | adis_report.Z_GYRO_LOW) / 65536.0f;
_px4_gyro.update(timestamp_sample, xraw_f, yraw_f, zraw_f);
}
perf_end(_sample_perf);
return OK;
}
void
ADIS16497::publish_accel(const hrt_abstime &t, const ADISReport &report)
{
float xraw_f = (int32_t(report.X_ACCEL_OUT) << 16 | report.X_ACCEL_LOW) / 65536.0f * _accel_range_scale;
float yraw_f = (int32_t(report.Y_ACCEL_OUT) << 16 | report.Y_ACCEL_LOW) / 65536.0f * _accel_range_scale;
float zraw_f = (int32_t(report.Z_ACCEL_OUT) << 16 | report.Z_ACCEL_LOW) / 65536.0f * _accel_range_scale;
// Apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
const float x_in_new = (xraw_f - _accel_scale.x_offset) * _accel_scale.x_scale;
const float y_in_new = (yraw_f - _accel_scale.y_offset) * _accel_scale.y_scale;
const float z_in_new = (zraw_f - _accel_scale.z_offset) * _accel_scale.z_scale;
matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
const matrix::Vector3f val_filt = _accel_filter.apply(aval);
sensor_accel_s arb{};
matrix::Vector3f aval_integrated;
if (_accel_int.put(t, aval, aval_integrated, arb.integral_dt)) {
arb.timestamp = t;
arb.device_id = _device_id.devid;
arb.error_count = perf_event_count(_bad_transfers);
// Raw sensor readings
arb.x_raw = report.X_ACCEL_OUT;
arb.y_raw = report.Y_ACCEL_OUT;
arb.z_raw = report.Z_ACCEL_OUT;
arb.scaling = _accel_range_scale;
// Filtered values for controls
arb.x = val_filt(0);
arb.y = val_filt(1);
arb.z = val_filt(2);
// Intgrated values for estimation
arb.x_integral = aval_integrated(0);
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.temperature = (int16_t(report.TEMP_OUT) * 0.0125f) + 25.0f; // 1 LSB = 0.0125°C, 0x0000 at 25°C
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
}
void
ADIS16497::publish_gyro(const hrt_abstime &t, const ADISReport &report)
{
float xraw_f = (int32_t(report.X_GYRO_OUT) << 16 | report.X_GYRO_LOW) / 65536.0f;
float yraw_f = (int32_t(report.Y_GYRO_OUT) << 16 | report.Y_GYRO_LOW) / 65536.0f;
float zraw_f = (int32_t(report.Z_GYRO_OUT) << 16 | report.Z_GYRO_LOW) / 65536.0f;
// Apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
const float x_gyro_in_new = (math::radians(xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
const float y_gyro_in_new = (math::radians(yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
const float z_gyro_in_new = (math::radians(zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
const matrix::Vector3f gval_filt = _gyro_filter.apply(gval);
sensor_gyro_s grb{};
matrix::Vector3f gval_integrated;
if (_gyro_int.put(t, gval, gval_integrated, grb.integral_dt)) {
grb.timestamp = t;
grb.device_id = _gyro->_device_id.devid;
grb.error_count = perf_event_count(_bad_transfers);
// Raw sensor readings
grb.x_raw = report.X_GYRO_OUT;
grb.y_raw = report.Y_GYRO_OUT;
grb.z_raw = report.Z_GYRO_OUT;
grb.scaling = math::radians(_gyro_range_scale);
// Filtered values for controls
grb.x = gval_filt(0);
grb.y = gval_filt(1);
grb.z = gval_filt(2);
// Unfiltered integrated values for estimation
grb.x_integral = gval_integrated(0);
grb.y_integral = gval_integrated(1);
grb.z_integral = gval_integrated(2);
grb.temperature = (int16_t(report.TEMP_OUT) * 0.0125f) + 25.0f; // 1 LSB = 0.0125°C, 0x0000 at 25°C
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
}
void
ADIS16497::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_sample_interval_perf);
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
_px4_accel.print_status();
_px4_gyro.print_status();
}
src/drivers/imu/adis16497/ADIS16497.hpp
+14 -75
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2019 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
@@ -36,29 +36,18 @@
*
*/
#ifndef DRIVERS_IMU_ADIS16497_ADIS16497_HPP_
#define DRIVERS_IMU_ADIS16497_ADIS16497_HPP_
#pragma once
#include <drivers/device/ringbuffer.h>
#include <drivers/device/spi.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <drivers/device/integrator.h>
#include <lib/conversion/rotation.h>
#include <perf/perf_counter.h>
#include <ecl/geo/geo.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#define ADIS16497_GYRO_DEFAULT_RATE 1000
#define ADIS16497_GYRO_DEFAULT_DRIVER_FILTER_FREQ 80
#define ADIS16497_ACCEL_DEFAULT_RATE 1000
#define ADIS16497_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
// TODO : This is a copy of the NuttX CRC32 table
static const uint32_t crc32_tab[] = {
static constexpr uint32_t crc32_tab[] = {
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3,
0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91,
0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
@@ -93,68 +82,30 @@ static const uint32_t crc32_tab[] = {
0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d
};
class ADIS16497_gyro;
class ADIS16497 : public device::SPI, public px4::ScheduledWorkItem
{
public:
ADIS16497(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation);
ADIS16497(int bus, uint32_t device, enum Rotation rotation = ROTATION_NONE);
virtual ~ADIS16497();
virtual int init();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
void print_info();
protected:
virtual int probe();
friend class ADIS16497_gyro;
virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
private:
ADIS16497_gyro *_gyro{nullptr};
unsigned _call_interval{1000};
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
struct gyro_calibration_s _gyro_scale {};
// gyro 0.025 °/sec/LSB
float _gyro_range_scale{0.025f};
float _gyro_range_rad_s{math::radians(500.0f)};
struct accel_calibration_s _accel_scale {};
// accel 1.25 mg/LSB
float _accel_range_scale{1.25f * CONSTANTS_ONE_G / 1000.0f};
float _accel_range_m_s2{40.0f * CONSTANTS_ONE_G};
orb_advert_t _accel_topic{nullptr};
int _accel_orb_class_instance{-1};
int _accel_class_instance{-1};
unsigned _sample_rate{1000};
perf_counter_t _sample_perf;
perf_counter_t _sample_interval_perf;
perf_counter_t _sample_perf;
perf_counter_t _bad_transfers;
math::LowPassFilter2pVector3f _gyro_filter{ADIS16497_GYRO_DEFAULT_RATE, ADIS16497_GYRO_DEFAULT_DRIVER_FILTER_FREQ};
math::LowPassFilter2pVector3f _accel_filter{ADIS16497_ACCEL_DEFAULT_RATE, ADIS16497_ACCEL_DEFAULT_DRIVER_FILTER_FREQ};
Integrator _accel_int{4000, false};
Integrator _gyro_int{4000, true};
enum Rotation _rotation;
#pragma pack(push, 1)
/**
* Report conversation with the ADIS16497, including command byte.
*/
// Report conversation with the ADIS16497, including command byte.
struct ADISReport {
uint16_t cmd;
uint16_t ZEROES;
@@ -182,12 +133,12 @@ private:
/**
* Start automatic measurement.
*/
void start();
void start();
/**
* Stop automatic measurement.
*/
void stop();
void stop();
/**
* Reset chip.
@@ -196,10 +147,6 @@ private:
*/
int reset();
/**
* Called by the ScheduledWorkItem at the specified rate if
* automatic polling is enabled.
*/
void Run() override;
static int data_ready_interrupt(int irq, void *context, void *arg);
@@ -209,21 +156,15 @@ private:
*/
int measure();
void publish_accel(const hrt_abstime &t, const ADISReport &report);
void publish_gyro(const hrt_abstime &t, const ADISReport &report);
uint16_t read_reg16(uint8_t reg);
void write_reg(uint8_t reg, uint8_t value);
void write_reg16(uint8_t reg, uint16_t value);
// ADIS16497 onboard self test
bool self_test_sensor();
bool self_test();
ADIS16497(const ADIS16497 &);
ADIS16497 operator=(const ADIS16497 &);
uint32_t crc32(const uint16_t *data, size_t len)
uint32_t crc32(const uint16_t *data, size_t len) const
{
uint32_t crc = 0xffffffff;
uint8_t tbl_idx;
@@ -245,5 +186,3 @@ private:
}
};
#endif /* DRIVERS_IMU_ADIS16497_ADIS16497_HPP_ */
src/drivers/imu/adis16497/ADIS16497_gyro.cpp
-79
View File
@@ -1,79 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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 "ADIS16497_gyro.hpp"
ADIS16497_gyro::ADIS16497_gyro(ADIS16497 *parent, const char *path) :
CDev("ADIS16497_gyro", path),
_parent(parent),
_gyro_topic(nullptr),
_gyro_orb_class_instance(-1),
_gyro_class_instance(-1)
{
}
ADIS16497_gyro::~ADIS16497_gyro()
{
if (_gyro_class_instance != -1) {
unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance);
}
}
int
ADIS16497_gyro::init()
{
int ret = CDev::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
return ret;
}
_gyro_class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
return ret;
}
int
ADIS16497_gyro::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case DEVIOCGDEVICEID:
return (int)CDev::ioctl(filp, cmd, arg);
break;
default:
return _parent->gyro_ioctl(filp, cmd, arg);
}
}
src/drivers/imu/adis16497/ADIS16497_gyro.hpp
-71
View File
@@ -1,71 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2018 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.
*
****************************************************************************/
#ifndef DRIVERS_IMU_ADIS16497_ADIS16497_GYRO_HPP_
#define DRIVERS_IMU_ADIS16497_ADIS16497_GYRO_HPP_
#include "ADIS16497.hpp"
#include <drivers/device/CDev.hpp>
#include <drivers/drv_gyro.h>
/**
* Helper class implementing the gyro driver node.
*/
class ADIS16497_gyro : public device::CDev
{
public:
ADIS16497_gyro(ADIS16497 *parent, const char *path);
virtual ~ADIS16497_gyro();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class ADIS16497;
private:
ADIS16497 *_parent{nullptr};
orb_advert_t _gyro_topic{nullptr};
int _gyro_orb_class_instance{-1};
int _gyro_class_instance{-1};
/* do not allow to copy this class due to pointer data members */
ADIS16497_gyro(const ADIS16497_gyro &);
ADIS16497_gyro operator=(const ADIS16497_gyro &);
};
#endif /* DRIVERS_IMU_ADIS16497_ADIS16497_GYRO_HPP_ */
src/drivers/imu/adis16497/CMakeLists.txt
+6 -3
View File
@@ -35,9 +35,12 @@ px4_add_module(
MAIN adis16497
STACK_MAIN 1200
COMPILE_FLAGS
-Wno-cast-align # TODO: fix and enable
-Wno-cast-align # TODO: fix and enable
SRCS
ADIS16497.cpp
ADIS16497_gyro.cpp
ADIS16497_main.cpp
ADIS16497.hpp
adis16497_main.cpp
DEPENDS
drivers_accelerometer
drivers_gyroscope
)
src/drivers/imu/adis16497/ADIS16497_main.cpp → src/drivers/imu/adis16497/adis16497_main.cpp
+16 -110
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018-2019 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
@@ -32,10 +32,8 @@
****************************************************************************/
#include "ADIS16497.hpp"
#include <systemlib/err.h>
#define ADIS16497_DEVICE_PATH_ACCEL "/dev/ADIS16497_accel"
#define ADIS16497_DEVICE_PATH_GYRO "/dev/ADIS16497_gyro"
#include <px4_getopt.h>
extern "C" { __EXPORT int adis16497_main(int argc, char *argv[]); }
@@ -45,11 +43,9 @@ extern "C" { __EXPORT int adis16497_main(int argc, char *argv[]); }
namespace adis16497
{
ADIS16497 *g_dev;
ADIS16497 *g_dev{nullptr};
void start(enum Rotation rotation);
void test();
void reset();
void info();
void usage();
/**
@@ -66,8 +62,7 @@ start(enum Rotation rotation)
/* create the driver */
#if defined(PX4_SPIDEV_EXTERNAL1_1)
g_dev = new ADIS16497(PX4_SPI_BUS_EXTERNAL1, ADIS16497_DEVICE_PATH_ACCEL, ADIS16497_DEVICE_PATH_GYRO,
PX4_SPIDEV_EXTERNAL1_1, rotation);
g_dev = new ADIS16497(PX4_SPI_BUS_EXTERNAL1, PX4_SPIDEV_EXTERNAL1_1, rotation);
#else
PX4_ERR("External SPI not available");
exit(0);
@@ -89,82 +84,7 @@ 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()
{
sensor_accel_s a_report{};
sensor_gyro_s g_report{};
ssize_t sz;
/* get the driver */
int fd = px4_open(ADIS16497_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0) {
err(1, "%s open failed", ADIS16497_DEVICE_PATH_ACCEL);
}
/* get the gyro driver */
int fd_gyro = px4_open(ADIS16497_DEVICE_PATH_GYRO, O_RDONLY);
if (fd_gyro < 0) {
err(1, "%s open failed", ADIS16497_DEVICE_PATH_GYRO);
}
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report)) {
PX4_ERR("ret: %d, expected: %d", sz, sizeof(a_report));
err(1, "immediate acc read failed");
}
print_message(a_report);
/* do a simple demand read */
sz = px4_read(fd_gyro, &g_report, sizeof(g_report));
if (sz != sizeof(g_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(g_report));
err(1, "immediate gyro read failed");
}
print_message(g_report);
px4_close(fd_gyro);
px4_close(fd);
reset();
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset()
{
int fd = px4_open(ADIS16497_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0) {
err(1, "open failed");
}
if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
px4_close(fd);
exit(0);
PX4_ERR("driver start failed");
}
/**
@@ -174,19 +94,16 @@ void
info()
{
if (g_dev == nullptr) {
errx(1, "driver not running");
PX4_WARN("driver not running");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
exit(0);
}
void
usage()
{
PX4_INFO("missing command: try 'start', 'test', 'info', 'reset'");
PX4_INFO("missing command: try 'start', 'info'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
}
@@ -198,22 +115,24 @@ int
adis16497_main(int argc, char *argv[])
{
enum Rotation rotation = ROTATION_NONE;
int ch;
int myoptind = 1;
int ch = 0;
const char *myoptarg = nullptr;
/* start options */
while ((ch = getopt(argc, argv, "R:")) != EOF) {
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'R':
rotation = (enum Rotation)atoi(optarg);
rotation = (enum Rotation)atoi(myoptarg);
break;
default:
adis16497::usage();
exit(0);
return 0;
}
}
const char *verb = argv[optind];
const char *verb = argv[myoptind];
/*
* Start/load the driver.
@@ -223,20 +142,6 @@ adis16497_main(int argc, char *argv[])
adis16497::start(rotation);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
adis16497::test();
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
adis16497::reset();
}
/*
* Print driver information.
*/
@@ -245,5 +150,6 @@ adis16497_main(int argc, char *argv[])
}
adis16497::usage();
exit(1);
return 0;
}
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
+5 -6
View File
@@ -104,19 +104,18 @@ void PX4Accelerometer::set_sample_rate(unsigned rate)
_filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq());
}
void PX4Accelerometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
void PX4Accelerometer::update(hrt_abstime timestamp, float x, float y, float z)
{
sensor_accel_s &report = _sensor_accel_pub.get();
report.timestamp = timestamp;
// Apply rotation (before scaling)
float xraw_f = x;
float yraw_f = y;
float zraw_f = z;
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
rotate_3f(_rotation, x, y, z);
const matrix::Vector3f raw{x, y, z};
// Apply range scale and the calibrating offset/scale
const matrix::Vector3f val_calibrated{(((matrix::Vector3f{xraw_f, yraw_f, zraw_f} * report.scaling) - _calibration_offset).emult(_calibration_scale))};
const matrix::Vector3f val_calibrated{(((raw * report.scaling) - _calibration_offset).emult(_calibration_scale))};
// Filtered values
const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
+1 -1
View File
@@ -60,7 +60,7 @@ public:
void set_sample_rate(unsigned rate);
void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
void update(hrt_abstime timestamp, float x, float y, float z);
void print_status();
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
+5 -6
View File
@@ -104,19 +104,18 @@ void PX4Gyroscope::set_sample_rate(unsigned rate)
_filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq());
}
void PX4Gyroscope::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
void PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
{
sensor_gyro_s &report = _sensor_gyro_pub.get();
report.timestamp = timestamp;
// Apply rotation (before scaling)
float xraw_f = x;
float yraw_f = y;
float zraw_f = z;
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
rotate_3f(_rotation, x, y, z);
const matrix::Vector3f raw{x, y, z};
// Apply range scale and the calibrating offset/scale
const matrix::Vector3f val_calibrated{(((matrix::Vector3f{xraw_f, yraw_f, zraw_f} * report.scaling) - _calibration_offset).emult(_calibration_scale))};
const matrix::Vector3f val_calibrated{(((raw * report.scaling) - _calibration_offset).emult(_calibration_scale))};
// Filtered values
const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
+1 -1
View File
@@ -60,7 +60,7 @@ public:
void set_sample_rate(unsigned rate);
void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
void update(hrt_abstime timestamp, float x, float y, float z);
void print_status();