ll40ls: astyle

This commit is contained in:
Ban Siesta
2015-05-24 09:44:10 +01:00
parent 3efaeabd5b
commit e67f681935
7 changed files with 667 additions and 628 deletions
+18 -4
View File
@@ -216,8 +216,7 @@ int LidarLiteI2C::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _reports->size(); return _reports->size();
default: default: {
{
int result = LidarLite::ioctl(filp, cmd, arg); int result = LidarLite::ioctl(filp, cmd, arg);
if (result == -EINVAL) { if (result == -EINVAL) {
@@ -308,12 +307,14 @@ int LidarLiteI2C::measure()
if (OK != ret) { if (OK != ret) {
perf_count(_comms_errors); perf_count(_comms_errors);
debug("i2c::transfer returned %d", ret); debug("i2c::transfer returned %d", ret);
// if we are getting lots of I2C transfer errors try // if we are getting lots of I2C transfer errors try
// resetting the sensor // resetting the sensor
if (perf_event_count(_comms_errors) % 10 == 0) { if (perf_event_count(_comms_errors) % 10 == 0) {
perf_count(_sensor_resets); perf_count(_sensor_resets);
reset_sensor(); reset_sensor();
} }
return ret; return ret;
} }
@@ -344,18 +345,23 @@ void LidarLiteI2C::print_registers()
printf("ll40ls registers\n"); printf("ll40ls registers\n");
// wait for a while to ensure the lidar is in a ready state // wait for a while to ensure the lidar is in a ready state
usleep(50000); usleep(50000);
for (uint8_t reg = 0; reg <= 0x67; reg++) { for (uint8_t reg = 0; reg <= 0x67; reg++) {
uint8_t val = 0; uint8_t val = 0;
int ret = transfer(&reg, 1, &val, 1); int ret = transfer(&reg, 1, &val, 1);
if (ret != OK) { if (ret != OK) {
printf("%02x:XX ", (unsigned)reg); printf("%02x:XX ", (unsigned)reg);
} else { } else {
printf("%02x:%02x ", (unsigned)reg, (unsigned)val); printf("%02x:%02x ", (unsigned)reg, (unsigned)val);
} }
if (reg % 16 == 15) { if (reg % 16 == 15) {
printf("\n"); printf("\n");
} }
} }
printf("\n"); printf("\n");
_pause_measurements = false; _pause_measurements = false;
} }
@@ -382,11 +388,13 @@ int LidarLiteI2C::collect()
*/ */
debug("error reading from sensor: %d", ret); debug("error reading from sensor: %d", ret);
perf_count(_comms_errors); perf_count(_comms_errors);
if (perf_event_count(_comms_errors) % 10 == 0) { if (perf_event_count(_comms_errors) % 10 == 0) {
perf_count(_sensor_resets); perf_count(_sensor_resets);
reset_sensor(); reset_sensor();
} }
} }
perf_end(_sample_perf); perf_end(_sample_perf);
// if we are getting lots of I2C transfer errors try // if we are getting lots of I2C transfer errors try
// resetting the sensor // resetting the sensor
@@ -399,6 +407,7 @@ int LidarLiteI2C::collect()
if (distance == 0) { if (distance == 0) {
_zero_counter++; _zero_counter++;
if (_zero_counter == 20) { if (_zero_counter == 20) {
/* we have had 20 zeros in a row - reset the /* we have had 20 zeros in a row - reset the
sensor. This is a known bad state of the sensor. This is a known bad state of the
@@ -412,6 +421,7 @@ int LidarLiteI2C::collect()
perf_count(_sensor_zero_resets); perf_count(_sensor_zero_resets);
return reset_sensor(); return reset_sensor();
} }
} else { } else {
_zero_counter = 0; _zero_counter = 0;
} }
@@ -424,10 +434,11 @@ int LidarLiteI2C::collect()
report.distance = si_units; report.distance = si_units;
report.minimum_distance = get_minimum_distance(); report.minimum_distance = get_minimum_distance();
report.maximum_distance = get_maximum_distance(); report.maximum_distance = get_maximum_distance();
if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) {
report.valid = 1; report.valid = 1;
}
else { } else {
report.valid = 0; report.valid = 0;
} }
@@ -495,11 +506,13 @@ void LidarLiteI2C::cycle()
/* try a collection */ /* try a collection */
if (OK != collect()) { if (OK != collect()) {
debug("collection error"); debug("collection error");
/* if we've been waiting more than 200ms then /* if we've been waiting more than 200ms then
send a new acquire */ send a new acquire */
if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) { if (hrt_absolute_time() - _acquire_time_usec > LL40LS_CONVERSION_TIMEOUT * 2) {
_collect_phase = false; _collect_phase = false;
} }
} else { } else {
/* next phase is measurement */ /* next phase is measurement */
_collect_phase = false; _collect_phase = false;
@@ -525,6 +538,7 @@ void LidarLiteI2C::cycle()
/* measurement phase */ /* measurement phase */
if (OK != measure()) { if (OK != measure()) {
debug("measure error"); debug("measure error");
} else { } else {
/* next phase is collection. Don't switch to /* next phase is collection. Don't switch to
collection phase until we have a successful collection phase until we have a successful
+1
View File
@@ -124,6 +124,7 @@ int LidarLitePWM::collect()
//Check PWM //Check PWM
bool update; bool update;
orb_check(_pwmSub, &update); orb_check(_pwmSub, &update);
if (update) { if (update) {
orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm); orb_copy(ORB_ID(pwm_input), _pwmSub, &_pwm);
_range.timestamp = hrt_absolute_time(); _range.timestamp = hrt_absolute_time();
+26 -2
View File
@@ -90,12 +90,16 @@ void start(int bus)
{ {
/* create the driver, attempt expansion bus first */ /* create the driver, attempt expansion bus first */
if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) {
if (g_dev_ext != nullptr) if (g_dev_ext != nullptr) {
errx(0, "already started external"); errx(0, "already started external");
}
g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT); g_dev_ext = new LidarLiteI2C(PX4_I2C_BUS_EXPANSION, LL40LS_DEVICE_PATH_EXT);
if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { if (g_dev_ext != nullptr && OK != g_dev_ext->init()) {
delete g_dev_ext; delete g_dev_ext;
g_dev_ext = nullptr; g_dev_ext = nullptr;
if (bus == PX4_I2C_BUS_EXPANSION) { if (bus == PX4_I2C_BUS_EXPANSION) {
goto fail; goto fail;
} }
@@ -103,11 +107,15 @@ void start(int bus)
} }
#ifdef PX4_I2C_BUS_ONBOARD #ifdef PX4_I2C_BUS_ONBOARD
/* if this failed, attempt onboard sensor */ /* if this failed, attempt onboard sensor */
if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) {
if (g_dev_int != nullptr) if (g_dev_int != nullptr) {
errx(0, "already started internal"); errx(0, "already started internal");
}
g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT); g_dev_int = new LidarLiteI2C(PX4_I2C_BUS_ONBOARD, LL40LS_DEVICE_PATH_INT);
if (g_dev_int != nullptr && OK != g_dev_int->init()) { if (g_dev_int != nullptr && OK != g_dev_int->init()) {
/* tear down the failing onboard instance */ /* tear down the failing onboard instance */
delete g_dev_int; delete g_dev_int;
@@ -117,20 +125,25 @@ void start(int bus)
goto fail; goto fail;
} }
} }
if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) {
goto fail; goto fail;
} }
} }
#endif #endif
/* set the poll rate to default, starts automatic data collection */ /* set the poll rate to default, starts automatic data collection */
if (g_dev_int != nullptr) { if (g_dev_int != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY); int fd = open(LL40LS_DEVICE_PATH_INT, O_RDONLY);
if (fd == -1) { if (fd == -1) {
goto fail; goto fail;
} }
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd); close(fd);
if (ret < 0) { if (ret < 0) {
goto fail; goto fail;
} }
@@ -138,11 +151,14 @@ void start(int bus)
if (g_dev_ext != nullptr) { if (g_dev_ext != nullptr) {
int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY); int fd = open(LL40LS_DEVICE_PATH_EXT, O_RDONLY);
if (fd == -1) { if (fd == -1) {
goto fail; goto fail;
} }
int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT); int ret = ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT);
close(fd); close(fd);
if (ret < 0) { if (ret < 0) {
goto fail; goto fail;
} }
@@ -151,10 +167,12 @@ void start(int bus)
exit(0); exit(0);
fail: fail:
if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) {
delete g_dev_int; delete g_dev_int;
g_dev_int = nullptr; g_dev_int = nullptr;
} }
if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) {
delete g_dev_ext; delete g_dev_ext;
g_dev_ext = nullptr; g_dev_ext = nullptr;
@@ -169,6 +187,7 @@ fail:
void stop(int bus) void stop(int bus)
{ {
LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext); LidarLiteI2C **g_dev = (bus == PX4_I2C_BUS_ONBOARD ? &g_dev_int : &g_dev_ext);
if (*g_dev != nullptr) { if (*g_dev != nullptr) {
delete *g_dev; delete *g_dev;
*g_dev = nullptr; *g_dev = nullptr;
@@ -279,6 +298,7 @@ void
info(int bus) info(int bus)
{ {
LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
if (g_dev == nullptr) { if (g_dev == nullptr) {
errx(1, "driver not running"); errx(1, "driver not running");
} }
@@ -296,6 +316,7 @@ void
regdump(int bus) regdump(int bus)
{ {
LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); LidarLiteI2C *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext);
if (g_dev == nullptr) { if (g_dev == nullptr) {
errx(1, "driver not running"); errx(1, "driver not running");
} }
@@ -328,13 +349,16 @@ ll40ls_main(int argc, char *argv[])
while ((ch = getopt(argc, argv, "XI")) != EOF) { while ((ch = getopt(argc, argv, "XI")) != EOF) {
switch (ch) { switch (ch) {
#ifdef PX4_I2C_BUS_ONBOARD #ifdef PX4_I2C_BUS_ONBOARD
case 'I': case 'I':
bus = PX4_I2C_BUS_ONBOARD; bus = PX4_I2C_BUS_ONBOARD;
break; break;
#endif #endif
case 'X': case 'X':
bus = PX4_I2C_BUS_EXPANSION; bus = PX4_I2C_BUS_EXPANSION;
break; break;
default: default:
ll40ls::usage(); ll40ls::usage();
exit(0); exit(0);