Merge remote-tracking branch 'upstream/master' into mtecs

8 files changed, 71 insertions, 58 deletions

diff --git a/.gitignore b/.gitignore
index 71326517f..5b345b34a 100644
--- a/.gitignore
+++ b/.gitignore
@@ -34,5 +34,6 @@ mavlink/include/mavlink/v0.9/
 /Documentation/html/
 /Documentation/doxygen*objdb*tmp
 .tags
+tags
 .tags_sorted_by_file
 .pydevproject
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 2ab1e2e96..ea5cf8deb 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/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,10 +264,10 @@ 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,23 +401,17 @@ then
 	#
 	if [ $MAVLINK_FLAGS == default ]
 	then
-		if [ $HIL == yes ]
+		# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
+		if [ $TTYS1_BUSY == yes ]
 		then
-			sleep 1
-			set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0"
+			# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
+			set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
+
+			# Exit from nsh to free port for mavlink
+			set EXIT_ON_END yes
 		else
-			# Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s
-			if [ $TTYS1_BUSY == yes ]
-			then
-				# Start MAVLink on ttyS0, because FMU ttyS1 pins configured as something else
-				set MAVLINK_FLAGS "-r 1000 -d /dev/ttyS0"
-				
-				# Exit from nsh to free port for mavlink
-				set EXIT_ON_END yes
-			else
-				# Start MAVLink on default port: ttyS1
-				set MAVLINK_FLAGS "-r 1000"
-			fi
+			# Start MAVLink on default port: ttyS1
+			set MAVLINK_FLAGS "-r 1000"
 		fi
 	fi
 
@@ -436,17 +430,15 @@ 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 ]
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ce6de88ef..819dfbe83 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -967,7 +967,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;
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 34d20e485..c132b0040 100644
--- a/src/modules/dataman/dataman.c
+++ b/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);
 }
-
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index a70638ccc..33c9fcd15 100644
--- a/src/modules/dataman/dataman.h
+++ b/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
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a9f5f4de7..227e99b48 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/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;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 58af1fcb6..678ce1645 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -271,7 +271,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,
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 2538dcd2f..28d0706ff 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -342,30 +342,30 @@ struct log_PARM_s {
 /* construct list of all message formats */
 static const struct log_format_s log_formats[] = {
 	/* business-level messages, ID < 0x80 */
-	LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
-	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, "ffffffffLLfBBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
-	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, "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(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(PWR, "fffBBBBB", "Periph_5V,Servo_5V,RSSI,USB_OK,BRICK_OK,SRV_OK,PERIPH_OC,HIPWR_OC"),
-	LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"),
+	LOG_FORMAT(ATT, "fffffffff",		"Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"),
+	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, "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, "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",		"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,nStat,statNaN,covNaN,kGainNaN"),
+	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 */
 	/* FMT: don't write format of format message, it's useless */