From 2993c5dd12a2faab625abce7b904e082ad5dd1e0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:18:24 +0200 Subject: [PATCH] L3GD20: Fixed code style --- src/drivers/l3gd20/l3gd20.cpp | 315 ++++++++++++++++++++-------------- 1 file changed, 189 insertions(+), 126 deletions(-) diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 43a24359eb..266c5283be 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -206,7 +206,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } class L3GD20 : public device::SPI { public: - L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation); + L3GD20(int bus, const char *path, spi_dev_e device, enum Rotation rotation); virtual ~L3GD20(); virtual int init(); @@ -389,11 +389,11 @@ private: * * @return 0 on success, 1 on failure */ - int self_test(); + int self_test(); /* this class does not allow copying */ - L3GD20(const L3GD20&); - L3GD20 operator=(const L3GD20&); + L3GD20(const L3GD20 &); + L3GD20 operator=(const L3GD20 &); }; /* @@ -401,16 +401,18 @@ private: that ADDR_WHO_AM_I must be first in the list. */ const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, - ADDR_CTRL_REG1, - ADDR_CTRL_REG2, - ADDR_CTRL_REG3, - ADDR_CTRL_REG4, - ADDR_CTRL_REG5, - ADDR_FIFO_CTRL_REG, - ADDR_LOW_ODR }; + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, + ADDR_CTRL_REG5, + ADDR_FIFO_CTRL_REG, + ADDR_LOW_ODR + }; -L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : - SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), +L3GD20::L3GD20(int bus, const char *path, spi_dev_e device, enum Rotation rotation) : + SPI("L3GD20", path, bus, device, SPIDEV_MODE3, + 11 * 1000 * 1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call{}, _call_interval(0), _reports(nullptr), @@ -456,11 +458,13 @@ L3GD20::~L3GD20() stop(); /* free any existing reports */ - if (_reports != nullptr) + if (_reports != nullptr) { delete _reports; + } - if (_class_instance != -1) + if (_class_instance != -1) { unregister_class_devname(GYRO_BASE_DEVICE_PATH, _class_instance); + } /* delete the perf counter */ perf_free(_sample_perf); @@ -475,14 +479,16 @@ L3GD20::init() int ret = ERROR; /* do SPI init (and probe) first */ - if (SPI::init() != OK) + if (SPI::init() != OK) { goto out; + } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); - if (_reports == nullptr) + if (_reports == nullptr) { goto out; + } _class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); @@ -495,7 +501,7 @@ L3GD20::init() _reports->get(&grp); _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, - &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); + &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { DEVICE_DEBUG("failed to create sensor_gyro publication"); @@ -517,13 +523,15 @@ L3GD20::probe() /* verify that the device is attached and functioning, accept * L3GD20, L3GD20H and L3G4200D */ - if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { + if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; - } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { + + } else if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; success = true; - } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { + + } else if ((v = read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { /* Detect the L3G4200D used on AeroCore */ _is_l3g4200d = true; _orientation = SENSOR_BOARD_ROTATION_DEFAULT; @@ -546,8 +554,9 @@ L3GD20::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_interval > 0) { @@ -588,28 +597,29 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case SENSORIOCSPOLLRATE: { switch (arg) { - /* switching to manual polling */ + /* switching to manual polling */ case SENSOR_POLLRATE_MANUAL: stop(); _call_interval = 0; return OK; - /* external signalling not supported */ + /* external signalling not supported */ case SENSOR_POLLRATE_EXTERNAL: - /* zero would be bad */ + /* zero would be bad */ case 0: return -EINVAL; - /* set default/max polling rate */ + /* set default/max polling rate */ case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_DEFAULT: if (_is_l3g4200d) { return ioctl(filp, SENSORIOCSPOLLRATE, L3G4200D_DEFAULT_RATE); } + return ioctl(filp, SENSORIOCSPOLLRATE, L3GD20_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_interval == 0); @@ -618,23 +628,25 @@ L3GD20::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... */ _call_interval = ticks; - _call.period = _call_interval - L3GD20_TIMER_REDUCTION; + _call.period = _call_interval - L3GD20_TIMER_REDUCTION; /* adjust filters */ float cutoff_freq_hz = _gyro_filter_x.get_cutoff_freq(); - float sample_rate = 1.0e6f/ticks; + float sample_rate = 1.0e6f / ticks; set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); /* if we need to start the poll state machine, do it */ - if (want_start) + if (want_start) { start(); + } return OK; } @@ -642,25 +654,29 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) } case SENSORIOCGPOLLRATE: - if (_call_interval == 0) + if (_call_interval == 0) { return SENSOR_POLLRATE_MANUAL; + } return 1000000 / _call_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 (!_reports->resize(arg)) { + irqrestore(flags); + return -ENOMEM; + } - irqstate_t flags = irqsave(); - if (!_reports->resize(arg)) { irqrestore(flags); - return -ENOMEM; - } - irqrestore(flags); - return OK; - } + return OK; + } case SENSORIOCGQUEUEDEPTH: return _reports->size(); @@ -676,12 +692,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return _current_rate; case GYROIOCSLOWPASS: { - float cutoff_freq_hz = arg; - float sample_rate = 1.0e6f / _call_interval; - set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); + float cutoff_freq_hz = arg; + float sample_rate = 1.0e6f / _call_interval; + set_driver_lowpass_filter(sample_rate, cutoff_freq_hz); - return OK; - } + return OK; + } case GYROIOCGLOWPASS: return static_cast(_gyro_filter_x.get_cutoff_freq()); @@ -741,7 +757,8 @@ void L3GD20::write_checked_reg(unsigned reg, uint8_t value) { write_reg(reg, value); - for (uint8_t i=0; iprint_info("report queue"); - ::printf("checked_next: %u\n", _checked_next); - for (uint8_t i=0; i zero or larger than 25 dps */ - if (fabsf(_gyro_scale.x_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.x_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.x_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.x_offset) < 0.000001f) { return 1; + } - if (fabsf(_gyro_scale.y_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.y_offset) < 0.000001f) - return 1; - if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) + if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f) { return 1; + } - if (fabsf(_gyro_scale.z_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.z_offset) < 0.000001f) + if (fabsf(_gyro_scale.y_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.y_offset) < 0.000001f) { return 1; - if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) + } + + if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f) { return 1; + } + + if (fabsf(_gyro_scale.z_offset) > L3GD20_MAX_OFFSET || fabsf(_gyro_scale.z_offset) < 0.000001f) { + return 1; + } + + if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f) { + return 1; + } return 0; } @@ -1165,36 +1204,42 @@ start(bool external_bus, enum Rotation rotation) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(0, "already started"); + } /* create the driver */ - if (external_bus) { + if (external_bus) { #ifdef PX4_SPI_BUS_EXT g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation); #else errx(0, "External SPI not available"); #endif + } else { g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation); } - if (g_dev == nullptr) + if (g_dev == nullptr) { 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(L3GD20_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } - close(fd); + close(fd); exit(0); fail: @@ -1222,18 +1267,21 @@ test() /* get the driver */ fd_gyro = open(L3GD20_DEVICE_PATH, O_RDONLY); - if (fd_gyro < 0) + if (fd_gyro < 0) { err(1, "%s open failed", L3GD20_DEVICE_PATH); + } /* reset to manual polling */ - if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) + if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MANUAL) < 0) { err(1, "reset to manual polling"); + } /* do a simple demand read */ sz = read(fd_gyro, &g_report, sizeof(g_report)); - if (sz != sizeof(g_report)) + if (sz != sizeof(g_report)) { err(1, "immediate gyro read failed"); + } warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x); warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y); @@ -1246,10 +1294,11 @@ test() warnx("gyro range: %8.4f rad/s (%d deg/s)", (double)g_report.range_rad_s, (int)((g_report.range_rad_s / M_PI_F) * 180.0f + 0.5f)); - if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd_gyro, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "reset to default polling"); + } - close(fd_gyro); + close(fd_gyro); /* XXX add poll-rate tests here too */ errx(0, "PASS"); @@ -1263,16 +1312,19 @@ reset() { int fd = open(L3GD20_DEVICE_PATH, 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); exit(0); } @@ -1283,8 +1335,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(); @@ -1298,8 +1351,9 @@ info() void regdump(void) { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", g_dev); g_dev->print_registers(); @@ -1313,8 +1367,9 @@ regdump(void) void test_error(void) { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("regdump @ %p\n", g_dev); g_dev->test_error(); @@ -1346,9 +1401,11 @@ l3gd20_main(int argc, char *argv[]) case 'X': external_bus = true; break; + case 'R': rotation = (enum Rotation)atoi(optarg); break; + default: l3gd20::usage(); exit(0); @@ -1361,38 +1418,44 @@ l3gd20_main(int argc, char *argv[]) * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { l3gd20::start(external_bus, rotation); + } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { l3gd20::test(); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { l3gd20::reset(); + } /* * Print driver information. */ - if (!strcmp(verb, "info")) + if (!strcmp(verb, "info")) { l3gd20::info(); + } /* * Print register information. */ - if (!strcmp(verb, "regdump")) + if (!strcmp(verb, "regdump")) { l3gd20::regdump(); + } /* * trigger an error */ - if (!strcmp(verb, "testerror")) + if (!strcmp(verb, "testerror")) { l3gd20::test_error(); + } errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); }