Updated C files for attitude estimator

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 d7f448861..d04556e12 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 321e6874d..bfbb3be01 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 8207aa5c5..56f02b4c8 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 abf1519a6..b72256a09 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 20186f183..efb465b25 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 458ddb9eb..d0bf625f0 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 a939eee20..1ad84575f 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 599a16884..bd83cc168 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 49f655ece..c88869453 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 844d8138f..92e3a884d 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 e54d52a38..522f6e285 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 a4a2a4c82..bb92fb242 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 aece401c2..e67071dce 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 e715ae1c3..c07a1bc97 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 532ba4397..8f05ef146 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 ceb90b6cf..ca525c600 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 2fcaf8cb6..d098162d5 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 a58faaa26..e807afcc8 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 8d8e4e110..97509a0c0 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 479503ae3..63294717f 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 42b3af501..f1bb71c87 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 20d8c7f05..6d32d51b5 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 d29aea34b..50581f34d 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 a14b6170b..12d8734f5 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 4cad63ada..419edf01c 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 bd04f52e6..42ff21d39 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 67b3ba205..60128156e 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 6feb2e1a9..2709915e9 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 011a7be97..578bcd875 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 7eca3031d..f9c06099e 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 b99c6652c..f1c1f9a23 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;