Merge branch 'navigator_cleanup' of github.com:PX4/Firmware into navigator_cleanup

ROMFS/px4fmu_common/init.d/3031_phantom

@@ -10,8 +10,8 @@ sh /etc/init.d/rc.fw_defaults
 if [ $DO_AUTOCONFIG == yes ]
 then
 	param set FW_AIRSPD_MIN 13
-	param set FW_AIRSPD_TRIM 18
+	param set FW_AIRSPD_TRIM 15
-	param set FW_AIRSPD_MAX 40
+	param set FW_AIRSPD_MAX 25
 	param set FW_ATT_TC 0.3
 	param set FW_L1_DAMPING 0.75
 	param set FW_L1_PERIOD 15
@@ -23,12 +23,12 @@ then
 	param set FW_P_LIM_MIN -50
 	param set FW_P_RMAX_NEG 0
 	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 0
+	param set FW_P_ROLLFF 1
 	param set FW_RR_FF 0.5
 	param set FW_RR_I 0.02
 	param set FW_RR_IMAX 0.2
 	param set FW_RR_P 0.08
-	param set FW_R_LIM 70
+	param set FW_R_LIM 50
 	param set FW_R_RMAX 0
 	param set FW_T_HRATE_P 0.01
 	param set FW_T_RLL2THR 15

ROMFS/px4fmu_common/init.d/3032_skywalker_x5

@@ -23,7 +23,7 @@ then
 	param set FW_P_LIM_MIN -45
 	param set FW_P_RMAX_NEG 0
 	param set FW_P_RMAX_POS 0
-	param set FW_P_ROLLFF 0
+	param set FW_P_ROLLFF 1
 	param set FW_RR_FF 0.3
 	param set FW_RR_I 0
 	param set FW_RR_IMAX 0.2

ROMFS/px4fmu_common/init.d/rcS

@@ -20,7 +20,7 @@ echo "[init] Looking for microSD..."
 if mount -t vfat /dev/mmcsd0 /fs/microsd
 then
 	set LOG_FILE /fs/microsd/bootlog.txt
-	echo "[init] microSD card mounted at /fs/microsd"
+	echo "[init] microSD mounted: /fs/microsd"
 	# Start playing the startup tune
 	tone_alarm start
 else
@@ -83,9 +83,9 @@ then
 	param select $PARAM_FILE
 	if param load
 	then
-		echo "[init] Parameters loaded: $PARAM_FILE"
+		echo "[init] Params loaded: $PARAM_FILE"
 	else
-		echo "[init] ERROR: Parameters loading failed: $PARAM_FILE"
+		echo "[init] ERROR: Params loading failed: $PARAM_FILE"
 	fi
 	
 	#
@@ -146,7 +146,7 @@ then
 	#
 	if param compare SYS_AUTOSTART 0
 	then
-		echo "[init] Don't try to find autostart script"
+		echo "[init] No autostart"
 	else
 		sh /etc/init.d/rc.autostart
 	fi
@@ -156,10 +156,10 @@ then
 	#
 	if [ -f $CONFIG_FILE ]
 	then
-		echo "[init] Reading config: $CONFIG_FILE"
+		echo "[init] Config: $CONFIG_FILE"
 		sh $CONFIG_FILE
 	else
-		echo "[init] Config file not found: $CONFIG_FILE"
+		echo "[init] Config not found: $CONFIG_FILE"
 	fi
 
 	#
@@ -264,11 +264,11 @@ then
 		then
 			set FMU_MODE serial
 		fi
-	else
-		# Try to get an USB console if not in HIL mode
-		nshterm /dev/ttyACM0 &
 	fi
 
+	# Try to get an USB console
+	nshterm /dev/ttyACM0 &
+	
 	#
 	# Start the Commander (needs to be this early for in-air-restarts)
 	#
@@ -401,11 +401,6 @@ then
 	#
 	if [ $MAVLINK_FLAGS == default ]
 	then
-		if [ $HIL == yes ]
-		then
-			sleep 1
-			set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
-		else
 		# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
 		if [ $TTYS1_BUSY == yes ]
 		then
@@ -419,7 +414,6 @@ then
 			set MAVLINK_FLAGS "-r 1000"
 		fi
 	fi
-	fi
 
 	mavlink start $MAVLINK_FLAGS
 	
@@ -436,18 +430,16 @@ then
 	#
 	# Sensors, Logging, GPS
 	#
