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

LSM303D: Fixed code style

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-19 13:18:47 +02:00
parent 7a33697b85
commit 19e3da7169
1 changed files with 249 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/lsm303d/lsm303d.cpp
+249 -161
View File
@@ -236,7 +236,7 @@ class LSM303D_mag;
class LSM303D : public device::SPI
{
public:
LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation);
LSM303D(int bus, const char *path, spi_dev_e device, enum Rotation rotation);
virtual ~LSM303D();
virtual int init();
@@ -497,8 +497,8 @@ private:
int mag_set_samplerate(unsigned frequency);
/* this class cannot be copied */
LSM303D(const LSM303D&);
LSM303D operator=(const LSM303D&);
LSM303D(const LSM303D &);
LSM303D operator=(const LSM303D &);
};
/*
@@ -512,7 +512,8 @@ const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADD
ADDR_CTRL_REG4,
ADDR_CTRL_REG5,
ADDR_CTRL_REG6,
ADDR_CTRL_REG7 };
ADDR_CTRL_REG7
};
/**
* Helper class implementing the mag driver node.
@@ -544,13 +545,14 @@ private:
void measure_trampoline(void *arg);
/* this class does not allow copying due to ptr data members */
LSM303D_mag(const LSM303D_mag&);
LSM303D_mag operator=(const LSM303D_mag&);
LSM303D_mag(const LSM303D_mag &);
LSM303D_mag operator=(const LSM303D_mag &);
};
LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
SPI("LSM303D", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
LSM303D::LSM303D(int bus, const char *path, spi_dev_e device, enum Rotation rotation) :
SPI("LSM303D", path, bus, device, SPIDEV_MODE3,
11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within safety margins for LSM303D */),
_mag(new LSM303D_mag(this)),
_accel_call{},
_mag_call{},
@@ -621,13 +623,17 @@ LSM303D::~LSM303D()
stop();
/* free any existing reports */
if (_accel_reports != nullptr)
if (_accel_reports != nullptr) {
delete _accel_reports;
if (_mag_reports != nullptr)
delete _mag_reports;
}
if (_accel_class_instance != -1)
if (_mag_reports != nullptr) {
delete _mag_reports;
}
if (_accel_class_instance != -1) {
unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _accel_class_instance);
}
delete _mag;
@@ -653,18 +659,21 @@ LSM303D::init()
/* allocate basic report buffers */
_accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr)
if (_accel_reports == nullptr) {
goto out;
}
_mag_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_mag_reports == nullptr)
if (_mag_reports == nullptr) {
goto out;
}
reset();
/* do CDev init for the mag device node */
ret = _mag->init();
if (ret != OK) {
warnx("MAG init failed");
goto out;
@@ -679,7 +688,7 @@ LSM303D::init()
/* measurement will have generated a report, publish */
_mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp,
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
&_mag->_mag_orb_class_instance, ORB_PRIO_LOW);
if (_mag->_mag_topic == nullptr) {
warnx("ADVERT ERR");
@@ -694,7 +703,7 @@ LSM303D::init()
/* measurement will have generated a report, publish */
_accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp,
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
&_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT);
if (_accel_topic == nullptr) {
warnx("ADVERT ERR");
@@ -762,7 +771,7 @@ LSM303D::probe()
if (success) {
_checked_values[0] = WHO_I_AM;
return OK;
}
}
return -EIO;
}
@@ -775,8 +784,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
int ret = 0;
/* buffer must be large enough */
if (count < 1)
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_call_accel_interval > 0) {
@@ -798,8 +808,9 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
measure();
/* measurement will have generated a report, copy it out */
if (_accel_reports->get(arb))
if (_accel_reports->get(arb)) {
ret = sizeof(*arb);
}
return ret;
}
@@ -812,8 +823,9 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
int ret = 0;
/* buffer must be large enough */
if (count < 1)
if (count < 1) {
return -ENOSPC;
}
/* if automatic measurement is enabled */
if (_call_mag_interval > 0) {
@@ -837,8 +849,9 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
_mag->measure();
/* measurement will have generated a report, copy it out */
if (_mag_reports->get(mrb))
if (_mag_reports->get(mrb)) {
ret = sizeof(*mrb);
}
return ret;
}
@@ -849,7 +862,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
@@ -871,56 +884,62 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSOR_POLLRATE_DEFAULT:
return ioctl(filp, SENSORIOCSPOLLRATE, LSM303D_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_accel_interval == 0);
/* do we need to start internal polling? */
bool want_start = (_call_accel_interval == 0);
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* convert hz to hrt interval via microseconds */
unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
if (ticks < 500)
return -EINVAL;
/* check against maximum sane rate */
if (ticks < 500) {
return -EINVAL;
}
/* adjust filters */
accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
/* adjust filters */
accel_set_driver_lowpass_filter((float)arg, _accel_filter_x.get_cutoff_freq());
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call_accel_interval = ticks;
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_call_accel_interval = ticks;
_accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION;
_accel_call.period = _call_accel_interval - LSM303D_TIMER_REDUCTION;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
/* if we need to start the poll state machine, do it */
if (want_start) {
start();
}
return OK;
return OK;
}
}
}
}
case SENSORIOCGPOLLRATE:
if (_call_accel_interval == 0)
if (_call_accel_interval == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / _call_accel_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_accel_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqstate_t flags = irqsave();
if (!_accel_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _accel_reports->size();
@@ -936,23 +955,25 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
return _accel_samplerate;
case ACCELIOCSLOWPASS: {
return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
}
return accel_set_driver_lowpass_filter((float)_accel_samplerate, (float)arg);
}
case ACCELIOCGLOWPASS:
return static_cast<int>(_accel_filter_x.get_cutoff_freq());
case ACCELIOCSSCALE: {
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) 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;
/* copy scale, but only if off by a few percent */
struct accel_scale *s = (struct accel_scale *) 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;
}
}
}
case ACCELIOCSRANGE:
/* arg needs to be in G */
@@ -960,7 +981,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
case ACCELIOCGRANGE:
/* convert to m/s^2 and return rounded in G */
return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f);
return (unsigned long)((_accel_range_m_s2) / LSM303D_ONE_G + 0.5f);
case ACCELIOCGSCALE:
/* copy scale out */
@@ -982,7 +1003,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
switch (cmd) {
case SENSORIOCSPOLLRATE: {
switch (arg) {
switch (arg) {
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
@@ -1012,16 +1033,18 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
unsigned ticks = 1000000 / arg;
/* check against maximum sane rate */
if (ticks < 1000)
if (ticks < 1000) {
return -EINVAL;
}
/* update interval for next measurement */
/* XXX this is a bit shady, but no other way to adjust... */
_mag_call.period = _call_mag_interval = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
if (want_start) {
start();
}
return OK;
}
@@ -1029,25 +1052,29 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
}
case SENSORIOCGPOLLRATE:
if (_call_mag_interval == 0)
if (_call_mag_interval == 0) {
return SENSOR_POLLRATE_MANUAL;
}
return 1000000 / _call_mag_interval;
case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100))
return -EINVAL;
/* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) {
return -EINVAL;
}
irqstate_t flags = irqsave();
if (!_mag_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqstate_t flags = irqsave();
if (!_mag_reports->resize(arg)) {
irqrestore(flags);
return -ENOMEM;
}
irqrestore(flags);
return OK;
}
return OK;
}
case SENSORIOCGQUEUEDEPTH:
return _mag_reports->size();
@@ -1102,24 +1129,34 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg)
int
LSM303D::accel_self_test()
{
if (_accel_read == 0)
if (_accel_read == 0) {
return 1;
}
/* inspect accel offsets */
if (fabsf(_accel_scale.x_offset) < 0.000001f)
return 1;
if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
if (fabsf(_accel_scale.x_offset) < 0.000001f) {
return 1;
}
if (fabsf(_accel_scale.y_offset) < 0.000001f)
return 1;
if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f) {
return 1;
}
if (fabsf(_accel_scale.z_offset) < 0.000001f)
if (fabsf(_accel_scale.y_offset) < 0.000001f) {
return 1;
if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
}
if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f) {
return 1;
}
if (fabsf(_accel_scale.z_offset) < 0.000001f) {
return 1;
}
if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f) {
return 1;
}
return 0;
}
@@ -1127,21 +1164,25 @@ LSM303D::accel_self_test()
int
LSM303D::mag_self_test()
{
if (_mag_read == 0)
if (_mag_read == 0) {
return 1;
}
/**
* inspect mag offsets
* don't check mag scale because it seems this is calibrated on chip
*/
if (fabsf(_mag_scale.x_offset) < 0.000001f)
if (fabsf(_mag_scale.x_offset) < 0.000001f) {
return 1;
}
if (fabsf(_mag_scale.y_offset) < 0.000001f)
if (fabsf(_mag_scale.y_offset) < 0.000001f) {
return 1;
}
if (fabsf(_mag_scale.z_offset) < 0.000001f)
if (fabsf(_mag_scale.z_offset) < 0.000001f) {
return 1;
}
return 0;
}
@@ -1174,7 +1215,8 @@ void
LSM303D::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
for (uint8_t i = 0; i < LSM303D_NUM_CHECKED_REGISTERS; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
}
@@ -1199,31 +1241,32 @@ LSM303D::accel_set_range(unsigned max_g)
uint8_t clearbits = REG2_FULL_SCALE_BITS_A;
float new_scale_g_digit = 0.0f;
if (max_g == 0)
if (max_g == 0) {
max_g = 16;
}
if (max_g <= 2) {
_accel_range_m_s2 = 2.0f*LSM303D_ONE_G;
_accel_range_m_s2 = 2.0f * LSM303D_ONE_G;
setbits |= REG2_FULL_SCALE_2G_A;
new_scale_g_digit = 0.061e-3f;
} else if (max_g <= 4) {
_accel_range_m_s2 = 4.0f*LSM303D_ONE_G;
_accel_range_m_s2 = 4.0f * LSM303D_ONE_G;
setbits |= REG2_FULL_SCALE_4G_A;
new_scale_g_digit = 0.122e-3f;
} else if (max_g <= 6) {
_accel_range_m_s2 = 6.0f*LSM303D_ONE_G;
_accel_range_m_s2 = 6.0f * LSM303D_ONE_G;
setbits |= REG2_FULL_SCALE_6G_A;
new_scale_g_digit = 0.183e-3f;
} else if (max_g <= 8) {
_accel_range_m_s2 = 8.0f*LSM303D_ONE_G;
_accel_range_m_s2 = 8.0f * LSM303D_ONE_G;
setbits |= REG2_FULL_SCALE_8G_A;
new_scale_g_digit = 0.244e-3f;
} else if (max_g <= 16) {
_accel_range_m_s2 = 16.0f*LSM303D_ONE_G;
_accel_range_m_s2 = 16.0f * LSM303D_ONE_G;
setbits |= REG2_FULL_SCALE_16G_A;
new_scale_g_digit = 0.732e-3f;
@@ -1246,8 +1289,9 @@ LSM303D::mag_set_range(unsigned max_ga)
uint8_t clearbits = REG6_FULL_SCALE_BITS_M;
float new_scale_ga_digit = 0.0f;
if (max_ga == 0)
if (max_ga == 0) {
max_ga = 12;
}
if (max_ga <= 2) {
_mag_range_ga = 2;
@@ -1286,8 +1330,9 @@ LSM303D::accel_set_onchip_lowpass_filter_bandwidth(unsigned bandwidth)
uint8_t setbits = 0;
uint8_t clearbits = REG2_ANTIALIAS_FILTER_BW_BITS_A;
if (bandwidth == 0)
if (bandwidth == 0) {
bandwidth = 773;
}
if (bandwidth <= 50) {
setbits |= REG2_AA_FILTER_BW_50HZ_A;
@@ -1369,8 +1414,9 @@ LSM303D::mag_set_samplerate(unsigned frequency)
uint8_t setbits = 0;
uint8_t clearbits = REG5_RATE_BITS_M;
if (frequency == 0)
if (frequency == 0) {
frequency = 100;
}
if (frequency <= 25) {
setbits |= REG5_RATE_25HZ_M;
@@ -1405,9 +1451,9 @@ LSM303D::start()
/* start polling at the specified rate */
hrt_call_every(&_accel_call,
1000,
_call_accel_interval - LSM303D_TIMER_REDUCTION,
(hrt_callout)&LSM303D::measure_trampoline, this);
1000,
_call_accel_interval - LSM303D_TIMER_REDUCTION,
(hrt_callout)&LSM303D::measure_trampoline, this);
hrt_call_every(&_mag_call, 1000, _call_mag_interval, (hrt_callout)&LSM303D::mag_measure_trampoline, this);
}
@@ -1447,7 +1493,8 @@ void
LSM303D::check_registers(void)
{
uint8_t v;
if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) {
if ((v = read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) {
/*
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
@@ -1465,9 +1512,11 @@ LSM303D::check_registers(void)
if (_checked_next != 0) {
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
}
_register_wait = 20;
}
_checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS;
}
_checked_next = (_checked_next + 1) % LSM303D_NUM_CHECKED_REGISTERS;
}
void
@@ -1505,11 +1554,11 @@ LSM303D::measure()
raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT;
transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report));
if (!(raw_accel_report.status & REG_STATUS_A_NEW_ZYXADA)) {
if (!(raw_accel_report.status & REG_STATUS_A_NEW_ZYXADA)) {
perf_end(_accel_sample_perf);
perf_count(_accel_duplicates);
return;
}
}
/*
* 1) Scale raw value to SI units using scaling from datasheet.
@@ -1536,7 +1585,7 @@ LSM303D::measure()
// register reads and bad values. This allows the higher level
// code to decide if it should use this sensor based on
// whether it has had failures
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values);
accel_report.x_raw = raw_accel_report.x;
accel_report.y_raw = raw_accel_report.y;
@@ -1565,9 +1614,11 @@ LSM303D::measure()
fabsf(y_in_new) > 20 &&
fabsf(z_in_new) > 20) {
_constant_accel_count += 1;
} else {
_constant_accel_count = 0;
}
if (_constant_accel_count > 100) {
// we've had 100 constant accel readings with large
// values. The sensor is almost certainly dead. We
@@ -1712,16 +1763,19 @@ LSM303D::print_info()
perf_print_counter(_accel_duplicates);
_accel_reports->print_info("accel reports");
_mag_reports->print_info("mag reports");
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i=0; i<LSM303D_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
}
}
::printf("checked_next: %u\n", _checked_next);
for (uint8_t i = 0; i < LSM303D_NUM_CHECKED_REGISTERS; i++) {
uint8_t v = read_reg(_checked_registers[i]);
if (v != _checked_values[i]) {
::printf("reg %02x:%02x should be %02x\n",
(unsigned)_checked_registers[i],
(unsigned)v,
(unsigned)_checked_values[i]);
}
}
::printf("temperature: %.2f\n", (double)_last_temperature);
}
@@ -1772,12 +1826,13 @@ LSM303D::print_registers()
{ ADDR_CLICK_SRC, "CLICK_SRC" },
{ ADDR_CLICK_THS, "CLICK_THS" },
{ ADDR_TIME_LIMIT, "TIME_LIMIT" },
{ ADDR_TIME_LATENCY,"TIME_LATENCY" },
{ ADDR_TIME_LATENCY, "TIME_LATENCY" },
{ ADDR_TIME_WINDOW, "TIME_WINDOW" },
{ ADDR_ACT_THS, "ACT_THS" },
{ ADDR_ACT_DUR, "ACT_DUR" }
};
for (uint8_t i=0; i<sizeof(regmap)/sizeof(regmap[0]); i++) {
for (uint8_t i = 0; i < sizeof(regmap) / sizeof(regmap[0]); i++) {
printf("0x%02x %s\n", read_reg(regmap[i].reg), regmap[i].name);
}
}
@@ -1786,7 +1841,7 @@ void
LSM303D::test_error()
{
// trigger an error
write_reg(ADDR_CTRL_REG3, 0);
write_reg(ADDR_CTRL_REG3, 0);
}
LSM303D_mag::LSM303D_mag(LSM303D *parent) :
@@ -1800,8 +1855,9 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) :
LSM303D_mag::~LSM303D_mag()
{
if (_mag_class_instance != -1)
if (_mag_class_instance != -1) {
unregister_class_devname(MAG_BASE_DEVICE_PATH, _mag_class_instance);
}
}
int
@@ -1810,8 +1866,10 @@ LSM303D_mag::init()
int ret;
ret = CDev::init();
if (ret != OK)
if (ret != OK) {
goto out;
}
_mag_class_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
@@ -1835,11 +1893,12 @@ int
LSM303D_mag::ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case DEVIOCGDEVICEID:
return (int)CDev::ioctl(filp, cmd, arg);
break;
default:
return _parent->mag_ioctl(filp, cmd, arg);
case DEVIOCGDEVICEID:
return (int)CDev::ioctl(filp, cmd, arg);
break;
default:
return _parent->mag_ioctl(filp, cmd, arg);
}
}
@@ -1881,17 +1940,19 @@ void
start(bool external_bus, enum Rotation rotation, unsigned range)
{
int fd, fd_mag;
if (g_dev != nullptr) {
errx(0, "already started");
}
/* create the driver */
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
if (external_bus) {
#ifdef PX4_SPI_BUS_EXT
g_dev = new LSM303D(PX4_SPI_BUS_EXT, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_EXT_ACCEL_MAG, rotation);
#else
#else
errx(0, "External SPI not available");
#endif
#endif
} else {
g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG, rotation);
}
@@ -1901,8 +1962,9 @@ start(bool external_bus, enum Rotation rotation, unsigned range)
goto fail;
}
if (OK != g_dev->init())
if (OK != g_dev->init()) {
goto fail;
}
/* set the poll rate to default, starts automatic data collection */
fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
@@ -1928,8 +1990,8 @@ start(bool external_bus, enum Rotation rotation, unsigned range)
}
}
close(fd);
close(fd_mag);
close(fd);
close(fd_mag);
exit(0);
fail:
@@ -1958,14 +2020,16 @@ test()
/* get the driver */
fd_accel = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd_accel < 0)
if (fd_accel < 0) {
err(1, "%s open failed", LSM303D_DEVICE_PATH_ACCEL);
}
/* do a simple demand read */
sz = read(fd_accel, &accel_report, sizeof(accel_report));
if (sz != sizeof(accel_report))
if (sz != sizeof(accel_report)) {
err(1, "immediate read failed");
}
warnx("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x);
@@ -1976,10 +2040,13 @@ test()
warnx("accel z: \t%d\traw", (int)accel_report.z_raw);
warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2);
if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0)))
if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) {
warnx("accel antialias filter bandwidth: fail");
else
} else {
warnx("accel antialias filter bandwidth: %d Hz", ret);
}
int fd_mag = -1;
struct mag_report m_report;
@@ -1987,19 +2054,23 @@ test()
/* get the driver */
fd_mag = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
if (fd_mag < 0)
if (fd_mag < 0) {
err(1, "%s open failed", LSM303D_DEVICE_PATH_MAG);
}
/* check if mag is onboard or external */
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0)
if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) {
errx(1, "failed to get if mag is onboard or external");
}
warnx("mag device active: %s", ret ? "external" : "onboard");
/* do a simple demand read */
sz = read(fd_mag, &m_report, sizeof(m_report));
if (sz != sizeof(m_report))
if (sz != sizeof(m_report)) {
err(1, "immediate read failed");
}
warnx("mag x: \t% 9.5f\tga", (double)m_report.x);
warnx("mag y: \t% 9.5f\tga", (double)m_report.y);
@@ -2014,8 +2085,8 @@ test()
err(1, "reset to default polling");
}
close(fd_accel);
close(fd_mag);
close(fd_accel);
close(fd_mag);
reset();
errx(0, "PASS");
@@ -2029,28 +2100,33 @@ reset()
{
int fd = open(LSM303D_DEVICE_PATH_ACCEL, O_RDONLY);
if (fd < 0)
if (fd < 0) {
err(1, "failed ");
}
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
err(1, "driver reset failed");
}
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "accel pollrate reset failed");
}
close(fd);
close(fd);
fd = open(LSM303D_DEVICE_PATH_MAG, O_RDONLY);
if (fd < 0) {
warnx("mag could not be opened, external mag might be used");
} else {
/* no need to reset the mag as well, the reset() is the same */
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
err(1, "mag pollrate reset failed");
}
}
close(fd);
close(fd);
exit(0);
}
@@ -2061,8 +2137,9 @@ reset()
void
info()
{
if (g_dev == nullptr)
if (g_dev == nullptr) {
errx(1, "driver not running\n");
}
printf("state @ %p\n", g_dev);
g_dev->print_info();
@@ -2076,8 +2153,9 @@ info()
void
regdump()
{
if (g_dev == nullptr)
if (g_dev == nullptr) {
errx(1, "driver not running\n");
}
printf("regdump @ %p\n", g_dev);
g_dev->print_registers();
@@ -2091,8 +2169,9 @@ regdump()
void
test_error()
{
if (g_dev == nullptr)
if (g_dev == nullptr) {
errx(1, "driver not running\n");
}
g_dev->test_error();
@@ -2124,12 +2203,15 @@ lsm303d_main(int argc, char *argv[])
case 'X':
external_bus = true;
break;
case 'R':
rotation = (enum Rotation)atoi(optarg);
break;
case 'a':
accel_range = atoi(optarg);
break;
default:
lsm303d::usage();
exit(0);
@@ -2142,38 +2224,44 @@ lsm303d_main(int argc, char *argv[])
* Start/load the driver.
*/
if (!strcmp(verb, "start"))
if (!strcmp(verb, "start")) {
lsm303d::start(external_bus, rotation, accel_range);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test"))
if (!strcmp(verb, "test")) {
lsm303d::test();
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset"))
if (!strcmp(verb, "reset")) {
lsm303d::reset();
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info"))
if (!strcmp(verb, "info")) {
lsm303d::info();
}
/*
* dump device registers
*/
if (!strcmp(verb, "regdump"))
if (!strcmp(verb, "regdump")) {
lsm303d::regdump();
}
/*
* trigger an error
*/
if (!strcmp(verb, "testerror"))
if (!strcmp(verb, "testerror")) {
lsm303d::test_error();
}
errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'");
}