Updated C files for attitude estimator

28 files changed, 37 insertions, 34 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

  *

  */