zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Analog Devices ADIS16497 move to PX4Accelerometer/PX4Gyroscope and cleanup

sbg
Daniel Agar 6 years ago committed by GitHub
parent
cdadb393b7
commit
9450496eb4
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 137 additions and 637 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 374
      src/drivers/imu/adis16497/ADIS16497.cpp
  2. 89
      src/drivers/imu/adis16497/ADIS16497.hpp
  3. 79
      src/drivers/imu/adis16497/ADIS16497_gyro.cpp
  4. 71
      src/drivers/imu/adis16497/ADIS16497_gyro.hpp
  5. 9
      src/drivers/imu/adis16497/CMakeLists.txt
  6. 126
      src/drivers/imu/adis16497/adis16497_main.cpp
  7. 11
      src/lib/drivers/accelerometer/PX4Accelerometer.cpp
  8. 2
      src/lib/drivers/accelerometer/PX4Accelerometer.hpp
  9. 11
      src/lib/drivers/gyroscope/PX4Gyroscope.cpp
  10. 2
      src/lib/drivers/gyroscope/PX4Gyroscope.hpp

374
src/drivers/imu/adis16497/ADIS16497.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -32,7 +32,6 @@
****************************************************************************/
#include "ADIS16497.hpp"
#include "ADIS16497_gyro.hpp"
#include <px4_config.h>
#include <ecl/geo/geo.h>
@ -41,7 +40,7 @@ @@ -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 @@ -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 */
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_ADIS16497;
#endif // GPIO_SPI1_RESET_ADIS16497
_gyro->_device_id.devid = _device_id.devid;
_gyro->_device_id.devid_s.devtype = DRV_GYR_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
// 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;
_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");
}
start();
if (ret == PX4_OK) {
start();
}
return ret;
return PX4_OK;
}
int ADIS16497::reset()
@ -201,22 +125,19 @@ 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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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) @@ -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) @@ -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() @@ -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) @@ -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) @@ -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() @@ -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() @@ -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
publish_accel(t, adis_report);
publish_gyro(t, adis_report);
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;
const uint64_t error_count = perf_event_count(_bad_transfers);
_px4_accel.set_error_count(error_count);
_px4_gyro.set_error_count(error_count);
arb.device_id = _device_id.devid;
arb.error_count = perf_event_count(_bad_transfers);
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);
// 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);
// 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);
}
}
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);
{
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);
}
grb.temperature = (int16_t(report.TEMP_OUT) * 0.0125f) + 25.0f; // 1 LSB = 0.0125°C, 0x0000 at 25°C
perf_end(_sample_perf);
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
return OK;
}
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();
}

89
src/drivers/imu/adis16497/ADIS16497.hpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -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[] = { @@ -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};
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};
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
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: @@ -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: @@ -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: @@ -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();
ADIS16497(const ADIS16497 &);
ADIS16497 operator=(const ADIS16497 &);
bool self_test();
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: @@ -245,5 +186,3 @@ private:
}
};
#endif /* DRIVERS_IMU_ADIS16497_ADIS16497_HPP_ */

79
src/drivers/imu/adis16497/ADIS16497_gyro.cpp
Unescape View File

@ -1,79 +0,0 @@ @@ -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);
}
}

71
src/drivers/imu/adis16497/ADIS16497_gyro.hpp
Unescape View File

@ -1,71 +0,0 @@ @@ -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_ */

9
src/drivers/imu/adis16497/CMakeLists.txt
Unescape View File

@ -35,9 +35,12 @@ px4_add_module( @@ -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
)

126
src/drivers/imu/adis16497/ADIS16497_main.cpp → src/drivers/imu/adis16497/adis16497_main.cpp
Unescape View File

@ -1,6 +1,6 @@ @@ -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 @@ @@ -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[]); } @@ -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) @@ -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: @@ -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 @@ -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 @@ -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[]) @@ -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[]) @@ -245,5 +150,6 @@ adis16497_main(int argc, char *argv[])
}
adis16497::usage();
exit(1);
return 0;
}

11
src/lib/drivers/accelerometer/PX4Accelerometer.cpp
Unescape View File

@ -104,19 +104,18 @@ void PX4Accelerometer::set_sample_rate(unsigned rate) @@ -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)};

2
src/lib/drivers/accelerometer/PX4Accelerometer.hpp
Unescape View File

@ -60,7 +60,7 @@ public: @@ -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();

11
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
Unescape View File

@ -104,19 +104,18 @@ void PX4Gyroscope::set_sample_rate(unsigned rate) @@ -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)};

2
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
Unescape View File

@ -60,7 +60,7 @@ public: @@ -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();

Write Preview
Loading…
Cancel
Save