Merge branch 'master' into fw_control

.gitignore

@@ -1,5 +1,5 @@
 .built
-.context
+*.context
 .depend
 .config
 .version
@@ -10,6 +10,7 @@ apps/namedapp/namedapp_proto.h
 Make.dep
 *.o
 *.a
+*~
 Images/*.bin
 Images/*.px4
 nuttx/Make.defs

ROMFS/scripts/rcS

@@ -20,6 +20,11 @@
 set MODE autostart
 set USB autoconnect
 
+#
+# Start playing the startup tune
+#
+tone_alarm start
+
 #
 # Try to mount the microSD card.
 #
@@ -31,9 +36,6 @@ else
 	echo "[init] no microSD card found"
 fi
 
-usleep 500
-
-
 #
 # Look for an init script on the microSD card.
 #

apps/ardrone_interface/ardrone_interface.c

@@ -286,7 +286,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 
 	/* close uarts */
 	close(ardrone_write);
-	//ar_multiplexing_deinit(gpios);
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
 	ardrone_write = ardrone_open_uart(device, &uart_config_original);

apps/attitude_estimator_ekf/Makefile

@@ -40,6 +40,7 @@ CSRCS		 = attitude_estimator_ekf_main.c \
 		   codegen/eye.c \
 		   codegen/attitudeKalmanfilter.c \
 		   codegen/mrdivide.c \
+		   codegen/rdivide.c \
 		   codegen/attitudeKalmanfilter_initialize.c \
 		   codegen/attitudeKalmanfilter_terminate.c \
 		   codegen/rt_nonfinite.c \
@@ -47,8 +48,9 @@ CSRCS		 = attitude_estimator_ekf_main.c \
 		   codegen/rtGetNaN.c \
 		   codegen/norm.c \
 		   codegen/diag.c \
-		   codegen/cross.c \
-		   codegen/power.c
+		   codegen/power.c \
+		   codegen/cross.c 
+
 
 # XXX this is *horribly* broken
 INCLUDES	+= $(TOPDIR)/../mavlink/include/mavlink

apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c

@@ -71,11 +71,10 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
 
 static unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
-static float dt = 1.0f;
+static float dt = 0.005f;
 /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
-static float z_k[9];					/**< Measurement vector */
-static float x_aposteriori_k[12];		/**< */
-static float x_aposteriori[12];
+static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f};					/**< Measurement vector */
+static float x_aposteriori_k[12];		/**< states */
 static float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
 		0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
 		0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
@@ -88,21 +87,10 @@ static float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0
 		0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
 		0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
 		0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
-				  };
+}; /**< init: diagonal matrix with big values */
 
-static float P_aposteriori[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-				   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-				   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
-				   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,
-				   0,   0,   0,   0,  100.f,  0,   0,   0,   0,   0,   0,   0,
-				   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,   0,
-				   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,   0,
-				   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,   0,
-				   0,   0,   0,   0,   0,   0,   0,   0, 100.f,   0,   0,   0,
-				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f, 100.0f,   0,   0,
-				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
-				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
-				  };	/**< init: diagonal matrix with big values */
+static float x_aposteriori[12];
+static float P_aposteriori[144];
 
 /* output euler angles */
 static float euler[3] = {0.0f, 0.0f, 0.0f};
@@ -110,7 +98,7 @@ static float euler[3] = {0.0f, 0.0f, 0.0f};
 static float Rot_matrix[9] = {1.f,  0,  0,
 		0,  1.f,  0,
 		0,  0,  1.f
-		     };		/**< init: identity matrix */
+};		/**< init: identity matrix */
 
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
@@ -236,7 +224,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 	/* advertise debug value */
 	struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
-	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
+	orb_advert_t pub_dbg = -1;
 
 	/* keep track of sensor updates */
 	uint32_t sensor_last_count[3] = {0, 0, 0};
@@ -250,6 +238,12 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	/* initialize parameter handles */
 	parameters_init(&ekf_param_handles);
 
+	uint64_t start_time = hrt_absolute_time();
+	bool initialized = false;
+
+	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
+	unsigned offset_count = 0;
+
 	/* Main loop*/
 	while (!thread_should_exit) {
 
@@ -282,6 +276,21 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 				/* get latest measurements */
 				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
 
+				if (!initialized) {
+
+					gyro_offsets[0] += raw.gyro_rad_s[0];
+					gyro_offsets[1] += raw.gyro_rad_s[1];
+					gyro_offsets[2] += raw.gyro_rad_s[2];
+					offset_count++;
+
+					if (hrt_absolute_time() - start_time > 3000000LL) {
+						initialized = true;
+						gyro_offsets[0] /= offset_count;
+						gyro_offsets[1] /= offset_count;
+						gyro_offsets[2] /= offset_count;
+					}
+				} else {
+
 					/* Calculate data time difference in seconds */
 					dt = (raw.timestamp - last_measurement) / 1000000.0f;
 					last_measurement = raw.timestamp;
@@ -295,9 +304,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
-				z_k[0] =  raw.gyro_rad_s[0];
-				z_k[1] =  raw.gyro_rad_s[1];
-				z_k[2] =  raw.gyro_rad_s[2];
+					z_k[0] =  raw.gyro_rad_s[0] - gyro_offsets[0];
+					z_k[1] =  raw.gyro_rad_s[1] - gyro_offsets[1];
+					z_k[2] =  raw.gyro_rad_s[2] - gyro_offsets[2];
 
 					/* update accelerometer measurements */
 					if (sensor_last_count[1] != raw.accelerometer_counter) {
@@ -341,7 +350,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					static bool const_initialized = false;
 
 					/* initialize with good values once we have a reasonable dt estimate */
-				if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) 
+					if (!const_initialized && dt < 0.05 && dt > 0.005)
 					{
 						dt = 0.005f;
 						parameters_update(&ekf_param_handles, &ekf_params);
@@ -367,16 +376,19 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 						continue;
 					}
 
-				dt = 0.004f;
-
 					uint64_t timing_start = hrt_absolute_time();
-				// attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
-				// 	Rot_matrix, x_aposteriori, P_aposteriori);
+					
 					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
 							euler, Rot_matrix, x_aposteriori, P_aposteriori);
-				/* swap values for next iteration */
+					/* swap values for next iteration, check for fatal inputs */
+					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {
 						memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
 						memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
+					} else {
+						/* due to inputs or numerical failure the output is invalid, skip it */
+						continue;
+					}
+					
 					uint64_t timing_diff = hrt_absolute_time() - timing_start;
 
 					// /* print rotation matrix every 200th time */
@@ -404,7 +416,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					// 		sprintf(name, "xapo #%d", i);
 					// 		memcpy(dbg.key, name, sizeof(dbg.key));
 					// 		dbg.value = x_aposteriori[i];
+					// if (pub_dbg < 0) {
+							// pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
+					// } else {
 					// 		orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
+					// }
 
 					printcounter++;
 
@@ -418,11 +434,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					att.pitch = euler[1];
 					att.yaw = euler[2];
 
-				// XXX replace with x_apo after fix to filter
-				att.rollspeed = raw.gyro_rad_s[0]; //x_aposteriori[0];
-				att.pitchspeed = raw.gyro_rad_s[1]; //x_aposteriori[1];
-				att.yawspeed = raw.gyro_rad_s[2]; //x_aposteriori[2];
-
+					att.rollspeed = x_aposteriori[0];
+					att.pitchspeed = x_aposteriori[1];
+					att.yawspeed = x_aposteriori[2];
+					//att.yawspeed =z_k[2] ;
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
 
@@ -434,6 +449,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 					orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
 				}
 			}
+		}
 
 		loopcounter++;
 	}

apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -61,13 +61,13 @@ PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f);
 PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f);
 
 /* gyro measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-2f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-2f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-2f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R0, 0.01f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R1, 0.01f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R2, 0.01f);
 /* accelerometer measurement noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e-1f);
-PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e1f);
 /* magnetometer measurement noise */
 PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
 PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -11,7 +11,7 @@
 #define __ATTITUDEKALMANFILTER_H__
 /* Include files */
 #include <math.h>
-#include <stdio.h>
+#include <stddef.h>
 #include <stdlib.h>
 #include <string.h>
 #include "rt_defines.h"

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_initialize'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_initialize'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -11,7 +11,7 @@
 #define __ATTITUDEKALMANFILTER_INITIALIZE_H__
 /* Include files */
 #include <math.h>
-#include <stdio.h>
+#include <stddef.h>
 #include <stdlib.h>
 #include <string.h>
 #include "rt_defines.h"

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_terminate'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter_terminate'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -11,7 +11,7 @@
 #define __ATTITUDEKALMANFILTER_TERMINATE_H__
 /* Include files */
 #include <math.h>
-#include <stdio.h>
+#include <stddef.h>
 #include <stdlib.h>
 #include <string.h>
 #include "rt_defines.h"

apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/cross.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'cross'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/cross.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'cross'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -11,7 +11,7 @@
 #define __CROSS_H__
 /* Include files */
 #include <math.h>
-#include <stdio.h>
+#include <stddef.h>
 #include <stdlib.h>
 #include <string.h>
 #include "rt_defines.h"

apps/attitude_estimator_ekf/codegen/diag.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'diag'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -30,7 +30,7 @@
 void b_diag(const real32_T v[9], real32_T d[81])
 {
   int32_T j;
-  memset((void *)&d[0], 0, 81U * sizeof(real32_T));
+  memset(&d[0], 0, 81U * sizeof(real32_T));
   for (j = 0; j < 9; j++) {
     d[j + 9 * j] = v[j];
   }
@@ -57,7 +57,7 @@ void c_diag(const real32_T v[3], real32_T d[9])
 void d_diag(const real32_T v[6], real32_T d[36])
 {
   int32_T j;
-  memset((void *)&d[0], 0, 36U * sizeof(real32_T));
+  memset(&d[0], 0, 36U * sizeof(real32_T));
   for (j = 0; j < 6; j++) {
     d[j + 6 * j] = v[j];
   }
@@ -69,7 +69,7 @@ void d_diag(const real32_T v[6], real32_T d[36])
 void diag(const real32_T v[12], real32_T d[144])
 {
   int32_T j;
-  memset((void *)&d[0], 0, 144U * sizeof(real32_T));
+  memset(&d[0], 0, 144U * sizeof(real32_T));
   for (j = 0; j < 12; j++) {
     d[j + 12 * j] = v[j];
   }

apps/attitude_estimator_ekf/codegen/diag.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'diag'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -11,7 +11,7 @@
 #define __DIAG_H__
 /* Include files */
 #include <math.h>
-#include <stdio.h>
+#include <stddef.h>
 #include <stdlib.h>
 #include <string.h>
 #include "rt_defines.h"

apps/attitude_estimator_ekf/codegen/eye.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'eye'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -30,7 +30,7 @@
 void b_eye(real_T I[144])
 {
   int32_T i;
-  memset((void *)&I[0], 0, 144U * sizeof(real_T));
+  memset(&I[0], 0, 144U * sizeof(real_T));
   for (i = 0; i < 12; i++) {
     I[i + 12 * i] = 1.0;
   }
@@ -42,7 +42,7 @@ void b_eye(real_T I[144])
 void eye(real_T I[9])
 {
   int32_T i;
-  memset((void *)&I[0], 0, 9U * sizeof(real_T));
+  memset(&I[0], 0, 9U * sizeof(real_T));
   for (i = 0; i < 3; i++) {
     I[i + 3 * i] = 1.0;
   }

apps/attitude_estimator_ekf/codegen/eye.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'eye'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -11,7 +11,7 @@
 #define __EYE_H__
 /* Include files */
 #include <math.h>
-#include <stdio.h>
+#include <stddef.h>
 #include <stdlib.h>
 #include <string.h>
 #include "rt_defines.h"

apps/attitude_estimator_ekf/codegen/mrdivide.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'mrdivide'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -29,9 +29,9 @@
  */
 void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
 {
+  real32_T b_A[9];
   int32_T rtemp;
   int32_T k;
-  real32_T b_A[9];
   real32_T b_B[36];
   int32_T r1;
   int32_T r2;
@@ -54,15 +54,15 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
   r1 = 0;
   r2 = 1;
   r3 = 2;
-  maxval = (real32_T)fabsf(b_A[0]);
-  a21 = (real32_T)fabsf(b_A[1]);
+  maxval = (real32_T)fabs(b_A[0]);
+  a21 = (real32_T)fabs(b_A[1]);
   if (a21 > maxval) {
     maxval = a21;
     r1 = 1;
     r2 = 0;
   }
 
-  if ((real32_T)fabsf(b_A[2]) > maxval) {
+  if ((real32_T)fabs(b_A[2]) > maxval) {
     r1 = 2;
     r2 = 1;
     r3 = 0;
@@ -74,7 +74,7 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
   b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
   b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
   b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
-  if ((real32_T)fabsf(b_A[3 + r3]) > (real32_T)fabsf(b_A[3 + r2])) {
+  if ((real32_T)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
     rtemp = r2;
     r2 = r3;
     r3 = rtemp;
@@ -107,51 +107,46 @@ void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36])
  */
 void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
 {
-  int32_T jy;
-  int32_T iy;
   real32_T b_A[36];
   int8_T ipiv[6];
+  int32_T i3;
+  int32_T iy;
   int32_T j;
-  int32_T mmj;
-  int32_T jj;
-  int32_T jp1j;
   int32_T c;
   int32_T ix;
   real32_T temp;
   int32_T k;
   real32_T s;
-  int32_T loop_ub;
+  int32_T jy;
+  int32_T ijA;
   real32_T Y[72];
-  for (jy = 0; jy < 6; jy++) {
+  for (i3 = 0; i3 < 6; i3++) {
     for (iy = 0; iy < 6; iy++) {
-      b_A[iy + 6 * jy] = B[jy + 6 * iy];
+      b_A[iy + 6 * i3] = B[i3 + 6 * iy];
     }
 
-    ipiv[jy] = (int8_T)(1 + jy);
+    ipiv[i3] = (int8_T)(1 + i3);
   }
 
   for (j = 0; j < 5; j++) {
-    mmj = -j;
-    jj = j * 7;
-    jp1j = jj + 1;
-    c = mmj + 6;
-    jy = 0;
-    ix = jj;
-    temp = (real32_T)fabsf(b_A[jj]);
-    for (k = 2; k <= c; k++) {
+    c = j * 7;
+    iy = 0;
+    ix = c;
+    temp = (real32_T)fabs(b_A[c]);
+    for (k = 2; k <= 6 - j; k++) {
       ix++;
-      s = (real32_T)fabsf(b_A[ix]);
+      s = (real32_T)fabs(b_A[ix]);
       if (s > temp) {
-        jy = k - 1;
+        iy = k - 1;
         temp = s;
       }
     }
 
-    if ((real_T)b_A[jj + jy] != 0.0) {
-      if (jy != 0) {
-        ipiv[j] = (int8_T)((j + jy) + 1);
+    if (b_A[c + iy] != 0.0F) {
+      if (iy != 0) {
+        ipiv[j] = (int8_T)((j + iy) + 1);
         ix = j;
-        iy = j + jy;
+        iy += j;
         for (k = 0; k < 6; k++) {
           temp = b_A[ix];
           b_A[ix] = b_A[iy];
@@ -161,42 +156,42 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
         }
       }
 
-      loop_ub = (jp1j + mmj) + 5;
-      for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
-        b_A[iy] /= b_A[jj];
+      i3 = (c - j) + 6;
+      for (jy = c + 1; jy + 1 <= i3; jy++) {
+        b_A[jy] /= b_A[c];
       }
     }
 
-    c = 5 - j;
-    jy = jj + 6;
-    for (iy = 1; iy <= c; iy++) {
-      if ((real_T)b_A[jy] != 0.0) {
-        temp = b_A[jy] * -1.0F;
-        ix = jp1j;
-        loop_ub = (mmj + jj) + 12;
-        for (k = 7 + jj; k + 1 <= loop_ub; k++) {
-          b_A[k] += b_A[ix] * temp;
+    iy = c;
+    jy = c + 6;
+    for (k = 1; k <= 5 - j; k++) {
+      temp = b_A[jy];
+      if (b_A[jy] != 0.0F) {
+        ix = c + 1;
+        i3 = (iy - j) + 12;
+        for (ijA = 7 + iy; ijA + 1 <= i3; ijA++) {
+          b_A[ijA] += b_A[ix] * -temp;
           ix++;
         }
       }
 
       jy += 6;
-      jj += 6;
+      iy += 6;
     }
   }
 
-  for (jy = 0; jy < 12; jy++) {
+  for (i3 = 0; i3 < 12; i3++) {
     for (iy = 0; iy < 6; iy++) {
-      Y[iy + 6 * jy] = A[jy + 12 * iy];
+      Y[iy + 6 * i3] = A[i3 + 12 * iy];
     }
   }
 
-  for (iy = 0; iy < 6; iy++) {
-    if (ipiv[iy] != iy + 1) {
+  for (jy = 0; jy < 6; jy++) {
+    if (ipiv[jy] != jy + 1) {
       for (j = 0; j < 12; j++) {
-        temp = Y[iy + 6 * j];
-        Y[iy + 6 * j] = Y[(ipiv[iy] + 6 * j) - 1];
-        Y[(ipiv[iy] + 6 * j) - 1] = temp;
+        temp = Y[jy + 6 * j];
+        Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
+        Y[(ipiv[jy] + 6 * j) - 1] = temp;
       }
     }
   }
@@ -204,10 +199,10 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
   for (j = 0; j < 12; j++) {
     c = 6 * j;
     for (k = 0; k < 6; k++) {
-      jy = 6 * k;
-      if ((real_T)Y[k + c] != 0.0) {
-        for (iy = k + 2; iy < 7; iy++) {
-          Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
+      iy = 6 * k;
+      if (Y[k + c] != 0.0F) {
+        for (jy = k + 2; jy < 7; jy++) {
+          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
         }
       }
     }
@@ -216,19 +211,19 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
   for (j = 0; j < 12; j++) {
     c = 6 * j;
     for (k = 5; k > -1; k += -1) {
-      jy = 6 * k;
-      if ((real_T)Y[k + c] != 0.0) {
-        Y[k + c] /= b_A[k + jy];
-        for (iy = 0; iy + 1 <= k; iy++) {
-          Y[iy + c] -= Y[k + c] * b_A[iy + jy];
+      iy = 6 * k;
+      if (Y[k + c] != 0.0F) {
+        Y[k + c] /= b_A[k + iy];
+        for (jy = 0; jy + 1 <= k; jy++) {
+          Y[jy + c] -= Y[k + c] * b_A[jy + iy];
         }
       }
     }
   }
 
-  for (jy = 0; jy < 6; jy++) {
+  for (i3 = 0; i3 < 6; i3++) {
     for (iy = 0; iy < 12; iy++) {
-      y[iy + 12 * jy] = Y[jy + 6 * iy];
+      y[iy + 12 * i3] = Y[i3 + 6 * iy];
     }
   }
 }
@@ -238,51 +233,46 @@ void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
  */
 void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
 {
-  int32_T jy;
-  int32_T iy;
   real32_T b_A[81];
   int8_T ipiv[9];
+  int32_T i2;
+  int32_T iy;
   int32_T j;
-  int32_T mmj;
-  int32_T jj;
-  int32_T jp1j;
   int32_T c;
   int32_T ix;
   real32_T temp;
   int32_T k;
   real32_T s;
-  int32_T loop_ub;
+  int32_T jy;
+  int32_T ijA;
   real32_T Y[108];
-  for (jy = 0; jy < 9; jy++) {
+  for (i2 = 0; i2 < 9; i2++) {
     for (iy = 0; iy < 9; iy++) {
-      b_A[iy + 9 * jy] = B[jy + 9 * iy];
+      b_A[iy + 9 * i2] = B[i2 + 9 * iy];
     }
 
-    ipiv[jy] = (int8_T)(1 + jy);
+    ipiv[i2] = (int8_T)(1 + i2);
   }
 
   for (j = 0; j < 8; j++) {
-    mmj = -j;
-    jj = j * 10;
-    jp1j = jj + 1;
-    c = mmj + 9;
-    jy = 0;
-    ix = jj;
-    temp = (real32_T)fabsf(b_A[jj]);
-    for (k = 2; k <= c; k++) {
+    c = j * 10;
+    iy = 0;
+    ix = c;
+    temp = (real32_T)fabs(b_A[c]);
+    for (k = 2; k <= 9 - j; k++) {
       ix++;
-      s = (real32_T)fabsf(b_A[ix]);
+      s = (real32_T)fabs(b_A[ix]);
       if (s > temp) {
-        jy = k - 1;
+        iy = k - 1;
         temp = s;
       }
     }
 
-    if ((real_T)b_A[jj + jy] != 0.0) {
-      if (jy != 0) {
-        ipiv[j] = (int8_T)((j + jy) + 1);
+    if (b_A[c + iy] != 0.0F) {
+      if (iy != 0) {
+        ipiv[j] = (int8_T)((j + iy) + 1);
         ix = j;
-        iy = j + jy;
+        iy += j;
         for (k = 0; k < 9; k++) {
           temp = b_A[ix];
           b_A[ix] = b_A[iy];
@@ -292,42 +282,42 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
         }
       }
 
-      loop_ub = (jp1j + mmj) + 8;
-      for (iy = jp1j; iy + 1 <= loop_ub; iy++) {
-        b_A[iy] /= b_A[jj];
+      i2 = (c - j) + 9;
+      for (jy = c + 1; jy + 1 <= i2; jy++) {
+        b_A[jy] /= b_A[c];
       }
     }
 
-    c = 8 - j;
-    jy = jj + 9;
-    for (iy = 1; iy <= c; iy++) {
-      if ((real_T)b_A[jy] != 0.0) {
-        temp = b_A[jy] * -1.0F;
-        ix = jp1j;
-        loop_ub = (mmj + jj) + 18;
-        for (k = 10 + jj; k + 1 <= loop_ub; k++) {
-          b_A[k] += b_A[ix] * temp;
+    iy = c;
+    jy = c + 9;
+    for (k = 1; k <= 8 - j; k++) {
+      temp = b_A[jy];
+      if (b_A[jy] != 0.0F) {
+        ix = c + 1;
+        i2 = (iy - j) + 18;
+        for (ijA = 10 + iy; ijA + 1 <= i2; ijA++) {
+          b_A[ijA] += b_A[ix] * -temp;
           ix++;
         }
       }
 
       jy += 9;
-      jj += 9;
+      iy += 9;
     }
   }
 
-  for (jy = 0; jy < 12; jy++) {
+  for (i2 = 0; i2 < 12; i2++) {
     for (iy = 0; iy < 9; iy++) {
-      Y[iy + 9 * jy] = A[jy + 12 * iy];
+      Y[iy + 9 * i2] = A[i2 + 12 * iy];
     }
   }
 
-  for (iy = 0; iy < 9; iy++) {
-    if (ipiv[iy] != iy + 1) {
+  for (jy = 0; jy < 9; jy++) {
+    if (ipiv[jy] != jy + 1) {
       for (j = 0; j < 12; j++) {
-        temp = Y[iy + 9 * j];
-        Y[iy + 9 * j] = Y[(ipiv[iy] + 9 * j) - 1];
-        Y[(ipiv[iy] + 9 * j) - 1] = temp;
+        temp = Y[jy + 9 * j];
+        Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
+        Y[(ipiv[jy] + 9 * j) - 1] = temp;
       }
     }
   }
@@ -335,10 +325,10 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
   for (j = 0; j < 12; j++) {
     c = 9 * j;
     for (k = 0; k < 9; k++) {
-      jy = 9 * k;
-      if ((real_T)Y[k + c] != 0.0) {
-        for (iy = k + 2; iy < 10; iy++) {
-          Y[(iy + c) - 1] -= Y[k + c] * b_A[(iy + jy) - 1];
+      iy = 9 * k;
+      if (Y[k + c] != 0.0F) {
+        for (jy = k + 2; jy < 10; jy++) {
+          Y[(jy + c) - 1] -= Y[k + c] * b_A[(jy + iy) - 1];
         }
       }
     }
@@ -347,19 +337,19 @@ void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
   for (j = 0; j < 12; j++) {
     c = 9 * j;
     for (k = 8; k > -1; k += -1) {
-      jy = 9 * k;
-      if ((real_T)Y[k + c] != 0.0) {
-        Y[k + c] /= b_A[k + jy];
-        for (iy = 0; iy + 1 <= k; iy++) {
-          Y[iy + c] -= Y[k + c] * b_A[iy + jy];
+      iy = 9 * k;
+      if (Y[k + c] != 0.0F) {
+        Y[k + c] /= b_A[k + iy];
+        for (jy = 0; jy + 1 <= k; jy++) {
+          Y[jy + c] -= Y[k + c] * b_A[jy + iy];
         }
       }
     }
   }
 
-  for (jy = 0; jy < 9; jy++) {
+  for (i2 = 0; i2 < 9; i2++) {
     for (iy = 0; iy < 12; iy++) {
-      y[iy + 12 * jy] = Y[jy + 9 * iy];
+      y[iy + 12 * i2] = Y[i2 + 9 * iy];
     }
   }
 }

apps/attitude_estimator_ekf/codegen/mrdivide.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'mrdivide'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -11,7 +11,7 @@
 #define __MRDIVIDE_H__
 /* Include files */
 #include <math.h>
-#include <stdio.h>
+#include <stddef.h>
 #include <stdlib.h>
 #include <string.h>
 #include "rt_defines.h"

apps/attitude_estimator_ekf/codegen/norm.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'norm'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -31,21 +31,14 @@ real32_T norm(const real32_T x[3])
 {
   real32_T y;
   real32_T scale;
-  boolean_T firstNonZero;
   int32_T k;
   real32_T absxk;
   real32_T t;
   y = 0.0F;
-  scale = 0.0F;
-  firstNonZero = TRUE;
+  scale = 1.17549435E-38F;
   for (k = 0; k < 3; k++) {
-    if (x[k] != 0.0F) {
-      absxk = (real32_T)fabsf(x[k]);
-      if (firstNonZero) {
-        scale = absxk;
-        y = 1.0F;
-        firstNonZero = FALSE;
-      } else if (scale < absxk) {
+    absxk = (real32_T)fabs(x[k]);
+    if (absxk > scale) {
       t = scale / absxk;
       y = 1.0F + y * t * t;
       scale = absxk;
@@ -54,9 +47,8 @@ real32_T norm(const real32_T x[3])
       y += t * t;
     }
   }
-  }
 
-  return scale * (real32_T)sqrtf(y);
+  return scale * (real32_T)sqrt(y);
 }
 
 /* End of code generation (norm.c) */

apps/attitude_estimator_ekf/codegen/norm.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'norm'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -11,7 +11,7 @@
 #define __NORM_H__
 /* Include files */
 #include <math.h>
-#include <stdio.h>
+#include <stddef.h>
 #include <stdlib.h>
 #include <string.h>
 #include "rt_defines.h"

apps/attitude_estimator_ekf/codegen/power.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'power'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -27,17 +27,17 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1);
 static real32_T rt_powf_snf(real32_T u0, real32_T u1)
 {
   real32_T y;
-  real32_T f0;
   real32_T f1;
+  real32_T f2;
   if (rtIsNaNF(u0) || rtIsNaNF(u1)) {
     y = ((real32_T)rtNaN);
   } else {
-    f0 = (real32_T)fabsf(u0);
-    f1 = (real32_T)fabsf(u1);
+    f1 = (real32_T)fabs(u0);
+    f2 = (real32_T)fabs(u1);
     if (rtIsInfF(u1)) {
-      if (f0 == 1.0F) {
+      if (f1 == 1.0F) {
         y = ((real32_T)rtNaN);
-      } else if (f0 > 1.0F) {
+      } else if (f1 > 1.0F) {
         if (u1 > 0.0F) {
           y = ((real32_T)rtInf);
         } else {
@@ -48,9 +48,9 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1)
       } else {
         y = ((real32_T)rtInf);
       }
-    } else if (f1 == 0.0F) {
+    } else if (f2 == 0.0F) {
       y = 1.0F;
-    } else if (f1 == 1.0F) {
+    } else if (f2 == 1.0F) {
       if (u1 > 0.0F) {
         y = u0;
       } else {
@@ -59,11 +59,11 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1)
     } else if (u1 == 2.0F) {
       y = u0 * u0;
     } else if ((u1 == 0.5F) && (u0 >= 0.0F)) {
-      y = (real32_T)sqrtf(u0);
-    } else if ((u0 < 0.0F) && (u1 > (real32_T)floorf(u1))) {
+      y = (real32_T)sqrt(u0);
+    } else if ((u0 < 0.0F) && (u1 > (real32_T)floor(u1))) {
       y = ((real32_T)rtNaN);
     } else {
-      y = (real32_T)powf(u0, u1);
+      y = (real32_T)pow(u0, u1);
     }
   }
 
@@ -73,11 +73,11 @@ static real32_T rt_powf_snf(real32_T u0, real32_T u1)
 /*
  *
  */
-void power(const real32_T a[12], real_T b, real32_T y[12])
+void power(const real32_T a[12], real32_T y[12])
 {
   int32_T k;
   for (k = 0; k < 12; k++) {
-    y[k] = rt_powf_snf(a[k], (real32_T)b);
+    y[k] = rt_powf_snf(a[k], 2.0F);
   }
 }
 

apps/attitude_estimator_ekf/codegen/power.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'power'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -11,7 +11,7 @@
 #define __POWER_H__
 /* Include files */
 #include <math.h>
-#include <stdio.h>
+#include <stddef.h>
 #include <stdlib.h>
 #include <string.h>
 #include "rt_defines.h"
@@ -29,6 +29,6 @@
 /* Variable Definitions */
 
 /* Function Declarations */
-extern void power(const real32_T a[12], real_T b, real32_T y[12]);
+extern void power(const real32_T a[12], real32_T y[12]);
 #endif
 /* End of code generation (power.h) */

apps/attitude_estimator_ekf/codegen/rdivide.c

@@ -0,0 +1,38 @@
+/*
+ * rdivide.c
+ *
+ * Code generation for function 'rdivide'
+ *
+ * C source code generated on: Tue Oct 16 15:27:58 2012
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "rdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void rdivide(const real32_T x[3], real32_T y, real32_T z[3])
+{
+  int32_T i;
+  for (i = 0; i < 3; i++) {
+    z[i] = x[i] / y;
+  }
+}
+
+/* End of code generation (rdivide.c) */

apps/attitude_estimator_ekf/codegen/rdivide.h

@@ -0,0 +1,34 @@
+/*
+ * rdivide.h
+ *
+ * Code generation for function 'rdivide'
+ *
+ * C source code generated on: Tue Oct 16 15:27:58 2012
+ *
+ */
+
+#ifndef __RDIVIDE_H__
+#define __RDIVIDE_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include "rt_defines.h"
+#include "rt_nonfinite.h"
+
+#include "rtwtypes.h"
+#include "attitudeKalmanfilter_types.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+extern void rdivide(const real32_T x[3], real32_T y, real32_T z[3]);
+#endif
+/* End of code generation (rdivide.h) */

apps/attitude_estimator_ekf/codegen/rtGetInf.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/rtGetInf.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/rtGetNaN.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/rtGetNaN.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/rt_defines.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/rt_nonfinite.c

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/rt_nonfinite.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 

apps/attitude_estimator_ekf/codegen/rtwtypes.h

@@ -3,7 +3,7 @@
  *
  * Code generation for function 'attitudeKalmanfilter'
  *
- * C source code generated on: Mon Oct 01 19:38:49 2012
+ * C source code generated on: Tue Oct 16 15:27:58 2012
  *
  */
 
@@ -26,8 +26,8 @@
  *   Number of bits:     char:   8    short:   16    int:  32
  *                       long:  32      native word size:  32
  *   Byte ordering: LittleEndian
- *   Signed integer division rounds to: Zero
- *   Shift right on a signed integer as arithmetic shift: on
+ *   Signed integer division rounds to: Undefined
+ *   Shift right on a signed integer as arithmetic shift: off
  *=======================================================================*/
 
 /*=======================================================================* 

apps/commander/calibration_routines.c

@@ -0,0 +1,175 @@
+#include <math.h>
+
+#include "calibration_routines.h"
+
+
+int sphere_fit_least_squares(const float x[], const float y[], const float z[],
+    unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius) {
+
+    float x_sumplain = 0.0f;
+    float x_sumsq = 0.0f;
+    float x_sumcube = 0.0f;
+
+    float y_sumplain = 0.0f;
+    float y_sumsq = 0.0f;
+    float y_sumcube = 0.0f;
+
+    float z_sumplain = 0.0f;
+    float z_sumsq = 0.0f;
+    float z_sumcube = 0.0f;
+
+    float xy_sum = 0.0f;
+    float xz_sum = 0.0f;
+    float yz_sum = 0.0f;
+
+    float x2y_sum = 0.0f;
+    float x2z_sum = 0.0f;
+    float y2x_sum = 0.0f;
+    float y2z_sum = 0.0f;
+    float z2x_sum = 0.0f;
+    float z2y_sum = 0.0f;
+
+    for (unsigned int i = 0; i < size; i++) {
+
+        float x2 = x[i] * x[i];
+        float y2 = y[i] * y[i];
+        float z2 = z[i] * z[i];
+
+        x_sumplain += x[i];
+        x_sumsq += x2;
+        x_sumcube += x2 * x[i];
+
+        y_sumplain += y[i];
+        y_sumsq += y2;
+        y_sumcube += y2 * y[i];
+
+        z_sumplain += z[i];
+        z_sumsq += z2;
+        z_sumcube += z2 * z[i];
+
+        xy_sum += x[i] * y[i];
+        xz_sum += x[i] * z[i];
+        yz_sum += y[i] * z[i];
+
+        x2y_sum += x2 * y[i];
+        x2z_sum += x2 * z[i];
+
+        y2x_sum += y2 * x[i];
+        y2z_sum += y2 * z[i];
+
+        z2x_sum += z2 * x[i];
+        z2y_sum += z2 * y[i];
+    }
+
+    //
+    //Least Squares Fit a sphere A,B,C with radius squared Rsq to 3D data
+    //
+    //    P is a structure that has been computed with the data earlier.
+    //    P.npoints is the number of elements; the length of X,Y,Z are identical.
+    //    P's members are logically named.
+    //
+    //    X[n] is the x component of point n
+    //    Y[n] is the y component of point n
+    //    Z[n] is the z component of point n
+    //
+    //    A is the x coordiante of the sphere
+    //    B is the y coordiante of the sphere
+    //    C is the z coordiante of the sphere
+    //    Rsq is the radius squared of the sphere.
+    //
+    //This method should converge; maybe 5-100 iterations or more.
+    //
+    float x_sum = x_sumplain / size;        //sum( X[n] )
+    float x_sum2 = x_sumsq / size;    //sum( X[n]^2 )
+    float x_sum3 = x_sumcube / size;    //sum( X[n]^3 )
+    float y_sum = y_sumplain / size;        //sum( Y[n] )
+    float y_sum2 = y_sumsq / size;    //sum( Y[n]^2 )
+    float y_sum3 = y_sumcube / size;    //sum( Y[n]^3 )
+    float z_sum = z_sumplain / size;        //sum( Z[n] )
+    float z_sum2 = z_sumsq / size;    //sum( Z[n]^2 )
+    float z_sum3 = z_sumcube / size;    //sum( Z[n]^3 )
+
+    float XY = xy_sum / size;        //sum( X[n] * Y[n] )
+    float XZ = xz_sum / size;        //sum( X[n] * Z[n] )
+    float YZ = yz_sum / size;        //sum( Y[n] * Z[n] )
+    float X2Y = x2y_sum / size;    //sum( X[n]^2 * Y[n] )
+    float X2Z = x2z_sum / size;    //sum( X[n]^2 * Z[n] )
+    float Y2X = y2x_sum / size;    //sum( Y[n]^2 * X[n] )
+    float Y2Z = y2z_sum / size;    //sum( Y[n]^2 * Z[n] )
+    float Z2X = z2x_sum / size;    //sum( Z[n]^2 * X[n] )
+    float Z2Y = z2y_sum / size;    //sum( Z[n]^2 * Y[n] )
+
+    //Reduction of multiplications
+    float F0 = x_sum2 + y_sum2 + z_sum2;
+    float F1 =  0.5f * F0;
+    float F2 = -8.0f * (x_sum3 + Y2X + Z2X);
+    float F3 = -8.0f * (X2Y + y_sum3 + Z2Y);
+    float F4 = -8.0f * (X2Z + Y2Z + z_sum3);
+
+    //Set initial conditions:
+    float A = x_sum;
+    float B = y_sum;
+    float C = z_sum;
+
+    //First iteration computation:
+    float A2 = A*A;
+    float B2 = B*B;
+    float C2 = C*C;
+    float QS = A2 + B2 + C2;
+    float QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
+
+    //Set initial conditions:
+    float Rsq = F0 + QB + QS;
+
+    //First iteration computation:
+    float Q0 = 0.5f * (QS - Rsq);
+    float Q1 = F1 + Q0;
+    float Q2 = 8.0f * ( QS - Rsq + QB + F0 );
+    float aA,aB,aC,nA,nB,nC,dA,dB,dC;
+
+    //Iterate N times, ignore stop condition.
+    int n = 0;
+    while( n < max_iterations ){
+        n++;
+
+        //Compute denominator:
+        aA = Q2 + 16.0f * (A2 - 2.0f * A*x_sum + x_sum2);
+        aB = Q2 + 16.0f *(B2 - 2.0f * B*y_sum + y_sum2);
+        aC = Q2 + 16.0f *(C2 - 2.0f * C*z_sum + z_sum2);
+        aA = (aA == 0.0f) ? 1.0f : aA;
+        aB = (aB == 0.0f) ? 1.0f : aB;
+        aC = (aC == 0.0f) ? 1.0f : aC;
+
+        //Compute next iteration
+        nA = A - ((F2 + 16.0f * ( B*XY + C*XZ + x_sum*(-A2 - Q0) + A*(x_sum2 + Q1 - C*z_sum - B*y_sum) ) )/aA);
+        nB = B - ((F3 + 16.0f * ( A*XY + C*YZ + y_sum*(-B2 - Q0) + B*(y_sum2 + Q1 - A*x_sum - C*z_sum) ) )/aB);
+        nC = C - ((F4 + 16.0f * ( A*XZ + B*YZ + z_sum*(-C2 - Q0) + C*(z_sum2 + Q1 - A*x_sum - B*y_sum) ) )/aC);
+
+        //Check for stop condition
+        dA = (nA - A);
+        dB = (nB - B);
+        dC = (nC - C);
+        if( (dA*dA + dB*dB + dC*dC) <= delta ){ break; }
+
+        //Compute next iteration's values
+        A = nA;
+        B = nB;
+        C = nC;
+        A2 = A*A;
+        B2 = B*B;
+        C2 = C*C;
+        QS = A2 + B2 + C2;
+        QB = -2.0f * (A*x_sum + B*y_sum + C*z_sum);
+        Rsq = F0 + QB + QS;
+        Q0 = 0.5f * (QS - Rsq);
+        Q1 = F1 + Q0;
+        Q2 = 8.0f * ( QS - Rsq + QB + F0 );
+    }
+
+    *sphere_x = A;
+    *sphere_y = B;
+    *sphere_z = C;
+    *sphere_radius = sqrtf(Rsq);
+
+    return 0;
+}

apps/commander/calibration_routines.h

@@ -0,0 +1,21 @@
+
+/**
+ * Least-squares fit of a sphere to a set of points.
+ *
+ * Fits a sphere to a set of points on the sphere surface.
+ *
+ * @param x point coordinates on the X axis
+ * @param y point coordinates on the Y axis
+ * @param z point coordinates on the Z axis
+ * @param size number of points
+ * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
+ * @param delta abort if error is below delta. If unsure, set to 0 to run max_iterations times.
+ * @param sphere_x coordinate of the sphere center on the X axis
+ * @param sphere_y coordinate of the sphere center on the Y axis
+ * @param sphere_z coordinate of the sphere center on the Z axis
+ * @param sphere_radius sphere radius
+ *
+ * @return 0 on success, 1 on failure
+ */
+int sphere_fit_least_squares(const float x[], const float y[], const float z[],
+    unsigned int size, unsigned int max_iterations, float delta, float *sphere_x, float *sphere_y, float *sphere_z, float *sphere_radius);

apps/commander/state_machine_helper.c

@@ -504,7 +504,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c
 	current_status->flag_control_manual_enabled = true;
 	/* enable attitude control per default */
 	current_status->flag_control_attitude_enabled = true;
-	current_status->flag_control_rates_enabled = false; // XXX
+	current_status->flag_control_rates_enabled = true;
 	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -519,7 +519,7 @@ void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_STABILIZED;
 	current_status->flag_control_manual_enabled = true;
 	current_status->flag_control_attitude_enabled = true;
-	current_status->flag_control_rates_enabled = false; // XXX
+	current_status->flag_control_rates_enabled = true;
 	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
@@ -534,7 +534,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur
 	current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
 	current_status->flag_control_manual_enabled = true;
 	current_status->flag_control_attitude_enabled = true;
-	current_status->flag_control_rates_enabled = false; // XXX
+	current_status->flag_control_rates_enabled = true;
 	if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
 
 	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {

apps/drivers/drv_mag.h

@@ -99,7 +99,13 @@ ORB_DECLARE(sensor_mag);
 /** copy the mag scaling constants to the structure pointed to by (arg) */
 #define MAGIOCGSCALE		_MAGIOC(3)
 
+/** set the measurement range to handle (at least) arg Gauss */
+#define MAGIOCSRANGE		_MAGIOC(4)
+
 /** perform self-calibration, update scale factors to canonical units */
-#define MAGIOCCALIBRATE		_MAGIOC(4)
+#define MAGIOCCALIBRATE		_MAGIOC(5)
+
+/** excite strap */
+#define MAGIOCEXSTRAP		_MAGIOC(6)
 
 #endif /* _DRV_MAG_H */

apps/drivers/drv_tone_alarm.h

@@ -63,8 +63,6 @@
 #define _TONE_ALARM_BASE	0x7400
 #define TONE_SET_ALARM		_IOC(_TONE_ALARM_BASE, 1)
 
-extern int	tone_alarm_init(void);
-
 /* structure describing one note in a tone pattern */
 struct tone_note {
 	uint8_t		pitch;

apps/drivers/hmc5883/Makefile

@@ -37,6 +37,6 @@
 
 APPNAME		 = hmc5883
 PRIORITY	 = SCHED_PRIORITY_DEFAULT
-STACKSIZE	 = 2048
+STACKSIZE	 = 4096
 
 include $(APPDIR)/mk/app.mk

apps/drivers/hmc5883/hmc5883.cpp

@@ -175,6 +175,36 @@ private:
 	 */
 	void			stop();
 
+	/**
+	 * Perform the on-sensor scale calibration routine.
+	 *
+	 * @note The sensor will continue to provide measurements, these
+	 *	 will however reflect the uncalibrated sensor state until
+	 *	 the calibration routine has been completed.
+	 *
+	 * @param enable set to 1 to enable self-test strap, 0 to disable
+	 */
+	int			calibrate(struct file *filp, unsigned enable);
+
+	/**
+	 * Perform the on-sensor scale calibration routine.
+	 *
+	 * @note The sensor will continue to provide measurements, these
+	 *	 will however reflect the uncalibrated sensor state until
+	 *	 the calibration routine has been completed.
+	 *
+	 * @param enable set to 1 to enable self-test positive strap, -1 to enable
+	 *        negative strap, 0 to set to normal mode
+	 */
+	int			set_excitement(unsigned enable);
+
+	/**
+	 * Set the sensor range.
+	 *
+	 * Sets the internal range to handle at least the argument in Gauss.
+	 */
+	int 			set_range(unsigned range);
+
 	/**
 	 * Perform a poll cycle; collect from the previous measurement
 	 * and start a new one.
@@ -254,9 +284,9 @@ HMC5883::HMC5883(int bus) :
 	_next_report(0),
 	_oldest_report(0),
 	_reports(nullptr),
+	_range_scale(0), /* default range scale from counts to gauss */
+	_range_ga(1.3f),
 	_mag_topic(-1),
-	_range_scale(1.0f / 1090.0f), /* default range scale from counts to gauss */
-	_range_ga(0.88f),
 	_sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")),
 	_buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows"))
@@ -308,11 +338,71 @@ HMC5883::init()
 	if (_mag_topic < 0)
 		debug("failed to create sensor_mag object");
 
+	/* set range */
+	set_range(_range_ga);
+
 	ret = OK;
 out:
 	return ret;
 }
 
+int HMC5883::set_range(unsigned range)
+{
+	uint8_t range_bits;
+
+	if (range < 1) {
+		range_bits = 0x00;
+		_range_scale = 1.0f / 1370.0f;
+		_range_ga = 0.88f;
+	} else if (range <= 1) {
+		range_bits = 0x01;
+		_range_scale = 1.0f / 1090.0f;
+		_range_ga = 1.3f;
+	} else if (range <= 2) {
+		range_bits = 0x02;
+		_range_scale = 1.0f / 820.0f;
+		_range_ga = 1.9f;
+	} else if (range <= 3) {
+		range_bits = 0x03;
+		_range_scale = 1.0f / 660.0f;
+		_range_ga = 2.5f;
+	} else if (range <= 4) {
+		range_bits = 0x04;
+		_range_scale = 1.0f / 440.0f;
+		_range_ga = 4.0f;
+	} else if (range <= 4.7f) {
+		range_bits = 0x05;
+		_range_scale = 1.0f / 390.0f;
+		_range_ga = 4.7f;
+	} else if (range <= 5.6f) {
+		range_bits = 0x06;
+		_range_scale = 1.0f / 330.0f;
+		_range_ga = 5.6f;
+	} else {
+		range_bits = 0x07;
+		_range_scale = 1.0f / 230.0f;
+		_range_ga = 8.1f;
+	}
+
+	int ret;
+
+	/*
+	 * Send the command to set the range
+	 */
+	ret = write_reg(ADDR_CONF_B, (range_bits << 5));
+
+	if (OK != ret)
+		perf_count(_comms_errors);
+
+	uint8_t range_bits_in;
+	ret = read_reg(ADDR_CONF_B, range_bits_in);
+
+	if (OK != ret)
+		perf_count(_comms_errors);
+
+	return !(range_bits_in == (range_bits << 5));
+}
+
 int
 HMC5883::probe()
 {
@@ -495,6 +585,9 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		/* not supported, always 1 sample per poll */
 		return -EINVAL;
 
+	case MAGIOCSRANGE:
+		return set_range(arg);
+
 	case MAGIOCSLOWPASS:
 		/* not supported, no internal filtering */
 		return -EINVAL;
@@ -510,8 +603,10 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
 		return 0;
 
 	case MAGIOCCALIBRATE:
-		/* XXX perform auto-calibration */
-		return -EINVAL;
+		return calibrate(filp, arg);
+
+	case MAGIOCEXSTRAP:
+		return set_excitement(arg);
 
 	default:
 		/* give it to the superclass */
@@ -718,6 +813,194 @@ out:
 	return ret;
 }
 
+int HMC5883::calibrate(struct file *filp, unsigned enable)
+{
+	struct mag_report report;
+	ssize_t sz;
+	int ret = 1;
+
+	// XXX do something smarter here
+	int fd = (int)enable;
+
+	struct mag_scale mscale_previous = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	struct mag_scale mscale_null = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	float avg_excited[3] = {0.0f, 0.0f, 0.0f};
+	unsigned i;
+
+	warnx("starting mag scale calibration");
+
+	/* do a simple demand read */
+	sz = read(filp, (char*)&report, sizeof(report));
+	if (sz != sizeof(report)) {
+		warn("immediate read failed");
+		ret = 1;
+		goto out;
+	}
+
+	warnx("current measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+	warnx("time:        %lld", report.timestamp);
+	warnx("sampling 500 samples for scaling offset");
+
+	/* set the queue depth to 10 */
+	if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
+		warn("failed to set queue depth");
+		ret = 1;
+		goto out;
+	}
+
+	/* start the sensor polling at 50 Hz */
+	if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
+		warn("failed to set 2Hz poll rate");
+		ret = 1;
+		goto out;
+	}
+
+	/* Set to 2.5 Gauss */
+	if (OK != ioctl(filp, MAGIOCSRANGE, 2)) {
+		warnx("failed to set 2.5 Ga range");
+		ret = 1;
+		goto out;
+	}
+
+	if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
+		warnx("failed to enable sensor calibration mode");
+		ret = 1;
+		goto out;
+	}
+
+	if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
+		warn("WARNING: failed to get scale / offsets for mag");
+		ret = 1;
+		goto out;
+	}
+
+	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
+		warn("WARNING: failed to set null scale / offsets for mag");
+		ret = 1;
+		goto out;
+	}
+
+	/* read the sensor 10x and report each value */
+	for (i = 0; i < 500; i++) {
+		struct pollfd fds;
+
+		/* wait for data to be ready */
+		fds.fd = fd;
+		fds.events = POLLIN;
+		ret = ::poll(&fds, 1, 2000);
+
+		if (ret != 1) {
+			warn("timed out waiting for sensor data");
+			goto out;
+		}
+
+		/* now go get it */
+		sz = ::read(fd, &report, sizeof(report));
+
+		if (sz != sizeof(report)) {
+			warn("periodic read failed");
+			goto out;
+		} else {
+			avg_excited[0] += report.x;
+			avg_excited[1] += report.y;
+			avg_excited[2] += report.z;
+		}
+
+		//warnx("periodic read %u", i);
+		//warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
+	}
+
+	avg_excited[0] /= i;
+	avg_excited[1] /= i;
+	avg_excited[2] /= i;
+
+	warnx("done. Performed %u reads", i);
+	warnx("measurement avg: %.6f  %.6f  %.6f", (double)avg_excited[0], (double)avg_excited[1], (double)avg_excited[2]);
+
+	float scaling[3];
+
+	/* calculate axis scaling */
+	scaling[0] = fabsf(1.16f / avg_excited[0]);
+	/* second axis inverted */
+	scaling[1] = fabsf(1.16f / -avg_excited[1]);
+	scaling[2] = fabsf(1.08f / avg_excited[2]);
+
+	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
+	
+	/* set back to normal mode */
+	/* Set to 1.1 Gauss */
+	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
+		warnx("failed to set 1.1 Ga range");
+		goto out;
+	}
+
+	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
+		warnx("failed to disable sensor calibration mode");
+		goto out;
+	}
+
+	/* set scaling in device */
+	mscale_previous.x_scale = scaling[0];
+	mscale_previous.y_scale = scaling[1];
+	mscale_previous.z_scale = scaling[2];
+
+	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
+		warn("WARNING: failed to set new scale / offsets for mag");
+		goto out;
+	}
+
+	ret = OK;
+
+	out:
+		if (ret == OK) {
+			warnx("mag scale calibration successfully finished.");
+		} else {
+			warnx("mag scale calibration failed.");
+		}
+	return ret;
+}
+
+int HMC5883::set_excitement(unsigned enable)
+{
+	int ret;
+	/* arm the excitement strap */
+	uint8_t conf_reg;
+	ret = read_reg(ADDR_CONF_A, conf_reg);
+	if (OK != ret)
+		perf_count(_comms_errors);
+	if (((int)enable) < 0) {
+		conf_reg |= 0x01;
+	} else if (enable > 0) {
+		conf_reg |= 0x02;
+	} else {
+		conf_reg &= ~0x03;
+	}
+	ret = write_reg(ADDR_CONF_A, conf_reg);
+	if (OK != ret)
+		perf_count(_comms_errors);
+
+	uint8_t conf_reg_ret;
+	read_reg(ADDR_CONF_A, conf_reg_ret);
+
+	return !(conf_reg == conf_reg_ret);
+}
+
 int
 HMC5883::write_reg(uint8_t reg, uint8_t val)
 {
@@ -775,6 +1058,7 @@ void	start();
 void	test();
 void	reset();
 void	info();
+int	calibrate();
 
 /**
  * Start the driver.
@@ -872,6 +1156,69 @@ test()
 	errx(0, "PASS");
 }
 
+
+/**
+ * Automatic scale calibration.
+ *
+ * Basic idea:
+ *
+ *   output = (ext field +- 1.1 Ga self-test) * scale factor
+ * 
+ * and consequently:
+ *
+ *   1.1 Ga = (excited - normal) * scale factor
+ *   scale factor = (excited - normal) / 1.1 Ga
+ *
+ *   sxy = (excited - normal) / 766	| for conf reg. B set to 0x60 / Gain = 3
+ *   sz  = (excited - normal) / 713	| for conf reg. B set to 0x60 / Gain = 3
+ *
+ * By subtracting the non-excited measurement the pure 1.1 Ga reading
+ * can be extracted and the sensitivity of all axes can be matched.
+ *
+ * SELF TEST OPERATION
+ * To check the HMC5883L for proper operation, a self test feature in incorporated
+ * in which the sensor offset straps are excited to create a nominal field strength
+ * (bias field) to be measured. To implement self test, the least significant bits
+ * (MS1 and MS0) of configuration register A are changed from 00 to 01 (positive bias)
+ * or 10 (negetive bias), e.g. 0x11 or 0x12.
+ * Then, by placing the mode register into single-measurement mode (0x01),
+ * two data acquisition cycles will be made on each magnetic vector.
+ * The first acquisition will be a set pulse followed shortly by measurement
+ * data of the external field. The second acquisition will have the offset strap
+ * excited (about 10 mA) in the positive bias mode for X, Y, and Z axes to create
+ * about a ±1.1 gauss self test field plus the external field. The first acquisition
+ * values will be subtracted from the second acquisition, and the net measurement
+ * will be placed into the data output registers.
+ * Since self test adds ~1.1 Gauss additional field to the existing field strength,
+ * using a reduced gain setting prevents sensor from being saturated and data registers
+ * overflowed. For example, if the configuration register B is set to 0x60 (Gain=3),
+ * values around +766 LSB (1.16 Ga * 660 LSB/Ga) will be placed in the X and Y data
+ * output registers and around +713 (1.08 Ga * 660 LSB/Ga) will be placed in Z data
+ * output register. To leave the self test mode, change MS1 and MS0 bit of the
+ * configuration register A back to 00 (Normal Measurement Mode), e.g. 0x10.
+ * Using the self test method described above, the user can scale sensor
+ */
+int calibrate()
+{
+	int ret;
+
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	if (fd < 0)
+		err(1, "%s open failed (try 'hmc5883 start' if the driver is not running", MAG_DEVICE_PATH);
+
+	if (OK != (ret = ioctl(fd, MAGIOCCALIBRATE, fd))) {
+		warnx("failed to enable sensor calibration mode");
+	}
+
+	close(fd);
+
+	if (ret == OK) {
+		errx(0, "PASS");
+	} else {
+		errx(1, "FAIL");
+	}
+}
+
 /**
  * Reset the driver.
  */
@@ -930,8 +1277,19 @@ hmc5883_main(int argc, char *argv[])
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
+	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
 		hmc5883::info();
 
-	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
+	/*
+	 * Autocalibrate the scaling
+	 */
+	if (!strcmp(argv[1], "calibrate")) {
+		if (hmc5883::calibrate() == 0) {
+			errx(0, "calibration successful");
+		} else {
+			errx(1, "calibration failed");
+		}
+	}
+
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' 'calibrate' or 'info'");
 }

apps/drivers/l3gd20/l3gd20.cpp

@@ -864,5 +864,5 @@ l3gd20_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "info"))
 		l3gd20::info();
 
-	errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'");
+	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
 }

apps/drivers/stm32/tone_alarm/Makefile

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Tone alarm driver
+#
+
+APPNAME		 = tone_alarm
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 2048
+INCLUDES	 = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common
+
+include $(APPDIR)/mk/app.mk

apps/fixedwing_control/fixedwing_control.c

@@ -2,6 +2,7 @@
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
  *   Author: @author Ivan Ovinnikov <oivan@ethz.ch>
+ *   Modifications: Doug Weibel <douglas.weibel@colorado.edu>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -31,7 +32,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-// Workflow test comment - DEW
+
 /**
  * @file fixedwing_control.c
  * Implementation of a fixed wing attitude and position controller.
@@ -86,27 +87,61 @@ static void usage(const char *reason);
  * Controller parameters, accessible via MAVLink
  *
  */
-PARAM_DEFINE_FLOAT(FW_ROLL_POS_P, 0.3f);
-PARAM_DEFINE_FLOAT(FW_ROLL_POS_I, 0.002f);
-PARAM_DEFINE_FLOAT(FW_ROLL_POS_AWU, 0.2f);
-PARAM_DEFINE_FLOAT(FW_ROLL_POS_LIM, 0.6f);
+// Roll control parameters
+PARAM_DEFINE_FLOAT(FW_ROLLRATE_P, 0.3f);
+// Need to add functionality to suppress integrator windup while on the ground
+// Suggested value of FW_ROLLRATE_I is 0.0 till this is in place
+PARAM_DEFINE_FLOAT(FW_ROLLRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_ROLLRATE_AWU, 0.0f);
+PARAM_DEFINE_FLOAT(FW_ROLLRATE_LIM, 0.7f);   // Roll rate limit in radians/sec
+PARAM_DEFINE_FLOAT(FW_ROLL_P, 0.3f);
+PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f);	// Roll angle limit in radians
+
+//Pitch control parameters
+PARAM_DEFINE_FLOAT(FW_PITCHRATE_P, 0.3f);
+// Need to add functionality to suppress integrator windup while on the ground
+// Suggested value of FW_PITCHRATE_I is 0.0 till this is in place
+PARAM_DEFINE_FLOAT(FW_PITCHRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PITCHRATE_AWU, 0.0f);
+PARAM_DEFINE_FLOAT(FW_PITCHRATE_LIM, 0.35f);   // Pitch rate limit in radians/sec
+PARAM_DEFINE_FLOAT(FW_PITCH_P, 0.3f);
+PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f);	// Pitch angle limit in radians
 
 struct fw_att_control_params {
-	float roll_pos_p;
-	float roll_pos_i;
-	float roll_pos_awu;
-	float roll_pos_lim;
+	float rollrate_p;
+	float rollrate_i;
+	float rollrate_awu;
+	float rollrate_lim;
+	float roll_p;
+	float roll_lim;
+	float pitchrate_p;
+	float pitchrate_i;
+	float pitchrate_awu;
+	float pitchrate_lim;
+	float pitch_p;
+	float pitch_lim;
 };
 
 struct fw_att_control_param_handles {
-	param_t roll_pos_p;
-	param_t roll_pos_i;
-	param_t roll_pos_awu;
-	param_t roll_pos_lim;
+	param_t rollrate_p;
+	param_t rollrate_i;
+	param_t rollrate_awu;
+	param_t rollrate_lim;
+	param_t roll_p;
+	param_t roll_lim;
+	param_t pitchrate_p;
+	param_t pitchrate_i;
+	param_t pitchrate_awu;
+	param_t pitchrate_lim;
+	param_t pitch_p;
+	param_t pitch_lim;
 };
 
 
-// XXX outsource position control to a separate app some time
+// TO_DO - Navigation control will be moved to a separate app
+// Attitude control will just handle the inner angle and rate loops
+// to control pitch and roll, and turn coordination via rudder and 
+// possibly throttle compensation for battery voltage sag.
 
 PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f);
 PARAM_DEFINE_FLOAT(FW_HEADING_LIM, 0.15f);
@@ -147,9 +182,9 @@ static int pos_parameters_update(const struct fw_pos_control_param_handles *h, s
 
 
 /**
- * The fixed wing control main loop.
+ * The fixed wing control main thread.
  *
- * This loop executes continously and calculates the control
+ * The main loop executes continously and calculates the control
  * response.
  *
  * @param argc number of arguments
@@ -223,19 +258,31 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 	pos_parameters_init(&hpos);
 	pos_parameters_update(&hpos, &ppos);
 
-	PID_t roll_pos_controller;
-	pid_init(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu,
-			PID_MODE_DERIVATIV_SET);
+// TO_DO  Fix output limit functionallity of PID controller or add that function elsewhere
+	PID_t roll_rate_controller;
+	pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, p.rollrate_awu,
+			p.rollrate_lim,PID_MODE_DERIVATIV_NONE);
+	PID_t roll_angle_controller;
+	pid_init(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 0.0f,
+			p.roll_lim,PID_MODE_DERIVATIV_NONE);
+			
+	PID_t pitch_rate_controller;
+	pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, p.pitchrate_awu,
+			p.pitchrate_lim,PID_MODE_DERIVATIV_NONE);
+	PID_t pitch_angle_controller;
+	pid_init(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 0.0f,
+			p.pitch_lim,PID_MODE_DERIVATIV_NONE);
 
 	PID_t heading_controller;
 	pid_init(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f,
-			PID_MODE_DERIVATIV_SET);
+			100.0f,PID_MODE_DERIVATIV_SET);		// Temporary arbitrarily large limit
 
 	// XXX remove in production
 	/* advertise debug value */
 	struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
 	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
 
+	// This is the top of the main loop
 	while(!thread_should_exit) {
 
 		struct pollfd fds[1] = {
@@ -251,11 +298,20 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 		} else {
 
 			// FIXME SUBSCRIBE
-			if (loopcounter % 20 == 0) {
+			if (loopcounter % 100 == 0) {
 				att_parameters_update(&h, &p);
 				pos_parameters_update(&hpos, &ppos);
-				pid_set_parameters(&roll_pos_controller, p.roll_pos_p, p.roll_pos_i, 0.0f, p.roll_pos_awu);
-				pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f);
+				pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0.0f, 
+									p.rollrate_awu, p.rollrate_lim);
+				pid_set_parameters(&roll_angle_controller, p.roll_p, 0.0f, 0.0f, 
+									0.0f, p.roll_lim);
+				pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0.0f, 
+									p.pitchrate_awu, p.pitchrate_lim);
+				pid_set_parameters(&pitch_angle_controller, p.pitch_p, 0.0f, 0.0f, 
+									0.0f, p.pitch_lim);
+				pid_set_parameters(&heading_controller, ppos.heading_p, 0.0f, 0.0f, 0.0f, 90.0f);
+//printf("[fixedwing control debug] p: %8.4f, i: %8.4f, limit: %8.4f  \n",
+//p.rollrate_p, p.rollrate_i, p.rollrate_lim);
 			}
 
 			/* if position updated, run position control loop */
@@ -359,12 +415,23 @@ int fixedwing_control_thread_main(int argc, char *argv[])
 				printf("[fixedwing control] roll sp: %8.4f, \n", att_sp.roll_body);
 			}
 
-			// actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body,
-			// 		att.roll, att.rollspeed, deltaTpos);
-						actuators.control[0] = pid_calculate(&roll_pos_controller, att_sp.roll_body,
-					att.roll, att.rollspeed, deltaTpos);
-			actuators.control[1] = 0;
-			actuators.control[2] = 0;
+			// actuator control[0] is aileron (or elevon roll control)
+			// Commanded roll rate from P controller on roll angle
+			float roll_rate_command = pid_calculate(&roll_angle_controller, att_sp.roll_body,
+					att.roll, 0.0f, deltaTpos);
+			// actuator control from PI controller on roll rate
+			actuators.control[0] = pid_calculate(&roll_rate_controller, roll_rate_command,
+					att.rollspeed, 0.0f, deltaTpos);
+					
+			// actuator control[1] is elevator (or elevon pitch control)
+			// Commanded pitch rate from P controller on pitch angle
+			float pitch_rate_command = pid_calculate(&pitch_angle_controller, att_sp.pitch_body,
+					att.pitch, 0.0f, deltaTpos);
+			// actuator control from PI controller on pitch rate
+			actuators.control[1] = pid_calculate(&pitch_rate_controller, pitch_rate_command,
+					att.pitchspeed, 0.0f, deltaTpos);
+					
+			// actuator control[3] is throttle
 			actuators.control[3] = att_sp.thrust;
 					
 			/* publish attitude setpoint (for MAVLink) */
@@ -446,20 +513,37 @@ int fixedwing_control_main(int argc, char *argv[])
 static int att_parameters_init(struct fw_att_control_param_handles *h)
 {
 	/* PID parameters */
-	h->roll_pos_p 	=	param_find("FW_ROLL_POS_P");
-	h->roll_pos_i 	=	param_find("FW_ROLL_POS_I");
-	h->roll_pos_lim =	param_find("FW_ROLL_POS_LIM");
-	h->roll_pos_awu =	param_find("FW_ROLL_POS_AWU");
+	
+	h->rollrate_p 			=	param_find("FW_ROLLRATE_P");
+	h->rollrate_i 			=	param_find("FW_ROLLRATE_I");
+	h->rollrate_awu 		=	param_find("FW_ROLLRATE_AWU");
+	h->rollrate_lim 		=	param_find("FW_ROLLRATE_LIM");
+	h->roll_p 				=	param_find("FW_ROLL_P");
+	h->roll_lim 			=	param_find("FW_ROLL_LIM");
+	h->pitchrate_p 			=	param_find("FW_PITCHRATE_P");
+	h->pitchrate_i 			=	param_find("FW_PITCHRATE_I");
+	h->pitchrate_awu 		=	param_find("FW_PITCHRATE_AWU");
+	h->pitchrate_lim 		=	param_find("FW_PITCHRATE_LIM");
+	h->pitch_p 				=	param_find("FW_PITCH_P");
+	h->pitch_lim 			=	param_find("FW_PITCH_LIM");
 	
 	return OK;
 }
 
 static int att_parameters_update(const struct fw_att_control_param_handles *h, struct fw_att_control_params *p)
 {
-	param_get(h->roll_pos_p, &(p->roll_pos_p));
-	param_get(h->roll_pos_i, &(p->roll_pos_i));
-	param_get(h->roll_pos_lim, &(p->roll_pos_lim));
-	param_get(h->roll_pos_awu, &(p->roll_pos_awu));
+	param_get(h->rollrate_p, &(p->rollrate_p));
+	param_get(h->rollrate_i, &(p->rollrate_i));
+	param_get(h->rollrate_awu, &(p->rollrate_awu));
+	param_get(h->rollrate_lim, &(p->rollrate_lim));
+	param_get(h->roll_p, &(p->roll_p));
+	param_get(h->roll_lim, &(p->roll_lim));
+	param_get(h->pitchrate_p, &(p->pitchrate_p));
+	param_get(h->pitchrate_i, &(p->pitchrate_i));
+	param_get(h->pitchrate_awu, &(p->pitchrate_awu));
+	param_get(h->pitchrate_lim, &(p->pitchrate_lim));
+	param_get(h->pitch_p, &(p->pitch_p));
+	param_get(h->pitch_lim, &(p->pitch_lim));
 	
 	return OK;
 }