Merge working changes into export-build branch.

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -0,0 +1,472 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/*
+ * @file attitude_estimator_ekf_main.c
+ *
+ * Extended Kalman Filter for Attitude Estimation.
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <v1.0/common/mavlink.h>
+#include <float.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <drivers/drv_hrt.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include "codegen/attitudeKalmanfilter_initialize.h"
+#include "codegen/attitudeKalmanfilter.h"
+#include "attitude_estimator_ekf_params.h"
+#ifdef __cplusplus
+}
+#endif
+
+extern "C" __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
+
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int attitude_estimator_ekf_task;				/**< Handle of deamon task / thread */
+
+/**
+ * Mainloop of attitude_estimator_ekf.
+ */
+int attitude_estimator_ekf_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+
+	fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The attitude_estimator_ekf app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int attitude_estimator_ekf_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("attitude_estimator_ekf already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
+					      SCHED_DEFAULT,
+					      SCHED_PRIORITY_MAX - 5,
+					      12400,
+					      attitude_estimator_ekf_thread_main,
+					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tattitude_estimator_ekf app is running\n");
+
+		} else {
+			printf("\tattitude_estimator_ekf app not started\n");
+		}
+
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
+
+/*
+ * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst)
+ */
+
+/*
+ * EKF Attitude Estimator main function.
+ *
+ * Estimates the attitude recursively once started.
+ *
+ * @param argc number of commandline arguments (plus command name)
+ * @param argv strings containing the arguments
+ */
+int attitude_estimator_ekf_thread_main(int argc, char *argv[])
+{
+
+const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
+
+	float dt = 0.005f;
+/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
+	float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f};					/**< Measurement vector */
+	float x_aposteriori_k[12];		/**< states */
+	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,
+				     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 */
+
+	float x_aposteriori[12];
+	float P_aposteriori[144];
+
+	/* output euler angles */
+	float euler[3] = {0.0f, 0.0f, 0.0f};
+
+	float Rot_matrix[9] = {1.f,  0,  0,
+			      0,  1.f,  0,
+			      0,  0,  1.f
+			     };		/**< init: identity matrix */
+
+	// print text
+	printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
+	fflush(stdout);
+
+	int overloadcounter = 19;
+
+	/* Initialize filter */
+	attitudeKalmanfilter_initialize();
+
+	/* store start time to guard against too slow update rates */
+	uint64_t last_run = hrt_absolute_time();
+
+	struct sensor_combined_s raw;
+	memset(&raw, 0, sizeof(raw));
+	struct vehicle_attitude_s att;
+	memset(&att, 0, sizeof(att));
+	struct vehicle_status_s state;
+	memset(&state, 0, sizeof(state));
+
+	uint64_t last_data = 0;
+	uint64_t last_measurement = 0;
+
+	/* 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, 4);
+
+	/* subscribe to param changes */
+	int sub_params = orb_subscribe(ORB_ID(parameter_update));
+
+	/* subscribe to system state*/
+	int sub_state = orb_subscribe(ORB_ID(vehicle_status));
+
+	/* advertise attitude */
+	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
+
+
+	int loopcounter = 0;
+	int printcounter = 0;
+
+	thread_running = true;
+
+	/* advertise debug value */
+	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
+	// orb_advert_t pub_dbg = -1;
+
+	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
+	// XXX write this out to perf regs
+
+	/* keep track of sensor updates */
+	uint32_t sensor_last_count[3] = {0, 0, 0};
+	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
+
+	struct attitude_estimator_ekf_params ekf_params;
+
+	struct attitude_estimator_ekf_param_handles ekf_param_handles;
+
+	/* 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;
+
+	/* register the perf counter */
+	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
+
+	/* Main loop*/
+	while (!thread_should_exit) {
+
+		struct pollfd fds[2];
+		fds[0].fd = sub_raw;
+		fds[0].events = POLLIN;
+		fds[1].fd = sub_params;
+		fds[1].events = POLLIN;
+		int ret = poll(fds, 2, 1000);
+
+		if (ret < 0) {
+			/* XXX this is seriously bad - should be an emergency */
+		} else if (ret == 0) {
+			/* check if we're in HIL - not getting sensor data is fine then */
+			orb_copy(ORB_ID(vehicle_status), sub_state, &state);
+
+			if (!state.flag_hil_enabled) {
+				fprintf(stderr,
+					"[att ekf] WARNING: Not getting sensors - sensor app running?\n");
+			}
+
+		} else {
+
+			/* only update parameters if they changed */
+			if (fds[1].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), sub_params, &update);
+
+				/* update parameters */
+				parameters_update(&ekf_param_handles, &ekf_params);
+			}
+
+			/* only run filter if sensor values changed */
+			if (fds[0].revents & POLLIN) {
+
+				/* 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 {
+
+					perf_begin(ekf_loop_perf);
+
+					/* Calculate data time difference in seconds */
+					dt = (raw.timestamp - last_measurement) / 1000000.0f;
+					last_measurement = raw.timestamp;
+					uint8_t update_vect[3] = {0, 0, 0};
+
+					/* Fill in gyro measurements */
+					if (sensor_last_count[0] != raw.gyro_counter) {
+						update_vect[0] = 1;
+						sensor_last_count[0] = raw.gyro_counter;
+						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
+						sensor_last_timestamp[0] = raw.timestamp;
+					}
+
+					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) {
+						update_vect[1] = 1;
+						sensor_last_count[1] = raw.accelerometer_counter;
+						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
+						sensor_last_timestamp[1] = raw.timestamp;
+					}
+
+					z_k[3] = raw.accelerometer_m_s2[0];
+					z_k[4] = raw.accelerometer_m_s2[1];
+					z_k[5] = raw.accelerometer_m_s2[2];
+
+					/* update magnetometer measurements */
+					if (sensor_last_count[2] != raw.magnetometer_counter) {
+						update_vect[2] = 1;
+						sensor_last_count[2] = raw.magnetometer_counter;
+						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
+						sensor_last_timestamp[2] = raw.timestamp;
+					}
+
+					z_k[6] = raw.magnetometer_ga[0];
+					z_k[7] = raw.magnetometer_ga[1];
+					z_k[8] = raw.magnetometer_ga[2];
+
+					uint64_t now = hrt_absolute_time();
+					unsigned int time_elapsed = now - last_run;
+					last_run = now;
+
+					if (time_elapsed > loop_interval_alarm) {
+						//TODO: add warning, cpu overload here
+						// if (overloadcounter == 20) {
+						// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
+						// 	overloadcounter = 0;
+						// }
+
+						overloadcounter++;
+					}
+
+					static bool const_initialized = false;
+
+					/* initialize with good values once we have a reasonable dt estimate */
+					if (!const_initialized && dt < 0.05f && dt > 0.005f) {
+						dt = 0.005f;
+						parameters_update(&ekf_param_handles, &ekf_params);
+
+						x_aposteriori_k[0] = z_k[0];
+						x_aposteriori_k[1] = z_k[1];
+						x_aposteriori_k[2] = z_k[2];
+						x_aposteriori_k[3] = 0.0f;
+						x_aposteriori_k[4] = 0.0f;
+						x_aposteriori_k[5] = 0.0f;
+						x_aposteriori_k[6] = z_k[3];
+						x_aposteriori_k[7] = z_k[4];
+						x_aposteriori_k[8] = z_k[5];
+						x_aposteriori_k[9] = z_k[6];
+						x_aposteriori_k[10] = z_k[7];
+						x_aposteriori_k[11] = z_k[8];
+
+						const_initialized = true;
+					}
+
+					/* do not execute the filter if not initialized */
+					if (!const_initialized) {
+						continue;
+					}
+
+					uint64_t timing_start = hrt_absolute_time();
+
+					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, 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;
+					}
+
+					if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
+
+					last_data = raw.timestamp;
+
+					/* send out */
+					att.timestamp = raw.timestamp;
+
+					// XXX Apply the same transformation to the rotation matrix
+					att.roll = euler[0] - ekf_params.roll_off;
+					att.pitch = euler[1] - ekf_params.pitch_off;
+					att.yaw = euler[2] - ekf_params.yaw_off;
+
+					att.rollspeed = x_aposteriori[0];
+					att.pitchspeed = x_aposteriori[1];
+					att.yawspeed = x_aposteriori[2];
+					att.rollacc = x_aposteriori[3];
+					att.pitchacc = x_aposteriori[4];
+					att.yawacc = x_aposteriori[5];
+
+					//att.yawspeed =z_k[2] ;
+					/* copy offsets */
+					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
+
+					/* copy rotation matrix */
+					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
+					att.R_valid = true;
+
+					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
+						// Broadcast
+						orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
+
+					} else {
+						warnx("NaN in roll/pitch/yaw estimate!");
+					}
+
+					perf_end(ekf_loop_perf);
+				}
+			}
+		}
+
+		loopcounter++;
+	}
+
+	thread_running = false;
+
+	return 0;
+}

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -0,0 +1,105 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/*
+ * @file attitude_estimator_ekf_params.c
+ *
+ * Parameters for EKF filter
+ */
+
+#include "attitude_estimator_ekf_params.h"
+
+/* Extended Kalman Filter covariances */
+
+/* gyro process noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f);
+/* gyro offsets process noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f);
+
+/* gyro measurement noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f);
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f);
+/* accelerometer measurement noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f);
+
+/* offsets in roll, pitch and yaw of sensor plane and body */
+PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
+
+int parameters_init(struct attitude_estimator_ekf_param_handles *h)
+{
+	/* PID parameters */
+	h->q0 	=	param_find("EKF_ATT_V2_Q0");
+	h->q1 	=	param_find("EKF_ATT_V2_Q1");
+	h->q2 	=	param_find("EKF_ATT_V2_Q2");
+	h->q3 	=	param_find("EKF_ATT_V2_Q3");
+	h->q4 	=	param_find("EKF_ATT_V2_Q4");
+
+	h->r0 	=	param_find("EKF_ATT_V2_R0");
+	h->r1 	=	param_find("EKF_ATT_V2_R1");
+	h->r2 	=	param_find("EKF_ATT_V2_R2");
+	h->r3 	=	param_find("EKF_ATT_V2_R3");
+
+	h->roll_off  =	param_find("ATT_ROLL_OFFS");
+	h->pitch_off =	param_find("ATT_PITCH_OFFS");
+	h->yaw_off   =	param_find("ATT_YAW_OFFS");
+
+	return OK;
+}
+
+int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p)
+{
+	param_get(h->q0, &(p->q[0]));
+	param_get(h->q1, &(p->q[1]));
+	param_get(h->q2, &(p->q[2]));
+	param_get(h->q3, &(p->q[3]));
+	param_get(h->q4, &(p->q[4]));
+
+	param_get(h->r0, &(p->r[0]));
+	param_get(h->r1, &(p->r[1]));
+	param_get(h->r2, &(p->r[2]));
+	param_get(h->r3, &(p->r[3]));
+
+	param_get(h->roll_off, &(p->roll_off));
+	param_get(h->pitch_off, &(p->pitch_off));
+	param_get(h->yaw_off, &(p->yaw_off));
+
+	return OK;
+}

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h

@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/*
+ * @file attitude_estimator_ekf_params.h
+ *
+ * Parameters for EKF filter
+ */
+
+#include <systemlib/param/param.h>
+
+struct attitude_estimator_ekf_params {
+	float r[9];
+	float q[12];
+	float roll_off;
+	float pitch_off;
+	float yaw_off;
+};
+
+struct attitude_estimator_ekf_param_handles {
+	param_t r0, r1, r2, r3;
+	param_t q0, q1, q2, q3, q4;
+	param_t roll_off, pitch_off, yaw_off;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct attitude_estimator_ekf_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p);

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h

@@ -0,0 +1,34 @@
+/*
+ * attitudeKalmanfilter.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_H__
+#define __ATTITUDEKALMANFILTER_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 attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
+#endif
+/* End of code generation (attitudeKalmanfilter.h) */

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c

@@ -0,0 +1,31 @@
+/*
+ * attitudeKalmanfilter_initialize.c
+ *
+ * Code generation for function 'attitudeKalmanfilter_initialize'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "attitudeKalmanfilter_initialize.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void attitudeKalmanfilter_initialize(void)
+{
+  rt_InitInfAndNaN(8U);
+}
+
+/* End of code generation (attitudeKalmanfilter_initialize.c) */

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h

@@ -0,0 +1,34 @@
+/*
+ * attitudeKalmanfilter_initialize.h
+ *
+ * Code generation for function 'attitudeKalmanfilter_initialize'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
+#define __ATTITUDEKALMANFILTER_INITIALIZE_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 attitudeKalmanfilter_initialize(void);
+#endif
+/* End of code generation (attitudeKalmanfilter_initialize.h) */

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c

@@ -0,0 +1,31 @@
+/*
+ * attitudeKalmanfilter_terminate.c
+ *
+ * Code generation for function 'attitudeKalmanfilter_terminate'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "attitudeKalmanfilter_terminate.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+void attitudeKalmanfilter_terminate(void)
+{
+  /* (no terminate code required) */
+}
+
+/* End of code generation (attitudeKalmanfilter_terminate.c) */

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h

@@ -0,0 +1,34 @@
+/*
+ * attitudeKalmanfilter_terminate.h
+ *
+ * Code generation for function 'attitudeKalmanfilter_terminate'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
+#define __ATTITUDEKALMANFILTER_TERMINATE_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 attitudeKalmanfilter_terminate(void);
+#endif
+/* End of code generation (attitudeKalmanfilter_terminate.h) */

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h

@@ -0,0 +1,16 @@
+/*
+ * attitudeKalmanfilter_types.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __ATTITUDEKALMANFILTER_TYPES_H__
+#define __ATTITUDEKALMANFILTER_TYPES_H__
+
+/* Type Definitions */
+
+#endif
+/* End of code generation (attitudeKalmanfilter_types.h) */

src/modules/attitude_estimator_ekf/codegen/cross.c

@@ -0,0 +1,37 @@
+/*
+ * cross.c
+ *
+ * Code generation for function 'cross'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "cross.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void cross(const real32_T a[3], const real32_T b[3], real32_T c[3])
+{
+  c[0] = a[1] * b[2] - a[2] * b[1];
+  c[1] = a[2] * b[0] - a[0] * b[2];
+  c[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+/* End of code generation (cross.c) */

src/modules/attitude_estimator_ekf/codegen/cross.h

@@ -0,0 +1,34 @@
+/*
+ * cross.h
+ *
+ * Code generation for function 'cross'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __CROSS_H__
+#define __CROSS_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 cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
+#endif
+/* End of code generation (cross.h) */

src/modules/attitude_estimator_ekf/codegen/eye.c

@@ -0,0 +1,51 @@
+/*
+ * eye.c
+ *
+ * Code generation for function 'eye'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "eye.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+void b_eye(real_T I[144])
+{
+  int32_T i;
+  memset(&I[0], 0, 144U * sizeof(real_T));
+  for (i = 0; i < 12; i++) {
+    I[i + 12 * i] = 1.0;
+  }
+}
+
+/*
+ *
+ */
+void eye(real_T I[9])
+{
+  int32_T i;
+  memset(&I[0], 0, 9U * sizeof(real_T));
+  for (i = 0; i < 3; i++) {
+    I[i + 3 * i] = 1.0;
+  }
+}
+
+/* End of code generation (eye.c) */

src/modules/attitude_estimator_ekf/codegen/eye.h

@@ -0,0 +1,35 @@
+/*
+ * eye.h
+ *
+ * Code generation for function 'eye'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __EYE_H__
+#define __EYE_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 b_eye(real_T I[144]);
+extern void eye(real_T I[9]);
+#endif
+/* End of code generation (eye.h) */

src/modules/attitude_estimator_ekf/codegen/mrdivide.c

@@ -0,0 +1,357 @@
+/*
+ * mrdivide.c
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "mrdivide.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+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_B[36];
+  int32_T r1;
+  int32_T r2;
+  int32_T r3;
+  real32_T maxval;
+  real32_T a21;
+  real32_T Y[36];
+  for (rtemp = 0; rtemp < 3; rtemp++) {
+    for (k = 0; k < 3; k++) {
+      b_A[k + 3 * rtemp] = B[rtemp + 3 * k];
+    }
+  }
+
+  for (rtemp = 0; rtemp < 12; rtemp++) {
+    for (k = 0; k < 3; k++) {
+      b_B[k + 3 * rtemp] = A[rtemp + 12 * k];
+    }
+  }
+
+  r1 = 0;
+  r2 = 1;
+  r3 = 2;
+  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)fabs(b_A[2]) > maxval) {
+    r1 = 2;
+    r2 = 1;
+    r3 = 0;
+  }
+
+  b_A[r2] /= b_A[r1];
+  b_A[r3] /= b_A[r1];
+  b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
+  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)fabs(b_A[3 + r3]) > (real32_T)fabs(b_A[3 + r2])) {
+    rtemp = r2;
+    r2 = r3;
+    r3 = rtemp;
+  }
+
+  b_A[3 + r3] /= b_A[3 + r2];
+  b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
+  for (k = 0; k < 12; k++) {
+    Y[3 * k] = b_B[r1 + 3 * k];
+    Y[1 + 3 * k] = b_B[r2 + 3 * k] - Y[3 * k] * b_A[r2];
+    Y[2 + 3 * k] = (b_B[r3 + 3 * k] - Y[3 * k] * b_A[r3]) - Y[1 + 3 * k] * b_A[3
+      + r3];
+    Y[2 + 3 * k] /= b_A[6 + r3];
+    Y[3 * k] -= Y[2 + 3 * k] * b_A[6 + r1];
+    Y[1 + 3 * k] -= Y[2 + 3 * k] * b_A[6 + r2];
+    Y[1 + 3 * k] /= b_A[3 + r2];
+    Y[3 * k] -= Y[1 + 3 * k] * b_A[3 + r1];
+    Y[3 * k] /= b_A[r1];
+  }
+
+  for (rtemp = 0; rtemp < 3; rtemp++) {
+    for (k = 0; k < 12; k++) {
+      y[k + 12 * rtemp] = Y[rtemp + 3 * k];
+    }
+  }
+}
+
+/*
+ *
+ */
+void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72])
+{
+  real32_T b_A[36];
+  int8_T ipiv[6];
+  int32_T i3;
+  int32_T iy;
+  int32_T j;
+  int32_T c;
+  int32_T ix;
+  real32_T temp;
+  int32_T k;
+  real32_T s;
+  int32_T jy;
+  int32_T ijA;
+  real32_T Y[72];
+  for (i3 = 0; i3 < 6; i3++) {
+    for (iy = 0; iy < 6; iy++) {
+      b_A[iy + 6 * i3] = B[i3 + 6 * iy];
+    }
+
+    ipiv[i3] = (int8_T)(1 + i3);
+  }
+
+  for (j = 0; j < 5; j++) {
+    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)fabs(b_A[ix]);
+      if (s > temp) {
+        iy = k - 1;
+        temp = s;
+      }
+    }
+
+    if (b_A[c + iy] != 0.0F) {
+      if (iy != 0) {
+        ipiv[j] = (int8_T)((j + iy) + 1);
+        ix = j;
+        iy += j;
+        for (k = 0; k < 6; k++) {
+          temp = b_A[ix];
+          b_A[ix] = b_A[iy];
+          b_A[iy] = temp;
+          ix += 6;
+          iy += 6;
+        }
+      }
+
+      i3 = (c - j) + 6;
+      for (jy = c + 1; jy + 1 <= i3; jy++) {
+        b_A[jy] /= b_A[c];
+      }
+    }
+
+    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;
+      iy += 6;
+    }
+  }
+
+  for (i3 = 0; i3 < 12; i3++) {
+    for (iy = 0; iy < 6; iy++) {
+      Y[iy + 6 * i3] = A[i3 + 12 * iy];
+    }
+  }
+
+  for (jy = 0; jy < 6; jy++) {
+    if (ipiv[jy] != jy + 1) {
+      for (j = 0; j < 12; j++) {
+        temp = Y[jy + 6 * j];
+        Y[jy + 6 * j] = Y[(ipiv[jy] + 6 * j) - 1];
+        Y[(ipiv[jy] + 6 * j) - 1] = temp;
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 6 * j;
+    for (k = 0; k < 6; k++) {
+      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];
+        }
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 6 * j;
+    for (k = 5; k > -1; k += -1) {
+      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 (i3 = 0; i3 < 6; i3++) {
+    for (iy = 0; iy < 12; iy++) {
+      y[iy + 12 * i3] = Y[i3 + 6 * iy];
+    }
+  }
+}
+
+/*
+ *
+ */
+void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108])
+{
+  real32_T b_A[81];
+  int8_T ipiv[9];
+  int32_T i2;
+  int32_T iy;
+  int32_T j;
+  int32_T c;
+  int32_T ix;
+  real32_T temp;
+  int32_T k;
+  real32_T s;
+  int32_T jy;
+  int32_T ijA;
+  real32_T Y[108];
+  for (i2 = 0; i2 < 9; i2++) {
+    for (iy = 0; iy < 9; iy++) {
+      b_A[iy + 9 * i2] = B[i2 + 9 * iy];
+    }
+
+    ipiv[i2] = (int8_T)(1 + i2);
+  }
+
+  for (j = 0; j < 8; j++) {
+    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)fabs(b_A[ix]);
+      if (s > temp) {
+        iy = k - 1;
+        temp = s;
+      }
+    }
+
+    if (b_A[c + iy] != 0.0F) {
+      if (iy != 0) {
+        ipiv[j] = (int8_T)((j + iy) + 1);
+        ix = j;
+        iy += j;
+        for (k = 0; k < 9; k++) {
+          temp = b_A[ix];
+          b_A[ix] = b_A[iy];
+          b_A[iy] = temp;
+          ix += 9;
+          iy += 9;
+        }
+      }
+
+      i2 = (c - j) + 9;
+      for (jy = c + 1; jy + 1 <= i2; jy++) {
+        b_A[jy] /= b_A[c];
+      }
+    }
+
+    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;
+      iy += 9;
+    }
+  }
+
+  for (i2 = 0; i2 < 12; i2++) {
+    for (iy = 0; iy < 9; iy++) {
+      Y[iy + 9 * i2] = A[i2 + 12 * iy];
+    }
+  }
+
+  for (jy = 0; jy < 9; jy++) {
+    if (ipiv[jy] != jy + 1) {
+      for (j = 0; j < 12; j++) {
+        temp = Y[jy + 9 * j];
+        Y[jy + 9 * j] = Y[(ipiv[jy] + 9 * j) - 1];
+        Y[(ipiv[jy] + 9 * j) - 1] = temp;
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 9 * j;
+    for (k = 0; k < 9; k++) {
+      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];
+        }
+      }
+    }
+  }
+
+  for (j = 0; j < 12; j++) {
+    c = 9 * j;
+    for (k = 8; k > -1; k += -1) {
+      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 (i2 = 0; i2 < 9; i2++) {
+    for (iy = 0; iy < 12; iy++) {
+      y[iy + 12 * i2] = Y[i2 + 9 * iy];
+    }
+  }
+}
+
+/* End of code generation (mrdivide.c) */

src/modules/attitude_estimator_ekf/codegen/mrdivide.h

@@ -0,0 +1,36 @@
+/*
+ * mrdivide.h
+ *
+ * Code generation for function 'mrdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __MRDIVIDE_H__
+#define __MRDIVIDE_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 b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
+extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
+extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
+#endif
+/* End of code generation (mrdivide.h) */

src/modules/attitude_estimator_ekf/codegen/norm.c

@@ -0,0 +1,54 @@
+/*
+ * norm.c
+ *
+ * Code generation for function 'norm'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* Include files */
+#include "rt_nonfinite.h"
+#include "attitudeKalmanfilter.h"
+#include "norm.h"
+
+/* Type Definitions */
+
+/* Named Constants */
+
+/* Variable Declarations */
+
+/* Variable Definitions */
+
+/* Function Declarations */
+
+/* Function Definitions */
+
+/*
+ *
+ */
+real32_T norm(const real32_T x[3])
+{
+  real32_T y;
+  real32_T scale;
+  int32_T k;
+  real32_T absxk;
+  real32_T t;
+  y = 0.0F;
+  scale = 1.17549435E-38F;
+  for (k = 0; k < 3; k++) {
+    absxk = (real32_T)fabs(x[k]);
+    if (absxk > scale) {
+      t = scale / absxk;
+      y = 1.0F + y * t * t;
+      scale = absxk;
+    } else {
+      t = absxk / scale;
+      y += t * t;
+    }
+  }
+
+  return scale * (real32_T)sqrt(y);
+}
+
+/* End of code generation (norm.c) */

src/modules/attitude_estimator_ekf/codegen/norm.h

@@ -0,0 +1,34 @@
+/*
+ * norm.h
+ *
+ * Code generation for function 'norm'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __NORM_H__
+#define __NORM_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 real32_T norm(const real32_T x[3]);
+#endif
+/* End of code generation (norm.h) */

src/modules/attitude_estimator_ekf/codegen/rdivide.c

@@ -0,0 +1,38 @@
+/*
+ * rdivide.c
+ *
+ * Code generation for function 'rdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/* 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) */

src/modules/attitude_estimator_ekf/codegen/rdivide.h

@@ -0,0 +1,34 @@
+/*
+ * rdivide.h
+ *
+ * Code generation for function 'rdivide'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#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) */

src/modules/attitude_estimator_ekf/codegen/rtGetInf.c

@@ -0,0 +1,139 @@
+/*
+ * rtGetInf.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ *       MATLAB for code generation function to initialize non-finite, Inf and MinusInf
+ */
+#include "rtGetInf.h"
+#define NumBitsPerChar	8U
+
+/* Function: rtGetInf ==================================================
+ * Abstract:
+ * Initialize rtInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetInf(void)
+{
+  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+  real_T inf = 0.0;
+  if (bitsPerReal == 32U) {
+    inf = rtGetInfF();
+  } else {
+    uint16_T one = 1U;
+    enum {
+      LittleEndian,
+      BigEndian
+    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+    switch (machByteOrder) {
+     case LittleEndian:
+      {
+        union {
+          LittleEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0x7FF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        inf = tmpVal.fltVal;
+        break;
+      }
+
+     case BigEndian:
+      {
+        union {
+          BigEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0x7FF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        inf = tmpVal.fltVal;
+        break;
+      }
+    }
+  }
+
+  return inf;
+}
+
+/* Function: rtGetInfF ==================================================
+ * Abstract:
+ * Initialize rtInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetInfF(void)
+{
+  IEEESingle infF;
+  infF.wordL.wordLuint = 0x7F800000U;
+  return infF.wordL.wordLreal;
+}
+
+/* Function: rtGetMinusInf ==================================================
+ * Abstract:
+ * Initialize rtMinusInf needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetMinusInf(void)
+{
+  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+  real_T minf = 0.0;
+  if (bitsPerReal == 32U) {
+    minf = rtGetMinusInfF();
+  } else {
+    uint16_T one = 1U;
+    enum {
+      LittleEndian,
+      BigEndian
+    } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+    switch (machByteOrder) {
+     case LittleEndian:
+      {
+        union {
+          LittleEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0xFFF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        minf = tmpVal.fltVal;
+        break;
+      }
+
+     case BigEndian:
+      {
+        union {
+          BigEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0xFFF00000U;
+        tmpVal.bitVal.words.wordL = 0x00000000U;
+        minf = tmpVal.fltVal;
+        break;
+      }
+    }
+  }
+
+  return minf;
+}
+
+/* Function: rtGetMinusInfF ==================================================
+ * Abstract:
+ * Initialize rtMinusInfF needed by the generated code.
+ * Inf is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetMinusInfF(void)
+{
+  IEEESingle minfF;
+  minfF.wordL.wordLuint = 0xFF800000U;
+  return minfF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetInf.c) */

src/modules/attitude_estimator_ekf/codegen/rtGetInf.h

@@ -0,0 +1,23 @@
+/*
+ * rtGetInf.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RTGETINF_H__
+#define __RTGETINF_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetInf(void);
+extern real32_T rtGetInfF(void);
+extern real_T rtGetMinusInf(void);
+extern real32_T rtGetMinusInfF(void);
+
+#endif
+/* End of code generation (rtGetInf.h) */

src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c

@@ -0,0 +1,96 @@
+/*
+ * rtGetNaN.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ *       MATLAB for code generation function to initialize non-finite, NaN
+ */
+#include "rtGetNaN.h"
+#define NumBitsPerChar	8U
+
+/* Function: rtGetNaN ==================================================
+ * Abstract:
+ * Initialize rtNaN needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real_T rtGetNaN(void)
+{
+  size_t bitsPerReal = sizeof(real_T) * (NumBitsPerChar);
+  real_T nan = 0.0;
+  if (bitsPerReal == 32U) {
+    nan = rtGetNaNF();
+  } else {
+  uint16_T one = 1U;
+  enum {
+    LittleEndian,
+    BigEndian
+  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+  switch (machByteOrder) {
+   case LittleEndian:
+    {
+      union {
+        LittleEndianIEEEDouble bitVal;
+        real_T fltVal;
+      } tmpVal;
+
+      tmpVal.bitVal.words.wordH = 0xFFF80000U;
+      tmpVal.bitVal.words.wordL = 0x00000000U;
+      nan = tmpVal.fltVal;
+      break;
+    }
+
+   case BigEndian:
+    {
+        union {
+          BigEndianIEEEDouble bitVal;
+          real_T fltVal;
+        } tmpVal;
+
+        tmpVal.bitVal.words.wordH = 0x7FFFFFFFU;
+        tmpVal.bitVal.words.wordL = 0xFFFFFFFFU;
+        nan = tmpVal.fltVal;
+        break;
+      }
+    }
+  }
+
+  return nan;
+}
+
+/* Function: rtGetNaNF ==================================================
+ * Abstract:
+ * Initialize rtNaNF needed by the generated code.
+ * NaN is initialized as non-signaling. Assumes IEEE.
+ */
+real32_T rtGetNaNF(void)
+{
+  IEEESingle nanF = { { 0 } };
+  uint16_T one = 1U;
+  enum {
+    LittleEndian,
+    BigEndian
+  } machByteOrder = (*((uint8_T *) &one) == 1U) ? LittleEndian : BigEndian;
+  switch (machByteOrder) {
+   case LittleEndian:
+    {
+      nanF.wordL.wordLuint = 0xFFC00000U;
+      break;
+    }
+
+   case BigEndian:
+    {
+      nanF.wordL.wordLuint = 0x7FFFFFFFU;
+      break;
+    }
+  }
+
+  return nanF.wordL.wordLreal;
+}
+
+/* End of code generation (rtGetNaN.c) */

src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h

@@ -0,0 +1,21 @@
+/*
+ * rtGetNaN.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RTGETNAN_H__
+#define __RTGETNAN_H__
+
+#include <stddef.h>
+#include "rtwtypes.h"
+#include "rt_nonfinite.h"
+
+extern real_T rtGetNaN(void);
+extern real32_T rtGetNaNF(void);
+
+#endif
+/* End of code generation (rtGetNaN.h) */

src/modules/attitude_estimator_ekf/codegen/rt_defines.h

@@ -0,0 +1,24 @@
+/*
+ * rt_defines.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RT_DEFINES_H__
+#define __RT_DEFINES_H__
+
+#include <stdlib.h>
+
+#define RT_PI	3.14159265358979323846
+#define RT_PIF	3.1415927F
+#define RT_LN_10	2.30258509299404568402
+#define RT_LN_10F	2.3025851F
+#define RT_LOG10E	0.43429448190325182765
+#define RT_LOG10EF	0.43429449F
+#define RT_E	2.7182818284590452354
+#define RT_EF	2.7182817F
+#endif
+/* End of code generation (rt_defines.h) */

src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c

@@ -0,0 +1,87 @@
+/*
+ * rt_nonfinite.c
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+/*
+ * Abstract:
+ *      MATLAB for code generation function to initialize non-finites,
+ *      (Inf, NaN and -Inf).
+ */
+#include "rt_nonfinite.h"
+#include "rtGetNaN.h"
+#include "rtGetInf.h"
+
+real_T rtInf;
+real_T rtMinusInf;
+real_T rtNaN;
+real32_T rtInfF;
+real32_T rtMinusInfF;
+real32_T rtNaNF;
+
+/* Function: rt_InitInfAndNaN ==================================================
+ * Abstract:
+ * Initialize the rtInf, rtMinusInf, and rtNaN needed by the
+ * generated code. NaN is initialized as non-signaling. Assumes IEEE.
+ */
+void rt_InitInfAndNaN(size_t realSize)
+{
+  (void) (realSize);
+  rtNaN = rtGetNaN();
+  rtNaNF = rtGetNaNF();
+  rtInf = rtGetInf();
+  rtInfF = rtGetInfF();
+  rtMinusInf = rtGetMinusInf();
+  rtMinusInfF = rtGetMinusInfF();
+}
+
+/* Function: rtIsInf ==================================================
+ * Abstract:
+ * Test if value is infinite
+ */
+boolean_T rtIsInf(real_T value)
+{
+  return ((value==rtInf || value==rtMinusInf) ? 1U : 0U);
+}
+
+/* Function: rtIsInfF =================================================
+ * Abstract:
+ * Test if single-precision value is infinite
+ */
+boolean_T rtIsInfF(real32_T value)
+{
+  return(((value)==rtInfF || (value)==rtMinusInfF) ? 1U : 0U);
+}
+
+/* Function: rtIsNaN ==================================================
+ * Abstract:
+ * Test if value is not a number
+ */
+boolean_T rtIsNaN(real_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+  return _isnan(value)? TRUE:FALSE;
+#else
+  return (value!=value)? 1U:0U;
+#endif
+}
+
+/* Function: rtIsNaNF =================================================
+ * Abstract:
+ * Test if single-precision value is not a number
+ */
+boolean_T rtIsNaNF(real32_T value)
+{
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+  return _isnan((real_T)value)? true:false;
+#else
+  return (value!=value)? 1U:0U;
+#endif
+}
+
+
+/* End of code generation (rt_nonfinite.c) */

src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h

@@ -0,0 +1,53 @@
+/*
+ * rt_nonfinite.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RT_NONFINITE_H__
+#define __RT_NONFINITE_H__
+
+#if defined(_MSC_VER) && (_MSC_VER <= 1200)
+#include <float.h>
+#endif
+#include <stddef.h>
+#include "rtwtypes.h"
+
+extern real_T rtInf;
+extern real_T rtMinusInf;
+extern real_T rtNaN;
+extern real32_T rtInfF;
+extern real32_T rtMinusInfF;
+extern real32_T rtNaNF;
+extern void rt_InitInfAndNaN(size_t realSize);
+extern boolean_T rtIsInf(real_T value);
+extern boolean_T rtIsInfF(real32_T value);
+extern boolean_T rtIsNaN(real_T value);
+extern boolean_T rtIsNaNF(real32_T value);
+
+typedef struct {
+  struct {
+    uint32_T wordH;
+    uint32_T wordL;
+  } words;
+} BigEndianIEEEDouble;
+
+typedef struct {
+  struct {
+    uint32_T wordL;
+    uint32_T wordH;
+  } words;
+} LittleEndianIEEEDouble;
+
+typedef struct {
+  union {
+    real32_T wordLreal;
+    uint32_T wordLuint;
+  } wordL;
+} IEEESingle;
+
+#endif
+/* End of code generation (rt_nonfinite.h) */

src/modules/attitude_estimator_ekf/codegen/rtwtypes.h

@@ -0,0 +1,159 @@
+/*
+ * rtwtypes.h
+ *
+ * Code generation for function 'attitudeKalmanfilter'
+ *
+ * C source code generated on: Sat Jan 19 15:25:29 2013
+ *
+ */
+
+#ifndef __RTWTYPES_H__
+#define __RTWTYPES_H__
+#ifndef TRUE
+# define TRUE (1U)
+#endif
+#ifndef FALSE
+# define FALSE (0U)
+#endif
+#ifndef __TMWTYPES__
+#define __TMWTYPES__
+
+#include <limits.h>
+
+/*=======================================================================* 
+ * Target hardware information
+ *   Device type: Generic->MATLAB Host Computer
+ *   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
+ *=======================================================================*/
+
+/*=======================================================================* 
+ * Fixed width word size data types:                                     * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *   real32_T, real64_T           - 32 and 64 bit floating point numbers * 
+ *=======================================================================*/
+
+typedef signed char int8_T;
+typedef unsigned char uint8_T;
+typedef short int16_T;
+typedef unsigned short uint16_T;
+typedef int int32_T;
+typedef unsigned int uint32_T;
+typedef float real32_T;
+typedef double real64_T;
+
+/*===========================================================================* 
+ * Generic type definitions: real_T, time_T, boolean_T, int_T, uint_T,       * 
+ *                           ulong_T, char_T and byte_T.                     * 
+ *===========================================================================*/
+
+typedef double real_T;
+typedef double time_T;
+typedef unsigned char boolean_T;
+typedef int int_T;
+typedef unsigned int uint_T;
+typedef unsigned long ulong_T;
+typedef char char_T;
+typedef char_T byte_T;
+
+/*===========================================================================* 
+ * Complex number type definitions                                           * 
+ *===========================================================================*/
+#define CREAL_T	
+   typedef struct {  
+     real32_T re;  
+     real32_T im;  
+   } creal32_T;  
+
+   typedef struct {  
+     real64_T re;  
+     real64_T im;  
+   } creal64_T;  
+
+   typedef struct {  
+     real_T re;  
+     real_T im;  
+   } creal_T;  
+
+   typedef struct {  
+     int8_T re;  
+     int8_T im;  
+   } cint8_T;  
+
+   typedef struct {  
+     uint8_T re;  
+     uint8_T im;  
+   } cuint8_T;  
+
+   typedef struct {  
+     int16_T re;  
+     int16_T im;  
+   } cint16_T;  
+
+   typedef struct {  
+     uint16_T re;  
+     uint16_T im;  
+   } cuint16_T;  
+
+   typedef struct {  
+     int32_T re;  
+     int32_T im;  
+   } cint32_T;  
+
+   typedef struct {  
+     uint32_T re;  
+     uint32_T im;  
+   } cuint32_T;  
+
+
+/*=======================================================================* 
+ * Min and Max:                                                          * 
+ *   int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     * 
+ *   uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   * 
+ *=======================================================================*/
+
+#define MAX_int8_T  	((int8_T)(127))
+#define MIN_int8_T  	((int8_T)(-128))
+#define MAX_uint8_T 	((uint8_T)(255))
+#define MIN_uint8_T 	((uint8_T)(0))
+#define MAX_int16_T 	((int16_T)(32767))
+#define MIN_int16_T 	((int16_T)(-32768))
+#define MAX_uint16_T	((uint16_T)(65535))
+#define MIN_uint16_T	((uint16_T)(0))
+#define MAX_int32_T 	((int32_T)(2147483647))
+#define MIN_int32_T 	((int32_T)(-2147483647-1))
+#define MAX_uint32_T	((uint32_T)(0xFFFFFFFFU))
+#define MIN_uint32_T	((uint32_T)(0))
+
+/* Logical type definitions */
+#if !defined(__cplusplus) && !defined(__true_false_are_keywords)
+#  ifndef false
+#   define false (0U)
+#  endif
+#  ifndef true
+#   define true (1U)
+#  endif
+#endif
+
+/*
+ * MATLAB for code generation assumes the code is compiled on a target using a 2's compliment representation
+ * for signed integer values.
+ */
+#if ((SCHAR_MIN + 1) != -SCHAR_MAX)
+#error "This code must be compiled using a 2's complement representation for signed integer values"
+#endif
+
+/*
+ * Maximum length of a MATLAB identifier (function/variable)
+ * including the null-termination character. Referenced by
+ * rt_logging.c and rt_matrx.c.
+ */
+#define TMW_NAME_LENGTH_MAX	64
+
+#endif
+#endif
+/* End of code generation (rtwtypes.h) */

src/modules/attitude_estimator_ekf/module.mk

@@ -0,0 +1,16 @@
+
+MODULE_NAME	 = attitude_estimator_ekf
+SRCS		 = attitude_estimator_ekf_main.cpp \
+			   attitude_estimator_ekf_params.c \
+			   codegen/attitudeKalmanfilter_initialize.c \
+			   codegen/attitudeKalmanfilter_terminate.c \
+			   codegen/attitudeKalmanfilter.c \
+			   codegen/cross.c \
+			   codegen/eye.c \
+			   codegen/mrdivide.c \
+			   codegen/norm.c \
+			   codegen/rdivide.c \
+			   codegen/rt_nonfinite.c \
+			   codegen/rtGetInf.c \
+			   codegen/rtGetNaN.c
+

src/modules/commander/calibration_routines.c

@@ -0,0 +1,219 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file calibration_routines.c
+ * Calibration routines implementations.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#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;
+}

