|
|
|
@ -62,7 +62,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
@@ -62,7 +62,7 @@ LidarLiteI2C::LidarLiteI2C(int bus, const char *path, int address) :
|
|
|
|
|
_collect_phase(false), |
|
|
|
|
_class_instance(-1), |
|
|
|
|
_orb_class_instance(-1), |
|
|
|
|
_distance_sensor_topic(-1), |
|
|
|
|
_distance_sensor_topic(nullptr), |
|
|
|
|
_sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_i2c_read")), |
|
|
|
|
_comms_errors(perf_alloc(PC_COUNT, "ll40ls_i2c_comms_errors")), |
|
|
|
|
_buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_i2c_overflows")), |
|
|
|
@ -132,7 +132,7 @@ int LidarLiteI2C::init()
@@ -132,7 +132,7 @@ int LidarLiteI2C::init()
|
|
|
|
|
_distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, |
|
|
|
|
&_orb_class_instance, ORB_PRIO_LOW); |
|
|
|
|
|
|
|
|
|
if (_distance_sensor_topic < 0) { |
|
|
|
|
if (_distance_sensor_topic == nullptr) { |
|
|
|
|
debug("failed to create distance_sensor object. Did you start uOrb?"); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
@ -442,7 +442,7 @@ int LidarLiteI2C::collect()
@@ -442,7 +442,7 @@ int LidarLiteI2C::collect()
|
|
|
|
|
report.id = 0; |
|
|
|
|
|
|
|
|
|
/* publish it, if we are the primary */ |
|
|
|
|
if (_distance_sensor_topic >= 0) { |
|
|
|
|
if (_distance_sensor_topic != nullptr) { |
|
|
|
|
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -467,22 +467,6 @@ void LidarLiteI2C::start()
@@ -467,22 +467,6 @@ void LidarLiteI2C::start()
|
|
|
|
|
|
|
|
|
|
/* schedule a cycle to start things */ |
|
|
|
|
work_queue(HPWORK, &_work, (worker_t)&LidarLiteI2C::cycle_trampoline, this, 1); |
|
|
|
|
|
|
|
|
|
/* notify about state change */ |
|
|
|
|
struct subsystem_info_s info = { |
|
|
|
|
true, |
|
|
|
|
true, |
|
|
|
|
true, |
|
|
|
|
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); |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void LidarLiteI2C::stop() |
|
|
|
|