Commit dcf7b81f authored by Lorenz Meier's avatar Lorenz Meier
Browse files

Airspeed: fix code style

parent 5c5ba1a8
/**************************************************************************** /****************************************************************************
* *
* Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
* *
* Redistribution and use in source and binary forms, with or without * Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions * modification, are permitted provided that the following conditions
...@@ -32,8 +32,9 @@ ...@@ -32,8 +32,9 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file ets_airspeed.cpp * @file airspeed.cpp
* @author Simon Wilks * @author Simon Wilks <simon@px4.io>
* @author Lorenz Meier <lorenz@px4.io>
* *
* Driver for the Eagle Tree Airspeed V3 connected via I2C. * Driver for the Eagle Tree Airspeed V3 connected via I2C.
*/ */
...@@ -76,7 +77,7 @@ ...@@ -76,7 +77,7 @@
#include <drivers/airspeed/airspeed.h> #include <drivers/airspeed/airspeed.h>
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char* path) : Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
I2C("Airspeed", path, bus, address, 100000), I2C("Airspeed", path, bus, address, 100000),
_reports(nullptr), _reports(nullptr),
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
...@@ -105,12 +106,14 @@ Airspeed::~Airspeed() ...@@ -105,12 +106,14 @@ Airspeed::~Airspeed()
/* make sure we are truly inactive */ /* make sure we are truly inactive */
stop(); stop();
if (_class_instance != -1) if (_class_instance != -1) {
unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
}
/* free any existing reports */ /* free any existing reports */
if (_reports != nullptr) if (_reports != nullptr) {
delete _reports; delete _reports;
}
// free perf counters // free perf counters
perf_free(_sample_perf); perf_free(_sample_perf);
...@@ -124,13 +127,16 @@ Airspeed::init() ...@@ -124,13 +127,16 @@ Airspeed::init()
int ret = ERROR; int ret = ERROR;
/* do I2C init (and probe) first */ /* do I2C init (and probe) first */
if (I2C::init() != OK) if (I2C::init() != OK) {
goto out; goto out;
}
/* allocate basic report buffers */ /* allocate basic report buffers */
_reports = new RingBuffer(2, sizeof(differential_pressure_s)); _reports = new RingBuffer(2, sizeof(differential_pressure_s));
if (_reports == nullptr)
if (_reports == nullptr) {
goto out; goto out;
}
/* register alternate interfaces if we have to */ /* register alternate interfaces if we have to */
_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
...@@ -146,8 +152,9 @@ Airspeed::init() ...@@ -146,8 +152,9 @@ Airspeed::init()
/* measurement will have generated a report, publish */ /* measurement will have generated a report, publish */
_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp); _airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
if (_airspeed_pub < 0) if (_airspeed_pub < 0) {
warnx("uORB started?"); warnx("uORB started?");
}
} }
ret = OK; ret = OK;
...@@ -166,7 +173,7 @@ Airspeed::probe() ...@@ -166,7 +173,7 @@ Airspeed::probe()
_retries = 4; _retries = 4;
int ret = measure(); int ret = measure();
// drop back to 2 retries once initialised // drop back to 2 retries once initialised
_retries = 2; _retries = 2;
return ret; return ret;
} }
...@@ -179,20 +186,20 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) ...@@ -179,20 +186,20 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCSPOLLRATE: { case SENSORIOCSPOLLRATE: {
switch (arg) { switch (arg) {
/* switching to manual polling */ /* switching to manual polling */
case SENSOR_POLLRATE_MANUAL: case SENSOR_POLLRATE_MANUAL:
stop(); stop();
_measure_ticks = 0; _measure_ticks = 0;
return OK; return OK;
/* external signalling (DRDY) not supported */ /* external signalling (DRDY) not supported */
case SENSOR_POLLRATE_EXTERNAL: case SENSOR_POLLRATE_EXTERNAL:
/* zero would be bad */ /* zero would be bad */
case 0: case 0:
return -EINVAL; return -EINVAL;
/* set default/max polling rate */ /* set default/max polling rate */
case SENSOR_POLLRATE_MAX: case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: { case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */ /* do we need to start internal polling? */
...@@ -202,13 +209,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) ...@@ -202,13 +209,14 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
_measure_ticks = USEC2TICK(_conversion_interval); _measure_ticks = USEC2TICK(_conversion_interval);
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) if (want_start) {
start(); start();
}
return OK; return OK;
} }
/* adjust to a legal polling interval in Hz */ /* adjust to a legal polling interval in Hz */
default: { default: {
/* do we need to start internal polling? */ /* do we need to start internal polling? */
bool want_start = (_measure_ticks == 0); bool want_start = (_measure_ticks == 0);
...@@ -217,15 +225,17 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) ...@@ -217,15 +225,17 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
unsigned ticks = USEC2TICK(1000000 / arg); unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */ /* check against maximum rate */
if (ticks < USEC2TICK(_conversion_interval)) if (ticks < USEC2TICK(_conversion_interval)) {
return -EINVAL; return -EINVAL;
}
/* update interval for next measurement */ /* update interval for next measurement */
_measure_ticks = ticks; _measure_ticks = ticks;
/* if we need to start the poll state machine, do it */ /* if we need to start the poll state machine, do it */
if (want_start) if (want_start) {
start(); start();
}
return OK; return OK;
} }
...@@ -233,21 +243,25 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) ...@@ -233,21 +243,25 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
} }
case SENSORIOCGPOLLRATE: case SENSORIOCGPOLLRATE:
if (_measure_ticks == 0) if (_measure_ticks == 0) {
return SENSOR_POLLRATE_MANUAL; return SENSOR_POLLRATE_MANUAL;
}
return (1000 / _measure_ticks); return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
if ((arg < 1) || (arg > 100)) if ((arg < 1) || (arg > 100)) {
return -EINVAL; return -EINVAL;
}
irqstate_t flags = irqsave(); irqstate_t flags = irqsave();
if (!_reports->resize(arg)) { if (!_reports->resize(arg)) {
irqrestore(flags); irqrestore(flags);
return -ENOMEM; return -ENOMEM;
} }
irqrestore(flags); irqrestore(flags);
return OK; return OK;
...@@ -261,16 +275,16 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg) ...@@ -261,16 +275,16 @@ Airspeed::ioctl(struct file *filp, int cmd, unsigned long arg)
return -EINVAL; return -EINVAL;
case AIRSPEEDIOCSSCALE: { case AIRSPEEDIOCSSCALE: {
struct airspeed_scale *s = (struct airspeed_scale*)arg; struct airspeed_scale *s = (struct airspeed_scale *)arg;
_diff_pres_offset = s->offset_pa; _diff_pres_offset = s->offset_pa;
return OK; return OK;
} }
case AIRSPEEDIOCGSCALE: { case AIRSPEEDIOCGSCALE: {
struct airspeed_scale *s = (struct airspeed_scale*)arg; struct airspeed_scale *s = (struct airspeed_scale *)arg;
s->offset_pa = _diff_pres_offset; s->offset_pa = _diff_pres_offset;
s->scale = 1.0f; s->scale = 1.0f;
return OK; return OK;
} }
default: default:
...@@ -287,8 +301,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen) ...@@ -287,8 +301,9 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
int ret = 0; int ret = 0;
/* buffer must be large enough */ /* buffer must be large enough */
if (count < 1) if (count < 1) {
return -ENOSPC; return -ENOSPC;
}
/* if automatic measurement is enabled */ /* if automatic measurement is enabled */
if (_measure_ticks > 0) { if (_measure_ticks > 0) {
...@@ -369,6 +384,7 @@ Airspeed::update_status() ...@@ -369,6 +384,7 @@ Airspeed::update_status()
if (_subsys_pub > 0) { if (_subsys_pub > 0) {
orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info);
} else { } else {
_subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info);
} }
...@@ -402,6 +418,7 @@ Airspeed::print_info() ...@@ -402,6 +418,7 @@ Airspeed::print_info()
void void
Airspeed::new_report(const differential_pressure_s &report) Airspeed::new_report(const differential_pressure_s &report)
{ {
if (!_reports->force(&report)) if (!_reports->force(&report)) {
perf_count(_buffer_overflows); perf_count(_buffer_overflows);
}
} }
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment