forked from Archive/PX4-Autopilot
Fix code style of mb12xx driver, no functional changes
This commit is contained in:
parent
501dc0cfa7
commit
5c86eb6dd0
|
@ -212,8 +212,9 @@ MB12XX::~MB12XX()
|
|||
stop();
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
if (_reports != nullptr) {
|
||||
delete _reports;
|
||||
}
|
||||
}
|
||||
|
||||
int
|
||||
|
@ -222,22 +223,25 @@ MB12XX::init()
|
|||
int ret = ERROR;
|
||||
|
||||
/* do I2C init (and probe) first */
|
||||
if (I2C::init() != OK)
|
||||
if (I2C::init() != OK) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer(2, sizeof(range_finder_report));
|
||||
|
||||
if (_reports == nullptr)
|
||||
if (_reports == nullptr) {
|
||||
goto out;
|
||||
}
|
||||
|
||||
/* get a publish handle on the range finder topic */
|
||||
struct range_finder_report zero_report;
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report);
|
||||
|
||||
if (_range_finder_topic < 0)
|
||||
if (_range_finder_topic < 0) {
|
||||
debug("failed to create sensor_range_finder object. Did you start uOrb?");
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but we don't really know if it is within range */
|
||||
|
@ -307,8 +311,9 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
if (want_start) {
|
||||
start();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
@ -322,15 +327,17 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
unsigned ticks = USEC2TICK(1000000 / arg);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
|
||||
if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
/* update interval for next measurement */
|
||||
_measure_ticks = ticks;
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
if (want_start) {
|
||||
start();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
@ -338,21 +345,25 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
}
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_measure_ticks == 0)
|
||||
if (_measure_ticks == 0) {
|
||||
return SENSOR_POLLRATE_MANUAL;
|
||||
}
|
||||
|
||||
return (1000 / _measure_ticks);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 1) || (arg > 100))
|
||||
if ((arg < 1) || (arg > 100)) {
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
irqstate_t flags = irqsave();
|
||||
|
||||
if (!_reports->resize(arg)) {
|
||||
irqrestore(flags);
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
irqrestore(flags);
|
||||
|
||||
return OK;
|
||||
|
@ -365,18 +376,18 @@ MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
/* XXX implement this */
|
||||
return -EINVAL;
|
||||
|
||||
case RANGEFINDERIOCSETMINIUMDISTANCE:
|
||||
{
|
||||
case RANGEFINDERIOCSETMINIUMDISTANCE: {
|
||||
set_minimum_distance(*(float *)arg);
|
||||
return 0;
|
||||
}
|
||||
break;
|
||||
case RANGEFINDERIOCSETMAXIUMDISTANCE:
|
||||
{
|
||||
|
||||
case RANGEFINDERIOCSETMAXIUMDISTANCE: {
|
||||
set_maximum_distance(*(float *)arg);
|
||||
return 0;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return I2C::ioctl(filp, cmd, arg);
|
||||
|
@ -391,8 +402,9 @@ MB12XX::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 (_measure_ticks > 0) {
|
||||
|
@ -453,12 +465,12 @@ MB12XX::measure()
|
|||
uint8_t cmd = MB12XX_TAKE_RANGE_REG;
|
||||
ret = transfer(&cmd, 1, nullptr, 0);
|
||||
|
||||
if (OK != ret)
|
||||
{
|
||||
if (OK != ret) {
|
||||
perf_count(_comms_errors);
|
||||
log("i2c::transfer returned %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
ret = OK;
|
||||
|
||||
return ret;
|
||||
|
@ -476,8 +488,7 @@ MB12XX::collect()
|
|||
|
||||
ret = transfer(nullptr, 0, &val[0], 2);
|
||||
|
||||
if (ret < 0)
|
||||
{
|
||||
if (ret < 0) {
|
||||
log("error reading from sensor: %d", ret);
|
||||
perf_count(_comms_errors);
|
||||
perf_end(_sample_perf);
|
||||
|
@ -485,7 +496,7 @@ MB12XX::collect()
|
|||
}
|
||||
|
||||
uint16_t distance = val[0] << 8 | val[1];
|
||||
float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
|
||||
float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
|
||||
struct range_finder_report report;
|
||||
|
||||
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
|
||||
|
@ -525,11 +536,13 @@ MB12XX::start()
|
|||
true,
|
||||
true,
|
||||
true,
|
||||
SUBSYSTEM_TYPE_RANGEFINDER};
|
||||
SUBSYSTEM_TYPE_RANGEFINDER
|
||||
};
|
||||
static orb_advert_t pub = -1;
|
||||
|
||||
if (pub > 0) {
|
||||
orb_publish(ORB_ID(subsystem_info), pub, &info);
|
||||
|
||||
} else {
|
||||
pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
||||
}
|
||||
|
@ -583,8 +596,9 @@ MB12XX::cycle()
|
|||
}
|
||||
|
||||
/* measurement phase */
|
||||
if (OK != measure())
|
||||
if (OK != measure()) {
|
||||
log("measure error");
|
||||
}
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
@ -635,33 +649,37 @@ start()
|
|||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
if (g_dev != nullptr) {
|
||||
errx(1, "already started");
|
||||
}
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new MB12XX(MB12XX_BUS);
|
||||
|
||||
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(RANGE_FINDER_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;
|
||||
}
|
||||
|
||||
exit(0);
|
||||
|
||||
fail:
|
||||
|
||||
if (g_dev != nullptr)
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
@ -674,15 +692,14 @@ fail:
|
|||
*/
|
||||
void stop()
|
||||
{
|
||||
if (g_dev != nullptr)
|
||||
{
|
||||
if (g_dev != nullptr) {
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
} else {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
@ -700,22 +717,25 @@ test()
|
|||
|
||||
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
if (fd < 0) {
|
||||
err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
|
||||
}
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
if (sz != sizeof(report)) {
|
||||
err(1, "immediate read failed");
|
||||
}
|
||||
|
||||
warnx("single read");
|
||||
warnx("measurement: %0.2f m", (double)report.distance);
|
||||
warnx("time: %lld", report.timestamp);
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
|
||||
errx(1, "failed to set 2Hz poll rate");
|
||||
}
|
||||
|
||||
/* read the sensor 5x and report each value */
|
||||
for (unsigned i = 0; i < 5; i++) {
|
||||
|
@ -726,14 +746,16 @@ test()
|
|||
fds.events = POLLIN;
|
||||
ret = poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1)
|
||||
if (ret != 1) {
|
||||
errx(1, "timed out waiting for sensor data");
|
||||
}
|
||||
|
||||
/* now go get it */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
if (sz != sizeof(report)) {
|
||||
err(1, "periodic read failed");
|
||||
}
|
||||
|
||||
warnx("periodic read %u", i);
|
||||
warnx("measurement: %0.3f", (double)report.distance);
|
||||
|
@ -751,14 +773,17 @@ reset()
|
|||
{
|
||||
int fd = open(RANGE_FINDER_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, "driver poll restart failed");
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
@ -769,8 +794,9 @@ reset()
|
|||
void
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr)
|
||||
if (g_dev == nullptr) {
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
@ -786,32 +812,37 @@ mb12xx_main(int argc, char *argv[])
|
|||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "start"))
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
mb12xx::start();
|
||||
}
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
mb12xx::stop();
|
||||
}
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[1], "test"))
|
||||
if (!strcmp(argv[1], "test")) {
|
||||
mb12xx::test();
|
||||
}
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "reset"))
|
||||
if (!strcmp(argv[1], "reset")) {
|
||||
mb12xx::reset();
|
||||
}
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
|
||||
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
|
||||
mb12xx::info();
|
||||
}
|
||||
|
||||
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue