From 8a11f76994f74e4b38e861d559b305c707d78190 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 21 Sep 2012 14:42:57 +0200
Subject: [PATCH] Updated C files for attitude estimator

---
 .../attitude_estimator_ekf_main.c               | 17 ++++++++++-------
 .../codegen/attitudeKalmanfilter.c              |  2 +-
 .../codegen/attitudeKalmanfilter.h              |  2 +-
 .../codegen/attitudeKalmanfilter_initialize.c   |  2 +-
 .../codegen/attitudeKalmanfilter_initialize.h   |  2 +-
 .../codegen/attitudeKalmanfilter_terminate.c    |  2 +-
 .../codegen/attitudeKalmanfilter_terminate.h    |  2 +-
 .../codegen/attitudeKalmanfilter_types.h        |  2 +-
 apps/attitude_estimator_ekf/codegen/cross.c     |  2 +-
 apps/attitude_estimator_ekf/codegen/cross.h     |  2 +-
 apps/attitude_estimator_ekf/codegen/diag.c      |  2 +-
 apps/attitude_estimator_ekf/codegen/diag.h      |  2 +-
 apps/attitude_estimator_ekf/codegen/eye.c       |  2 +-
 apps/attitude_estimator_ekf/codegen/eye.h       |  2 +-
 apps/attitude_estimator_ekf/codegen/find.c      |  2 +-
 apps/attitude_estimator_ekf/codegen/find.h      |  2 +-
 apps/attitude_estimator_ekf/codegen/mrdivide.c  |  2 +-
 apps/attitude_estimator_ekf/codegen/mrdivide.h  |  2 +-
 apps/attitude_estimator_ekf/codegen/norm.c      |  2 +-
 apps/attitude_estimator_ekf/codegen/norm.h      |  2 +-
 apps/attitude_estimator_ekf/codegen/rtGetInf.c  |  2 +-
 apps/attitude_estimator_ekf/codegen/rtGetInf.h  |  2 +-
 apps/attitude_estimator_ekf/codegen/rtGetNaN.c  |  2 +-
 apps/attitude_estimator_ekf/codegen/rtGetNaN.h  |  2 +-
 .../attitude_estimator_ekf/codegen/rt_defines.h |  2 +-
 .../codegen/rt_nonfinite.c                      |  2 +-
 .../codegen/rt_nonfinite.h                      |  2 +-
 apps/attitude_estimator_ekf/codegen/rtwtypes.h  |  2 +-
 apps/commander/commander.c                      |  6 ++++++
 apps/mavlink/mavlink.c                          | 12 ++++++------
 apps/sdlog/sdlog.c                              |  4 +++-
 31 files changed, 52 insertions(+), 41 deletions(-)

diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index d7f448861c..d04556e125 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -70,7 +70,7 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
 // #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
 // #define REPROJECTION_COUNTER_LIMIT 125
 
-static unsigned int loop_interval_alarm = 4500;	// loop interval in microseconds
+static unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 static float dt = 1;
 /* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */
@@ -205,6 +205,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
+	/* rate-limit raw data updates to 200Hz */
+	orb_set_interval(sub_raw, 5);
+
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
@@ -222,11 +225,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 		};
 		int ret = poll(fds, 1, 1000);
 
-		/* check for a timeout */
-		if (ret == 0) {
-			/* */
-
-			/* update successful, copy data on every 2nd run and execute filter */
+		if (ret < 0) {
+			/* XXX this is seriously bad - should be an emergency */
+		} else if (ret == 0) {
+			/* XXX this means no sensor data - should be critical or emergency */
+			printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n");
 		} else {
 
 			orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
@@ -256,7 +259,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			if (time_elapsed > loop_interval_alarm) {
 				//TODO: add warning, cpu overload here
 				if (overloadcounter == 20) {
-					printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
+					printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
 					overloadcounter = 0;
 				}
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
index 321e6874d0..bfbb3be012 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:42 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
index 8207aa5c53..56f02b4c81 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
index abf1519a67..b72256a09e 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_initialize'
  *
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
index 20186f183c..efb465b255 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_initialize'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
index 458ddb9eb0..d0bf625f09 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_terminate'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
index a939eee200..1ad84575f8 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_terminate'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
index 599a168848..bd83cc168a 100755
--- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
+++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:42 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c
index 49f655ece8..c888694536 100755
--- a/apps/attitude_estimator_ekf/codegen/cross.c
+++ b/apps/attitude_estimator_ekf/codegen/cross.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'cross'
  *
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h
index 844d8138fd..92e3a884d0 100755
--- a/apps/attitude_estimator_ekf/codegen/cross.h
+++ b/apps/attitude_estimator_ekf/codegen/cross.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'cross'
  *
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c
index e54d52a384..522f6e2851 100755
--- a/apps/attitude_estimator_ekf/codegen/diag.c
+++ b/apps/attitude_estimator_ekf/codegen/diag.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'diag'
  *
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h
index a4a2a4c829..bb92fb2424 100755
--- a/apps/attitude_estimator_ekf/codegen/diag.h
+++ b/apps/attitude_estimator_ekf/codegen/diag.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'diag'
  *
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c
index aece401c25..e67071dcea 100755
--- a/apps/attitude_estimator_ekf/codegen/eye.c
+++ b/apps/attitude_estimator_ekf/codegen/eye.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'eye'
  *
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h
index e715ae1c39..c07a1bc970 100755
--- a/apps/attitude_estimator_ekf/codegen/eye.h
+++ b/apps/attitude_estimator_ekf/codegen/eye.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'eye'
  *
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/find.c b/apps/attitude_estimator_ekf/codegen/find.c
index 532ba43970..8f05ef146e 100755
--- a/apps/attitude_estimator_ekf/codegen/find.c
+++ b/apps/attitude_estimator_ekf/codegen/find.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'find'
  *
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/find.h b/apps/attitude_estimator_ekf/codegen/find.h
index ceb90b6cf6..ca525c6009 100755
--- a/apps/attitude_estimator_ekf/codegen/find.h
+++ b/apps/attitude_estimator_ekf/codegen/find.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'find'
  *
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c
index 2fcaf8cb67..d098162d54 100755
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.c
+++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'mrdivide'
  *
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h
index a58faaa262..e807afcc8b 100755
--- a/apps/attitude_estimator_ekf/codegen/mrdivide.h
+++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'mrdivide'
  *
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c
index 8d8e4e1109..97509a0c0c 100755
--- a/apps/attitude_estimator_ekf/codegen/norm.c
+++ b/apps/attitude_estimator_ekf/codegen/norm.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'norm'
  *
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h
index 479503ae36..63294717f4 100755
--- a/apps/attitude_estimator_ekf/codegen/norm.h
+++ b/apps/attitude_estimator_ekf/codegen/norm.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'norm'
  *
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c
index 42b3af501f..f1bb71c873 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c
+++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:45 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h
index 20d8c7f058..6d32d51b56 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h
+++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:45 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
index d29aea34b6..50581f34df 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
+++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:45 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
index a14b6170b8..12d8734f5b 100755
--- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
+++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h
index 4cad63ada3..419edf01c9 100755
--- a/apps/attitude_estimator_ekf/codegen/rt_defines.h
+++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
index bd04f52e66..42ff21d39e 100755
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
+++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
index 67b3ba205d..60128156ea 100755
--- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
+++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h
index 6feb2e1a99..2709915e95 100755
--- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h
+++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h
@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
  *
  */
 
diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 011a7be97b..578bcd8752 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -1365,12 +1365,18 @@ int commander_thread_main(int argc, char *argv[])
 				//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
 
 				if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
+					current_status.flag_control_attitude_enabled = true;
+					current_status.flag_control_rates_enabled = false;
 					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
 
 				} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
+					current_status.flag_control_attitude_enabled = true;
+					current_status.flag_control_rates_enabled = false;
 					update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
 
 				} else {
+					current_status.flag_control_attitude_enabled = true;
+					current_status.flag_control_rates_enabled = false;
 					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
 				}
 
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 7eca3031d9..f9c06099e9 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -1077,8 +1077,8 @@ static void *uorb_receiveloop(void *arg)
 			if (fds[ifds++].revents & POLLIN) {
 				/* copy local position data into local buffer */
 				orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control);
-				mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll,
-					buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 0);
+				mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll*1000,
+					buf.man_control.pitch*1000, buf.man_control.yaw*1000, buf.man_control.throttle*1000, 0);
 			}
 
 			/* --- ACTUATOR ARMED --- */
@@ -1427,10 +1427,10 @@ void handleMessage(mavlink_message_t *msg)
 			struct manual_control_setpoint_s mc;
 			static orb_advert_t mc_pub = 0;
 
-			mc.roll = man.x;
-			mc.pitch = man.y;
-			mc.yaw = man.r;
-			mc.roll = man.z;
+			mc.roll = man.x*1000;
+			mc.pitch = man.y*1000;
+			mc.yaw = man.r*1000;
+			mc.roll = man.z*1000;
 
 			/* fake RC channels with manual control input from simulator */
 
diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c
index b99c6652c8..f1c1f9a239 100644
--- a/apps/sdlog/sdlog.c
+++ b/apps/sdlog/sdlog.c
@@ -289,6 +289,8 @@ int sdlog_thread_main(int argc, char *argv[]) {
 	/* subscribe to ORB for sensors raw */
 	subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
 	fds[fdsc_count].fd = subs.sensor_sub;
+	/* rate-limit raw data updates to 200Hz */
+	orb_set_interval(subs.sensor_sub, 5);
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
@@ -459,7 +461,7 @@ int sdlog_thread_main(int argc, char *argv[]) {
 
 		sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector));
 
-		usleep(4900);
+		usleep(10000);
 	}
 
 	unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes;