Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 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 14:25:18 -04: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
File diff suppressed because it is too large Load Diff
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.
*/