yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

distance_sensor: adapted message to float for distance and adapted the drivers

Browse Source
This commit is contained in:
Ban Siesta
2015-05-24 14:28:19 +01:00
parent a0011d2163
commit 103d59e9be
4 changed files with 24 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
msg/distance_sensor.msg
View File

@@ -1,17 +1,17 @@
# DISTANCE_SENSOR message data
uint32 time_boot_ms # Time since system boot (us)
uint64 timestamp # Microseconds since system boot
uint16 min_distance # Minimum distance the sensor can measure (in SI)
uint16 max_distance # Maximum distance the sensor can measure (in SI)
uint16 current_distance # Current distance reading (in SI)
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
uint8 type # Type from MAV_DISTANCE_SENSOR enum
uint8 type # Type from MAV_DISTANCE_SENSOR enum
uint8 MAV_DISTANCE_SENSOR_LASER = 0
uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1
uint8 MAV_DISTANCE_SENSOR_INFRARED = 2
uint8 id # Onboard ID of the sensor
uint8 id # Onboard ID of the sensor
uint8 orientation # Direction the sensor faces from MAV_SENSOR_ORIENTATION enum
uint8 covariance # Measurement covariance (in SI), 0 for unknown / invalid readings

12
src/drivers/mb12xx/mb12xx.cpp
View File

@@ -570,18 +570,18 @@ MB12XX::collect()
return ret;
}
uint16_t distance = val[0] << 8 | val[1];
float si_units = (distance * 1.0f) / 100.0f; /* cm to m */
struct distance_sensor_s report;
uint16_t distance_cm = val[0] << 8 | val[1];
float distance_m = float(distance_cm) * 1e-2f;
report.time_boot_ms = hrt_absolute_time();
struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.id = 1;
report.type = 1;
report.orientation = 8;
report.current_distance = si_units;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0;
report.covariance = 0.0f;
/* publish it, if we are the primary */
if (_distance_sensor_topic >= 0) {

12
src/drivers/sf0x/sf0x.cpp
View File

@@ -557,11 +557,11 @@ SF0X::collect()
_last_read = hrt_absolute_time();
float si_units;
float distance_m = -1.0f;
bool valid = false;
for (int i = 0; i < ret; i++) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
valid = true;
}
}
@@ -570,18 +570,18 @@ SF0X::collect()
return -EAGAIN;
}
debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));
debug("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));
struct distance_sensor_s report;
report.time_boot_ms = hrt_absolute_time();
report.timestamp = hrt_absolute_time();
report.id = 2;
report.type = 0;
report.orientation = 8;
report.current_distance = si_units;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0;
report.covariance = 0.0f;
/* publish it */
orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);

10
src/drivers/trone/trone.cpp
View File

@@ -567,18 +567,18 @@ TRONE::collect()
return ret;
}
uint16_t distance = (val[0] << 8) | val[1];
float si_units = distance * 0.001f; /* mm to m */
uint16_t distance_mm = (val[0] << 8) | val[1];
float distance_m = float(distance_mm) * 1e-3f;
struct distance_sensor_s report;
report.time_boot_ms = hrt_absolute_time();
report.timestamp = hrt_absolute_time();
report.id = 3;
report.type = 0;
report.orientation = 8;
report.current_distance = si_units;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.covariance = 0.0;
report.covariance = 0.0f;
/* publish it, if we are the primary */
if (_distance_sensor_topic >= 0) {