-	echo "[init] Start sensors"
 	sh /etc/init.d/rc.sensors
 
 	if [ $HIL == no ]
 	then
 		echo "[init] Start logging"
 		sh /etc/init.d/rc.logging
-		
-		echo "[init] Start GPS"
-		gps start
 	fi
 
+	gps start
+	
 	#
 	# Start up ARDrone Motor interface
 	#
@@ -561,7 +553,7 @@ then
 		echo "[init] Starting addons script: $EXTRAS_FILE"
 		sh $EXTRAS_FILE
 	else
-		echo "[init] Addons script not found: $EXTRAS_FILE"
+		echo "[init] No addons script: $EXTRAS_FILE"
 	fi
 
 	if [ $EXIT_ON_END == yes ]

nuttx-configs/px4fmu-v1/nsh/defconfig

@@ -405,7 +405,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=36
+CONFIG_NFILE_DESCRIPTORS=38
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -439,7 +439,7 @@ CONFIG_SIG_SIGWORK=4
 CONFIG_MAX_TASKS=32
 CONFIG_MAX_TASK_ARGS=10
 CONFIG_NPTHREAD_KEYS=4
-CONFIG_NFILE_DESCRIPTORS=36
+CONFIG_NFILE_DESCRIPTORS=38
 CONFIG_NFILE_STREAMS=8
 CONFIG_NAME_MAX=32
 CONFIG_PREALLOC_MQ_MSGS=4

src/drivers/blinkm/blinkm.cpp

@@ -48,11 +48,14 @@
  * The recognized number off cells, will be blinked 5 times in purple color.
  * 2 Cells = 2 blinks
  * ...
- * 5 Cells = 5 blinks
+ * 6 Cells = 6 blinks
  * Now the Application will show the actual selected Flightmode, GPS-Fix and Battery Warnings and Alerts.
  *
- * System disarmed:
+ * System disarmed and safe:
- * The BlinkM should lit solid red.
+ * The BlinkM should light solid cyan.
+ *
+ * System safety off but not armed:
+ * The BlinkM should light flashing orange
  *
  * System armed:
  * One message is made of 4 Blinks and a pause in the same length as the 4 blinks.
@@ -67,10 +70,10 @@
  * (X = on, _=off)
  *
  * The first 3 blinks indicates the status of the GPS-Signal (red):
- * 0-4 satellites = X-X-X-X-_-_-_-_-_-_-
+ * 0-4 satellites = X-X-X-X-X-_-_-_-_-_-
- *   5 satellites = X-X-_-X-_-_-_-_-_-_-
+ *   5 satellites = X-X-_-X-X-_-_-_-_-_-
- *   6 satellites = X-_-_-X-_-_-_-_-_-_-
+ *   6 satellites = X-_-_-X-X-_-_-_-_-_-
- * >=7 satellites = _-_-_-X-_-_-_-_-_-_-
+ * >=7 satellites = _-_-_-X-X-_-_-_-_-_-
  * If no GPS is found the first 3 blinks are white
  *
  * The fourth Blink indicates the Flightmode:
@@ -119,6 +122,7 @@
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/safety.h>
 
 static const float MAX_CELL_VOLTAGE	= 4.3f;
 static const int LED_ONTIME = 120;
@@ -166,10 +170,12 @@ private:
 	enum ledColors {
 		LED_OFF,
 		LED_RED,
+		LED_ORANGE,
 		LED_YELLOW,
 		LED_PURPLE,
 		LED_GREEN,
 		LED_BLUE,
+		LED_CYAN,
 		LED_WHITE,
 		LED_AMBER
 	};
@@ -380,6 +386,7 @@ BlinkM::led()
 	static int vehicle_control_mode_sub_fd;
 	static int vehicle_gps_position_sub_fd;
 	static int actuator_armed_sub_fd;
+	static int safety_sub_fd;
 
 	static int num_of_cells = 0;
 	static int detected_cells_runcount = 0;
@@ -402,16 +409,20 @@ BlinkM::led()
 
 	if(!topic_initialized) {
 		vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status));
-		orb_set_interval(vehicle_status_sub_fd, 1000);
+		orb_set_interval(vehicle_status_sub_fd, 250);
 
 		vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode));
-		orb_set_interval(vehicle_control_mode_sub_fd, 1000);
+		orb_set_interval(vehicle_control_mode_sub_fd, 250);
 
 		actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed));
