diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 9a05890cd0..150465ec5b 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -132,7 +132,7 @@ int LidarLiteI2C::init() measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); @@ -165,12 +165,16 @@ int LidarLiteI2C::lidar_transfer(const uint8_t *send, unsigned send_len, uint8_t { if (send != NULL && send_len > 0) { int ret = transfer(send, send_len, NULL, 0); - if (ret != OK) + + if (ret != OK) { return ret; + } } + if (recv != NULL && recv_len > 0) { return transfer(NULL, 0, recv, recv_len); } + return OK; } @@ -345,8 +349,10 @@ int LidarLiteI2C::measure() int LidarLiteI2C::reset_sensor() { int ret = write_reg(LL40LS_MEASURE_REG, LL40LS_MSRREG_RESET); - if (ret != OK) + + if (ret != OK) { return ret; + } // wait for sensor reset to complete usleep(1000); @@ -447,7 +453,7 @@ int LidarLiteI2C::collect() } _last_distance = distance_cm; - + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); diff --git a/src/drivers/ll40ls/LidarLiteI2C.h b/src/drivers/ll40ls/LidarLiteI2C.h index 51cca0862f..1c6de3c9b1 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.h +++ b/src/drivers/ll40ls/LidarLiteI2C.h @@ -51,7 +51,7 @@ #include #include - + /* Configuration Constants */ #define LL40LS_BUS PX4_I2C_BUS_EXPANSION @@ -117,8 +117,8 @@ private: uint16_t _zero_counter; uint64_t _acquire_time_usec; volatile bool _pause_measurements; - uint8_t _hw_version; - uint8_t _sw_version; + uint8_t _hw_version; + uint8_t _sw_version; /**< the bus the device is connected to */ int _bus; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 3d17d5b36b..e2a993ad14 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -114,7 +114,7 @@ int LidarLitePWM::init() measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + &_orb_class_instance, ORB_PRIO_LOW); if (_distance_sensor_topic == nullptr) { DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index a4c8720dbc..8591c4dee3 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -80,7 +80,7 @@ LidarLiteI2C *g_dev_int; LidarLiteI2C *g_dev_ext; LidarLitePWM *g_dev_pwm; -LidarLite * get_dev(const bool use_i2c, const int bus); +LidarLite *get_dev(const bool use_i2c, const int bus); void start(const bool use_i2c, const int bus); void stop(const bool use_i2c, const int bus); void test(const bool use_i2c, const int bus); @@ -92,19 +92,25 @@ void usage(); /** * Get the correct device pointer */ -LidarLite * get_dev(const bool use_i2c, const int bus) { - LidarLite * g_dev = nullptr; +LidarLite *get_dev(const bool use_i2c, const int bus) +{ + LidarLite *g_dev = nullptr; + if (use_i2c) { - g_dev = static_cast(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + g_dev = static_cast(bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + if (g_dev == nullptr) { errx(1, "i2c driver not running"); } + } else { - g_dev = static_cast(g_dev_pwm); + g_dev = static_cast(g_dev_pwm); + if (g_dev == nullptr) { errx(1, "pwm driver not running"); } } + return g_dev; }; @@ -114,7 +120,7 @@ LidarLite * get_dev(const bool use_i2c, const int bus) { void start(const bool use_i2c, const int bus) { if (g_dev_int != nullptr || g_dev_ext != nullptr || g_dev_pwm != nullptr) { - errx(1,"driver already started"); + errx(1, "driver already started"); } if (use_i2c) { @@ -254,18 +260,21 @@ void stop(const bool use_i2c, const int bus) delete g_dev_int; g_dev_int = nullptr; } + } else { if (g_dev_ext != nullptr) { delete g_dev_ext; g_dev_ext = nullptr; } } + } else { if (g_dev_pwm != nullptr) { delete g_dev_pwm; g_dev_pwm = nullptr; } } + exit(0); } @@ -334,7 +343,7 @@ test(const bool use_i2c, const int bus) warnx("periodic read %u", i); warnx("valid %u", (float)report.current_distance > report.min_distance - && (float)report.current_distance < report.max_distance ? 1 : 0); + && (float)report.current_distance < report.max_distance ? 1 : 0); warnx("measurement: %0.3f m", (double)report.current_distance); warnx("time: %lld", report.timestamp); } @@ -386,7 +395,7 @@ reset(const bool use_i2c, const int bus) void info(const bool use_i2c, const int bus) { - LidarLite * g_dev = get_dev(use_i2c, bus); + LidarLite *g_dev = get_dev(use_i2c, bus); printf("state @ %p\n", g_dev); g_dev->print_info(); exit(0); @@ -398,7 +407,7 @@ info(const bool use_i2c, const int bus) void regdump(const bool use_i2c, const int bus) { - LidarLite * g_dev = get_dev(use_i2c, bus); + LidarLite *g_dev = get_dev(use_i2c, bus); printf("regdump @ %p\n", g_dev); g_dev->print_registers(); exit(0);