src/modules/commander/calibration_routines.h

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file calibration_routines.h
+ * Calibration routines definitions.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+/**
+ * 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);

src/modules/commander/commander.h

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *           Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander.h
+ * Main system state machine definition.
+ *
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ *
+ */
+
+#ifndef COMMANDER_H_
+#define COMMANDER_H_
+
+#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
+#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
+
+void tune_confirm(void);
+void tune_error(void);
+
+#endif /* COMMANDER_H_ */

src/modules/commander/module.mk

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (C) 2012-2013 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.
+#
+############################################################################
+
+#
+# Main system state machine
+#
+
+MODULE_COMMAND	 = commander
+SRCS		 = commander.c \
+			   state_machine_helper.c \
+			   calibration_routines.c

src/modules/commander/state_machine_helper.c

@@ -0,0 +1,752 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file state_machine_helper.c
+ * State machine helper functions implementations
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/actuator_controls.h>
+#include <systemlib/systemlib.h>
+#include <drivers/drv_hrt.h>
+#include <mavlink/mavlink_log.h>
+
+#include "state_machine_helper.h"
+
+static const char *system_state_txt[] = {
+	"SYSTEM_STATE_PREFLIGHT",
+	"SYSTEM_STATE_STANDBY",
+	"SYSTEM_STATE_GROUND_READY",
+	"SYSTEM_STATE_MANUAL",
+	"SYSTEM_STATE_STABILIZED",
+	"SYSTEM_STATE_AUTO",
+	"SYSTEM_STATE_MISSION_ABORT",
+	"SYSTEM_STATE_EMCY_LANDING",
+	"SYSTEM_STATE_EMCY_CUTOFF",
+	"SYSTEM_STATE_GROUND_ERROR",
+	"SYSTEM_STATE_REBOOT",
+
+};
+
+/**
+ * Transition from one state to another
+ */
+int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state)
+{
+	int invalid_state = false;
+	int ret = ERROR;
+
+	commander_state_machine_t old_state = current_status->state_machine;
+
+	switch (new_state) {
+	case SYSTEM_STATE_MISSION_ABORT: {
+			/* Indoor or outdoor */
+			// if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+			ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING);
+
+			// } else {
+			// 	ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF);
+			// }
+		}
+		break;
+
+	case SYSTEM_STATE_EMCY_LANDING:
+		/* Tell the controller to land */
+
+		/* set system flags according to state */
+		current_status->flag_system_armed = true;
+
+		warnx("EMERGENCY LANDING!\n");
+		mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!");
+		break;
+
+	case SYSTEM_STATE_EMCY_CUTOFF:
+		/* Tell the controller to cutoff the motors (thrust = 0) */
+
+		/* set system flags according to state */
+		current_status->flag_system_armed = false;
+
+		warnx("EMERGENCY MOTOR CUTOFF!\n");
+		mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!");
+		break;
+
+	case SYSTEM_STATE_GROUND_ERROR:
+
+		/* set system flags according to state */
+
+		/* prevent actuators from arming */
+		current_status->flag_system_armed = false;
+
+		warnx("GROUND ERROR, locking down propulsion system\n");
+		mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system");
+		break;
+
+	case SYSTEM_STATE_PREFLIGHT:
+		if (current_status->state_machine == SYSTEM_STATE_STANDBY
+		    || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+			/* set system flags according to state */
+			current_status->flag_system_armed = false;
+			mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state");
+
+		} else {
+			invalid_state = true;
+			mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state");
+		}
+
+		break;
+
+	case SYSTEM_STATE_REBOOT:
+		if (current_status->state_machine == SYSTEM_STATE_STANDBY
+			|| current_status->state_machine == SYSTEM_STATE_PREFLIGHT
+			|| current_status->flag_hil_enabled) {
+			invalid_state = false;
+			/* set system flags according to state */
+			current_status->flag_system_armed = false;
+			mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
+			usleep(500000);
+			up_systemreset();
+			/* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */
+
+		} else {
+			invalid_state = true;
+			mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT");
+		}
+
+		break;
+
+	case SYSTEM_STATE_STANDBY:
+		/* set system flags according to state */
+
+		/* standby enforces disarmed */
+		current_status->flag_system_armed = false;
+
+		mavlink_log_critical(mavlink_fd, "Switched to STANDBY state");
+		break;
+
+	case SYSTEM_STATE_GROUND_READY:
+
+		/* set system flags according to state */
+
+		/* ground ready has motors / actuators armed */
+		current_status->flag_system_armed = true;
+
+		mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state");
+		break;
+
+	case SYSTEM_STATE_AUTO:
+
+		/* set system flags according to state */
+
+		/* auto is airborne and in auto mode, motors armed */
+		current_status->flag_system_armed = true;
+
+		mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode");
+		break;
+
+	case SYSTEM_STATE_STABILIZED:
+
+		/* set system flags according to state */
+		current_status->flag_system_armed = true;
+
+		mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode");
+		break;
+
+	case SYSTEM_STATE_MANUAL:
+
+		/* set system flags according to state */
+		current_status->flag_system_armed = true;
+
+		mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode");
+		break;
+
+	default:
+		invalid_state = true;
+		break;
+	}
+
+	if (invalid_state == false || old_state != new_state) {
+		current_status->state_machine = new_state;
+		state_machine_publish(status_pub, current_status, mavlink_fd);
+		publish_armed_status(current_status);
+		ret = OK;
+	}
+
+	if (invalid_state) {
+		mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition");
+		ret = ERROR;
+	}
+
+	return ret;
+}
+
+void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	/* publish the new state */
+	current_status->counter++;
+	current_status->timestamp = hrt_absolute_time();
+
+	/* assemble state vector based on flag values */
+	if (current_status->flag_control_rates_enabled) {
+		current_status->onboard_control_sensors_present |= 0x400;
+
+	} else {
+		current_status->onboard_control_sensors_present &= ~0x400;
+	}
+
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
+	current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
+
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
+	current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
+
+	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+	printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]);
+}
+
+void publish_armed_status(const struct vehicle_status_s *current_status)
+{
+	struct actuator_armed_s armed;
+	armed.armed = current_status->flag_system_armed;
+	/* lock down actuators if required, only in HIL */
+	armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
+	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
+	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
+}
+
+
+/*
+ * Private functions, update the state machine
+ */
+void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	warnx("EMERGENCY HANDLER\n");
+	/* Depending on the current state go to one of the error states */
+
+	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR);
+
+	} else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) {
+
+		// DO NOT abort mission
+		//do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT);
+
+	} else {
+		warnx("Unknown system state: #%d\n", current_status->state_machine);
+	}
+}
+
+void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors
+{
+	if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself
+		state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd);
+
+	} else {
+		//global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL);
+	}
+
+}
+
+
+
+// /*
+//  * Wrapper functions (to be used in the commander), all functions assume lock on current_status
+//  */
+
+// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR
+//  *
+//  * START SUBSYSTEM/EMERGENCY FUNCTIONS
+//  * */
+
+// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_present |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if a subsystem was removed something went completely wrong */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		{
+// 			uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV];
+
+// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// 				//global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY);
+// 				state_machine_emergency(status_pub, current_status);
+// 			}
+// 		}
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+// }
+
+// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if a subsystem was disabled something went completely wrong */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY);
+// 		state_machine_emergency_always_critical(status_pub, current_status);
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		{
+// 			uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]);
+
+// 			if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) {
+// 				//global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY);
+// 				state_machine_emergency(status_pub, current_status);
+// 			}
+// 		}
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+
+// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	current_status->onboard_control_sensors_health |= 1 << *subsystem_type;
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//TODO state machine change (recovering)
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//TODO state machine change
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//TODO state machine change
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// 		//TODO state machine change
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+
+// }
+
+
+// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type)
+// {
+// 	bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type);
+// 	current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type);
+// 	current_status->counter++;
+// 	current_status->timestamp = hrt_absolute_time();
+// 	orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
+
+// 	/* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */
+
+// 	switch (*subsystem_type) {
+// 	case SUBSYSTEM_TYPE_GYRO:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL);
+
+// 		if (previosly_healthy) 	//only throw emergency if previously healthy
+// 			state_machine_emergency_always_critical(status_pub, current_status);
+
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_ACC:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL);
+
+// 		if (previosly_healthy) 	//only throw emergency if previously healthy
+// 			state_machine_emergency_always_critical(status_pub, current_status);
+
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_MAG:
+// 		//global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL);
+
+// 		if (previosly_healthy) 	//only throw emergency if previously healthy
+// 			state_machine_emergency_always_critical(status_pub, current_status);
+
+// 		break;
+
+// 	case SUBSYSTEM_TYPE_GPS:
+// //			//TODO: remove this block
+// //			break;
+// //			///////////////////
+// 		//global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL);
+
+// //				printf("previosly_healthy = %u\n", previosly_healthy);
+// 		if (previosly_healthy) 	//only throw emergency if previously healthy
+// 			state_machine_emergency(status_pub, current_status);
+
+// 		break;
+
+// 	default:
+// 		break;
+// 	}
+
+// }
+
+
+/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/
+
+
+void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	/* Depending on the current state switch state */
+	if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+	}
+}
+
+void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	/* Depending on the current state switch state */
+	if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		state_machine_emergency(status_pub, current_status, mavlink_fd);
+	}
+}
+
+void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	if (current_status->state_machine == SYSTEM_STATE_STANDBY) {
+		printf("[cmd] arming\n");
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+	}
+}
+
+void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+		printf("[cmd] going standby\n");
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+
+	} else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		printf("[cmd] MISSION ABORT!\n");
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+	}
+}
+
+void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	int old_mode = current_status->flight_mode;
+	current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
+
+	current_status->flag_control_manual_enabled = true;
+
+	/* set behaviour based on airframe */
+	if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
+	    (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) {
+
+		/* assuming a rotary wing, set to SAS */
+		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
+
+	} else {
+
+		/* assuming a fixed wing, set to direct pass-through */
+		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT;
+		current_status->flag_control_attitude_enabled = false;
+		current_status->flag_control_rates_enabled = false;
+	}
+
+	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) {
+		printf("[cmd] manual mode\n");
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
+	}
+}
+
+void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		int old_mode = current_status->flight_mode;
+		int old_manual_control_mode = current_status->manual_control_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL;
+		current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
+		current_status->flag_control_manual_enabled = true;
+
+		if (old_mode != current_status->flight_mode ||
+		    old_manual_control_mode != current_status->manual_control_mode) {
+			printf("[cmd] att stabilized mode\n");
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
+			state_machine_publish(status_pub, current_status, mavlink_fd);
+		}
+
+	}
+}
+
+void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	if (!current_status->flag_vector_flight_mode_ok) {
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE");
+		tune_error();
+		return;
+	}
+
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) {
+		printf("[cmd] position guided mode\n");
+		int old_mode = current_status->flight_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB;
+		current_status->flag_control_manual_enabled = false;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED);
+
+		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
+
+	}
+}
+
+void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
+{
+	if (!current_status->flag_vector_flight_mode_ok) {
+		mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE");
+		return;
+	}
+
+	if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) {
+		printf("[cmd] auto mode\n");
+		int old_mode = current_status->flight_mode;
+		current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO;
+		current_status->flag_control_manual_enabled = false;
+		current_status->flag_control_attitude_enabled = true;
+		current_status->flag_control_rates_enabled = true;
+		do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
+
+		if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd);
+	}
+}
+
+
+uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode)
+{
+	uint8_t ret = 1;
+
+	/* Switch on HIL if in standby and not already in HIL mode */
+	if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED)
+	    && !current_status->flag_hil_enabled) {
+		if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) {
+			/* Enable HIL on request */
+			current_status->flag_hil_enabled = true;
+			ret = OK;
+			state_machine_publish(status_pub, current_status, mavlink_fd);
+			publish_armed_status(current_status);
+			printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n");
+
+		} else if (current_status->state_machine != SYSTEM_STATE_STANDBY &&
+			   current_status->flag_system_armed) {
+
+			mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!")
+
+		} else {
+
+			mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.")
+		}
+	}
+
+	/* switch manual / auto */
+	if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) {
+		update_state_machine_mode_auto(status_pub, current_status, mavlink_fd);
+
+	} else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) {
+		update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd);
+
+	} else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) {
+		update_state_machine_mode_guided(status_pub, current_status, mavlink_fd);
+
+	} else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+		update_state_machine_mode_manual(status_pub, current_status, mavlink_fd);
+	}
+
+	/* vehicle is disarmed, mode requests arming */
+	if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+		/* only arm in standby state */
+		// XXX REMOVE
+		if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) {
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY);
+			ret = OK;
+			printf("[cmd] arming due to command request\n");
+		}
+	}
+
+	/* vehicle is armed, mode requests disarming */
+	if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) {
+		/* only disarm in ground ready */
+		if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) {
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY);
+			ret = OK;
+			printf("[cmd] disarming due to command request\n");
+		}
+	}
+
+	/* NEVER actually switch off HIL without reboot */
+	if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) {
+		warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n");
+		mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL");
+		ret = ERROR;
+	}
+
+	return ret;
+}
+
+uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations
+{
+	commander_state_machine_t current_system_state = current_status->state_machine;
+
+	uint8_t ret = 1;
+
+	switch (custom_mode) {
+	case SYSTEM_STATE_GROUND_READY:
+		break;
+
+	case SYSTEM_STATE_STANDBY:
+		break;
+
+	case SYSTEM_STATE_REBOOT:
+		printf("try to reboot\n");
+
+		if (current_system_state == SYSTEM_STATE_STANDBY
+				|| current_system_state == SYSTEM_STATE_PREFLIGHT
+				|| current_status->flag_hil_enabled) {
+			printf("system will reboot\n");
+			mavlink_log_critical(mavlink_fd, "Rebooting..");
+			usleep(200000);
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT);
+			ret = 0;
+		}
+
+		break;
+
+	case SYSTEM_STATE_AUTO:
+		printf("try to switch to auto/takeoff\n");
+
+		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) {
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO);
+			printf("state: auto\n");
+			ret = 0;
+		}
+
+		break;
+
+	case SYSTEM_STATE_MANUAL:
+		printf("try to switch to manual\n");
+
+		if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) {
+			do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL);
+			printf("state: manual\n");
+			ret = 0;
+		}
+
+		break;
+
+	default:
+		break;
+	}
+
+	return ret;
+}
+