-		orb_set_interval(actuator_armed_sub_fd, 1000);
+		orb_set_interval(actuator_armed_sub_fd, 250);
 
 		vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position));
-		orb_set_interval(vehicle_gps_position_sub_fd, 1000);
+		orb_set_interval(vehicle_gps_position_sub_fd, 250);
+
+		/* Subscribe to safety topic */
+		safety_sub_fd = orb_subscribe(ORB_ID(safety));
+		orb_set_interval(safety_sub_fd, 250);
 
 		topic_initialized = true;
 	}
@@ -433,7 +444,9 @@ BlinkM::led()
 			if(num_of_cells > 4) {
 				t_led_color[4] = LED_PURPLE;
 			}
-			t_led_color[5] = LED_OFF;
+			if(num_of_cells > 5) {
+				t_led_color[5] = LED_PURPLE;
+			}
 			t_led_color[6] = LED_OFF;
 			t_led_color[7] = LED_OFF;
 			t_led_blink = LED_BLINK;
@@ -467,14 +480,17 @@ BlinkM::led()
 		struct vehicle_control_mode_s vehicle_control_mode;
 		struct actuator_armed_s actuator_armed;
 		struct vehicle_gps_position_s vehicle_gps_position_raw;
+		struct safety_s safety;
 
 		memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw));
 		memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw));
+		memset(&safety, 0, sizeof(safety));
 
 		bool new_data_vehicle_status;
 		bool new_data_vehicle_control_mode;
 		bool new_data_actuator_armed;
 		bool new_data_vehicle_gps_position;
+		bool new_data_safety;
 
 		orb_check(vehicle_status_sub_fd, &new_data_vehicle_status);
 
@@ -520,7 +536,12 @@ BlinkM::led()
 				no_data_vehicle_gps_position = 3;
 		}
 
+		/* update safety topic */
+		orb_check(safety_sub_fd, &new_data_safety);
 
+		if (new_data_safety) {
+			orb_copy(ORB_ID(safety), safety_sub_fd, &safety);
+		}
 
 		/* get number of used satellites in navigation */
 		num_of_used_sats = 0;
@@ -541,19 +562,7 @@ BlinkM::led()
 				printf("<blinkm> cells found:%d\n", num_of_cells);
 
 			} else {
-				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+				if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
-					/* LED Pattern for battery low warning */
-					led_color_1 = LED_YELLOW;
-					led_color_2 = LED_YELLOW;
-					led_color_3 = LED_YELLOW;
-					led_color_4 = LED_YELLOW;
-					led_color_5 = LED_YELLOW;
-					led_color_6 = LED_YELLOW;
-					led_color_7 = LED_YELLOW;
-					led_color_8 = LED_YELLOW;
-					led_blink = LED_BLINK;
-
-				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
 					/* LED Pattern for battery critical alerting */
 					led_color_1 = LED_RED;
 					led_color_2 = LED_RED;
@@ -565,21 +574,56 @@ BlinkM::led()
 					led_color_8 = LED_RED;
 					led_blink = LED_BLINK;
 
+				} else if(vehicle_status_raw.rc_signal_lost) {
+					/* LED Pattern for FAILSAFE */
+					led_color_1 = LED_BLUE;
+					led_color_2 = LED_BLUE;
+					led_color_3 = LED_BLUE;
+					led_color_4 = LED_BLUE;
+					led_color_5 = LED_BLUE;
+					led_color_6 = LED_BLUE;
+					led_color_7 = LED_BLUE;
+					led_color_8 = LED_BLUE;
+					led_blink = LED_BLINK;
+
+				} else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+					/* LED Pattern for battery low warning */
+					led_color_1 = LED_YELLOW;
+					led_color_2 = LED_YELLOW;
+					led_color_3 = LED_YELLOW;
+					led_color_4 = LED_YELLOW;
+					led_color_5 = LED_YELLOW;
+					led_color_6 = LED_YELLOW;
+					led_color_7 = LED_YELLOW;
+					led_color_8 = LED_YELLOW;
+					led_blink = LED_BLINK;
+
 				} else {
 					/* no battery warnings here */
 
 					if(actuator_armed.armed == false) {
 						/* system not armed */
-						led_color_1 = LED_RED;
+						if(safety.safety_off){
-						led_color_2 = LED_RED;
+							led_color_1 = LED_ORANGE;
-						led_color_3 = LED_RED;
+							led_color_2 = LED_ORANGE;
-						led_color_4 = LED_RED;
+							led_color_3 = LED_ORANGE;
-						led_color_5 = LED_RED;
+							led_color_4 = LED_ORANGE;
-						led_color_6 = LED_RED;
+							led_color_5 = LED_ORANGE;
-						led_color_7 = LED_RED;
+							led_color_6 = LED_ORANGE;
-						led_color_8 = LED_RED;
+							led_color_7 = LED_ORANGE;
+							led_color_8 = LED_ORANGE;
+							led_blink = LED_BLINK;
+						}else{
+							led_color_1 = LED_CYAN;
+							led_color_2 = LED_CYAN;
+							led_color_3 = LED_CYAN;
+							led_color_4 = LED_CYAN;
+							led_color_5 = LED_CYAN;
+							led_color_6 = LED_CYAN;
+							led_color_7 = LED_CYAN;
+							led_color_8 = LED_CYAN;
 							led_blink = LED_NOBLINK;
-
+						}
 					} else {
 						/* armed system - initial led pattern */
 						led_color_1 = LED_RED;
@@ -593,23 +637,22 @@ BlinkM::led()
 						led_blink = LED_BLINK;
 
 						if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
-
+							/* indicate main control state */
-							//XXX please check
+							if (vehicle_status_raw.main_state == MAIN_STATE_EASY)
-							if (vehicle_control_mode.flag_control_position_enabled)
 								led_color_4 = LED_GREEN;
-							else if (vehicle_control_mode.flag_control_velocity_enabled)
+							else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
 								led_color_4 = LED_BLUE;
-							else if (vehicle_control_mode.flag_control_attitude_enabled)
+							else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT)
 								led_color_4 = LED_YELLOW;
-							else if (vehicle_control_mode.flag_control_manual_enabled)
+							else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
-								led_color_4 = LED_AMBER;
+								led_color_4 = LED_WHITE;
 							else 
 								led_color_4 = LED_OFF;
-								
+							led_color_5 = led_color_4;
 						}
 
 						if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) {
-							/* handling used sat<EFBFBD>s */
+							/* handling used satus */
 							if(num_of_used_sats >= 7) {
 								led_color_1 = LED_OFF;
 								led_color_2 = LED_OFF;
@@ -690,8 +733,11 @@ void BlinkM::setLEDColor(int ledcolor) {
 		case LED_RED:	// red
 			set_rgb(255,0,0);
 			break;
+		case LED_ORANGE:	// orange
+			set_rgb(255,150,0);
+			break;
 		case LED_YELLOW:	// yellow
-			set_rgb(255,70,0);
+			set_rgb(200,200,0);
 			break;
 		case LED_PURPLE:	// purple
 			set_rgb(255,0,255);
@@ -702,11 +748,14 @@ void BlinkM::setLEDColor(int ledcolor) {
 		case LED_BLUE:	// blue
 			set_rgb(0,0,255);
 			break;
+		case LED_CYAN:	// cyan
+			set_rgb(0,128,128);
+			break;
 		case LED_WHITE:	// white
 			set_rgb(255,255,255);
 			break;
 		case LED_AMBER:	// amber
-			set_rgb(255,20,0);
+			set_rgb(255,65,0);
 			break;
 	}
 }

src/drivers/drv_range_finder.h

@@ -46,6 +46,10 @@
 
 #define RANGE_FINDER_DEVICE_PATH	"/dev/range_finder"
 
+enum RANGE_FINDER_TYPE {
+	RANGE_FINDER_TYPE_LASER = 0,
+};
+
 /**
  * range finder report structure.  Reads from the device must be in multiples of this
  * structure.
@@ -53,8 +57,11 @@
 struct range_finder_report {
 	uint64_t timestamp;
 	uint64_t error_count;
-	float distance; 			/** in meters */
+	unsigned type;				/**< type, following RANGE_FINDER_TYPE enum */
-	uint8_t valid;				/** 1 == within sensor range, 0 = outside sensor range */
+	float distance; 			/**< in meters */
+	float minimum_distance;			/**< minimum distance the sensor can measure */
+	float maximum_distance;			/**< maximum distance the sensor can measure */
+	uint8_t valid;				/**< 1 == within sensor range, 0 = outside sensor range */
 };
 
 /*

src/drivers/px4io/px4io_uploader.cpp

@@ -201,9 +201,14 @@ PX4IO_Uploader::upload(const char *filenames[])
 			continue;
 		}
 
-		if (bl_rev <= 2)
+		if (bl_rev <= 2) {
 			ret = verify_rev2(fw_size);
-		else if(bl_rev == 3) {
+		} else if(bl_rev == 3) {
+			ret = verify_rev3(fw_size);
+		} else {
+			/* verify rev 4 and higher still uses the same approach and
+			 * every version *needs* to be verified.
+			 */
 			ret = verify_rev3(fw_size);
 		}
 

