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

bma180: move to PX4Accelerometer and cleanup

Browse Source
This commit is contained in:
Daniel Agar
2020-04-15 23:33:41 -04:00
parent 9f7c3b9f06
commit 47098f026a
1 changed files with 54 additions and 252 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/bma180/bma180.cpp
+54 -252
View File
@@ -40,40 +40,14 @@
#include <px4_platform_common/defines.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <ecl/geo/geo.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 <nuttx/arch.h>
#include <nuttx/clock.h>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include <board_config.h>
#include <drivers/device/spi.h>
#include <drivers/drv_accel.h>
#include <drivers/device/ringbuffer.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#define ACCEL_DEVICE_PATH "/dev/bma180"
#define DIR_READ (1<<7)
#define DIR_WRITE (0<<7)
static constexpr uint8_t DIR_READ{1 << 7};
#define ADDR_CHIP_ID 0x00
#define CHIP_ID 0x03
@@ -121,8 +95,6 @@
#define ADDR_OFFSET_T 0x37
#define OFFSET_T_READOUT_12BIT (1<<0)
extern "C" __EXPORT int bma180_main(int argc, char *argv[]);
class BMA180 : public device::SPI, public I2CSPIDriver<BMA180>
{
public:
@@ -132,30 +104,15 @@ public:
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
static void print_usage();
int init() override;
ssize_t read(struct file *filp, char *buffer, size_t buflen) override;
int ioctl(struct file *filp, int cmd, unsigned long arg) override;
void print_status() override;
void print_status() override;
void RunImpl();
protected:
int probe() override;
private:
int probe() override;
unsigned _call_interval;
ringbuffer::RingBuffer *_reports;
struct accel_calibration_s _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
int _class_instance;
PX4Accelerometer _px4_accel;
unsigned _current_lowpass;
unsigned _current_range;
@@ -178,7 +135,7 @@ private:
* @param The register to read.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg);
uint8_t read_reg(uint8_t reg);
/**
* Write a register in the BMA180
@@ -186,7 +143,7 @@ private:
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
void write_reg(uint8_t reg, uint8_t value);
/**
* Modify a register in the BMA180
@@ -197,7 +154,7 @@ private:
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
void modify_reg(uint8_t reg, uint8_t clearbits, uint8_t setbits);
/**
* Set the BMA180 measurement range.
@@ -222,38 +179,18 @@ BMA180::BMA180(I2CSPIBusOption bus_option, int bus, int32_t device, enum Rotatio
spi_mode_e spi_mode) :
SPI(DRV_ACC_DEVTYPE_BMA180, MODULE_NAME, bus, device, spi_mode, bus_frequency),
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
_call_interval(0),
_reports(nullptr),
_accel_range_scale(0.0f),
_accel_range_m_s2(0.0f),
_accel_topic(nullptr),
_class_instance(-1),
_current_lowpass(0),
_current_range(0),
_sample_perf(perf_alloc(PC_ELAPSED, "bma180_read"))
_px4_accel(get_device_id(), ORB_PRIO_MAX, rotation),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read"))
{
// default 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;
}
BMA180::~BMA180()
{
/* free any existing reports */
if (_reports != nullptr) {
delete _reports;
}
/* delete the perf counter */
perf_free(_sample_perf);
}
int
BMA180::init()
int BMA180::init()
{
int ret = PX4_ERROR;
@@ -262,13 +199,6 @@ BMA180::init()
goto out;
}
/* allocate basic report buffers */
_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s));
if (_reports == nullptr) {
goto out;
}
/* perform soft reset (p48) */
write_reg(ADDR_RESET, SOFT_RESET);
@@ -293,9 +223,13 @@ BMA180::init()
/* disable writing to chip config */
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
if (set_range(4)) { warnx("Failed setting range"); }
if (set_range(4)) {
PX4_ERR("Failed setting range");
}
if (set_lowpass(75)) { warnx("Failed setting lowpass"); }
if (set_lowpass(75)) {
PX4_ERR("Failed setting lowpass");
}
if (read_reg(ADDR_CHIP_ID) == CHIP_ID) {
ret = OK;
@@ -304,28 +238,16 @@ BMA180::init()
ret = PX4_ERROR;
}
_class_instance = register_class_devname(ACCEL_DEVICE_PATH);
/* advertise sensor topic, measure manually to initialize valid report */
measure();
if (_class_instance == CLASS_DEVICE_PRIMARY) {
sensor_accel_s arp;
_reports->get(&arp);
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &arp);
}
_call_interval = 1000000 / 250;
start();
out:
return ret;
}
int
BMA180::probe()
int BMA180::probe()
{
/* dummy read to ensure SPI state machine is sane */
read_reg(ADDR_CHIP_ID);
@@ -337,100 +259,29 @@ BMA180::probe()
return -EIO;
}
ssize_t
BMA180::read(struct file *filp, char *buffer, size_t buflen)
uint8_t BMA180::read_reg(uint8_t reg)
{
unsigned count = buflen / sizeof(sensor_accel_s);
sensor_accel_s *arp = reinterpret_cast<sensor_accel_s *>(buffer);
int ret = 0;
/* buffer must be large enough */
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_call_interval > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
* Note that we may be pre-empted by the measurement code while we are doing this;
* we are careful to avoid racing with it.
*/
while (count--) {
if (_reports->get(arp)) {
ret += sizeof(*arp);
arp++;
}
}
/* if there was no data, warn the caller */
return ret ? ret : -EAGAIN;
}
/* manual measurement */
_reports->flush();
measure();
/* measurement will have generated a report, copy it out */
if (_reports->get(arp)) {
ret = sizeof(*arp);
}
return ret;
}
int
BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case ACCELIOCSSCALE:
/* copy scale in */
memcpy(&_accel_scale, (struct accel_calibration_s *) arg, sizeof(_accel_scale));
return OK;
default:
/* give it to the superclass */
return SPI::ioctl(filp, cmd, arg);
}
}
uint8_t
BMA180::read_reg(unsigned reg)
{
uint8_t cmd[2];
uint8_t cmd[2] {};
cmd[0] = reg | DIR_READ;
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
void
BMA180::write_reg(unsigned reg, uint8_t value)
void BMA180::write_reg(uint8_t reg, uint8_t value)
{
uint8_t cmd[2];
cmd[0] = reg | DIR_WRITE;
cmd[1] = value;
uint8_t cmd[2] {reg, value};
transfer(cmd, nullptr, sizeof(cmd));
}
void
BMA180::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
void BMA180::modify_reg(uint8_t reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val;
val = read_reg(reg);
uint8_t val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_reg(reg, val);
}
int
BMA180::set_range(unsigned max_g)
int BMA180::set_range(unsigned max_g)
{
uint8_t rangebits;
@@ -467,8 +318,8 @@ BMA180::set_range(unsigned max_g)
}
/* set new range scaling factor */
_accel_range_m_s2 = _current_range * CONSTANTS_ONE_G;
_accel_range_scale = _accel_range_m_s2 / 8192.0f;
float accel_range_m_s2 = _current_range * CONSTANTS_ONE_G;
_px4_accel.set_scale(accel_range_m_s2 / 8192.f);
/* enable writing to chip config */
modify_reg(ADDR_CTRL_REG0, 0, REG0_WRITE_ENABLE);
@@ -484,8 +335,7 @@ BMA180::set_range(unsigned max_g)
(OFFSET_LSB1_RANGE_MASK & rangebits));
}
int
BMA180::set_lowpass(unsigned frequency)
int BMA180::set_lowpass(unsigned frequency)
{
uint8_t bwbits;
@@ -527,112 +377,64 @@ BMA180::set_lowpass(unsigned frequency)
modify_reg(ADDR_CTRL_REG0, REG0_WRITE_ENABLE, 0);
/* check if wanted value is now in register */
return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) ==
(BW_TCS_BW_MASK & bwbits));
return !((read_reg(ADDR_BW_TCS) & BW_TCS_BW_MASK) == (BW_TCS_BW_MASK & bwbits));
}
void
BMA180::start()
void BMA180::start()
{
/* reset the report ring */
_reports->flush();
/* start polling at the specified rate */
ScheduleOnInterval(_call_interval, 10000);
ScheduleOnInterval(1000, 10000);
}
void
BMA180::RunImpl()
void BMA180::RunImpl()
{
/* make another measurement */
measure();
}
void
BMA180::measure()
void BMA180::measure()
{
/* BMA180 measurement registers */
// #pragma pack(push, 1)
// struct {
// uint8_t cmd;
// int16_t x;
// int16_t y;
// int16_t z;
// } raw_report;
// #pragma pack(pop)
sensor_accel_s report;
/* start the performance counter */
perf_begin(_sample_perf);
/*
* Fetch the full set of measurements from the BMA180 in one pass;
* starting from the X LSB.
*/
//raw_report.cmd = ADDR_ACC_X_LSB;
// XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));
const hrt_abstime timestamp_sample = hrt_absolute_time();
/*
* Adjust and scale results to SI units.
*
* Note that we ignore the "new data" bits. At any time we read, each
* of the axis measurements are the "most recent", even if we've seen
* them before. There is no good way to synchronise with the internal
* measurement flow without using the external interrupt.
*/
report.timestamp = hrt_absolute_time();
report.error_count = 0;
/*
* y of board is x of sensor and x of board is -y of sensor
* perform only the axis assignment here.
* Two non-value bits are discarded directly
*/
report.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
report.x_raw = read_reg(ADDR_ACC_X_LSB + 2);
report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
report.z_raw = read_reg(ADDR_ACC_X_LSB + 4);
report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
int16_t y_raw = read_reg(ADDR_ACC_X_LSB + 0);
y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
int16_t x_raw = read_reg(ADDR_ACC_X_LSB + 2);
x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
int16_t z_raw = read_reg(ADDR_ACC_X_LSB + 4);
z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
/* discard two non-value bits in the 16 bit measurement */
report.x_raw = (report.x_raw / 4);
report.y_raw = (report.y_raw / 4);
report.z_raw = (report.z_raw / 4);
x_raw = (x_raw / 4);
y_raw = (y_raw / 4);
z_raw = (z_raw / 4);
/* invert y axis, due to 14 bit data no overflow can occur in the negation */
report.y_raw = -report.y_raw;
y_raw = -y_raw;
report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
report.scaling = _accel_range_scale;
_reports->force(&report);
/* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */
if (_accel_topic != nullptr) {
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
}
_px4_accel.update(timestamp_sample, x_raw, y_raw, z_raw);
/* stop the perf counter */
perf_end(_sample_perf);
}
void
BMA180::print_status()
void BMA180::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
_reports->print_info("report queue");
_px4_accel.print_status();
}
void
BMA180::print_usage()
void BMA180::print_usage()
{
PRINT_MODULE_USAGE_NAME("bma180", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
@@ -661,7 +463,7 @@ I2CSPIDriverBase *BMA180::instantiate(const BusCLIArguments &cli, const BusInsta
return instance;
}
extern "C" int bma180_main(int argc, char *argv[])
extern "C" __EXPORT int bma180_main(int argc, char *argv[])
{
int ch;
using ThisDriver = BMA180;