From 103d59e9bee7a147429a9e70f1ec17c89be57059 Mon Sep 17 00:00:00 2001
From: Ban Siesta <bansiesta@gmail.com>
Date: Sun, 24 May 2015 14:28:19 +0100
Subject: [PATCH] distance_sensor: adapted message to float for distance and
 adapted the drivers

---
 msg/distance_sensor.msg       | 14 +++++++-------
 src/drivers/mb12xx/mb12xx.cpp | 12 ++++++------
 src/drivers/sf0x/sf0x.cpp     | 12 ++++++------
 src/drivers/trone/trone.cpp   | 10 +++++-----
 4 files changed, 24 insertions(+), 24 deletions(-)

diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg
index 8e44d76f79..ad01ce63e0 100644
--- a/msg/distance_sensor.msg
+++ b/msg/distance_sensor.msg
@@ -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
\ No newline at end of file
diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp
index 432b579193..7ecfa21c9d 100644
--- a/src/drivers/mb12xx/mb12xx.cpp
+++ b/src/drivers/mb12xx/mb12xx.cpp
@@ -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) {
diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index 4c85bc8c4c..86a4967f19 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -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);
diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp
index 460f5570a8..ffc262a570 100644
--- a/src/drivers/trone/trone.cpp
+++ b/src/drivers/trone/trone.cpp
@@ -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) {