src/modules/commander/commander.cpp

@@ -969,7 +969,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;

src/modules/dataman/dataman.c

@@ -44,7 +44,9 @@
 #include <stdlib.h>
 #include <fcntl.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
 #include <queue.h>
+#include <string.h>
 
 #include "dataman.h"
 
@@ -594,6 +596,20 @@ task_main(int argc, char *argv[])
 
 	sem_init(&g_work_queued_sema, 1, 0);
 
+	/* See if the data manage file exists and is a multiple of the sector size */
+	g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
+	if (g_task_fd >= 0) {
+		/* File exists, check its size */
+		int file_size = lseek(g_task_fd, 0, SEEK_END);
+		if ((file_size % k_sector_size) != 0) {
+			warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
+			close(g_task_fd);
+			unlink(k_data_manager_device_path);
+		}
+		else
+			close(g_task_fd);
+	}
+
 	/* Open or create the data manager file */
 	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);
 
@@ -603,7 +619,7 @@ task_main(int argc, char *argv[])
 		return -1;
 	}
 
-	if (lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
+	if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
 		close(g_task_fd);
 		warnx("Could not seek data manager file %s", k_data_manager_device_path);
 		sem_post(&g_init_sema); /* Don't want to hang startup */
@@ -776,4 +792,3 @@ dataman_main(int argc, char *argv[])
 
 	exit(1);
 }
-

src/modules/dataman/dataman.h

@@ -79,7 +79,7 @@ extern "C" {
 	} dm_reset_reason;
 
 	/* Maximum size in bytes of a single item instance */
-	#define DM_MAX_DATA_SIZE 126
+	#define DM_MAX_DATA_SIZE 124
 
 	/* Retrieve from the data manager store */
 	__EXPORT ssize_t

src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp

@@ -964,7 +964,7 @@ FixedwingEstimator::task_main()
 			}
 
 			// Publish results
-			if (_initialized) {
+			if (_initialized && (check == OK)) {
 
 
 

src/modules/mavlink/mavlink_main.cpp

@@ -574,6 +574,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	/* open uart */
 	_uart_fd = open(uart_name, O_RDWR | O_NOCTTY);
 
+	if (_uart_fd < 0) {
+		return _uart_fd;
+	}
+
+
 	/* Try to set baud rate */
 	struct termios uart_config;
 	int termios_state;
@@ -1964,6 +1969,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
 		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
 		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
+		configure_stream("DISTANCE_SENSOR", 0.5f);
 		break;
 
 	case MAVLINK_MODE_CAMERA:

src/modules/mavlink/mavlink_messages.cpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/navigation_capabilities.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
+#include <drivers/drv_range_finder.h>
 
 #include "mavlink_messages.h"
 
@@ -271,7 +272,7 @@ protected:
 						status->load * 1000.0f,
 						status->battery_voltage * 1000.0f,
 						status->battery_current * 1000.0f,
-						status->battery_remaining,
+						status->battery_remaining * 100.0f,
 						status->drop_rate_comm,
 						status->errors_comm,
 						status->errors_count1,
@@ -1313,6 +1314,51 @@ protected:
 	}
 };
 
+class MavlinkStreamDistanceSensor : public MavlinkStream
+{
+public:
+	const char *get_name()
+	{
+		return "DISTANCE_SENSOR";
+	}
+
+	MavlinkStream *new_instance()
+	{
+		return new MavlinkStreamDistanceSensor();
+	}
+
+private:
+	MavlinkOrbSubscription *range_sub;
+	struct range_finder_report *range;
+
+protected:
+	void subscribe(Mavlink *mavlink)
+	{
+		range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
+		range = (struct range_finder_report *)range_sub->get_data();
+	}
+
+	void send(const hrt_abstime t)
+	{
+		(void)range_sub->update(t);
+
+		uint8_t type;
+
+		switch (range->type) {
+			case RANGE_FINDER_TYPE_LASER:
+			type = MAV_DISTANCE_SENSOR_LASER;
+			break;
+		}
+
+		uint8_t id = 0;
+		uint8_t orientation = 0;
+		uint8_t covariance = 20;
+
+		mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation, 
+			range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
+	}
+};
+
 MavlinkStream *streams_list[] = {
 	new MavlinkStreamHeartbeat(),
 	new MavlinkStreamSysStatus(),
@@ -1339,6 +1385,7 @@ MavlinkStream *streams_list[] = {
 	new MavlinkStreamAttitudeControls(),
 	new MavlinkStreamNamedValueFloat(),
 	new MavlinkStreamCameraCapture(),
+	new MavlinkStreamDistanceSensor(),
 	new MavlinkStreamViconPositionEstimate(),
 	nullptr
 };

src/modules/sdlog2/sdlog2.c

@@ -908,9 +908,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 	hrt_abstime barometer_timestamp = 0;
 	hrt_abstime differential_pressure_timestamp = 0;
 
-	/* track changes in distance status */
-	bool dist_bottom_present = false;
-
 	/* enable logging on start if needed */
 	if (log_on_start) {
 		/* check GPS topic to get GPS time */
@@ -1099,6 +1096,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_LPOS.x = buf.local_pos.x;
 			log_msg.body.log_LPOS.y = buf.local_pos.y;
 			log_msg.body.log_LPOS.z = buf.local_pos.z;
+			log_msg.body.log_LPOS.ground_dist = buf.local_pos.dist_bottom;
+			log_msg.body.log_LPOS.ground_dist_rate = buf.local_pos.dist_bottom_rate;
 			log_msg.body.log_LPOS.vx = buf.local_pos.vx;
 			log_msg.body.log_LPOS.vy = buf.local_pos.vy;
 			log_msg.body.log_LPOS.vz = buf.local_pos.vz;
@@ -1108,19 +1107,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
 			log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
 			log_msg.body.log_LPOS.landed = buf.local_pos.landed;
+			log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
 			LOGBUFFER_WRITE_AND_COUNT(LPOS);
-
-			if (buf.local_pos.dist_bottom_valid) {
-				dist_bottom_present = true;
-			}
-
-			if (dist_bottom_present) {
-				log_msg.msg_type = LOG_DIST_MSG;
-				log_msg.body.log_DIST.bottom = buf.local_pos.dist_bottom;
-				log_msg.body.log_DIST.bottom_rate = buf.local_pos.dist_bottom_rate;
-				log_msg.body.log_DIST.flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
-				LOGBUFFER_WRITE_AND_COUNT(DIST);
-			}
 		}
 
 		/* --- LOCAL POSITION SETPOINT --- */

src/modules/sdlog2/sdlog2_messages.h

@@ -101,6 +101,8 @@ struct log_LPOS_s {
 	float x;
 	float y;
 	float z;
+	float ground_dist;
+	float ground_dist_rate;
 	float vx;
 	float vy;
 	float vz;
@@ -110,6 +112,7 @@ struct log_LPOS_s {
 	uint8_t xy_flags;
 	uint8_t z_flags;
 	uint8_t landed;
+	uint8_t ground_dist_flags;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -344,25 +347,25 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff",		"RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
-	LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
+	LOG_FORMAT(LPOS, "ffffffffLLfBBBB",	"X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"),
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff",		"GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBfBB",		"MainState,ArmState,BatRem,BatWarn,Landed"),
-	LOG_FORMAT(RC, "ffffffffBB", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
+	LOG_FORMAT(RC, "ffffffffB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count"),
 	LOG_FORMAT(OUT0, "ffffffff",		"Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "fff",			"IndSpeed,TrueSpeed,AirTemp"),
 	LOG_FORMAT(ARSP, "fff",			"RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB",		"RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLfffffB",		"Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"),
 	LOG_FORMAT(GPSP, "BLLffBfbf",		"NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"),
-	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
+	LOG_FORMAT(ESC, "HBBBHHHHHHfH",		"count,nESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff",			"VX,VY,VZ"),
 	LOG_FORMAT(BATT, "ffff",		"V,VFilt,C,Discharged"),
 	LOG_FORMAT(DIST, "ffB",			"Bottom,BottomRate,Flags"),
 	LOG_FORMAT(TELE, "BBBBHHB",		"RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"),
-	LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,n_states,states_nan,cov_nan,kgain_nan"),
+	LOG_FORMAT(ESTM, "ffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
-	LOG_FORMAT(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
+	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
 	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
 
 	/* system-level messages, ID >= 0x80 */