mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-24 10:03:35 +08:00
ll40ls: astyle
This commit is contained in:
@@ -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(®, 1, &val, 1);
|
int ret = transfer(®, 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
|
||||||
|
|||||||
@@ -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();
|
||||||
|
|||||||
@@ -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);
|
||||||
|
|||||||
Reference in New Issue
Block a user