src/modules/commander/state_machine_helper.h

@@ -0,0 +1,209 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *
+ * 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file state_machine_helper.h
+ * State machine helper functions definitions
+ */
+
+#ifndef STATE_MACHINE_HELPER_H_
+#define STATE_MACHINE_HELPER_H_
+
+#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor)
+#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_status.h>
+
+/**
+ * Switch to new state with no checking.
+ *
+ * do_state_update: this is the functions that all other functions have to call in order to update the state.
+ * the function does not question the state change, this must be done before
+ * The function performs actions that are connected with the new state (buzzer, reboot, ...)
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ *
+ * @return 0 (macro OK) or 1 on error (macro ERROR)
+ */
+int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state);
+
+/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */
+// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+
+// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+
+// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type);
+
+
+/**
+ * Handle state machine if got position fix
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
+ * Handle state machine if position fix lost
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
+ * Handle state machine if user wants to arm
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
+ * Handle state machine if user wants to disarm
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
+ * Handle state machine if mode switch is manual
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
+ * Handle state machine if mode switch is stabilized
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
+ * Handle state machine if mode switch is guided
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
+ * Handle state machine if mode switch is auto
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
+ * Publish current state information
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+
+/*
+ *  Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app).
+ *  If the request is obeyed the functions return 0
+ *
+ */
+
+/**
+ * Handles *incoming request* to switch to a specific state, if state change is successful returns 0
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode);
+
+/**
+ * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode);
+
+/**
+ * Always switches to critical mode under any circumstances.
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
+ * Switches to emergency if required.
+ *
+ * @param status_pub file descriptor for state update topic publication
+ * @param current_status pointer to the current state machine to operate on
+ * @param mavlink_fd file descriptor for MAVLink statustext messages
+ */
+void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
+
+/**
+ * Publish the armed state depending on the current system state
+ *
+ * @param current_status the current system status
+ */
+void publish_armed_status(const struct vehicle_status_s *current_status);
+
+
+
+#endif /* STATE_MACHINE_HELPER_H_ */