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

bmi160 move to PX4Accelerometer and PX4Gyroscope and cleanup (#12128)

Browse Source
This commit is contained in:
Daniel Agar
2019-06-05 11:25:18 -07:00
committed by David Sidrane
parent ac4458460d
commit 9d726cb0c4
6 changed files with 69 additions and 793 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/bmi160/CMakeLists.txt
+4 -1
View File
@@ -38,6 +38,9 @@ px4_add_module(
-Wno-cast-align # TODO: fix and enable
SRCS
bmi160.cpp
bmi160_gyro.cpp
bmi160_main.cpp
DEPENDS
px4_work_queue
drivers_accelerometer
drivers_gyroscope
)
src/drivers/imu/bmi160/bmi160.cpp
+44 -471
View File
@@ -1,5 +1,4 @@
#include "bmi160.hpp"
#include "bmi160_gyro.hpp"
#include <ecl/geo/geo.h>
/*
@@ -18,26 +17,11 @@ const uint8_t BMI160::_checked_registers[BMI160_NUM_CHECKED_REGISTERS] = { BM
BMIREG_NV_CONF
};
BMI160::BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation) :
SPI("BMI160", path_accel, bus, device, SPIDEV_MODE3, BMI160_BUS_SPEED),
BMI160::BMI160(int bus, uint32_t device, enum Rotation rotation) :
SPI("BMI160", nullptr, bus, device, SPIDEV_MODE3, BMI160_BUS_SPEED),
ScheduledWorkItem(px4::device_bus_to_wq(this->get_device_id())),
_gyro(new BMI160_gyro(this, path_gyro)),
_whoami(0),
_call_interval(0),
_accel_reports(nullptr),
_accel_scale{},
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(nullptr),
_accel_orb_class_instance(-1),
_accel_class_instance(-1),
_gyro_reports(nullptr),
_gyro_scale{},
_gyro_range_scale(0.0f),
_gyro_range_rad_s(0.0f),
_dlpf_freq(0),
_accel_sample_rate(BMI160_ACCEL_DEFAULT_RATE),
_gyro_sample_rate(BMI160_GYRO_DEFAULT_RATE),
_px4_accel(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
_px4_gyro(get_device_id(), (external() ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1), rotation),
_accel_reads(perf_alloc(PC_COUNT, "bmi160_accel_read")),
_gyro_reads(perf_alloc(PC_COUNT, "bmi160_gyro_read")),
_sample_perf(perf_alloc(PC_ELAPSED, "bmi160_read")),
@@ -45,71 +29,20 @@ BMI160::BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t
_bad_registers(perf_alloc(PC_COUNT, "bmi160_bad_registers")),
_good_transfers(perf_alloc(PC_COUNT, "bmi160_good_transfers")),
_reset_retries(perf_alloc(PC_COUNT, "bmi160_reset_retries")),
_duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates")),
_register_wait(0),
_reset_wait(0),
_accel_filter_x(BMI160_ACCEL_DEFAULT_RATE, BMI160_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_y(BMI160_ACCEL_DEFAULT_RATE, BMI160_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_accel_filter_z(BMI160_ACCEL_DEFAULT_RATE, BMI160_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_x(BMI160_GYRO_DEFAULT_RATE, BMI160_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(BMI160_GYRO_DEFAULT_RATE, BMI160_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(BMI160_GYRO_DEFAULT_RATE, BMI160_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_accel_int(1000000 / BMI160_ACCEL_MAX_PUBLISH_RATE),
_gyro_int(1000000 / BMI160_GYRO_MAX_PUBLISH_RATE, true),
_rotation(rotation),
_checked_next(0),
_last_temperature(0),
_last_accel{},
_got_duplicate(false)
_duplicates(perf_alloc(PC_COUNT, "bmi160_duplicates"))
{
// disable debug() calls
_debug_enabled = false;
_px4_accel.set_device_type(DRV_ACC_DEVTYPE_BMI160);
_px4_accel.set_sample_rate(BMI160_ACCEL_DEFAULT_RATE);
_device_id.devid_s.devtype = DRV_ACC_DEVTYPE_BMI160;
/* Prime _gyro with parents devid. */
_gyro->_device_id.devid = _device_id.devid;
_gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_BMI160;
// default accel scale factors
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
// default gyro scale factors
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_BMI160);
_px4_accel.set_sample_rate(BMI160_GYRO_DEFAULT_RATE);
}
BMI160::~BMI160()
{
/* make sure we are truly inactive */
stop();
/* delete the gyro subdriver */
delete _gyro;
/* free any existing reports */
if (_accel_reports != nullptr) {
delete _accel_reports;
}
if (_gyro_reports != nullptr) {
delete _gyro_reports;
}
if (_accel_class_instance != -1) {
unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance);
}
/* delete the perf counter */
perf_free(_sample_perf);
perf_free(_accel_reads);
@@ -124,10 +57,8 @@ BMI160::~BMI160()
int
BMI160::init()
{
int ret;
/* do SPI init (and probe) first */
ret = SPI::init();
int ret = SPI::init();
/* if probe/setup failed, bail now */
if (ret != OK) {
@@ -135,81 +66,17 @@ BMI160::init()
return ret;
}
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s));
ret = reset();
if (_accel_reports == nullptr) {
goto out;
}
_gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s));
if (_gyro_reports == nullptr) {
goto out;
}
if (reset() != OK) {
goto out;
}
/* Initialize offsets and scales */
_accel_scale.x_offset = 0;
_accel_scale.x_scale = 1.0f;
_accel_scale.y_offset = 0;
_accel_scale.y_scale = 1.0f;
_accel_scale.z_offset = 0;
_accel_scale.z_scale = 1.0f;
_gyro_scale.x_offset = 0;
_gyro_scale.x_scale = 1.0f;
_gyro_scale.y_offset = 0;
_gyro_scale.y_scale = 1.0f;
_gyro_scale.z_offset = 0;
_gyro_scale.z_scale = 1.0f;
/* do CDev init for the gyro device node, keep it optional */
ret = _gyro->init();
/* if probe/setup failed, bail now */
if (ret != OK) {
DEVICE_DEBUG("gyro init failed");
if (ret != PX4_OK) {
return ret;
}
_accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
start();
measure();
/* advertise sensor topic, measure manually to initialize valid report */
sensor_accel_s arp;
_accel_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_accel_topic == nullptr) {
warnx("ADVERT FAIL");
}
/* advertise sensor topic, measure manually to initialize valid report */
sensor_gyro_s grp;
_gyro_reports->get(&grp);
_gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp,
&_gyro->_gyro_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1);
if (_gyro->_gyro_topic == nullptr) {
warnx("ADVERT FAIL");
}
out:
return ret;
}
int BMI160::reset()
{
write_reg(BMIREG_CONF, (1 << 1)); //Enable NVM programming
@@ -439,57 +306,6 @@ BMI160::_set_dlpf_filter(uint16_t bandwidth)
modify_reg(CTRL_REG6_XL, clearbits, setbits);*/
}
ssize_t
BMI160::read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(sensor_accel_s);
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
if (_call_interval == 0) {
_accel_reports->flush();
measure();
}
/* if no data, error (we could block here) */
if (_accel_reports->empty()) {
return -EAGAIN;
}
perf_count(_accel_reads);
/* copy reports out of our buffer to the caller */
sensor_accel_s *arp = reinterpret_cast<sensor_accel_s *>(buffer);
int transferred = 0;
while (count--) {
if (!_accel_reports->get(arp)) {
break;
}
transferred++;
arp++;
}
/* return the number of bytes transferred */
return (transferred * sizeof(sensor_accel_s));
}
int
BMI160::self_test()
{
if (perf_event_count(_sample_perf) == 0) {
measure();
}
/* return 0 on success, 1 else */
return (perf_event_count(_sample_perf) > 0) ? 0 : 1;
}
/*
deliberately trigger an error in the sensor to trigger recovery
*/
@@ -501,153 +317,6 @@ BMI160::test_error()
print_registers();
}
ssize_t
BMI160::gyro_read(struct file *filp, char *buffer, size_t buflen)
{
unsigned count = buflen / sizeof(sensor_gyro_s);
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
if (_call_interval == 0) {
_gyro_reports->flush();
measure();
}
/* if no data, error (we could block here) */
if (_gyro_reports->empty()) {
return -EAGAIN;
}
perf_count(_gyro_reads);
/* copy reports out of our buffer to the caller */
sensor_gyro_s *grp = reinterpret_cast<sensor_gyro_s *>(buffer);
int transferred = 0;
while (count--) {
if (!_gyro_reports->get(grp)) {
break;
}
transferred++;
grp++;
}
/* return the number of bytes transferred */
return (transferred * sizeof(sensor_gyro_s));
}
int
BMI160::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case SENSORIOCRESET:
return reset();
case SENSORIOCSPOLLRATE: {
switch (arg) {
/* zero would be bad */
case 0:
return -EINVAL;
/* set default polling rate */
case SENSOR_POLLRATE_DEFAULT:
if (BMI160_GYRO_DEFAULT_RATE > BMI160_ACCEL_DEFAULT_RATE) {
return ioctl(filp, SENSORIOCSPOLLRATE, BMI160_GYRO_DEFAULT_RATE);
} else {
return ioctl(filp, SENSORIOCSPOLLRATE,
BMI160_ACCEL_DEFAULT_RATE); //Polling at the highest frequency. We may get duplicate values on the sensors
}
/* 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;
}
// adjust filters
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
float sample_rate = 1.0e6f / interval;
_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call_interval = interval;
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
}
}
}
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_calibration_s *s = (struct accel_calibration_s *) arg;
float sum = s->x_scale + s->y_scale + s->z_scale;
if (sum > 2.0f && sum < 4.0f) {
memcpy(&_accel_scale, s, sizeof(_accel_scale));
return OK;
} else {
return -EINVAL;
}
}
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
int
BMI160::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
/* these are shared with the accel side */
case SENSORIOCSPOLLRATE:
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);
}
}
uint8_t
BMI160::read_reg(unsigned reg)
{
@@ -709,29 +378,28 @@ BMI160::set_accel_range(unsigned max_g)
uint8_t setbits = 0;
uint8_t clearbits = BMI_ACCEL_RANGE_2_G | BMI_ACCEL_RANGE_16_G;
float lsb_per_g;
float max_accel_g;
if (max_g == 0) {
max_g = 16;
}
if (max_g <= 2) {
max_accel_g = 2;
//max_accel_g = 2;
setbits |= BMI_ACCEL_RANGE_2_G;
lsb_per_g = 16384;
} else if (max_g <= 4) {
max_accel_g = 4;
//max_accel_g = 4;
setbits |= BMI_ACCEL_RANGE_4_G;
lsb_per_g = 8192;
} else if (max_g <= 8) {
max_accel_g = 8;
//max_accel_g = 8;
setbits |= BMI_ACCEL_RANGE_8_G;
lsb_per_g = 4096;
} else if (max_g <= 16) {
max_accel_g = 16;
//max_accel_g = 16;
setbits |= BMI_ACCEL_RANGE_16_G;
lsb_per_g = 2048;
@@ -739,8 +407,7 @@ BMI160::set_accel_range(unsigned max_g)
return -EINVAL;
}
_accel_range_scale = (CONSTANTS_ONE_G / lsb_per_g);
_accel_range_m_s2 = max_accel_g * CONSTANTS_ONE_G;
_px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
modify_reg(BMIREG_ACC_RANGE, clearbits, setbits);
@@ -753,34 +420,34 @@ BMI160::set_gyro_range(unsigned max_dps)
uint8_t setbits = 0;
uint8_t clearbits = BMI_GYRO_RANGE_125_DPS | BMI_GYRO_RANGE_250_DPS;
float lsb_per_dps;
float max_gyro_dps;
//float max_gyro_dps;
if (max_dps == 0) {
max_dps = 2000;
}
if (max_dps <= 125) {
max_gyro_dps = 125;
//max_gyro_dps = 125;
lsb_per_dps = 262.4;
setbits |= BMI_GYRO_RANGE_125_DPS;
} else if (max_dps <= 250) {
max_gyro_dps = 250;
//max_gyro_dps = 250;
lsb_per_dps = 131.2;
setbits |= BMI_GYRO_RANGE_250_DPS;
} else if (max_dps <= 500) {
max_gyro_dps = 500;
//max_gyro_dps = 500;
lsb_per_dps = 65.6;
setbits |= BMI_GYRO_RANGE_500_DPS;
} else if (max_dps <= 1000) {
max_gyro_dps = 1000;
//max_gyro_dps = 1000;
lsb_per_dps = 32.8;
setbits |= BMI_GYRO_RANGE_1000_DPS;
} else if (max_dps <= 2000) {
max_gyro_dps = 2000;
//max_gyro_dps = 2000;
lsb_per_dps = 16.4;
setbits |= BMI_GYRO_RANGE_2000_DPS;
@@ -788,8 +455,7 @@ BMI160::set_gyro_range(unsigned max_dps)
return -EINVAL;
}
_gyro_range_rad_s = (max_gyro_dps / 180.0f * M_PI_F);
_gyro_range_scale = (M_PI_F / (180.0f * lsb_per_dps));
_px4_gyro.set_scale(M_PI_F / (180.0f * lsb_per_dps));
modify_reg(BMIREG_GYR_RANGE, clearbits, setbits);
@@ -802,12 +468,8 @@ BMI160::start()
/* make sure we are stopped first */
stop();
/* discard any stale data in the buffers */
_accel_reports->flush();
_gyro_reports->flush();
/* start polling at the specified rate */
ScheduleOnInterval(_call_interval - BMI160_TIMER_REDUCTION, 1000);
ScheduleOnInterval((1_s / BMI160_GYRO_DEFAULT_RATE) - BMI160_TIMER_REDUCTION, 1000);
reset();
}
@@ -876,7 +538,7 @@ BMI160::measure()
return;
}
struct BMIReport bmi_report;
struct BMIReport bmi_report {};
struct Report {
int16_t accel_x;
@@ -898,6 +560,8 @@ BMI160::measure()
uint8_t status = read_reg(BMIREG_STATUS);
const hrt_abstime timestamp_sample = hrt_absolute_time();
if (OK != transfer((uint8_t *)&bmi_report, ((uint8_t *)&bmi_report), sizeof(bmi_report))) {
return;
}
@@ -921,6 +585,8 @@ BMI160::measure()
report.temp = ((temp_h << 8) + temp_l);
report.accel_x = bmi_report.accel_x;
report.accel_y = bmi_report.accel_y;
report.accel_z = bmi_report.accel_z;
@@ -936,6 +602,7 @@ BMI160::measure()
report.gyro_x == 0 &&
report.gyro_y == 0 &&
report.gyro_z == 0) {
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
@@ -956,22 +623,17 @@ BMI160::measure()
return;
}
/*
* Report buffers.
*/
sensor_accel_s arb{};
sensor_gyro_s grb{};
/*
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
_px4_accel.set_error_count(error_count);
_px4_gyro.set_error_count(error_count);
const float temperature = 23.0f + report.temp * 1.0f / 512.0f;
_px4_accel.set_temperature(temperature);
_px4_gyro.set_temperature(temperature);
/*
* 1) Scale raw value to SI units using scaling from datasheet.
@@ -991,97 +653,8 @@ BMI160::measure()
/* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x;
arb.y_raw = report.accel_y;
arb.z_raw = report.accel_z;
float xraw_f = report.accel_x;
float yraw_f = report.accel_y;
float zraw_f = report.accel_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
arb.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
matrix::Vector3f aval_integrated;
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
arb.x_integral = aval_integrated(0);
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
_last_temperature = 23 + report.temp * 1.0f / 512.0f;
arb.temperature = _last_temperature;
/* return device ID */
arb.device_id = _device_id.devid;
grb.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z;
xraw_f = report.gyro_x;
yraw_f = report.gyro_y;
zraw_f = report.gyro_z;
// apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
matrix::Vector3f gval_integrated;
bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
grb.x_integral = gval_integrated(0);
grb.y_integral = gval_integrated(1);
grb.z_integral = gval_integrated(2);
grb.scaling = _gyro_range_scale;
grb.temperature = _last_temperature;
/* return device ID */
grb.device_id = _gyro->_device_id.devid;
_accel_reports->force(&arb);
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
if (accel_notify) {
poll_notify(POLLIN);
}
if (gyro_notify) {
_gyro->parent_poll_notify();
}
if (accel_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (gyro_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}
_px4_accel.update(timestamp_sample, bmi_report.accel_x, bmi_report.accel_y, bmi_report.accel_z);
_px4_gyro.update(timestamp_sample, bmi_report.gyro_x, bmi_report.gyro_y, bmi_report.gyro_z);
/* stop measuring */
perf_end(_sample_perf);
@@ -1098,8 +671,7 @@ BMI160::print_info()
perf_print_counter(_good_transfers);
perf_print_counter(_reset_retries);
perf_print_counter(_duplicates);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i = 0; i < BMI160_NUM_CHECKED_REGISTERS; i++) {
@@ -1120,7 +692,8 @@ BMI160::print_info()
}
}
::printf("temperature: %.1f\n", (double)_last_temperature);
_px4_accel.print_status();
_px4_gyro.print_status();
}
void
src/drivers/imu/bmi160/bmi160.hpp
+17 -93
View File
@@ -2,51 +2,18 @@
#define BMI160_HPP_
#include <px4_config.h>
#include <sys/types.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 <perf/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/conversions.h>
#include <nuttx/arch.h>
#include <nuttx/clock.h>
#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/integrator.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>
#include <px4_work_queue/ScheduledWorkItem.hpp>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#define DIR_READ 0x80
#define DIR_WRITE 0x00
#define BMI160_DEVICE_PATH_ACCEL "/dev/bmi160_accel"
#define BMI160_DEVICE_PATH_GYRO "/dev/bmi160_gyro"
#define BMI160_DEVICE_PATH_MAG "/dev/bmi160_mag"
#define BMI160_DEVICE_PATH_ACCEL_EXT "/dev/bmi160_accel_ext"
#define BMI160_DEVICE_PATH_GYRO_EXT "/dev/bmi160_gyro_ext"
#define BMI160_DEVICE_PATH_MAG_EXT "/dev/bmi160_mag_ext"
// BMI 160 registers
#define BMIREG_CHIP_ID 0x00
@@ -242,19 +209,16 @@
#define BMI160_TIMER_REDUCTION 200
class BMI160_gyro;
using namespace time_literals;
class BMI160 : public device::SPI, public px4::ScheduledWorkItem
{
public:
BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t device, enum Rotation rotation);
BMI160(int bus, uint32_t device, enum Rotation rotation);
virtual ~BMI160();
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.
*/
@@ -268,36 +232,18 @@ public:
protected:
virtual int probe();
friend class BMI160_gyro;
virtual ssize_t gyro_read(struct file *filp, char *buffer, size_t buflen);
virtual int gyro_ioctl(struct file *filp, int cmd, unsigned long arg);
private:
BMI160_gyro *_gyro;
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
uint8_t _whoami; /** whoami result */
unsigned _call_interval;
ringbuffer::RingBuffer *_accel_reports;
struct accel_calibration_s _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _accel_orb_class_instance;
int _accel_class_instance;
ringbuffer::RingBuffer *_gyro_reports;
struct gyro_calibration_s _gyro_scale;
float _gyro_range_scale;
float _gyro_range_rad_s;
unsigned _dlpf_freq;
float _accel_sample_rate;
float _gyro_sample_rate;
float _accel_sample_rate{BMI160_ACCEL_DEFAULT_RATE};
float _gyro_sample_rate{BMI160_GYRO_DEFAULT_RATE};
perf_counter_t _accel_reads;
perf_counter_t _gyro_reads;
perf_counter_t _sample_perf;
@@ -307,36 +253,21 @@ private:
perf_counter_t _reset_retries;
perf_counter_t _duplicates;
uint8_t _register_wait;
uint64_t _reset_wait;
math::LowPassFilter2p _accel_filter_x;
math::LowPassFilter2p _accel_filter_y;
math::LowPassFilter2p _accel_filter_z;
math::LowPassFilter2p _gyro_filter_x;
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
Integrator _accel_int;
Integrator _gyro_int;
enum Rotation _rotation;
uint8_t _register_wait{0};
uint64_t _reset_wait{0};
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
#define BMI160_NUM_CHECKED_REGISTERS 10
static constexpr int BMI160_NUM_CHECKED_REGISTERS{10};
static const uint8_t _checked_registers[BMI160_NUM_CHECKED_REGISTERS];
uint8_t _checked_values[BMI160_NUM_CHECKED_REGISTERS];
uint8_t _checked_bad[BMI160_NUM_CHECKED_REGISTERS];
uint8_t _checked_next;
// last temperature reading for print_info()
float _last_temperature;
uint8_t _checked_next{0};
// keep last accel reading for duplicate detection
uint16_t _last_accel[3];
bool _got_duplicate;
uint16_t _last_accel[3] {};
bool _got_duplicate{false};
/**
* Start automatic measurement.
@@ -413,13 +344,6 @@ private:
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
/**
* Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
/*
set low pass filter frequency
*/
src/drivers/imu/bmi160/bmi160_gyro.cpp
-63
View File
@@ -1,63 +0,0 @@
#include "bmi160_gyro.hpp"
#include "bmi160.hpp"
BMI160_gyro::BMI160_gyro(BMI160 *parent, const char *path) : CDev("BMI160_gyro", path),
_parent(parent),
_gyro_topic(nullptr),
_gyro_orb_class_instance(-1),
_gyro_class_instance(-1)
{
}
BMI160_gyro::~BMI160_gyro()
{
if (_gyro_class_instance != -1) {
unregister_class_devname(GYRO_BASE_DEVICE_PATH, _gyro_class_instance);
}
}
int
BMI160_gyro::init()
{
int ret;
// do base class init
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;
}
void
BMI160_gyro::parent_poll_notify()
{
poll_notify(POLLIN);
}
ssize_t
BMI160_gyro::read(struct file *filp, char *buffer, size_t buflen)
{
return _parent->gyro_read(filp, buffer, buflen);
}
int
BMI160_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/bmi160/bmi160_gyro.hpp
-41
View File
@@ -1,41 +0,0 @@
#ifndef BMI160_GYRO_HPP_
#define BMI160_GYRO_HPP_
#include <px4_config.h>
#include "bmi160.hpp"
/**
* Helper class implementing the gyro driver node.
*/
class BMI160_gyro : public device::CDev
{
public:
BMI160_gyro(BMI160 *parent, const char *path);
~BMI160_gyro();
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
virtual int init();
protected:
friend class BMI160;
void parent_poll_notify();
private:
BMI160 *_parent;
orb_advert_t _gyro_topic;
int _gyro_orb_class_instance;
int _gyro_class_instance;
/* do not allow to copy this class due to pointer data members */
BMI160_gyro(const BMI160_gyro &);
BMI160_gyro operator=(const BMI160_gyro &);
};
#endif /* BMI160_GYRO_HPP_ */
src/drivers/imu/bmi160/bmi160_main.cpp
+4 -124
View File
@@ -1,7 +1,7 @@
#include "bmi160.hpp"
#include <board_config.h>
#include <px4_config.h>
#include <px4_getopt.h>
/** driver 'main' command */
@@ -18,8 +18,6 @@ BMI160 *g_dev_ext; // on external bus
void start(bool, enum Rotation);
void stop(bool);
void test(bool);
void reset(bool);
void info(bool);
void regdump(bool);
void testerror(bool);
@@ -35,10 +33,7 @@ void usage();
void
start(bool external_bus, enum Rotation rotation)
{
int fd;
BMI160 **g_dev_ptr = external_bus ? &g_dev_ext : &g_dev_int;
const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus ? BMI160_DEVICE_PATH_GYRO_EXT : BMI160_DEVICE_PATH_GYRO;
if (*g_dev_ptr != nullptr)
/* if already started, the still command succeeded */
@@ -49,14 +44,14 @@ start(bool external_bus, enum Rotation rotation)
/* create the driver */
if (external_bus) {
#if defined(PX4_SPI_BUS_EXT) && defined(PX4_SPIDEV_EXT_BMI)
*g_dev_ptr = new BMI160(PX4_SPI_BUS_EXT, path_accel, path_gyro, PX4_SPIDEV_EXT_BMI, rotation);
*g_dev_ptr = new BMI160(PX4_SPI_BUS_EXT, PX4_SPIDEV_EXT_BMI, rotation);
#else
errx(0, "External SPI not available");
#endif
} else {
#if defined(PX4_SPIDEV_BMI)
*g_dev_ptr = new BMI160(PX4_SPI_BUS_SENSORS, path_accel, path_gyro, PX4_SPIDEV_BMI, rotation);
*g_dev_ptr = new BMI160(PX4_SPI_BUS_SENSORS, PX4_SPIDEV_BMI, rotation);
#else
errx(0, "No Internal SPI CS");
#endif
@@ -70,19 +65,6 @@ start(bool external_bus, enum Rotation rotation)
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(path_accel, O_RDONLY);
if (fd < 0) {
goto fail;
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
goto fail;
}
close(fd);
exit(0);
fail:
@@ -111,94 +93,6 @@ stop(bool external_bus)
exit(0);
}
/**
* Perform some basic functional tests on the driver;
* make sure we can collect data from the sensor in polled
* and automatic modes.
*/
void
test(bool external_bus)
{
const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
const char *path_gyro = external_bus ? BMI160_DEVICE_PATH_GYRO_EXT : BMI160_DEVICE_PATH_GYRO;
sensor_accel_s a_report{};
sensor_gyro_s g_report{};
ssize_t sz;
/* get the driver */
int fd = open(path_accel, O_RDONLY);
if (fd < 0)
err(1, "%s open failed (try 'bmi160 start')",
path_accel);
/* get the driver */
int fd_gyro = open(path_gyro, O_RDONLY);
if (fd_gyro < 0) {
err(1, "%s open failed", path_gyro);
}
/* do a simple demand read */
sz = read(fd, &a_report, sizeof(a_report));
if (sz != sizeof(a_report)) {
warnx("ret: %d, expected: %d", sz, sizeof(a_report));
err(1, "immediate acc read failed");
}
print_message(a_report);
/* do a simple demand read */
sz = 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);
/* reset to default polling */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "reset to default polling");
}
close(fd);
close(fd_gyro);
/* XXX add poll-rate tests here too */
reset(external_bus);
errx(0, "PASS");
}
/**
* Reset the driver.
*/
void
reset(bool external_bus)
{
const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL;
int fd = open(path_accel, O_RDONLY);
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "driver poll restart failed");
}
close(fd);
exit(0);
}
/**
* Print a little info about the driver.
*/
@@ -256,7 +150,7 @@ testerror(bool external_bus)
void
usage()
{
warnx("missing command: try 'start', 'info', 'test', 'stop',\n'reset', 'regdump', 'testerror'");
warnx("missing command: try 'start', 'info', 'stop', 'regdump', 'testerror'");
warnx("options:");
warnx(" -X (external bus)");
warnx(" -R rotation");
@@ -307,20 +201,6 @@ bmi160_main(int argc, char *argv[])
bmi160::stop(external_bus);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test")) {
bmi160::test(external_bus);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset")) {
bmi160::reset(external_bus);
}
/*
* Print driver information.
*/