Merge branch 'master' of https://github.com/PX4/Firmware into fw_control

13 files changed, 334 insertions, 186 deletions

diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m
index 579a582d3..88bcfec96 100644
--- a/ROMFS/logging/logconv.m
+++ b/ROMFS/logging/logconv.m
@@ -4,162 +4,232 @@ close all
 %%%%%%%%%%%%%%%%%%%%%%%
 % SYSTEM VECTOR
 %
-% 			All measurements in NED frame
-% 			
-% 			uint64_t timestamp;
-% 			float gyro[3]; in rad/s
-% 			float accel[3]; in m/s^2
-% 			float mag[3]; in Gauss
-% 			float baro; pressure in millibar
-% 			float baro_alt; altitude above MSL in meters
-% 			float baro_temp; in degrees celcius
-% 			float control[4]; roll, pitch, yaw [-1..1], thrust [0..1]
-% 			float actuators[8]; motor 1-8, in motor units (PWM: 1000-2000,
-% 			AR.Drone: 0-512
-% 			float vbat; battery voltage in volt
-% 			float adc[3]; remaining auxiliary ADC ports in volt
-%           float local_position
-%           int32 gps_raw_position
-
-
-if exist('sysvector.bin', 'file')
-    % Read actuators file
-    myFile = java.io.File('sysvector.bin')
-    fileSize = length(myFile)
-
-    fid = fopen('sysvector.bin', 'r');
-    elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
-
-    for i=1:elements
-        % timestamp
-        sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-        % actuators 1-16
-        % quadrotor: motor 1-4 on the first four positions
-        sysvector(i, 2:32) = fread(fid,  28+3, 'float', 'ieee-le');
-        sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
-    end
-    
-    sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
-    sysvector_minutes = sysvector_interval_seconds / 60
-    
-    % Normalize time
-    sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
-    
-    % Create some basic plots
-    
-    % Remove zero rows from GPS
-    gps = sysvector(:,33:35);
-    gps(~any(gps,2), :) = [];
-    
-    all_data = figure('Name', 'GPS RAW');
-    gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
-    
-    
-    all_data = figure('Name', 'Complete Log Data (exc. GPS)');
-    plot(sysvector(:,1), sysvector(:,2:32));
-    
-    actuator_inputs = figure('Name', 'Attitude controller outputs');
-    plot(sysvector(:,1), sysvector(:,14:17));
-    legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
+% //All measurements in NED frame
+%
+% uint64_t timestamp; //[us]
+% float gyro[3]; //[rad/s]
+% float accel[3]; //[m/s^2]
+% float mag[3]; //[gauss] 
+% float baro; //pressure [millibar]
+% float baro_alt; //altitude above MSL [meter]
+% float baro_temp; //[degree celcius]
+% float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
+% float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
+% float vbat; //battery voltage in [volt]
+% float adc[3]; //remaining auxiliary ADC ports [volt]
+% float local_position[3]; //tangent plane mapping into x,y,z [m]
+% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
+% float attitude[3]; //pitch, roll, yaw [rad]
+% float rotMatrix[9]; //unitvectors
+
+%myPath = '..\LOG30102012\session0002\';    %set relative path here
+myPath = '.\';                             
+myFile = 'sysvector.bin';
+filePath = strcat(myPath,myFile);
+
+if exist(filePath, 'file')
+    fileInfo = dir(filePath);
+    fileSize = fileInfo.bytes;
     
-    actuator_outputs = figure('Name', 'Actuator outputs');
-    plot(sysvector(:,1), sysvector(:,18:25));
-    legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
+    fid = fopen(filePath, 'r');
+    elements = int64(fileSize./(16*4+8))/4
     
-end
-
-if exist('actuator_outputs0.bin', 'file')
-    % Read actuators file
-    myFile = java.io.File('actuator_outputs0.bin')
-    fileSize = length(myFile)
-
-    fid = fopen('actuator_outputs0.bin', 'r');
-    elements = int64(fileSize./(16*4+8))
-
-    for i=1:elements
+    for i=1:elements       
         % timestamp
-        actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-        % actuators 1-16
-        % quadrotor: motor 1-4 on the first four positions
-        actuators(i, 2:17) = fread(fid,  16, 'float', 'ieee-le');
-    end
-end
-
-if exist('actuator_controls0.bin', 'file')
-    % Read actuators file
-    myFile = java.io.File('actuator_controls0.bin')
-    fileSize = length(myFile)
-
-    fid = fopen('actuator_controls0.bin', 'r');
-    elements = int64(fileSize./(8*4+8))
-
-    for i=1:elements
-        % timestamp
-        actuator_controls(i,1) = fread(fid, 1, 'uint64', 0, 'ieee-le.l64');
-        % actuators 1-16
-        % quadrotor: motor 1-4 on the first four positions
-        actuator_controls(i, 2:9) = fread(fid,  8, 'float', 'ieee-le');
+        sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
+        
+        % gyro (3 channels)
+        sensors(i,2:4) = fread(fid, 3, 'float', 0, 'ieee-le');
+       
+        % accelerometer (3 channels)
+        sensors(i,5:7) = fread(fid, 3, 'float', 0, 'ieee-le');
+      
+        % mag (3 channels)
+        sensors(i,8:10) = fread(fid, 3, 'float', 0, 'ieee-le');
+       
+        % baro pressure
+        sensors(i,11) = fread(fid, 1, 'float', 0, 'ieee-le');
+        
+        % baro alt
+        sensors(i,12) = fread(fid, 1, 'float', 0, 'ieee-le');
+         
+        % baro temp
+        sensors(i,13) = fread(fid, 1, 'float', 0, 'ieee-le');
+        
+        % actuator control (4 channels) 
+        sensors(i,14:17) = fread(fid, 4, 'float', 0, 'ieee-le');
+        
+        % actuator outputs (8 channels)
+        sensors(i,18:25) = fread(fid, 8, 'float', 0, 'ieee-le');
+                
+        % vbat
+        sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
+        
+        % adc voltage (3 channels)
+        sensors(i,27:29) = fread(fid, 3, 'float', 0, 'ieee-le');
+        
+        % local position (3 channels)
+        sensors(i,30:32) = fread(fid, 3, 'float', 0, 'ieee-le');
+        
+        % gps_raw_position (3 channels)
+        sensors(i,33:35) = fread(fid, 3, 'uint32', 0, 'ieee-le');
+        
+        % attitude (3 channels)
+        sensors(i,36:38) = fread(fid, 3, 'float', 0, 'ieee-le');
+        
+        % RotMatrix (9 channels)
+        sensors(i,39:47) = fread(fid, 9, 'float', 0, 'ieee-le');
     end
+    time_us = sensors(elements,1) - sensors(1,1);
+    time_s = time_us*1e-6
+    time_m = time_s/60
+    disp(['end log2matlab conversion' char(10)]);
+else
+    disp(['file: ' filePath ' does not exist' char(10)]);
 end
 
 
-if exist('sensor_combined.bin', 'file')
-    % Read sensor combined file
-    % Type definition: Firmware/apps/uORB/topics/sensor_combined.h
-    % Struct: sensor_combined_s
-    fileInfo = dir('sensor_combined.bin');
-    fileSize = fileInfo.bytes;
+%% old version of reading in different files from sdlog.c
+% if exist('sysvector.bin', 'file')
+%     % Read actuators file
+%     myFile = java.io.File('sysvector.bin')
+%     fileSize = length(myFile)
+% 
+%     fid = fopen('sysvector.bin', 'r');
+%     elements = int64(fileSize./(8+(3+3+3+1+1+1+4+8+4+3+3)*4));
+% 
+%     for i=1:elements
+%         % timestamp
+%         sysvector(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
+%         % actuators 1-16
+%         % quadrotor: motor 1-4 on the first four positions
+%         sysvector(i, 2:32) = fread(fid, 28+3, 'float', 'ieee-le');
+%         sysvector(i,33:35) = fread(fid, 3, 'int32', 'ieee-le');
+%     end
+%     
+%     sysvector_interval_seconds = (sysvector(end,1) - sysvector(1:1)) / 1000000
+%     sysvector_minutes = sysvector_interval_seconds / 60
+%     
+%     % Normalize time
+%     sysvector(:,1) = (sysvector(:,1) - sysvector(1,1)) / 1000000;
+%     
+%     % Create some basic plots
+%     
+%     % Remove zero rows from GPS
+%     gps = sysvector(:,33:35);
+%     gps(~any(gps,2), :) = [];
+%     
+%     all_data = figure('Name', 'GPS RAW');
+%     gps_position = plot3(gps(:,1), gps(:,2), gps(:,3));
+%     
+%     
+%     all_data = figure('Name', 'Complete Log Data (exc. GPS)');
+%     plot(sysvector(:,1), sysvector(:,2:32));
+%     
+%     actuator_inputs = figure('Name', 'Attitude controller outputs');
+%     plot(sysvector(:,1), sysvector(:,14:17));
+%     legend('roll motor setpoint', 'pitch motor setpoint', 'yaw motor setpoint', 'throttle motor setpoint');
+%     
+%     actuator_outputs = figure('Name', 'Actuator outputs');
+%     plot(sysvector(:,1), sysvector(:,18:25));
+%     legend('actuator 0', 'actuator 1', 'actuator 2', 'actuator 3', 'actuator 4', 'actuator 5', 'actuator 6', 'actuator 7');
+%     
+% end
+% 
+% if exist('actuator_outputs0.bin', 'file')
+%     % Read actuators file
+%     myFile = java.io.File('actuator_outputs0.bin')
+%     fileSize = length(myFile)
+% 
+%     fid = fopen('actuator_outputs0.bin', 'r');
+%     elements = int64(fileSize./(16*4+8))
+% 
+%     for i=1:elements
+%         % timestamp
+%         actuators(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
+%         % actuators 1-16
+%         % quadrotor: motor 1-4 on the first four positions
+%         actuators(i, 2:17) = fread(fid, 16, 'float', 'ieee-le');
+%     end
+% end
+% 
+% if exist('actuator_controls0.bin', 'file')
+%     % Read actuators file
+%     myFile = java.io.File('actuator_controls0.bin')
+%     fileSize = length(myFile)
+% 
+%     fid = fopen('actuator_controls0.bin', 'r');
+%     elements = int64(fileSize./(8*4+8))
+% 
+%     for i=1:elements
+%         % timestamp
+%         actuator_controls(i,1) = fread(fid, 1, 'uint64', 0, 'ieee-le.l64');
+%         % actuators 1-16
+%         % quadrotor: motor 1-4 on the first four positions
+%         actuator_controls(i, 2:9) = fread(fid, 8, 'float', 'ieee-le');
+%     end
+% end
+% 
+% 
+% if exist('sensor_combined.bin', 'file')
+%     % Read sensor combined file
+%     % Type definition: Firmware/apps/uORB/topics/sensor_combined.h
+%     % Struct: sensor_combined_s
+%     fileInfo = dir('sensor_combined.bin');
+%     fileSize = fileInfo.bytes;
+% 
+%     fid = fopen('sensor_combined.bin', 'r');
+% 
+%     for i=1:elements
+%         % timestamp
+%         sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
+%         % gyro raw
+%         sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le');
+%         % gyro counter
+%         sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le');
+%         % gyro in rad/s
+%         sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le');
+% 
+%         % accelerometer raw
+%         sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
+%         % padding bytes
+%         fread(fid, 1, 'int16', 0, 'ieee-le');
+%         % accelerometer counter
+%         sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le');
+%         % accel in m/s2
+%         sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le');
+%         % accel mode
+%         sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le');
+%         % accel range
+%         sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le');
+% 
+%         % mag raw
+%         sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
+%         % padding bytes
+%         fread(fid, 1, 'int16', 0, 'ieee-le');
+%         % mag in Gauss
+%         sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
+%         % mag mode
+%         sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
+%         % mag range
+%         sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
+%         % mag cuttoff freq
+%         sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
+%         % mag counter
+%         sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le');
+% 
+%         % baro pressure millibar
+%         % baro alt meter
+%         % baro temp celcius
+%         % battery voltage
+%         % adc voltage (3 channels)
+%         sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
+%         % baro counter and battery counter
+%         sensors(i,35:36) = fread(fid, 2, 'uint32', 0, 'ieee-le');
+%         % battery voltage valid flag
+%         sensors(i,37) = fread(fid, 1, 'uint32', 0, 'ieee-le');
+% 
+%     end
+% end
 
-    fid = fopen('sensor_combined.bin', 'r');
 
-    for i=1:elements
-        % timestamp
-        sensors(i,1) = double(fread(fid, 1, '*uint64', 0, 'ieee-le.l64'));
-        % gyro raw
-        sensors(i,2:4) = fread(fid, 3, 'int16', 0, 'ieee-le');
-        % gyro counter
-        sensors(i,5) = fread(fid, 1, 'uint16', 0, 'ieee-le');
-        % gyro in rad/s
-        sensors(i,6:8) = fread(fid, 3, 'float', 0, 'ieee-le');
-
-        % accelerometer raw
-        sensors(i,9:11) = fread(fid, 3, 'int16', 0, 'ieee-le');
-        % padding bytes
-        fread(fid, 1, 'int16', 0, 'ieee-le');
-        % accelerometer counter
-        sensors(i,12) = fread(fid, 1, 'uint32', 0, 'ieee-le');
-        % accel in m/s2
-        sensors(i,13:15) = fread(fid, 3, 'float', 0, 'ieee-le');
-        % accel mode
-        sensors(i,16) = fread(fid, 1, 'int32', 0, 'ieee-le');
-        % accel range
-        sensors(i,17) = fread(fid, 1, 'float', 0, 'ieee-le');
-
-        % mag raw
-        sensors(i,18:20) = fread(fid, 3, 'int16', 0, 'ieee-le');
-        % padding bytes
-        fread(fid, 1, 'int16', 0, 'ieee-le');
-        % mag in Gauss
-        sensors(i,21:23) = fread(fid, 3, 'float', 0, 'ieee-le');
-        % mag mode
-        sensors(i,24) = fread(fid, 1, 'int32', 0, 'ieee-le');
-        % mag range
-        sensors(i,25) = fread(fid, 1, 'float', 0, 'ieee-le');
-        % mag cuttoff freq
-        sensors(i,26) = fread(fid, 1, 'float', 0, 'ieee-le');
-        % mag counter
-        sensors(i,27) = fread(fid, 1, 'int32', 0, 'ieee-le');
-
-        % baro pressure millibar
-        % baro alt meter
-        % baro temp celcius
-        % battery voltage
-        % adc voltage (3 channels)
-        sensors(i,28:34) = fread(fid, 7, 'float', 0, 'ieee-le');
-        % baro counter and battery counter
-        sensors(i,35:36) = fread(fid, 2, 'uint32', 0, 'ieee-le');
-        % battery voltage valid flag
-        sensors(i,37) = fread(fid, 1, 'uint32', 0, 'ieee-le');
-
-    end
-end
\ No newline at end of file
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 9dbdf49e2..2b59f709a 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -149,7 +149,6 @@ static void led_deinit(void);
 static int led_toggle(int led);
 static int led_on(int led);
 static int led_off(int led);
-static int pm_save_eeprom(bool only_unsaved);
 static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status);
 static void do_mag_calibration(int status_pub, struct vehicle_status_s *status);
 static void do_accel_calibration(int status_pub, struct vehicle_status_s *status);
diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c
index a64d99cd4..891efe9d7 100644
--- a/apps/commander/state_machine_helper.c
+++ b/apps/commander/state_machine_helper.c
@@ -579,6 +579,8 @@ uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_
 		state_machine_publish(status_pub, current_status, mavlink_fd);
 		publish_armed_status(current_status);
 		printf("[commander] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
+	} else if (current_status->state_machine != SYSTEM_STATE_STANDBY) {
+		mavlink_log_critical(mavlink_fd, "[commander] REJECTING switch to HIL, not in standby.")
 	}
 
 	/* NEVER actually switch off HIL without reboot */
diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp
index bef21848b..67b16aa42 100644
--- a/apps/drivers/hil/hil.cpp
+++ b/apps/drivers/hil/hil.cpp
@@ -474,6 +474,7 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg)
 		ret = HIL::pwm_ioctl(filp, cmd, arg);
 		break;
 	default:
+		ret = -ENOTTY;
 		debug("not in a PWM mode");
 		break;
 	}
@@ -489,7 +490,7 @@ int
 HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
-	int channel;
+	// int channel;
 
 	lock();
 
@@ -824,6 +825,7 @@ hil_main(int argc, char *argv[])
 			// XXX all modes have PWM settings
 			if (argc > i + 1) {
 				pwm_update_rate_in_hz = atoi(argv[i + 1]);
+				printf("pwm update rate: %d Hz\n", pwm_update_rate_in_hz);
 			} else {
 				fprintf(stderr, "missing argument for pwm update rate (-u)\n");
 				return 1;
diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp
index 72c7e34a1..0e84760d5 100644
--- a/apps/drivers/px4io/px4io.cpp
+++ b/apps/drivers/px4io/px4io.cpp
@@ -467,6 +467,7 @@ PX4IO::rx_callback(const uint8_t *buffer, size_t bytes_received)
 
 	/* publish raw rc channel values from IO */
 	_input_rc.timestamp = hrt_absolute_time();
+	_input_rc.channel_count = rep->channel_count;
 	for (int i = 0; i < rep->channel_count; i++)
 	{
 		_input_rc.values[i] = rep->rc_channel[i];
diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c
index 374280d28..604dba05c 100644
--- a/apps/gps/mtk.c
+++ b/apps/gps/mtk.c
@@ -418,10 +418,11 @@ void *mtk_watchdog_loop(void *args)
 				mtk_gps->satellite_info_available = 0;
 				// global_data_send_subsystem_info(&mtk_present_enabled_healthy);
 				mavlink_log_info(mavlink_fd, "[gps] MTK custom binary module found, status ok\n");
-				mtk_healthy = true;
-				mtk_fail_count = 0;
-				once_ok = true;
 			}
+
+			mtk_healthy = true;
+			mtk_fail_count = 0;
+			once_ok = true;
 		}
 
 		usleep(MTK_WATCHDOG_WAIT_TIME_MICROSECONDS);
diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c
index 03ae622a1..21e917bf8 100644
--- a/apps/gps/ubx.c
+++ b/apps/gps/ubx.c
@@ -272,7 +272,8 @@ int ubx_parse(uint8_t b,  char *gps_rx_buffer)
 
 						ubx_gps->timestamp = hrt_absolute_time();
 						ubx_gps->counter++;
-
+						ubx_gps->s_variance = packet->sAcc;
+						ubx_gps->p_variance = packet->pAcc;
 
 						//pthread_mutex_lock(ubx_mutex);
 						ubx_state->last_message_timestamps[NAV_SOL - 1] = hrt_absolute_time();
@@ -785,10 +786,6 @@ void *ubx_watchdog_loop(void *args)
 			sleep(1);
 
 		} else {
-			/* gps healthy */
-			ubx_success_count++;
-			ubx_healthy = true;
-			ubx_fail_count = 0;
 
 			if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
 				//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);
@@ -799,6 +796,10 @@ void *ubx_watchdog_loop(void *args)
 				once_ok = true;
 			}
 
+			/* gps healthy */
+			ubx_success_count++;
+			ubx_healthy = true;
+			ubx_fail_count = 0;
 		}
 
 		usleep(UBX_WATCHDOG_WAIT_TIME_MICROSECONDS);
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 2527e0b0f..991bbfbab 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -163,8 +163,8 @@ set_hil_on_off(bool hil_enabled)
 			/* 20 Hz */
 			hil_rate_interval = 50;
 		} else {
-			/* 100 Hz */
-			hil_rate_interval = 10;
+			/* 200 Hz */
+			hil_rate_interval = 5;
 		}
 
 		orb_set_interval(mavlink_subs.spa_sub, hil_rate_interval);
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index 7d2f6afba..eed72125a 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -488,23 +488,25 @@ int sdlog_thread_main(int argc, char *argv[]) {
 		orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf.gps_pos);
 		orb_copy(ORB_ID(vehicle_local_position), subs.local_pos_sub, &buf.local_pos);
 		orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
+		orb_copy(ORB_ID(vehicle_attitude), subs.att_sub, &buf.att);
 
 		#pragma pack(push, 1)
 		struct {
-			uint64_t timestamp;
-			float gyro[3];
-			float accel[3];
-			float mag[3];
-			float baro;
-			float baro_alt;
-			float baro_temp;
-			float control[4];
-
-			float actuators[8];
-			float vbat;
-			float adc[3];
-			float local_pos[3];
-			int32_t gps_pos[3];
+			uint64_t timestamp; //[us]
+			float gyro[3]; //[rad/s]
+			float accel[3]; //[m/s^2]
+			float mag[3]; //[gauss]
+			float baro; //pressure [millibar]
+			float baro_alt; //altitude above MSL [meter]
+			float baro_temp; //[degree celcius]
+			float control[4]; //roll, pitch, yaw [-1..1], thrust [0..1]
+			float actuators[8]; //motor 1-8, in motor units (PWM: 1000-2000,AR.Drone: 0-512)
+			float vbat; //battery voltage in [volt]
+			float adc[3]; //remaining auxiliary ADC ports [volt]
+			float local_position[3]; //tangent plane mapping into x,y,z [m]
+			int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
+			float attitude[3]; //pitch, roll, yaw [rad]
+			float rotMatrix[9]; //unitvectors
 		} sysvector = {
 			.timestamp = buf.raw.timestamp,
 			.gyro = {buf.raw.gyro_rad_s[0], buf.raw.gyro_rad_s[1], buf.raw.gyro_rad_s[2]},
@@ -518,14 +520,16 @@ int sdlog_thread_main(int argc, char *argv[]) {
 					buf.act_outputs.output[4], buf.act_outputs.output[5], buf.act_outputs.output[6], buf.act_outputs.output[7]},
 			.vbat = buf.raw.battery_voltage_v,
 			.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
-			.local_pos = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
-			.gps_pos = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}
+			.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
+			.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
+			.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
+			.rotMatrix = {buf.att.R[0][0], buf.att.R[0][1], buf.att.R[0][2], buf.att.R[1][0], buf.att.R[1][1], buf.att.R[1][2], buf.att.R[2][0], buf.att.R[2][1], buf.att.R[2][2]}
 		};
 		#pragma pack(pop)
 
 		sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
 
