distance sensors: Improve variable naming / description

This commit is contained in:
Philipp Oettershagen 2018-07-12 21:50:47 +02:00 committed by Paul Riseborough
parent 35bde5c9fc
commit da2fbf60b5
16 changed files with 20 additions and 20 deletions

View File

@ -5,7 +5,7 @@ float32 min_distance # Minimum distance the sensor can measure (in m)
float32 max_distance # Maximum distance the sensor can measure (in m)
float32 current_distance # Current distance reading (in m)
float32 covariance # Measurement covariance (in m), 0 for unknown / invalid readings
int8 signal_strength # Signal strength in percent (0...100%) or -1 if unknown
int8 signal_quality # Signal quality in percent (0...100%), where 0 = invalid signal, 100 = perfect signal, and -1 = unknown signal quality.
uint8 type # Type from MAV_DISTANCE_SENSOR enum
uint8 MAV_DISTANCE_SENSOR_LASER = 0

View File

@ -411,7 +411,7 @@ void leddar_one::_publish(uint16_t distance_mm)
report.min_distance = MIN_DISTANCE;
report.max_distance = MAX_DISTANCE;
report.covariance = 0.0f;
report.signal_strength = -1;
report.signal_quality = -1;
report.id = 0;
_reports->force(&report);

View File

@ -515,14 +515,14 @@ int LidarLiteI2C::collect()
/* Final data quality evaluation. This is based on the datasheet and simple heuristics retrieved from experiments*/
// Step 1: Normalize signal strength to 0...100 percent using the absolute signal peak strength.
uint8_t signal_strength = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW,
0) / (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW);
uint8_t signal_quality = 100 * math::max(ll40ls_peak_strength - LL40LS_PEAK_STRENGTH_LOW,
0) / (LL40LS_PEAK_STRENGTH_HIGH - LL40LS_PEAK_STRENGTH_LOW);
// Step 2: Also use ll40ls_signal_strength (a relative measure, i.e. peak strength to noise!) to reject potentially ambiguous measurements
if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW) { signal_strength = 0; }
if (ll40ls_signal_strength <= LL40LS_SIGNAL_STRENGTH_LOW) { signal_quality = 0; }
// Step 3: Filter physically impossible measurements, which removes some crazy outliers that appear on LL40LS.
if (distance_m < LL40LS_MIN_DISTANCE) { signal_strength = 0; }
if (distance_m < LL40LS_MIN_DISTANCE) { signal_quality = 0; }
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
report.timestamp = hrt_absolute_time();
@ -530,7 +530,7 @@ int LidarLiteI2C::collect()
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_strength = signal_strength;
report.signal_quality = signal_quality;
report.type =
distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; // the sensor is in fact a laser + sonar but there is no enum for this
report.orientation = _rotation;

View File

@ -556,7 +556,7 @@ MB12XX::collect()
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_strength = -1;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;

View File

@ -616,7 +616,7 @@ SF0X::collect()
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_strength = -1;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;

View File

@ -552,7 +552,7 @@ SF1XX::collect()
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_strength = -1;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;

View File

@ -559,7 +559,7 @@ SRF02::collect()
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_strength = -1;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;

View File

@ -633,7 +633,7 @@ TERARANGER::collect()
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_strength = -1;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;

View File

@ -580,7 +580,7 @@ TFMINI::collect()
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0f;
report.signal_strength = -1;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;

View File

@ -675,7 +675,7 @@ VL53LXX::collect()
report.min_distance = VL53LXX_MIN_RANGING_DISTANCE;
report.max_distance = VL53LXX_MAX_RANGING_DISTANCE;
report.covariance = 0.0f;
report.signal_strength = -1;
report.signal_quality = -1;
/* TODO: set proper ID */
report.id = 0;

View File

@ -595,7 +595,7 @@ PX4FLOW::collect()
distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
distance_report.current_distance = report.ground_distance_m;
distance_report.covariance = 0.0f;
distance_report.signal_strength = -1;
distance_report.signal_quality = -1;
distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
/* TODO: the ID needs to be properly set */
distance_report.id = 0;

View File

@ -942,7 +942,7 @@ void Ekf2::run()
// check if distance sensor is within working boundaries
if (range_finder.min_distance >= range_finder.current_distance ||
range_finder.max_distance <= range_finder.current_distance ||
range_finder.signal_strength == 0) {
range_finder.signal_quality == 0) {
// use rng_gnd_clearance if on ground
if (_ekf.get_in_air_status()) {
range_finder_updated = false;

View File

@ -1173,7 +1173,7 @@ int Simulator::publish_distance_topic(mavlink_distance_sensor_t *dist_mavlink)
dist.id = dist_mavlink->id;
dist.orientation = dist_mavlink->orientation;
dist.covariance = dist_mavlink->covariance / 100.0f;
dist.signal_strength = -1;
dist.signal_quality = -1;
int dist_multi;
orb_publish_auto(ORB_ID(distance_sensor), &_dist_pub, &dist, &dist_multi, ORB_PRIO_HIGH);

View File

@ -162,7 +162,7 @@ int DfBebopRangeFinderWrapper::_publish(struct bebop_range &data)
distance_data.covariance = 1.0f; // TODO set correct value
distance_data.signal_strength = -1;
distance_data.signal_quality = -1;
if (_range_topic == nullptr) {
_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &distance_data,

View File

@ -169,7 +169,7 @@ int DfISL29501Wrapper::_publish(struct range_sensor_data &data)
d.covariance = 0.0f;
d.signal_strength = -1;
d.signal_quality = -1;
if (_range_topic == nullptr) {
_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,

View File

@ -175,7 +175,7 @@ int DfTROneWrapper::_publish(struct range_sensor_data &data)
d.covariance = 0.0f;
d.signal_strength = -1;
d.signal_quality = -1;
if (_range_topic == nullptr) {
_range_topic = orb_advertise_multi(ORB_ID(distance_sensor), &d,