diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index bd72971b94..128e8fc0d2 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -48,14 +48,10 @@ #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" #define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected -#define range_finder_report range_finder_s -#define __orb_sensor_range_finder __orb_range_finder - -#include - -#ifndef RANGE_FINDER_TYPE_LASER -#define RANGE_FINDER_TYPE_LASER 0 -#endif +/* + * ObjDev tag for px4flow data. + */ +ORB_DECLARE(distance_sensor); /* * ioctl() definitions diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index b3368406a1..6a38baf875 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -68,6 +68,7 @@ #include #include +#include #include @@ -144,7 +145,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -224,7 +225,7 @@ LL40LS::LL40LS(int bus, const char *path, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ll40ls_read")), _comms_errors(perf_alloc(PC_COUNT, "ll40ls_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "ll40ls_buffer_overflows")), @@ -278,7 +279,7 @@ LL40LS::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); if (_reports == nullptr) { goto out; @@ -288,12 +289,12 @@ LL40LS::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report; + struct distance_sensor_s rf_report; measure(); _reports->get(&rf_report); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { debug("failed to create sensor_range_finder object. Did you start uOrb?"); } } @@ -497,8 +498,8 @@ LL40LS::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t LL40LS::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -664,8 +665,8 @@ LL40LS::collect() } uint16_t distance = (val[0] << 8) | val[1]; - float si_units = distance * 0.01f; /* cm to m */ - struct range_finder_report report; + float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ + struct distance_sensor_s report; if (distance == 0) { _zero_counter++; @@ -688,22 +689,18 @@ LL40LS::collect() _last_distance = distance; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.distance = si_units; - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - if (si_units > get_minimum_distance() && si_units < get_maximum_distance()) { - report.valid = 1; - } - else { - report.valid = 0; - } + report.time_boot_ms = hrt_absolute_time(); + report.id = 0; + report.type = 1; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -958,7 +955,7 @@ void stop(int bus) void test(int bus) { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; const char *path = (bus==PX4_I2C_BUS_ONBOARD?LL40LS_DEVICE_PATH_INT:LL40LS_DEVICE_PATH_EXT); @@ -977,8 +974,8 @@ test(int bus) } warnx("single read"); - warnx("measurement: %0.2f m", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.2f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -1006,8 +1003,8 @@ test(int bus) } warnx("periodic read %u", i); - warnx("measurement: %0.3f", (double)report.distance); - warnx("time: %lld", report.timestamp); + warnx("measurement: %0.3f m", (double)report.current_distance); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to default rate */ diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index 1a4ea17cf1..edbb4eefe2 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -70,6 +70,7 @@ #include #include +#include #include @@ -131,7 +132,7 @@ private: bool _collect_phase; int _class_instance; - orb_advert_t _range_finder_topic; + orb_advert_t _distance_sensor_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -208,7 +209,7 @@ MB12XX::MB12XX(int bus, int address) : _measure_ticks(0), _collect_phase(false), _class_instance(-1), - _range_finder_topic(-1), + _distance_sensor_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")), @@ -255,7 +256,7 @@ MB12XX::init() } /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + _reports = new RingBuffer(2, sizeof(distance_sensor_s)); _index_counter = MB12XX_BASEADDR; /* set temp sonar i2c address to base adress */ set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ @@ -268,11 +269,11 @@ MB12XX::init() if (_class_instance == CLASS_DEVICE_PRIMARY) { /* get a publish handle on the range finder topic */ - struct range_finder_report rf_report = {}; + struct distance_sensor_s rf_report = {}; - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &rf_report); + _distance_sensor_topic = orb_advertise(ORB_ID(distance_sensor), &rf_report); - if (_range_finder_topic < 0) { + if (_distance_sensor_topic < 0) { log("failed to create sensor_range_finder object. Did you start uOrb?"); } } @@ -469,8 +470,8 @@ ssize_t MB12XX::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct range_finder_report); - struct range_finder_report *rbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(struct distance_sensor_s); + struct distance_sensor_s *rbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -571,51 +572,20 @@ MB12XX::collect() uint16_t distance = val[0] << 8 | val[1]; float si_units = (distance * 1.0f) / 100.0f; /* cm to m */ - struct range_finder_report report; + struct distance_sensor_s report; - /* this should be fairly close to the end of the measurement, so the best approximation of the time */ - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - - /* if only one sonar, write it to the original distance parameter so that it's still used as altitude sonar */ - if (addr_ind.size() == 1) { - report.distance = si_units; - - for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS); i++) { - report.distance_vector[i] = 0; - } - - report.just_updated = 0; - - } else { - /* for multiple sonars connected */ - - /* don't use the orginial single sonar variable */ - report.distance = 0; - - /* intermediate vector _latest_sonar_measurements is used to store the measurements as every cycle the other sonar values of the report are thrown away and/or filled in with garbage. We don't want this. We want the report to give the latest value for each connected sonar */ - _latest_sonar_measurements[_cycle_counter] = si_units; - - for (unsigned i = 0; i < (_latest_sonar_measurements.size()); i++) { - report.distance_vector[i] = _latest_sonar_measurements[i]; - } - - /* a just_updated variable is added to indicate to autopilot (ardupilot or whatever) which sonar has most recently been collected as this could be of use for Kalman filters */ - report.just_updated = _index_counter; - - /* Make sure all elements of the distance vector for which no sonar is connected are zero to prevent strange numbers */ - for (unsigned i = 0; i < (MB12XX_MAX_RANGEFINDERS - addr_ind.size()); i++) { - report.distance_vector[addr_ind.size() + i] = 0; - } - } - - report.minimum_distance = get_minimum_distance(); - report.maximum_distance = get_maximum_distance(); - report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + report.time_boot_ms = hrt_absolute_time(); + report.id = 0; + report.type = 1; + report.orientation = 8; + report.current_distance = si_units; + report.min_distance = get_minimum_distance(); + report.max_distance = get_maximum_distance(); + report.covariance = 0.0; /* publish it, if we are the primary */ - if (_range_finder_topic >= 0) { - orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report); + if (_distance_sensor_topic >= 0) { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report); } if (_reports->force(&report)) { @@ -840,7 +810,7 @@ void stop() void test() { - struct range_finder_report report; + struct distance_sensor_s report; ssize_t sz; int ret; @@ -858,8 +828,7 @@ test() } warnx("single read"); - warnx("measurement: %0.2f of sonar %d", (double)report.distance_vector[report.just_updated], report.just_updated); - warnx("time: %lld", report.timestamp); + warnx("time: %d", report.time_boot_ms); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -887,13 +856,7 @@ test() } warnx("periodic read %u", i); - - /* Print the sonar rangefinder report sonar distance vector */ - for (uint8_t count = 0; count < MB12XX_MAX_RANGEFINDERS; count++) { - warnx("measurement: %0.3f of sonar %u", (double)report.distance_vector[count], count + 1); - } - - warnx("time: %lld", report.timestamp); + warnx("time: %d", report.time_boot_ms); } /* reset the sensor polling to default rate */ diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 4d5e56a961..ee6fcb3ac5 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -58,13 +58,13 @@ #include #include #include +#include #include #include #include #include #include -#include #include #include @@ -162,7 +162,7 @@ private: struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ struct wind_estimate_s _wind; /**< wind estimate */ - struct range_finder_report _distance; /**< distance estimate */ + struct distance_sensor_s _distance; /**< distance estimate */ struct vehicle_land_detected_s _landDetector; struct actuator_armed_s _armed; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 1b6e17ac37..3c3e2d2f35 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -501,7 +501,7 @@ void AttitudePositionEstimatorEKF::task_main() /* * do subscriptions */ - _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); + _distance_sub = orb_subscribe(ORB_ID(distance_sensor)); _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); @@ -1471,11 +1471,11 @@ void AttitudePositionEstimatorEKF::pollData() orb_check(_distance_sub, &_newRangeData); if (_newRangeData) { - orb_copy(ORB_ID(sensor_range_finder), _distance_sub, &_distance); - - if (_distance.valid) { - _ekf->rngMea = _distance.distance; - _distance_last_valid = _distance.timestamp; + orb_copy(ORB_ID(distance_sensor), _distance_sub, &_distance); + if ((_distance.current_distance > _distance.min_distance) + && (_distance.current_distance < _distance.max_distance)) { + _ekf->rngMea = _distance.current_distance; + _distance_last_valid = _distance.time_boot_ms; } else { _newRangeData = false; diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 58f5dc2362..650e9bf81a 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -71,7 +71,6 @@ #include #include #include -#include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ac7ab99c05..8588e4047f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -56,7 +56,6 @@ #include #include #include -#include #include #include #include @@ -109,7 +108,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _battery_pub(-1), _cmd_pub(-1), _flow_pub(-1), - _range_pub(-1), + _distance_sensor_pub(-1), _offboard_control_mode_pub(-1), _actuator_controls_pub(-1), _global_vel_sp_pub(-1), @@ -134,8 +133,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _att_sp{}, _rates_sp{}, _time_offset_avg_alpha(0.6), - _time_offset(0), - _distance_sensor_pub(-1) + _time_offset(0) { } @@ -417,6 +415,25 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) } else { orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } + + /* Use distance value for distance sensor topic */ + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); + + d.time_boot_ms = hrt_absolute_time(); + d.min_distance = 3.0f; + d.max_distance = 50.0f; + d.current_distance = flow.distance; + d.type = 1; + d.id = 0; + d.orientation = 8; + d.covariance = 0.0; + + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); + } else { + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); + } } void @@ -449,22 +466,23 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) orb_publish(ORB_ID(optical_flow), _flow_pub, &f); } - /* Use distance value for range finder report */ - struct range_finder_report r; - memset(&r, 0, sizeof(r)); + /* Use distance value for distance sensor topic */ + struct distance_sensor_s d; + memset(&d, 0, sizeof(d)); - r.timestamp = hrt_absolute_time(); - r.error_count = 0; - r.type = RANGE_FINDER_TYPE_LASER; - r.distance = flow.distance; - r.minimum_distance = 0.0f; - r.maximum_distance = 40.0f; // this is set to match the typical range of real sensors, could be made configurable - r.valid = (r.distance > r.minimum_distance) && (r.distance < r.maximum_distance); + d.time_boot_ms = hrt_absolute_time(); + d.min_distance = 3.0f; + d.max_distance = 50.0f; + d.current_distance = flow.distance; + d.type = 1; + d.id = 0; + d.orientation = 8; + d.covariance = 0.0; - if (_range_pub < 0) { - _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); + if (_distance_sensor_pub < 0) { + _distance_sensor_pub = orb_advertise(ORB_ID(distance_sensor), &d); } else { - orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); + orb_publish(ORB_ID(distance_sensor), _distance_sensor_pub, &d); } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4be73bc990..016305e796 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -166,7 +166,7 @@ private: orb_advert_t _battery_pub; orb_advert_t _cmd_pub; orb_advert_t _flow_pub; - orb_advert_t _range_pub; + orb_advert_t _distance_sensor_pub; orb_advert_t _offboard_control_mode_pub; orb_advert_t _actuator_controls_pub; orb_advert_t _global_vel_sp_pub; @@ -192,7 +192,6 @@ private: struct vehicle_rates_setpoint_s _rates_sp; double _time_offset_avg_alpha; uint64_t _time_offset; - orb_advert_t _distance_sensor_pub; /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver &); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b34f9a742d..f900543bc2 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -57,9 +57,6 @@ ORB_DEFINE(sensor_gyro, struct gyro_report); #include ORB_DEFINE(sensor_baro, struct baro_report); -#include -ORB_DEFINE(sensor_range_finder, struct range_finder_report); - #include ORB_DEFINE(output_pwm, struct pwm_output_values);