-		usleep(10000);
+		usleep(10000);   //10000 corresponds in reality to ca. 76 Hz
 	}
 
 	fsync(sysvector_file);
@@ -602,3 +606,4 @@ int file_copy(const char* file_old, const char* file_new)
    return ret;
 }
 
+
diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp
index 675a8602a..7c1503f0d 100644
--- a/apps/sensors/sensors.cpp
+++ b/apps/sensors/sensors.cpp
@@ -93,6 +93,8 @@
 #define BAT_VOL_LOWPASS_2 0.01f
 #define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f
 
+#define PPM_INPUT_TIMEOUT_INTERVAL	50000 /**< 50 ms timeout / 20 Hz */
+
 /**
  * Sensor app start / stop handling function
  *
@@ -865,8 +867,10 @@ Sensors::ppm_poll()
 	struct rc_input_values raw;
 
 	raw.timestamp = ppm_last_valid_decode;
+	/* we are accepting this message */
+	_ppm_last_valid = ppm_last_valid_decode;
 
-	if (ppm_decoded_channels > 1) {
+	if (ppm_decoded_channels > 2 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
 
 		for (int i = 0; i < ppm_decoded_channels; i++) {
 			raw.values[i] = ppm_buffer[i];
diff --git a/apps/systemlib/perf_counter.c b/apps/systemlib/perf_counter.c
index e6c49d432..ff15ef479 100644
--- a/apps/systemlib/perf_counter.c
+++ b/apps/systemlib/perf_counter.c
@@ -74,6 +74,20 @@ struct perf_ctr_elapsed {
 };
 
 /**
+ * PC_INTERVAL counter.
+ */
+struct perf_ctr_interval {
+	struct perf_ctr_header	hdr;
+	uint64_t		event_count;
+	uint64_t		time_event;
+	uint64_t		time_first;
+	uint64_t		time_last;
+	uint64_t		time_least;
+	uint64_t		time_most;
+
+};
+
+/**
  * List of all known counters.
  */
 static sq_queue_t	perf_counters;
@@ -93,6 +107,10 @@ perf_alloc(enum perf_counter_type type, const char *name)
 		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_elapsed), 1);
 		break;
 
+	case PC_INTERVAL:
+		ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1);
+		break;
+
 	default:
 		break;
 	}
@@ -127,6 +145,32 @@ perf_count(perf_counter_t handle)
 		((struct perf_ctr_count *)handle)->event_count++;
 		break;
 
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+		hrt_abstime now = hrt_absolute_time();
+
+		switch (pci->event_count) {
+		case 0:
+			pci->time_first = now;
+			break;
+		case 1:
+			pci->time_least = now - pci->time_last;
+			pci->time_most = now - pci->time_last;
+			break;
+		default: {
+			hrt_abstime interval = now - pci->time_last;
+			if (interval < pci->time_least)
+				pci->time_least = interval;
+			if (interval > pci->time_most)
+				pci->time_most = interval;
+			break;
+		}
+		}
+		pci->time_last = now;
+		pci->event_count++;
+		break;
+	}
+
 	default:
 		break;
 	}
@@ -187,13 +231,29 @@ perf_print_counter(perf_counter_t handle)
 		       ((struct perf_ctr_count *)handle)->event_count);
 		break;
 
-	case PC_ELAPSED:
+	case PC_ELAPSED: {
+		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+
 		printf("%s: %llu events, %lluus elapsed, min %lluus max %lluus\n",
 		       handle->name,
-		       ((struct perf_ctr_elapsed *)handle)->event_count,
-		       ((struct perf_ctr_elapsed *)handle)->time_total,
-		       ((struct perf_ctr_elapsed *)handle)->time_least,
-		       ((struct perf_ctr_elapsed *)handle)->time_most);
+		       pce->event_count,
+		       pce->time_total,
+		       pce->time_least,
+		       pce->time_most);
+		break;
+	}
+
+	case PC_INTERVAL: {
+		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+
+		printf("%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		       handle->name,
+		       pci->event_count,
+		       (pci->time_last - pci->time_first) / pci->event_count,
+		       pci->time_least,
+		       pci->time_most);
+		break;
+	}
 
 	default:
 		break;
diff --git a/apps/systemlib/perf_counter.h b/apps/systemlib/perf_counter.h
index 5dc441056..6e6c80d5b 100644
--- a/apps/systemlib/perf_counter.h
+++ b/apps/systemlib/perf_counter.h
@@ -44,7 +44,8 @@
  */
 enum perf_counter_type {
 	PC_COUNT,		/**< count the number of times an event occurs */
-	PC_ELAPSED		/**< measure the time elapsed performing an event */
+	PC_ELAPSED,		/**< measure the time elapsed performing an event */
+	PC_INTERVAL		/**< measure the interval between instances of an event */
 };
 
 struct perf_ctr_header;
diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h
index 8e55ef148..ebd83e1a9 100644
--- a/apps/uORB/topics/vehicle_gps_position.h
+++ b/apps/uORB/topics/vehicle_gps_position.h
@@ -65,6 +65,8 @@ struct vehicle_gps_position_s
 	uint16_t counter_pos_valid;		/**< is only increased when new lat/lon/alt information was added   */
 	uint16_t eph;				/**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME   */
 	uint16_t epv;				/**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535   */
+	float s_variance;			// XXX testing
+	float p_variance;			// XXX testing
 	uint16_t vel;				/**< GPS ground speed (m/s * 100). If unknown, set to: 65535   */
 	float vel_n;				/**< GPS ground speed in m/s */
 	float vel_e;				/**< GPS ground speed in m/s */