Merge branch 'master' of github.com:PX4/Firmware into control_state

src/modules/attitude_estimator_ekf/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015 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
@@ -30,18 +30,16 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-
-#
-# Fixedwing attitude control application
-#
-
-MODULE_COMMAND	= fw_att_control
-
-SRCS		= fw_att_control_main.cpp \
-		  fw_att_control_params.c
-
-# Startup handler, the actual app stack size is
-# in the task_spawn command
-MODULE_STACKSIZE = 1200
-
-MAXOPTIMIZATION = -Os
+px4_add_module(
+	MODULE modules__attitude_estimator_ekf
+	MAIN attitude_estimator_ekf
+	STACK 1200
+	COMPILE_FLAGS
+		-Wno-float-equal
+	SRCS
+		attitude_estimator_ekf_main.cpp
+		codegen/AttitudeEKF.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -108,6 +108,46 @@ usage(const char *reason)
 	fprintf(stderr, "usage: attitude_estimator_ekf {start|stop|status} [-p <additional params>]\n\n");
 }
 
+int parameters_init(struct attitude_estimator_ekf_param_handles *h)
+{
+	/* PID parameters */
+	h->q0 	=	param_find("EKF_ATT_V3_Q0");
+	h->q1 	=	param_find("EKF_ATT_V3_Q1");
+	h->q2 	=	param_find("EKF_ATT_V3_Q2");
+	h->q3 	=	param_find("EKF_ATT_V3_Q3");
+
+	h->r0 	=	param_find("EKF_ATT_V4_R0");
+	h->r1 	=	param_find("EKF_ATT_V4_R1");
+	h->r2 	=	param_find("EKF_ATT_V4_R2");
+
+	h->moment_inertia_J[0]  =   param_find("ATT_J11");
+	h->moment_inertia_J[1]  =   param_find("ATT_J22");
+	h->moment_inertia_J[2]  =   param_find("ATT_J33");
+	h->use_moment_inertia	=   param_find("ATT_J_EN");
+
+	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->r0, &(p->r[0]));
+	param_get(h->r1, &(p->r[1]));
+	param_get(h->r2, &(p->r[2]));
+
+	for (int i = 0; i < 3; i++) {
+		param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
+	}
+
+	param_get(h->use_moment_inertia, &(p->use_moment_inertia));
+
+	return OK;
+}
+
 /**
  * The attitude_estimator_ekf app only briefly exists to start
  * the background job. The stack size assigned in the

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -95,19 +95,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
  */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
 
-/**
- * EKF attitude estimator enabled
- *
- * If enabled, it uses the older EKF filter.
- * However users can enable the new quaternion
- * based complimentary filter by setting EKF_ATT_ENABLED = 0.
- *
- * @min 0
- * @max 1
- * @group Attitude EKF estimator
- */
-PARAM_DEFINE_INT32(EKF_ATT_ENABLED, 1);
-
 /**
  * Moment of inertia matrix diagonal entry (1, 1)
  *
@@ -142,43 +129,3 @@ PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
  * @max 1
  */
 PARAM_DEFINE_INT32(ATT_J_EN, 0);
-
-int parameters_init(struct attitude_estimator_ekf_param_handles *h)
-{
-	/* PID parameters */
-	h->q0 	=	param_find("EKF_ATT_V3_Q0");
-	h->q1 	=	param_find("EKF_ATT_V3_Q1");
-	h->q2 	=	param_find("EKF_ATT_V3_Q2");
-	h->q3 	=	param_find("EKF_ATT_V3_Q3");
-
-	h->r0 	=	param_find("EKF_ATT_V4_R0");
-	h->r1 	=	param_find("EKF_ATT_V4_R1");
-	h->r2 	=	param_find("EKF_ATT_V4_R2");
-
-	h->moment_inertia_J[0]  =   param_find("ATT_J11");
-	h->moment_inertia_J[1]  =   param_find("ATT_J22");
-	h->moment_inertia_J[2]  =   param_find("ATT_J33");
-	h->use_moment_inertia	=   param_find("ATT_J_EN");
-
-	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->r0, &(p->r[0]));
-	param_get(h->r1, &(p->r[1]));
-	param_get(h->r2, &(p->r[2]));
-
-	for (int i = 0; i < 3; i++) {
-		param_get(h->moment_inertia_J[i], &(p->moment_inertia_J[3 * i + i]));
-	}
-
-	param_get(h->use_moment_inertia, &(p->use_moment_inertia));
-
-	return OK;
-}

src/modules/attitude_estimator_ekf/module.mk

@@ -1,50 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Attitude estimator (Extended Kalman Filter)
-#
-
-MODULE_COMMAND	 = attitude_estimator_ekf
-
-SRCS		 = attitude_estimator_ekf_main.cpp \
-		   attitude_estimator_ekf_params.c \
-		   codegen/AttitudeEKF.c
-
-MODULE_STACKSIZE = 1200
-
-EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3700
-
-ifeq ($(PX4_TARGET_OS),nuttx)
-EXTRACXXFLAGS = -Wframe-larger-than=2600
-endif

src/modules/attitude_estimator_q/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+#############################################################################
+set(MODULE_CFLAGS)
+if (${OS} STREQUAL "nuttx")
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=1400)
+endif()
+px4_add_module(
+	MODULE modules__attitude_estimator_q
+	MAIN attitude_estimator_q
+	COMPILE_FLAGS ${MODULE_CFLAGS}
+	STACK 1200
+	SRCS
+		attitude_estimator_q_main.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -40,6 +40,7 @@
  */
 
 #include <px4_config.h>
+#include <px4_posix.h>
 #include <unistd.h>
 #include <stdlib.h>
 #include <stdio.h>

src/modules/attitude_estimator_q/attitude_estimator_q_params.c

@@ -116,4 +116,4 @@ PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);
  * @min 0.001
  * @max 100
  */
-PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.1f);
+PARAM_DEFINE_FLOAT(ATT_VIBE_THRESH, 0.2f);

src/modules/bottle_drop/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__bottle_drop
+	MAIN bottle_drop
+	STACK 1200
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		bottle_drop.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/bottle_drop/module.mk

@@ -1,45 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2013, 2014 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.
-#
-############################################################################
-
-#
-# Daemon application
-#
-
-MODULE_COMMAND		= bottle_drop
-
-SRCS			= bottle_drop.cpp \
-			  bottle_drop_params.c
-
-MAXOPTIMIZATION	 = -Os
-
-MODULE_STACKSIZE = 1200

src/modules/commander/CMakeLists.txt

@@ -0,0 +1,60 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+set(MODULE_CFLAGS -Os)
+if(${OS} STREQUAL "nuttx")
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=2300)
+endif()
+px4_add_module(
+	MODULE modules__commander
+	MAIN commander
+	STACK 1200
+	COMPILE_FLAGS
+		${MODULE_CFLAGS}
+		-Os
+	SRCS
+		commander.cpp
+		state_machine_helper.cpp
+		commander_helper.cpp
+		calibration_routines.cpp
+		accelerometer_calibration.cpp
+		gyro_calibration.cpp
+		mag_calibration.cpp
+		baro_calibration.cpp
+		rc_calibration.cpp
+		airspeed_calibration.cpp
+		esc_calibration.cpp
+		PreflightCheck.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/commander/commander.cpp

@@ -45,6 +45,7 @@
 #include <px4_config.h>
 #include <px4_posix.h>
 #include <px4_time.h>
+#include <px4_tasks.h>
 #include <pthread.h>
 #include <stdio.h>
 #include <sys/stat.h>
@@ -57,9 +58,6 @@
 #include <systemlib/err.h>
 #include <systemlib/circuit_breaker.h>
 //#include <debug.h>
-#ifndef __PX4_QURT
-#include <sys/prctl.h>
-#endif
 #include <sys/stat.h>
 #include <string.h>
 #include <math.h>
@@ -197,6 +195,13 @@ static struct offboard_control_mode_s offboard_control_mode;
 static struct home_position_s _home;
 
 static unsigned _last_mission_instance = 0;
+static uint64_t last_manual_input = 0;
+static switch_pos_t last_offboard_switch = 0;
+static switch_pos_t last_return_switch = 0;
+static switch_pos_t last_mode_switch = 0;
+static switch_pos_t last_acro_switch = 0;
+static switch_pos_t last_posctl_switch = 0;
+static switch_pos_t last_loiter_switch = 0;
 
 struct vtol_vehicle_status_s vtol_status;
 
@@ -410,7 +415,8 @@ void usage(const char *reason)
 void print_status()
 {
 	warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion");
-	warnx("usb powered: %s", (status.usb_connected) ? "yes" : "no");
+	warnx("power: USB: %s, BRICK: %s", (status.usb_connected) ? "[OK]" : "[NO]",
+		(status.condition_power_input_valid) ? " [OK]" : "[NO]");
 	warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage);
 	warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", _home.lat, _home.lon, (double)_home.alt);
 	warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z);
@@ -587,6 +593,14 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 
 			} else {
 				cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+
+				if (arming_ret == TRANSITION_DENIED) {
+					mavlink_log_critical(mavlink_fd, "Rejecting arming cmd");
+				}
+
+				if (main_ret == TRANSITION_DENIED) {
+					mavlink_log_critical(mavlink_fd, "Rejecting mode switch cmd");
+				}
 			}
 		}
 		break;
@@ -911,16 +925,16 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_eph = param_find("COM_HOME_H_T");
 	param_t _param_epv = param_find("COM_HOME_V_T");
 
-	const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
-	main_states_str[vehicle_status_s::MAIN_STATE_MANUAL]			= "MANUAL";
-	main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL]			= "ALTCTL";
-	main_states_str[vehicle_status_s::MAIN_STATE_POSCTL]			= "POSCTL";
-	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION]		= "AUTO_MISSION";
-	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
-	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
-	main_states_str[vehicle_status_s::MAIN_STATE_ACRO]			= "ACRO";
-	main_states_str[vehicle_status_s::MAIN_STATE_STAB]			= "STAB";
-	main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD]			= "OFFBOARD";
+	// const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX];
+	// main_states_str[vehicle_status_s::MAIN_STATE_MANUAL]			= "MANUAL";
+	// main_states_str[vehicle_status_s::MAIN_STATE_ALTCTL]			= "ALTCTL";
+	// main_states_str[vehicle_status_s::MAIN_STATE_POSCTL]			= "POSCTL";
+	// main_states_str[vehicle_status_s::MAIN_STATE_AUTO_MISSION]		= "AUTO_MISSION";
+	// main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
+	// main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
+	// main_states_str[vehicle_status_s::MAIN_STATE_ACRO]			= "ACRO";
+	// main_states_str[vehicle_status_s::MAIN_STATE_STAB]			= "STAB";
+	// main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD]			= "OFFBOARD";
 
 	const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
 	arming_states_str[vehicle_status_s::ARMING_STATE_INIT]			= "INIT";
@@ -1222,13 +1236,13 @@ int commander_thread_main(int argc, char *argv[])
 	// Run preflight check
 	int32_t rc_in_off = 0;
 	param_get(_param_autostart_id, &autostart_id);
+	param_get(_param_rc_in_off, &rc_in_off);
+	status.rc_input_mode = rc_in_off;
 	if (is_hil_setup(autostart_id)) {
 		// HIL configuration selected: real sensors will be disabled
 		status.condition_system_sensors_initialized = false;
 		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
 	} else {
-		param_get(_param_rc_in_off, &rc_in_off);
-			status.rc_input_mode = rc_in_off;
 		status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true,
 			checkAirspeed, (status.rc_input_mode == vehicle_status_s::RC_IN_MODE_DEFAULT), !status.circuit_breaker_engaged_gpsfailure_check);
 		if (!status.condition_system_sensors_initialized) {
@@ -1745,6 +1759,9 @@ int commander_thread_main(int argc, char *argv[])
 
 			if (arming_ret == TRANSITION_CHANGED) {
 				arming_state_changed = true;
+			} else if (arming_ret == TRANSITION_DENIED) {
+				/* do not complain if not allowed into standby */
+				arming_ret = TRANSITION_NOT_CHANGED;
 			}
 
 		}
@@ -1885,7 +1902,7 @@ int commander_thread_main(int argc, char *argv[])
 
 			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
 			 * do it only for rotary wings */
-			if (status.is_rotary_wing &&
+			if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF &&
 			    (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
 			    (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL ||
 			    	status.main_state == vehicle_status_s::MAIN_STATE_ACRO ||
@@ -1915,7 +1932,7 @@ int commander_thread_main(int argc, char *argv[])
 			}
 
 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
-			if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
+			if (sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF ) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 
 					/* we check outside of the transition function here because the requirement
@@ -1932,9 +1949,9 @@ int commander_thread_main(int argc, char *argv[])
 
 						if (arming_ret == TRANSITION_CHANGED) {
 							arming_state_changed = true;
+						} else {
+							print_reject_arm("NOT ARMING: Configuration error");
 						}
-					} else {
-						print_reject_arm("NOT ARMING: Configuration error");
 					}
 
 					stick_on_counter = 0;
@@ -2184,7 +2201,6 @@ int commander_thread_main(int argc, char *argv[])
 		// TODO handle mode changes by commands
 		if (main_state_changed || nav_state_changed) {
 			status_changed = true;
-			warnx("main state: %s nav state: %s", main_states_str[status.main_state], nav_states_str[status.nav_state]);
 			mavlink_log_info(mavlink_fd, "Flight mode: %s", nav_states_str[status.nav_state]);
 			main_state_changed = false;
 		}
@@ -2357,7 +2373,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu
 			} else if (status_local->battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 				rgbled_set_color(RGBLED_COLOR_RED);
 			} else {
-				if (status_local->condition_global_position_valid) {
+				if (status_local->condition_home_position_valid) {
 					rgbled_set_color(RGBLED_COLOR_GREEN);
 
 				} else {
@@ -2413,6 +2429,27 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		return main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
 	}
 
+	/* manual setpoint has not updated, do not re-evaluate it */
+	if ((last_manual_input == sp_man->timestamp) ||
+		((last_offboard_switch == sp_man->offboard_switch) &&
+		 (last_return_switch == sp_man->return_switch) &&
+		 (last_mode_switch == sp_man->mode_switch) &&
+		 (last_acro_switch == sp_man->acro_switch) &&
+		 (last_posctl_switch == sp_man->posctl_switch) &&
+		 (last_loiter_switch == sp_man->loiter_switch))) {
+
+		/* no timestamp change or no switch change -> nothing changed */
+		return TRANSITION_NOT_CHANGED;
+	}
+
+	last_manual_input = sp_man->timestamp;
+	last_offboard_switch = sp_man->offboard_switch;
+	last_return_switch = sp_man->return_switch;
+	last_mode_switch = sp_man->mode_switch;
+	last_acro_switch = sp_man->acro_switch;
+	last_posctl_switch = sp_man->posctl_switch;
+	last_loiter_switch = sp_man->loiter_switch;
+
 	/* offboard switch overrides main switch */
 	if (sp_man->offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
 		res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_OFFBOARD);
@@ -2804,7 +2841,7 @@ void answer_command(struct vehicle_command_s &cmd, unsigned result)
 
 void *commander_low_prio_loop(void *arg)
 {
-#ifndef __PX4_QURT
+#if defined(__PX4_LINUX) || defined(__PX4_NUTTX)
 	/* Set thread name */
 	prctl(PR_SET_NAME, "commander_low_prio", getpid());
 #endif

src/modules/commander/commander_tests/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__commander__commander_tests
+	MAIN commander_tests
+	SRCS
+		commander_tests.cpp
+		state_machine_helper_test.cpp
+		../state_machine_helper.cpp
+		../PreflightCheck.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/commander/module.mk

@@ -1,60 +0,0 @@
-############################################################################
-#
-#   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.cpp \
-			commander_params.c \
-			state_machine_helper.cpp \
-			commander_helper.cpp \
-			calibration_routines.cpp \
-			accelerometer_calibration.cpp \
-			gyro_calibration.cpp \
-			mag_calibration.cpp \
-			baro_calibration.cpp \
-			rc_calibration.cpp \
-			airspeed_calibration.cpp \
-			esc_calibration.cpp \
-			PreflightCheck.cpp
-
-MODULE_STACKSIZE = 5000
-
-MAXOPTIMIZATION	 = -Os
-
-ifeq ($(PX4_TARGET_OS),nuttx)
-EXTRACXXFLAGS = -Wframe-larger-than=2400
-endif
-

src/modules/commander/state_machine_helper.cpp

@@ -48,6 +48,8 @@
 #include <string.h>
 #include <math.h>
 
+#include <px4_posix.h>
+
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
@@ -64,12 +66,6 @@
 #include "commander_helper.h"
 #include "PreflightCheck.h"
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 // This array defines the arming state transitions. The rows are the new state, and the columns
 // are the current state. Using new state and current  state you can index into the array which
 // will be true for a valid transition or false for a invalid transition. In some cases even
@@ -97,6 +93,8 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = {
 	"ARMING_STATE_IN_AIR_RESTORE",
 };
 
+static bool sensor_feedback_provided = false;
+
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status,		///< current vehicle status
 			const struct safety_s   *safety,		///< current safety settings
@@ -245,10 +243,12 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			(new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) &&
 			(status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) &&
 			(!status->condition_system_sensors_initialized)) {
-			mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
+			if (!sensor_feedback_provided || (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED)) {
+				mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection");
+				sensor_feedback_provided = true;
+			}
 			feedback_provided = true;
 			valid_transition = false;
-			status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
 		}
 
 		// Finish up the state transition
@@ -259,6 +259,12 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 			status->arming_state = new_arming_state;
 		}
 
+		/* reset feedback state */
+		if (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR &&
+			valid_transition) {
+			sensor_feedback_provided = false;
+		}
+
 		/* end of atomic state update */
 		#ifdef __PX4_NUTTX
 		irqrestore(flags);
@@ -266,15 +272,10 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 	}
 
 	if (ret == TRANSITION_DENIED) {
-#define		WARNSTR "INVAL: %s - %s"
-		/* only print to console here by default as this is too technical to be useful during operation */
-		warnx(WARNSTR, state_names[status->arming_state], state_names[new_arming_state]);
-
-		/* print to MAVLink if we didn't provide any feedback yet */
+		/* print to MAVLink and console if we didn't provide any feedback yet */
 		if (!feedback_provided) {
-			mavlink_log_critical(mavlink_fd, WARNSTR, state_names[status->arming_state], state_names[new_arming_state]);
+			mavlink_and_console_log_critical(mavlink_fd, "INVAL: %s - %s", state_names[status->arming_state], state_names[new_arming_state]);
 		}
-#undef WARNSTR
 	}
 
 	return ret;

src/modules/controllib/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__controllib
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		test_params.c
+		block/Block.cpp
+		block/BlockParam.cpp
+		uorb/blocks.cpp
+		blocks.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/controllib/block/Block.hpp

@@ -39,6 +39,7 @@
 
 #pragma once
 
+#define __STDC_FORMAT_MACROS
 #include <stdint.h>
 #include <inttypes.h>
 

src/modules/controllib/blocks.cpp

@@ -39,6 +39,7 @@
 
 #include <math.h>
 #include <stdio.h>
+#include <float.h>
 
 #include "blocks.hpp"
 
@@ -51,6 +52,7 @@ int basicBlocksTest()
 	blockLimitSymTest();
 	blockLowPassTest();
 	blockHighPassTest();
+	blockLowPass2Test();
 	blockIntegralTest();
 	blockIntegralTrapTest();
 	blockDerivativeTest();
@@ -198,6 +200,47 @@ int blockHighPassTest()
 	return 0;
 }
 
+float BlockLowPass2::update(float input)
+{
+	if (!isfinite(getState())) {
+		setState(input);
+	}
+
+	if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
+		_lp.set_cutoff_frequency(_fs, getFCutParam());
+	}
+	_state = _lp.apply(input);
+	return _state;
+}
+
+int blockLowPass2Test()
+{
+	printf("Test BlockLowPass2\t\t: ");
+	BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
+	// test initial state
+	ASSERT(equal(10.0f, lowPass.getFCutParam()));
+	ASSERT(equal(0.0f, lowPass.getState()));
+	ASSERT(equal(0.0f, lowPass.getDt()));
+	// set dt
+	lowPass.setDt(0.1f);
+	ASSERT(equal(0.1f, lowPass.getDt()));
+	// set state
+	lowPass.setState(1.0f);
+	ASSERT(equal(1.0f, lowPass.getState()));
+	// test update
+	ASSERT(equal(1.06745527f, lowPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		lowPass.update(2.0f);
+	}
+
+	ASSERT(equal(2.0f, lowPass.getState()));
+	ASSERT(equal(2.0f, lowPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+};
+
 float BlockIntegral::update(float input)
 {
 	// trapezoidal integration

src/modules/controllib/blocks.hpp

@@ -45,6 +45,7 @@
 #include <stdlib.h>
 #include <math.h>
 #include <mathlib/math/test/test.hpp>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #include "block/Block.hpp"
 #include "block/BlockParam.hpp"
@@ -163,6 +164,36 @@ protected:
 
 int __EXPORT blockHighPassTest();
 
+/**
+ * A 2nd order low pass filter block which uses the default px4 2nd order low pass filter
+ */
+class __EXPORT BlockLowPass2 : public Block
+{
+public:
+// methods
+	BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) :
+		Block(parent, name),
+		_state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */),
+		_fCut(this, ""), // only one parameter, no need to name
+		_fs(sample_freq),
+		_lp(_fs, _fCut.get())
+	{};
+	virtual ~BlockLowPass2() {};
+	float update(float input);
+// accessors
+	float getState() { return _state; }
+	float getFCutParam() { return _fCut.get(); }
+	void setState(float state) { _state = _lp.reset(state); }
+protected:
+// attributes
+	float _state;
+	control::BlockParamFloat _fCut;
+	float _fs;
+	math::LowPassFilter2p _lp;
+};
+
+int __EXPORT blockLowPass2Test();
+
 /**
  * A rectangular integrator.
  * A limiter is built into the class to bound the
@@ -263,6 +294,7 @@ public:
 	void setU(float u) {_u = u;}
 	float getU() {return _u;}
 	float getLP() {return _lowPass.getFCut();}
+	float getO() { return _lowPass.getState(); }
 protected:
 // attributes
 	float _u; /**< previous input */

src/modules/controllib/module.mk

@@ -1,43 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Control library
-#
-SRCS		 = test_params.c \
-		   block/Block.cpp \
-		   block/BlockParam.cpp \
-		   uorb/blocks.cpp \
-		   blocks.cpp
-
-MAXOPTIMIZATION = -Os

src/modules/dataman/CMakeLists.txt

@@ -30,14 +30,15 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-
-#
-# Attitude estimator (quaternion based)
-#
-
-MODULE_COMMAND	 = attitude_estimator_q
-
-SRCS		 = attitude_estimator_q_main.cpp \
-		   attitude_estimator_q_params.c
-
-MODULE_STACKSIZE = 1200
+px4_add_module(
+	MODULE modules__dataman
+	MAIN dataman
+	STACK 1200
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		dataman.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/dataman/dataman.c

@@ -42,6 +42,7 @@
 
 #include <px4_config.h>
 #include <px4_defines.h>
+#include <px4_posix.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <fcntl.h>
@@ -82,7 +83,7 @@ typedef enum {
 /** Work task work item */
 typedef struct {
 	sq_entry_t link;	/**< list linkage */
-	sem_t wait_sem;
+	px4_sem_t wait_sem;
 	unsigned char first;
 	unsigned char func;
 	ssize_t result;
@@ -128,8 +129,8 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
 static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
 
 /* Item type lock mutexes */
-static sem_t *g_item_locks[DM_KEY_NUM_KEYS];
-static sem_t g_sys_state_mutex;
+static px4_sem_t *g_item_locks[DM_KEY_NUM_KEYS];
+static px4_sem_t g_sys_state_mutex;
 
 /* The data manager store file handle and file name */
 static int g_fd = -1, g_task_fd = -1;
@@ -140,7 +141,7 @@ static char *k_data_manager_device_path = NULL;
 
 typedef struct {
 	sq_queue_t q;		/* Nuttx queue */
-	sem_t mutex;		/* Mutual exclusion on work queue adds and deletes */
+	px4_sem_t mutex;		/* Mutual exclusion on work queue adds and deletes */
 	unsigned size;		/* Current size of queue */
 	unsigned max_size;	/* Maximum queue size reached */
 } work_q_t;
@@ -148,8 +149,8 @@ typedef struct {
 static work_q_t g_free_q;	/* queue of free work items. So that we don't always need to call malloc and free*/
 static work_q_t g_work_q;	/* pending work items. To be consumed by worker thread */
 
-sem_t g_work_queued_sema;	/* To notify worker thread a work item has been queued */
-sem_t g_init_sema;
+static px4_sem_t g_work_queued_sema;	/* To notify worker thread a work item has been queued */
+static px4_sem_t g_init_sema;
 
 static bool g_task_should_exit;	/**< if true, dataman task should exit */
 
@@ -159,26 +160,26 @@ static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /*
 static void init_q(work_q_t *q)
 {
 	sq_init(&(q->q));		/* Initialize the NuttX queue structure */
-	sem_init(&(q->mutex), 1, 1);	/* Queue is initially unlocked */
+	px4_sem_init(&(q->mutex), 1, 1);	/* Queue is initially unlocked */
 	q->size = q->max_size = 0;	/* Queue is initially empty */
 }
 
 static inline void
 destroy_q(work_q_t *q)
 {
-	sem_destroy(&(q->mutex));	/* Destroy the queue lock */
+	px4_sem_destroy(&(q->mutex));	/* Destroy the queue lock */
 }
 
 static inline void
 lock_queue(work_q_t *q)
 {
-	sem_wait(&(q->mutex));	/* Acquire the queue lock */
+	px4_sem_wait(&(q->mutex));	/* Acquire the queue lock */
 }
 
 static inline void
 unlock_queue(work_q_t *q)
 {
-	sem_post(&(q->mutex));	/* Release the queue lock */
+	px4_sem_post(&(q->mutex));	/* Release the queue lock */
 }
 
 static work_q_item_t *
@@ -221,7 +222,7 @@ create_work_item(void)
 
 	/* If we got one then lock the item*/
 	if (item) {
-		sem_init(&item->wait_sem, 1, 0);        /* Caller will wait on this... initially locked */
+		px4_sem_init(&item->wait_sem, 1, 0);        /* Caller will wait on this... initially locked */
 	}
 
 	/* return the item pointer, or NULL if all failed */
@@ -233,7 +234,7 @@ create_work_item(void)
 static inline void
 destroy_work_item(work_q_item_t *item)
 {
-	sem_destroy(&item->wait_sem); /* Destroy the item lock */
+	px4_sem_destroy(&item->wait_sem); /* Destroy the item lock */
 	/* Return the item to the free item queue for later reuse */
 	lock_queue(&g_free_q);
 	sq_addfirst(&item->link, &(g_free_q.q));
@@ -277,10 +278,10 @@ enqueue_work_item_and_wait_for_result(work_q_item_t *item)
 	unlock_queue(&g_work_q);
 
 	/* tell the work thread that work is available */
-	sem_post(&g_work_queued_sema);
+	px4_sem_post(&g_work_queued_sema);
 
 	/* wait for the result */
-	sem_wait(&item->wait_sem);
+	px4_sem_wait(&item->wait_sem);
 
 	int result = item->result;
 
@@ -628,7 +629,7 @@ dm_lock(dm_item_t item)
 	}
 
 	if (g_item_locks[item]) {
-		sem_wait(g_item_locks[item]);
+		px4_sem_wait(g_item_locks[item]);
 	}
 }
 
@@ -645,7 +646,7 @@ dm_unlock(dm_item_t item)
 	}
 
 	if (g_item_locks[item]) {
-		sem_post(g_item_locks[item]);
+		px4_sem_post(g_item_locks[item]);
 	}
 }
 
@@ -691,7 +692,7 @@ task_main(int argc, char *argv[])
 	}
 
 	/* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */
-	sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
+	px4_sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
 
 	for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) {
 		g_item_locks[i] = NULL;
@@ -704,7 +705,7 @@ task_main(int argc, char *argv[])
 	init_q(&g_work_q);
 	init_q(&g_free_q);
 
-	sem_init(&g_work_queued_sema, 1, 0);
+	px4_sem_init(&g_work_queued_sema, 1, 0);
 
 	/* See if the data manage file exists and is a multiple of the sector size */
 	g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
@@ -729,14 +730,14 @@ task_main(int argc, char *argv[])
 
 	if (g_task_fd < 0) {
 		warnx("Could not open data manager file %s", k_data_manager_device_path);
-		sem_post(&g_init_sema); /* Don't want to hang startup */
+		px4_sem_post(&g_init_sema); /* Don't want to hang startup */
 		return -1;
 	}
 
 	if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
 		close(g_task_fd);
 		warnx("Could not seek data manager file %s", k_data_manager_device_path);
-		sem_post(&g_init_sema); /* Don't want to hang startup */
+		px4_sem_post(&g_init_sema); /* Don't want to hang startup */
 		return -1;
 	}
 
@@ -771,7 +772,7 @@ task_main(int argc, char *argv[])
 	printf(", data manager file '%s' size is %d bytes\n", k_data_manager_device_path, max_offset);
 
 	/* Tell startup that the worker thread has completed its initialization */
-	sem_post(&g_init_sema);
+	px4_sem_post(&g_init_sema);
 
 	/* Start the endless loop, waiting for then processing work requests */
 	while (true) {
@@ -784,7 +785,7 @@ task_main(int argc, char *argv[])
 
 		if (!g_task_should_exit) {
 			/* wait for work */
-			sem_wait(&g_work_queued_sema);
+			px4_sem_wait(&g_work_queued_sema);
 		}
 
 		/* Empty the work queue */
@@ -821,7 +822,7 @@ task_main(int argc, char *argv[])
 			}
 
 			/* Inform the caller that work is done */
-			sem_post(&work->wait_sem);
+			px4_sem_post(&work->wait_sem);
 		}
 
 		/* time to go???? */
@@ -846,8 +847,8 @@ task_main(int argc, char *argv[])
 
 	destroy_q(&g_work_q);
 	destroy_q(&g_free_q);
-	sem_destroy(&g_work_queued_sema);
-	sem_destroy(&g_sys_state_mutex);
+	px4_sem_destroy(&g_work_queued_sema);
+	px4_sem_destroy(&g_sys_state_mutex);
 
 	return 0;
 }
@@ -857,17 +858,17 @@ start(void)
 {
 	int task;
 
-	sem_init(&g_init_sema, 1, 0);
+	px4_sem_init(&g_init_sema, 1, 0);
 
-	/* start the worker thread */
-	if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1500, task_main, NULL)) <= 0) {
+	/* start the worker thread with low priority for disk IO */
+	if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 10, 1200, task_main, NULL)) <= 0) {
 		warn("task start failed");
 		return -1;
 	}
 
 	/* wait for the thread to actually initialize */
-	sem_wait(&g_init_sema);
-	sem_destroy(&g_init_sema);
+	px4_sem_wait(&g_init_sema);
+	px4_sem_destroy(&g_init_sema);
 
 	return 0;
 }
@@ -888,7 +889,7 @@ stop(void)
 {
 	/* Tell the worker task to shut down */
 	g_task_should_exit = true;
-	sem_post(&g_work_queued_sema);
+	px4_sem_post(&g_work_queued_sema);
 }
 
 static void

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -134,6 +134,8 @@ public:
      */
     int set_debuglevel(unsigned debug) { _debug = debug; return 0; }
 
+    static constexpr unsigned MAX_PREDICITION_STEPS = 3; /**< maximum number of prediction steps between updates */
+
 private:
     bool        _task_should_exit;      /**< if true, sensor task should exit */
     bool        _task_running;          /**< if true, task is running in its mainloop */
@@ -177,6 +179,7 @@ private:
 
     hrt_abstime _last_accel;
     hrt_abstime _last_mag;
+    unsigned _prediction_steps;
 
     struct sensor_combined_s            _sensor_combined;
 

src/modules/ekf_att_pos_estimator/CMakeLists.txt

@@ -0,0 +1,50 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+set(MODULE_CFLAGS )
+if(NOT ${OS} STREQUAL "qurt")
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=3400)
+endif()
+
+px4_add_module(
+	MODULE modules__ekf_att_pos_estimator
+	MAIN ekf_att_pos_estimator
+	COMPILE_FLAGS ${MODULE_CFLAGS}
+	SRCS
+		ekf_att_pos_estimator_main.cpp
+		estimator_22states.cpp
+		estimator_utilities.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -109,12 +109,6 @@ uint64_t getMicros()
 namespace estimator
 {
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 AttitudePositionEstimatorEKF	*g_estimator = nullptr;
 }
 
@@ -164,6 +158,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 
 	_last_accel(0),
 	_last_mag(0),
+	_prediction_steps(0),
 
 	_sensor_combined{},
 
@@ -1073,6 +1068,14 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 	_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
 	_covariancePredictionDt += _ekf->dtIMU;
 
+	// only fuse every few steps
+	if (_prediction_steps < MAX_PREDICITION_STEPS) {
+		_prediction_steps++;
+		return;
+	} else {
+		_prediction_steps = 0;
+	}
+
 	// perform a covariance prediction if the total delta angle has exceeded the limit
 	// or the time limit will be exceeded at the next IMU update
 	if ((_covariancePredictionDt >= (_ekf->covTimeStepMax - _ekf->dtIMU))
@@ -1189,7 +1192,7 @@ int AttitudePositionEstimatorEKF::start()
 	_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
-					 4800,
+					 4200,
 					 (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);
 
@@ -1249,11 +1252,12 @@ void AttitudePositionEstimatorEKF::print_status()
 		PX4_INFO("states (terrain)      [22]: %8.4f", (double)_ekf->states[22]);
 
 	} else {
-		PX4_INFO("states (wind)      [13-14]: %8.4f, %8.4f", (double)_ekf->states[13], (double)_ekf->states[14]);
-		PX4_INFO("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[15], (double)_ekf->states[16],
-		       (double)_ekf->states[17]);
-		PX4_INFO("states (body mag)  [18-20]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[18], (double)_ekf->states[19],
-		       (double)_ekf->states[20]);
+		PX4_INFO("states (accel offs)   [13]: %8.4f", (double)_ekf->states[13]);
+		PX4_INFO("states (wind)      [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]);
+		PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17],
+		       (double)_ekf->states[18]);
+		PX4_INFO("states (mag bias)  [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20],
+		       (double)_ekf->states[21]);
 	}
 
 	PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s",
@@ -1332,7 +1336,7 @@ void AttitudePositionEstimatorEKF::pollData()
 		} else {
 			_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
 			_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
-			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
+			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) + 9.80665f;
 		}
 
 		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
@@ -1400,8 +1404,8 @@ void AttitudePositionEstimatorEKF::pollData()
 	// leave this in as long as larger improvements are still being made.
 	#if 0
 
-	float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f;
-	float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f;
+	float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f;
+	float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f;
 
 	static unsigned dtoverflow5 = 0;
 	static unsigned dtoverflow10 = 0;
@@ -1412,13 +1416,21 @@ void AttitudePositionEstimatorEKF::pollData()
 			(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
 			dtoverflow5, dtoverflow10);
 
-		warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
+		warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
-		(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z);
+		(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
 
-		warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
+		warnx("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
+		(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
+		(double)(_sensor_combined.accelerometer_integral_m_s[0]),
+		(double)(_sensor_combined.accelerometer_integral_m_s[1]),
+		(double)(_sensor_combined.accelerometer_integral_m_s[2]));
+
+		warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
-		(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
+		(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
+		(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
+		(double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT));
 
 		lastprint = hrt_absolute_time();
 	}

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c

@@ -36,13 +36,9 @@
  *
  * Parameters defined by the attitude and position estimator task
  *
- * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Lorenz Meier <lorenz@px4.io>
  */
 
-#include <px4_config.h>
-
-#include <systemlib/param/param.h>
-
 /*
  * Estimator parameters, accessible via MAVLink
  *

src/modules/ekf_att_pos_estimator/module.mk

@@ -1,46 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2013, 2014 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 EKF Attitude and Position Estimator
-#
-
-MODULE_COMMAND	= ekf_att_pos_estimator
-
-SRCS		= ekf_att_pos_estimator_main.cpp \
-		  ekf_att_pos_estimator_params.c \
-		  estimator_22states.cpp \
-		  estimator_utilities.cpp
-
-EXTRACXXFLAGS	= -Weffc++ -Wframe-larger-than=3400
-

src/modules/fixedwing_backside/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__fixedwing_backside
+	MAIN fixedwing_backside
+	SRCS
+		fixedwing_backside_main.cpp
+		fixedwing.cpp
+		params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/fixedwing_backside/module.mk

@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# Fixedwing backside controller
-#
-
-MODULE_COMMAND	= fixedwing_backside
-
-SRCS		= fixedwing_backside_main.cpp \
-		  fixedwing.cpp \
-		  params.c

src/modules/fw_att_control/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__fw_att_control
+	MAIN fw_att_control
+	STACK 1200
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		fw_att_control_main.cpp
+		fw_att_control_params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -1112,7 +1112,7 @@ FixedwingAttitudeControl::start()
 	_control_task = px4_task_spawn_cmd("fw_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       1600,
+				       1300,
 				       (px4_main_t)&FixedwingAttitudeControl::task_main_trampoline,
 				       nullptr);
 

src/modules/fw_pos_control_l1/CMakeLists.txt

@@ -0,0 +1,52 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__fw_pos_control_l1
+	MAIN fw_pos_control_l1
+	STACK 1200
+	COMPILE_FLAGS
+		-Wno-float-equal
+		-Os
+	SRCS
+		fw_pos_control_l1_main.cpp
+		fw_pos_control_l1_params.c
+		landingslope.cpp
+		mtecs/mTecs.cpp
+		mtecs/limitoverride.cpp
+		mtecs/mTecs_params.c
+	DEPENDS
+		platforms__common
+		lib__external_lgpl
+		lib__ecl
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -80,6 +80,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/tecs_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
@@ -1947,7 +1948,7 @@ FixedwingPositionControl::start()
 	_control_task = px4_task_spawn_cmd("fw_pos_control_l1",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       1600,
+				       1300,
 				       (px4_main_t)&FixedwingPositionControl::task_main_trampoline,
 				       nullptr);
 

src/modules/fw_pos_control_l1/module.mk

@@ -1,53 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2013, 2014 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.
-#
-############################################################################
-
-#
-# Fixedwing L1 position control application
-#
-
-MODULE_COMMAND	= fw_pos_control_l1
-
-SRCS		= fw_pos_control_l1_main.cpp \
-		  fw_pos_control_l1_params.c \
-		  landingslope.cpp \
-		  mtecs/mTecs.cpp \
-		  mtecs/limitoverride.cpp \
-		  mtecs/mTecs_params.c
-
-# Startup handler, the actual app stack size is
-# in the task_spawn command
-MODULE_STACKSIZE = 1200
-
-MAXOPTIMIZATION	 = -Os
-
-EXTRACXXFLAGS = -Wno-float-equal

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -52,15 +52,17 @@ mTecs::mTecs() :
 	_mTecsEnabled(this, "ENABLED"),
 	_airspeedMin(this, "FW_AIRSPD_MIN", false),
 	/* Publications */
+#if defined(__PX4_NUTTX)
 	_status(ORB_ID(tecs_status), &getPublications()),
+#endif // defined(__PX4_NUTTX)
 	/* control blocks */
 	_controlTotalEnergy(this, "THR"),
 	_controlEnergyDistribution(this, "PIT", true),
 	_controlAltitude(this, "FPA", true),
 	_controlAirSpeed(this, "ACC"),
-	_flightPathAngleLowpass(this, "FPA_LP"),
-	_altitudeLowpass(this, "ALT_LP"),
-	_airspeedLowpass(this, "A_LP"),
+	_flightPathAngleLowpass(this, "FPA_LP", 50),
+	_altitudeLowpass(this, "ALT_LP", 50),
+	_airspeedLowpass(this, "A_LP", 50),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
 	_pitchSp(0.0f),
@@ -107,9 +109,11 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* Write part of the status message */
 	_status.altitudeSp = altitudeSp;
 	_status.altitude_filtered = altitudeFiltered;
+#endif // defined(__PX4_NUTTX)
 
 
 	/* use flightpath angle setpoint for total energy control */
@@ -143,9 +147,11 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 				(double)airspeedFiltered, (double)accelerationLongitudinalSp);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* Write part of the status message */
 	_status.airspeedSp = airspeedSp;
 	_status.airspeed_filtered = airspeedFiltered;
+#endif // defined(__PX4_NUTTX)
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
@@ -200,23 +206,23 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	}
 
 	/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
-	if (mode != tecs_status_s::TECS_MODE_LAND && mode != tecs_status_s::TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
-		mode = tecs_status_s::TECS_MODE_UNDERSPEED;
+	if (mode != MTECS_MODE_LAND && mode != MTECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
+		mode = MTECS_MODE_UNDERSPEED;
 	}
 
-	/* Set special output limiters if we are not in TECS_MODE_NORMAL */
+	/* Set special output limiters if we are not in MTECS_MODE_NORMAL */
 	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
 	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
-	if (mode == tecs_status_s::TECS_MODE_TAKEOFF) {
+	if (mode == MTECS_MODE_TAKEOFF) {
 		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_LAND) {
+	} else if (mode == MTECS_MODE_LAND) {
 		// only limit pitch but do not limit throttle
 		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM) {
+	} else if (mode == MTECS_MODE_LAND_THROTTLELIM) {
 		outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_UNDERSPEED) {
+	} else if (mode == MTECS_MODE_UNDERSPEED) {
 		outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
 	}
@@ -226,6 +232,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	 * is running) */
 	limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
+// #if defined(__PX4_NUTTX)
 	// /* Write part of the status message */
 	// _status.flightPathAngleSp = flightPathAngleSp;
 	// _status.flightPathAngle = flightPathAngle;
@@ -237,6 +244,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	// _status.energyDistributionRateSp = energyDistributionRateSp;
 	// _status.energyDistributionRate = energyDistributionRate;
 	// _status.mode = mode;
+// #endif // defined(__PX4_NUTTX)
 
 	/** update control blocks **/
 	/* update total energy rate control block */
@@ -253,8 +261,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 				(double)accelerationLongitudinalSp, (double)airspeedDerivative);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* publish status message */
 	_status.update();
+#endif // defined(__PX4_NUTTX)
 
 	/* clean up */
 	_firstIterationAfterReset = false;

src/modules/fw_pos_control_l1/mtecs/mTecs.h

@@ -49,11 +49,25 @@
 #include <controllib/block/BlockParam.hpp>
 #include <drivers/drv_hrt.h>
 #include <uORB/Publication.hpp>
+
+#if defined(__PX4_NUTTX)
 #include <uORB/topics/tecs_status.h>
+#endif // defined(__PX4_NUTTX)
 
 namespace fwPosctrl
 {
 
+/* corresponds to TECS_MODE in tecs_status.msg */
+enum MTECS_MODE {
+	MTECS_MODE_NORMAL = 0,
+	MTECS_MODE_UNDERSPEED = 1,
+	MTECS_MODE_TAKEOFF = 2,
+	MTECS_MODE_LAND = 3,
+	MTECS_MODE_LAND_THROTTLELIM = 4,
+	MTECS_MODE_BAD_DESCENT = 5,
+	MTECS_MODE_CLIMBOUT = 6
+};
+
 /* Main class of the mTecs */
 class mTecs : public control::SuperBlock
 {
@@ -94,6 +108,10 @@ public:
 	float getThrottleSetpoint() { return _throttleSp; }
 	float getPitchSetpoint() { return _pitchSp; }
 	float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
+	float getFlightPathAngleLowpassState() { return _flightPathAngleLowpass.getState(); }
+	float getAltitudeLowpassState() { return _altitudeLowpass.getState(); }
+	float getAirspeedLowpassState() { return _airspeedLowpass.getState(); }
+	float getAirspeedDerivativeLowpassState() { return _airspeedDerivative.getO(); }
 
 protected:
 	/* parameters */
@@ -101,7 +119,9 @@ protected:
 	control::BlockParamFloat _airspeedMin;		/**< minimal airspeed */
 
 	/* Publications */
+#if defined(__PX4_NUTTX)
 	uORB::Publication<tecs_status_s> _status;	/**< publish internal values for logging */
+#endif // defined(__PX4_NUTTX)
 
 	/* control blocks */
 	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output
@@ -114,9 +134,9 @@ protected:
 								  setpoint */
 
 	/* Other calculation Blocks */
-	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
-	control::BlockLowPass _altitudeLowpass;	/**< low pass filter for altitude */
-	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
+	control::BlockLowPass2 _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
+	control::BlockLowPass2 _altitudeLowpass;	/**< low pass filter for altitude */
+	control::BlockLowPass2 _airspeedLowpass;		/**< low pass filter for airspeed */
 	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
 	/* Output setpoints */

src/modules/gpio_led/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015 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
@@ -30,12 +30,14 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-
-#
-# Status LED via GPIO driver
-#
-
-MODULE_COMMAND	= gpio_led
-SRCS			= gpio_led.c
-
-MAXOPTIMIZATION	 = -Os
+px4_add_module(
+	MODULE modules__gpio_led
+	MAIN gpio_led
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		gpio_led.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/land_detector/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__land_detector
+	MAIN land_detector
+	STACK 1200
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		land_detector_main.cpp
+		LandDetector.cpp
+		MulticopterLandDetector.cpp
+		FixedwingLandDetector.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -40,6 +40,8 @@
 
 #include "FixedwingLandDetector.h"
 
+#include <px4_config.h>
+#include <px4_defines.h>
 #include <cmath>
 #include <drivers/drv_hrt.h>
 

src/modules/land_detector/LandDetector.cpp

@@ -42,65 +42,77 @@
 #include "LandDetector.h"
 #include <unistd.h>                 //usleep
 #include <drivers/drv_hrt.h>
+#include <px4_config.h>
+#include <px4_defines.h>
 
 LandDetector::LandDetector() :
 	_landDetectedPub(0),
 	_landDetected({0, false}),
 	_arming_time(0),
 	_taskShouldExit(false),
-	_taskIsRunning(false)
+	_taskIsRunning(false),
+	_work{}
 {
 	// ctor
 }
 
 LandDetector::~LandDetector()
 {
+	work_cancel(LPWORK, &_work);
 	_taskShouldExit = true;
 }
 
+int LandDetector::start()
+{
+	/* schedule a cycle to start things */
+	work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, 0);
+
+	return 0;
+}
+
 void LandDetector::shutdown()
 {
 	_taskShouldExit = true;
 }
 
-void LandDetector::start()
+void
+LandDetector::cycle_trampoline(void *arg)
 {
-	// make sure this method has not already been called by another thread
-	if (isRunning()) {
-		return;
+	LandDetector *dev = reinterpret_cast<LandDetector *>(arg);
+
+	dev->cycle();
+}
+
+void LandDetector::cycle()
+{
+	if (!_taskIsRunning) {
+		// advertise the first land detected uORB
+		_landDetected.timestamp = hrt_absolute_time();
+		_landDetected.landed = false;
+		_landDetectedPub = orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
+
+		// initialize land detection algorithm
+		initialize();
+
+		// task is now running, keep doing so until shutdown() has been called
+		_taskIsRunning = true;
+		_taskShouldExit = false;
 	}
 
-	// advertise the first land detected uORB
-	_landDetected.timestamp = hrt_absolute_time();
-	_landDetected.landed = false;
-	_landDetectedPub = (uintptr_t)orb_advertise(ORB_ID(vehicle_land_detected), &_landDetected);
+	bool landDetected = update();
 
-	// initialize land detection algorithm
-	initialize();
+	// publish if land detection state has changed
+	if (_landDetected.landed != landDetected) {
+		_landDetected.timestamp = hrt_absolute_time();
+		_landDetected.landed = landDetected;
 
-	// task is now running, keep doing so until shutdown() has been called
-	_taskIsRunning = true;
-	_taskShouldExit = false;
-
-	while (isRunning()) {
-
-		bool landDetected = update();
-
-		// publish if land detection state has changed
-		if (_landDetected.landed != landDetected) {
-			_landDetected.timestamp = hrt_absolute_time();
-			_landDetected.landed = landDetected;
-
-			// publish the land detected broadcast
-			orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected);
-		}
-
-		// limit loop rate
-		usleep(1000000 / LAND_DETECTOR_UPDATE_RATE);
+		// publish the land detected broadcast
+		orb_publish(ORB_ID(vehicle_land_detected), (orb_advert_t)_landDetectedPub, &_landDetected);
 	}
 
-	_taskIsRunning = false;
-	_exit(0);
+	if (!_taskShouldExit) {
+		work_queue(LPWORK, &_work, (worker_t)&LandDetector::cycle_trampoline, this, USEC2TICK(1000000 / LAND_DETECTOR_UPDATE_RATE));
+	}
 }
 
 bool LandDetector::orb_update(const struct orb_metadata *meta, int handle, void *buffer)

src/modules/land_detector/LandDetector.h

@@ -41,6 +41,7 @@
 #ifndef __LAND_DETECTOR_H__
 #define __LAND_DETECTOR_H__
 
+#include <px4_workqueue.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_land_detected.h>
 
@@ -70,7 +71,9 @@ public:
 	 * @brief Blocking function that should be called from it's own task thread. This method will
 	 *        run the underlying algorithm at the desired update rate and publish if the landing state changes.
 	 **/
-	void start();
+	int start();
+
+	static void	cycle_trampoline(void *arg);
 
 protected:
 
@@ -99,13 +102,16 @@ protected:
 	static constexpr uint64_t LAND_DETECTOR_ARM_PHASE_TIME = 1000000;	/**< time interval in which wider acceptance thresholds are used after arming */
 
 protected:
-	uintptr_t				_landDetectedPub;		/**< publisher for position in local frame */
+	orb_advert_t				_landDetectedPub;		/**< publisher for position in local frame */
 	struct vehicle_land_detected_s		_landDetected;			/**< local vehicle position */
 	uint64_t				_arming_time;			/**< timestamp of arming time */
 
 private:
 	bool _taskShouldExit;                                               /**< true if it is requested that this task should exit */
 	bool _taskIsRunning;                                                /**< task has reached main loop and is currently running */
+	struct work_s	_work;
+
+	void		cycle();
 };
 
 #endif //__LAND_DETECTOR_H__

src/modules/land_detector/land_detector_main.cpp

@@ -67,47 +67,32 @@ extern "C" __EXPORT int land_detector_main(int argc, char *argv[]);
 
 //Private variables
 static LandDetector *land_detector_task = nullptr;
-static int _landDetectorTaskID = -1;
 static char _currentMode[12];
 
-/**
-* Deamon thread function
-**/
-static void land_detector_deamon_thread(int argc, char *argv[])
-{
-	land_detector_task->start();
-}
-
 /**
 * Stop the task, force killing it if it doesn't stop by itself
 **/
 static void land_detector_stop()
 {
-	if (land_detector_task == nullptr || _landDetectorTaskID == -1) {
+	if (land_detector_task == nullptr) {
 		warnx("not running");
 		return;
 	}
 
 	land_detector_task->shutdown();
 
-	//Wait for task to die
+	// Wait for task to die
 	int i = 0;
 
 	do {
 		/* wait 20ms */
 		usleep(20000);
 
-		/* if we have given up, kill it */
-		if (++i > 50) {
-			px4_task_delete(_landDetectorTaskID);
-			break;
-		}
-	} while (land_detector_task->isRunning());
+	} while (land_detector_task->isRunning() && ++i < 50);
 
 
 	delete land_detector_task;
 	land_detector_task = nullptr;
-	_landDetectorTaskID = -1;
 	warnx("land_detector has been stopped");
 }
 
@@ -116,7 +101,7 @@ static void land_detector_stop()
 **/
 static int land_detector_start(const char *mode)
 {
-	if (land_detector_task != nullptr || _landDetectorTaskID != -1) {
+	if (land_detector_task != nullptr) {
 		warnx("already running");
 		return -1;
 	}
@@ -140,14 +125,9 @@ static int land_detector_start(const char *mode)
 	}
 
 	//Start new thread task
-	_landDetectorTaskID = px4_task_spawn_cmd("land_detector",
-					     SCHED_DEFAULT,
-					     SCHED_PRIORITY_DEFAULT,
-					     1000,
-					     (px4_main_t)&land_detector_deamon_thread,
-					     nullptr);
+	int ret = land_detector_task->start();
 
-	if (_landDetectorTaskID < 0) {
+	if (ret) {
 		warnx("task start failed: %d", -errno);
 		return -1;
 	}

src/modules/land_detector/land_detector_params.c

@@ -38,8 +38,6 @@
  * @author Johan Jansen <jnsn.johan@gmail.com>
  */
 
-#include <systemlib/param/param.h>
-
 /**
  * Multicopter max climb rate
  *

src/modules/land_detector/module.mk

@@ -1,17 +0,0 @@
-#
-# Land detector
-#
-
-MODULE_COMMAND	= land_detector
-
-SRCS		= land_detector_main.cpp \
-              land_detector_params.c \
-              LandDetector.cpp \
-			  MulticopterLandDetector.cpp \
-              FixedwingLandDetector.cpp
-
-EXTRACXXFLAGS   = -Weffc++ -Os
-
-# Startup handler, the actual app stack size is
-# in the task_spawn command
-MODULE_STACKSIZE = 1200

src/modules/mavlink/CMakeLists.txt

@@ -0,0 +1,63 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+if (${OS} STREQUAL "nuttx")
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=1500)
+endif()
+px4_add_module(
+	MODULE modules__mavlink
+	MAIN mavlink
+	STACK 1200
+	COMPILE_FLAGS
+		${MODULE_CFLAGS}
+		-Wno-attributes
+		-Wno-packed
+		-DMAVLINK_COMM_NUM_BUFFERS=3
+		-Wno-packed
+		-Wno-tautological-constant-out-of-range-compare
+		-Os
+	SRCS
+		mavlink.c
+		mavlink_main.cpp
+		mavlink_mission.cpp
+		mavlink_parameters.cpp
+		mavlink_orb_subscription.cpp
+		mavlink_messages.cpp
+		mavlink_stream.cpp
+		mavlink_rate_limiter.cpp
+		mavlink_receiver.cpp
+		mavlink_ftp.cpp
+		mavlink_params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/mavlink/mavlink.c

@@ -46,75 +46,6 @@
 #include "mavlink_bridge_header.h"
 #include <systemlib/param/param.h>
 
-/**
- * MAVLink system ID
- * @group MAVLink
- * @min 1
- * @max 250
- */
-PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
-
-/**
- * MAVLink component ID
- * @group MAVLink
- * @min 1
- * @max 250
- */
-PARAM_DEFINE_INT32(MAV_COMP_ID, 1);
-
-/**
- * MAVLink Radio ID
- *
- * When non-zero the MAVLink app will attempt to configure the
- * radio to this ID and re-set the parameter to 0. If the value
- * is negative it will reset the complete radio config to
- * factory defaults.
- *
- * @group MAVLink
- * @min -1
- * @max 240
- */
-PARAM_DEFINE_INT32(MAV_RADIO_ID, 0);
-
-/**
- * MAVLink airframe type
- *
- * @min 1
- * @group MAVLink
- */
-PARAM_DEFINE_INT32(MAV_TYPE, 1);
-
-/**
- * Use/Accept HIL GPS message even if not in HIL mode
- *
- * If set to 1 incoming HIL GPS messages are parsed.
- *
- * @group MAVLink
- */
-PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
-
-/**
- * Forward external setpoint messages
- *
- * If set to 1 incoming external setpoint messages will be directly forwarded
- * to the controllers if in offboard control mode
- *
- * @group MAVLink
- */
-PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
-
-/**
- * Test parameter
- *
- * This parameter is not actively used by the system. Its purpose is to allow
- * testing the parameter interface on the communication level.
- *
- * @group MAVLink
- * @min -1000
- * @max 1000
- */
-PARAM_DEFINE_INT32(MAV_TEST_PAR, 1);
-
 mavlink_system_t mavlink_system = {
 	100,
 	50

src/modules/mavlink/mavlink_main.cpp

@@ -178,6 +178,11 @@ Mavlink::Mavlink() :
 	_rate_tx(0.0f),
 	_rate_txerr(0.0f),
 	_rate_rx(0.0f),
+#ifdef __PX4_POSIX
+	_myaddr{},
+	_src_addr{},
+	_bcast_addr{},
+#endif
 	_socket_fd(-1),
 	_protocol(SERIAL),
 	_network_port(14556),
@@ -695,7 +700,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		_is_usb_uart = true;
 	}
 
-#ifdef __PX4_LINUX
+#if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
 	/* Put in raw mode */
 	cfmakeraw(&uart_config);
 #endif
@@ -796,7 +801,7 @@ Mavlink::get_free_tx_buf()
 #ifndef __PX4_POSIX
 
 // No FIONWRITE on Linux
-#if !defined(__PX4_LINUX)
+#if !defined(__PX4_LINUX) && !defined(__PX4_DARWIN)
 	(void) ioctl(_uart_fd, FIONWRITE, (unsigned long)&buf_free);
 #endif
 
@@ -883,8 +888,23 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 #else
 	if (get_protocol() == UDP) {
 		ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
+
+		struct telemetry_status_s &tstatus = get_rx_status();
+
+		/* resend heartbeat via broadcast */
+		if (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) ||
+			(tstatus.heartbeat_time == 0)) &&
+			msgid == MAVLINK_MSG_ID_HEARTBEAT) {
+
+			int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
+
+			if (bret <= 0) {
+				PX4_WARN("sending broadcast failed");
+			}
+		}
+
 	} else if (get_protocol() == TCP) {
-		// not implemented, but possible to do so
+		/* not implemented, but possible to do so */
 		warnx("TCP transport pending implementation");
 	}
 #endif
@@ -957,8 +977,8 @@ Mavlink::resend_message(mavlink_message_t *msg)
 void
 Mavlink::init_udp()
 {
-#ifdef __PX4_LINUX
-	PX4_INFO("Setting up UDP w/port %d\n",_network_port);
+#if defined (__PX4_LINUX) || defined (__PX4_DARWIN)
+	PX4_INFO("Setting up UDP w/port %d",_network_port);
 
 	memset((char *)&_myaddr, 0, sizeof(_myaddr));
 	_myaddr.sin_family = AF_INET;
@@ -966,21 +986,33 @@ Mavlink::init_udp()
 	_myaddr.sin_port = htons(_network_port);
 
 	if ((_socket_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
-		PX4_WARN("create socket failed\n");
+		PX4_WARN("create socket failed");
+		return;
+	}
+
+	int broadcast_opt = 1;
+	if (setsockopt(_socket_fd, SOL_SOCKET, SO_BROADCAST, &broadcast_opt, sizeof(broadcast_opt)) < 0) {
+		PX4_WARN("setting broadcast permission failed");
 		return;
 	}
 
 	if (bind(_socket_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) {
-		PX4_WARN("bind failed\n");
+		PX4_WARN("bind failed");
 		return;
 	}
 
-	// set default target address
+	/* set default target address */
 	memset((char *)&_src_addr, 0, sizeof(_src_addr));
 	_src_addr.sin_family = AF_INET;
 	inet_aton("127.0.0.1", &_src_addr.sin_addr);
 	_src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
 
+	/* default broadcast address */
+	memset((char *)&_bcast_addr, 0, sizeof(_bcast_addr));
+	_bcast_addr.sin_family = AF_INET;
+	inet_aton("255.255.255.255", &_bcast_addr.sin_addr);
+	_bcast_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
+
 #endif
 }
 
@@ -1637,20 +1669,21 @@ Mavlink::task_main(int argc, char *argv[])
 	switch (_mode) {
 	case MAVLINK_MODE_NORMAL:
 		configure_stream("SYS_STATUS", 1.0f);
-		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
+		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("HIGHRES_IMU", 2.0f);
 		configure_stream("ATTITUDE", 20.0f);
 		configure_stream("VFR_HUD", 8.0f);
 		configure_stream("GPS_RAW_INT", 1.0f);
 		configure_stream("GLOBAL_POSITION_INT", 3.0f);
 		configure_stream("LOCAL_POSITION_NED", 3.0f);
+		configure_stream("NAMED_VALUE_FLOAT", 2.0f);
 		configure_stream("RC_CHANNELS", 1.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
 		configure_stream("ATTITUDE_TARGET", 8.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
 		configure_stream("OPTICAL_FLOW_RAD", 5.0f);
-		configure_stream("VTOL_STATE", 0.5f);
+		configure_stream("EXTENDED_SYS_STATE", 1.0f);
 		break;
 
 	case MAVLINK_MODE_ONBOARD:
@@ -1660,7 +1693,9 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GPS_RAW_INT", 5.0f);
 		configure_stream("GLOBAL_POSITION_INT", 50.0f);
 		configure_stream("LOCAL_POSITION_NED", 30.0f);
+		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
 		configure_stream("CAMERA_CAPTURE", 2.0f);
+		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("ATTITUDE_TARGET", 10.0f);
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
 		configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f);
@@ -1674,7 +1709,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
 		/* camera trigger is rate limited at the source, do not limit here */
 		configure_stream("CAMERA_TRIGGER", 500.0f);
-		configure_stream("VTOL_STATE", 2.0f);
+		configure_stream("EXTENDED_SYS_STATE", 2.0f);
 		break;
 
 	case MAVLINK_MODE_OSD:
@@ -1683,19 +1718,24 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("VFR_HUD", 5.0f);
 		configure_stream("GPS_RAW_INT", 1.0f);
 		configure_stream("GLOBAL_POSITION_INT", 10.0f);
+		configure_stream("HOME_POSITION", 0.5f);
 		configure_stream("ATTITUDE_TARGET", 10.0f);
 		configure_stream("BATTERY_STATUS", 1.0f);
 		configure_stream("SYSTEM_TIME", 1.0f);
 		configure_stream("RC_CHANNELS", 5.0f);
 		configure_stream("SERVO_OUTPUT_RAW_0", 1.0f);
-		configure_stream("VTOL_STATE", 0.5f);
+		configure_stream("EXTENDED_SYS_STATE", 1.0f);
 		break;
 
 	case MAVLINK_MODE_CONFIG:
 		// Enable a number of interesting streams we want via USB
+		configure_stream("SYS_STATUS", 1.0f);
+		configure_stream("HOME_POSITION", 0.5f);
+		configure_stream("GLOBAL_POSITION_INT", 10.0f);
+		configure_stream("ATTITUDE_TARGET", 8.0f);
 		configure_stream("PARAM_VALUE", 300.0f);
 		configure_stream("MISSION_ITEM", 50.0f);
-		configure_stream("NAMED_VALUE_FLOAT", 10.0f);
+		configure_stream("NAMED_VALUE_FLOAT", 50.0f);
 		configure_stream("OPTICAL_FLOW_RAD", 10.0f);
 		configure_stream("DISTANCE_SENSOR", 10.0f);
 		configure_stream("VFR_HUD", 20.0f);
@@ -1707,10 +1747,10 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
 		configure_stream("LOCAL_POSITION_NED", 30.0f);
 		configure_stream("MANUAL_CONTROL", 5.0f);
-		configure_stream("HIGHRES_IMU", 100.0f);
+		configure_stream("HIGHRES_IMU", 50.0f);
 		configure_stream("GPS_RAW_INT", 20.0f);
 		configure_stream("CAMERA_TRIGGER", 500.0f);
-		configure_stream("VTOL_STATE", 2.0f);
+		configure_stream("EXTENDED_SYS_STATE", 2.0f);
 
 	default:
 		break;

src/modules/mavlink/mavlink_main.h

@@ -417,6 +417,7 @@ private:
 #ifdef __PX4_POSIX
 	struct sockaddr_in _myaddr;
 	struct sockaddr_in _src_addr;
+	struct sockaddr_in _bcast_addr;
 
 #endif
 	int _socket_fd;

src/modules/mavlink/mavlink_messages.cpp

@@ -72,6 +72,7 @@
 #include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/camera_trigger.h>
+#include <uORB/topics/vehicle_land_detected.h>
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_pwm_output.h>
 #include <systemlib/err.h>
@@ -555,7 +556,7 @@ protected:
 			msg.errors_count2 = status.errors_count2;
 			msg.errors_count3 = status.errors_count3;
 			msg.errors_count4 = status.errors_count4;
-			msg.battery_remaining = (msg.voltage_battery > 0) ?
+			msg.battery_remaining = (status.condition_battery_voltage_valid) ?
 							status.battery_remaining * 100.0f : -1;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_SYS_STATUS, &msg);
@@ -570,14 +571,22 @@ protected:
 				if (i < status.battery_cell_count) {
 					bat_msg.voltages[i] = (status.battery_voltage / status.battery_cell_count) * 1000.0f;
 				} else {
-					bat_msg.voltages[i] = 0;
+					bat_msg.voltages[i] = UINT16_MAX;
 				}
 			}
-			bat_msg.current_battery = status.battery_current * 100.0f;
-			bat_msg.current_consumed = status.battery_discharged_mah;
+
+			if (status.condition_battery_voltage_valid) {
+				bat_msg.current_battery = (bat_msg.current_battery >= 0.0f) ?
+								status.battery_current * 100.0f : -1;
+				bat_msg.current_consumed = (bat_msg.current_consumed >= 0.0f) ?
+								status.battery_discharged_mah : -1;
+				bat_msg.battery_remaining = status.battery_remaining * 100.0f;
+			} else {
+				bat_msg.current_battery = -1.0f;
+				bat_msg.current_consumed = -1.0f;
+				bat_msg.battery_remaining = -1.0f;
+			}
 			bat_msg.energy_consumed = -1.0f;
-			bat_msg.battery_remaining = (status.battery_voltage > 0) ?
-							status.battery_remaining * 100.0f : -1;
 
 			_mavlink->send_message(MAVLINK_MSG_ID_BATTERY_STATUS, &bat_msg);
 		}
@@ -1326,43 +1335,43 @@ protected:
 };
 
 
-class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
+class MavlinkStreamHomePosition : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamGPSGlobalOrigin::get_name_static();
+		return MavlinkStreamHomePosition::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "GPS_GLOBAL_ORIGIN";
+		return "HOME_POSITION";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
+		return MAVLINK_MSG_ID_HOME_POSITION;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamGPSGlobalOrigin(mavlink);
+		return new MavlinkStreamHomePosition(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+		return _home_sub->is_published() ? (MAVLINK_MSG_ID_HOME_POSITION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
 	}
 
 private:
 	MavlinkOrbSubscription *_home_sub;
 
 	/* do not allow top copying this class */
-	MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &);
-	MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &);
+	MavlinkStreamHomePosition(MavlinkStreamHomePosition &);
+	MavlinkStreamHomePosition& operator = (const MavlinkStreamHomePosition &);
 
 protected:
-	explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink),
+	explicit MavlinkStreamHomePosition(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position)))
 	{}
 
@@ -1374,13 +1383,26 @@ protected:
 			struct home_position_s home;
 
 			if (_home_sub->update(&home)) {
-				mavlink_gps_global_origin_t msg;
+				mavlink_home_position_t msg;
 
 				msg.latitude = home.lat * 1e7;
 				msg.longitude = home.lon * 1e7;
 				msg.altitude = home.alt * 1e3f;
 
-				_mavlink->send_message(MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, &msg);
+				msg.x = home.x;
+				msg.y = home.y;
+				msg.z = home.z;
+
+				msg.q[0] = 1.0f;
+				msg.q[1] = 0.0f;
+				msg.q[2] = 0.0f;
+				msg.q[3] = 0.0f;
+
+				msg.approach_x = 0.0f;
+				msg.approach_y = 0.0f;
+				msg.approach_z = 0.0f;
+
+				_mavlink->send_message(MAVLINK_MSG_ID_HOME_POSITION, &msg);
 			}
 		}
 	}
@@ -2348,76 +2370,96 @@ protected:
 	}
 };
 
-class MavlinkStreamVtolState : public MavlinkStream
+class MavlinkStreamExtendedSysState : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamVtolState::get_name_static();
+		return MavlinkStreamExtendedSysState::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "VTOL_STATE";
+		return "EXTENDED_SYS_STATE";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_VTOL_STATE;
+		return MAVLINK_MSG_ID_EXTENDED_SYS_STATE;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamVtolState(mavlink);
+		return new MavlinkStreamExtendedSysState(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_VTOL_STATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return MAVLINK_MSG_ID_EXTENDED_SYS_STATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
 	MavlinkOrbSubscription *_status_sub;
+	MavlinkOrbSubscription *_landed_sub;
+	mavlink_extended_sys_state_t _msg;
 
 	/* do not allow top copying this class */
-	MavlinkStreamVtolState(MavlinkStreamVtolState &);
-	MavlinkStreamVtolState &operator = (const MavlinkStreamVtolState &);
+	MavlinkStreamExtendedSysState(MavlinkStreamExtendedSysState &);
+	MavlinkStreamExtendedSysState &operator = (const MavlinkStreamExtendedSysState &);
 
 protected:
-	explicit MavlinkStreamVtolState(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status)))
-	{}
+	explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))),
+		_landed_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))),
+		_msg{}
+	{
+
+		_msg.vtol_state = MAV_VTOL_STATE_UNDEFINED;
+		_msg.landed_state = MAV_LANDED_STATE_UNDEFINED;
+	}
 
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_status_s status;
+		struct vehicle_land_detected_s land_detected;
+		bool updated = false;
 
 		if (_status_sub->update(&status)) {
-			mavlink_vtol_state_t msg;
+			updated = true;
 
 			if (status.is_vtol) {
 				if (status.is_rotary_wing) {
 					if (status.in_transition_mode) {
-						msg.state = MAV_VTOL_STATE_TRANSITION_TO_FW;
+						_msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_FW;
 
 					} else {
-						msg.state = MAV_VTOL_STATE_MC;
+						_msg.vtol_state = MAV_VTOL_STATE_MC;
 					}
 
 				} else {
 					if (status.in_transition_mode) {
-						msg.state = MAV_VTOL_STATE_TRANSITION_TO_MC;
+						_msg.vtol_state = MAV_VTOL_STATE_TRANSITION_TO_MC;
 
 					} else {
-						msg.state = MAV_VTOL_STATE_FW;
+						_msg.vtol_state = MAV_VTOL_STATE_FW;
 					}
 				}
-
-			} else {
-				msg.state = MAV_VTOL_STATE_UNDEFINED;
 			}
+		}
 
-			_mavlink->send_message(MAVLINK_MSG_ID_VTOL_STATE, &msg);
+		if (_landed_sub->update(&land_detected)) {
+			updated = true;
+
+			if (land_detected.landed) {
+				_msg.landed_state = MAV_LANDED_STATE_ON_GROUND;
+			
+			} else {
+				_msg.landed_state = MAV_LANDED_STATE_IN_AIR;
+			}
+		}
+
+		if (updated) {
+			_mavlink->send_message(MAVLINK_MSG_ID_EXTENDED_SYS_STATE, &_msg);
 		}
 	}
 };
@@ -2437,7 +2479,7 @@ const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
 	new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
 	new StreamListItem(&MavlinkStreamAttPosMocap::new_instance, &MavlinkStreamAttPosMocap::get_name_static),
-	new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
+	new StreamListItem(&MavlinkStreamHomePosition::new_instance, &MavlinkStreamHomePosition::get_name_static),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
@@ -2457,6 +2499,6 @@ const StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
 	new StreamListItem(&MavlinkStreamCameraTrigger::new_instance, &MavlinkStreamCameraTrigger::get_name_static),
 	new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
-	new StreamListItem(&MavlinkStreamVtolState::new_instance, &MavlinkStreamVtolState::get_name_static),
+	new StreamListItem(&MavlinkStreamExtendedSysState::new_instance, &MavlinkStreamExtendedSysState::get_name_static),
 	nullptr
 };

src/modules/mavlink/mavlink_mission.cpp

@@ -779,13 +779,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		mission_item->altitude_is_relative = true;
 		break;
 
-	case MAV_FRAME_LOCAL_NED:
-	case MAV_FRAME_LOCAL_ENU:
-		return MAV_MISSION_UNSUPPORTED_FRAME;
-
 	case MAV_FRAME_MISSION:
+		// This is a mission item with no coordinate
+		break;
+			
 	default:
-		return MAV_MISSION_ERROR;
+		return MAV_MISSION_UNSUPPORTED_FRAME;
 	}
 
 	switch (mavlink_mission_item->command) {

src/modules/mavlink/mavlink_parameters.cpp

@@ -44,6 +44,8 @@
 #include "mavlink_parameters.h"
 #include "mavlink_main.h"
 
+#define HASH_PARAM "_HASH_CHECK"
+
 MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_send_all_index(-1),
 	_rc_param_map_pub(nullptr),
@@ -121,13 +123,27 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 
 				/* when no index is given, loop through string ids and compare them */
 				if (req_read.param_index < 0) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter and send it */
-					send_param(param_find_no_notification(name));
+					if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) {
+						/* return hash check for cached params */
+						uint32_t hash = param_hash_check();
+
+						/* build the one-off response message */
+						mavlink_param_value_t msg;
+						msg.param_count = param_count_used();
+						msg.param_index = -1;
+						strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+						msg.param_type = MAV_PARAM_TYPE_UINT32;
+						memcpy(&msg.param_value, &hash, sizeof(hash));
+						_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
+					} else {
+						/* local name buffer to enforce null-terminated string */
+						char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+						strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+						/* enforce null termination */
+						name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+						/* attempt to find parameter and send it */
+						send_param(param_find_no_notification(name));
+					}
 
 				} else {
 					/* when index is >= 0, send this parameter again */

src/modules/mavlink/mavlink_params.c

@@ -0,0 +1,104 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2015 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.
+ *
+ ****************************************************************************/
+
+#include <systemlib/param/param.h>
+
+/**
+ * MAVLink system ID
+ * @group MAVLink
+ * @min 1
+ * @max 250
+ */
+PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
+
+/**
+ * MAVLink component ID
+ * @group MAVLink
+ * @min 1
+ * @max 250
+ */
+PARAM_DEFINE_INT32(MAV_COMP_ID, 1);
+
+/**
+ * MAVLink Radio ID
+ *
+ * When non-zero the MAVLink app will attempt to configure the
+ * radio to this ID and re-set the parameter to 0. If the value
+ * is negative it will reset the complete radio config to
+ * factory defaults.
+ *
+ * @group MAVLink
+ * @min -1
+ * @max 240
+ */
+PARAM_DEFINE_INT32(MAV_RADIO_ID, 0);
+
+/**
+ * MAVLink airframe type
+ *
+ * @min 1
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_TYPE, 1);
+
+/**
+ * Use/Accept HIL GPS message even if not in HIL mode
+ *
+ * If set to 1 incoming HIL GPS messages are parsed.
+ *
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
+
+/**
+ * Forward external setpoint messages
+ *
+ * If set to 1 incoming external setpoint messages will be directly forwarded
+ * to the controllers if in offboard control mode
+ *
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
+
+/**
+ * Test parameter
+ *
+ * This parameter is not actively used by the system. Its purpose is to allow
+ * testing the parameter interface on the communication level.
+ *
+ * @group MAVLink
+ * @min -1000
+ * @max 1000
+ */
+PARAM_DEFINE_INT32(MAV_TEST_PAR, 1);
+

src/modules/mavlink/mavlink_receiver.cpp

@@ -49,7 +49,6 @@
 #include <math.h>
 #include <stdbool.h>
 #include <fcntl.h>
-#include <mqueue.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_accel.h>
@@ -62,7 +61,6 @@
 #include <float.h>
 #include <unistd.h>
 #ifndef __PX4_POSIX
-#include <sys/prctl.h>
 #include <termios.h>
 #endif
 #include <errno.h>
@@ -128,8 +126,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
 	_old_timestamp(0),
+	_hil_last_frame(0),
 	_hil_local_proj_inited(0),
 	_hil_local_alt0(0.0f),
+	_hil_prev_gyro{},
+	_hil_prev_accel{},
 	_hil_local_proj_ref{},
 	_offboard_control_mode{},
 	_att_sp{},
@@ -309,6 +310,10 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg)
 		} else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
 			/* send autopilot version message */
 			_mavlink->send_autopilot_capabilites();
+
+		} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
+			_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
+
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
@@ -365,6 +370,13 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg)
 			/* terminate other threads and this thread */
 			_mavlink->_task_should_exit = true;
 
+		} else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) {
+			/* send autopilot version message */
+			_mavlink->send_autopilot_capabilites();
+
+		} else if (cmd_mavlink.command == MAV_CMD_GET_HOME_POSITION) {
+			_mavlink->configure_stream_threadsafe("HOME_POSITION", 0.5f);
+
 		} else {
 
 			if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) {
@@ -1292,6 +1304,17 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	uint64_t timestamp = hrt_absolute_time();
 
+	float dt;
+
+	if (_hil_last_frame == 0 ||
+	   (imu.time_usec - _hil_last_frame) > (0.1f * 1e6f) ||
+	   (_hil_last_frame >= imu.time_usec)) {
+		dt = 0.01f; /* default to 100 Hz */
+	} else {
+		dt = (imu.time_usec - _hil_last_frame) / 1e6f;
+	}
+	_hil_last_frame = imu.time_usec;
+
 	/* airspeed */
 	{
 		struct airspeed_s airspeed;
@@ -1407,20 +1430,32 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f;
 		hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f;
 		hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f;
-		hil_sensors.gyro_rad_s[0] = imu.xgyro;
+		hil_sensors.gyro_rad_s[0] = ((imu.xgyro * dt + _hil_prev_gyro[0]) / 2.0f) / dt;
 		hil_sensors.gyro_rad_s[1] = imu.ygyro;
 		hil_sensors.gyro_rad_s[2] = imu.zgyro;
+		hil_sensors.gyro_integral_rad[0] = (hil_sensors.gyro_rad_s[0] * dt + _hil_prev_gyro[0]) / 2.0f;
+		hil_sensors.gyro_integral_rad[1] = (hil_sensors.gyro_rad_s[1] * dt + _hil_prev_gyro[1]) / 2.0f;
+		hil_sensors.gyro_integral_rad[2] = (hil_sensors.gyro_rad_s[2] * dt + _hil_prev_gyro[2]) / 2.0f;
+		memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_integral_rad[0], sizeof(_hil_prev_gyro));
+		hil_sensors.gyro_integral_dt[0] = dt * 1e6f;
 		hil_sensors.gyro_timestamp[0] = timestamp;
+		hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH;
 
 		hil_sensors.accelerometer_raw[0] = imu.xacc / mg2ms2;
 		hil_sensors.accelerometer_raw[1] = imu.yacc / mg2ms2;
 		hil_sensors.accelerometer_raw[2] = imu.zacc / mg2ms2;
-		hil_sensors.accelerometer_m_s2[0] = imu.xacc;
-		hil_sensors.accelerometer_m_s2[1] = imu.yacc;
-		hil_sensors.accelerometer_m_s2[2] = imu.zacc;
+		hil_sensors.accelerometer_m_s2[0] = ((imu.xacc * dt + _hil_prev_accel[0]) / 2.0f) / dt;
+		hil_sensors.accelerometer_m_s2[1] = ((imu.yacc * dt + _hil_prev_accel[1]) / 2.0f) / dt;
+		hil_sensors.accelerometer_m_s2[2] = (((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f) / dt - 9.80665f;
+		hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f;
+		hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f;
+		hil_sensors.accelerometer_integral_m_s[2] = ((imu.zacc + 9.80665f) * dt + _hil_prev_accel[2]) / 2.0f;
+		memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel));
+		hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f;
 		hil_sensors.accelerometer_mode[0] = 0; // TODO what is this?
 		hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16
 		hil_sensors.accelerometer_timestamp[0] = timestamp;
+		hil_sensors.accelerometer_priority[0] = ORB_PRIO_HIGH;
 
 		hil_sensors.adc_voltage_v[0] = 0.0f;
 		hil_sensors.adc_voltage_v[1] = 0.0f;
@@ -1436,6 +1471,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.magnetometer_mode[0] = 0; // TODO what is this
 		hil_sensors.magnetometer_cuttoff_freq_hz[0] = 50.0f;
 		hil_sensors.magnetometer_timestamp[0] = timestamp;
+		hil_sensors.magnetometer_priority[0] = ORB_PRIO_HIGH;
 
 		hil_sensors.baro_pres_mbar[0] = imu.abs_pressure;
 		hil_sensors.baro_alt_meter[0] = imu.pressure_alt;

src/modules/mavlink/mavlink_receiver.h

@@ -196,8 +196,11 @@ private:
 	int _control_mode_sub;
 	int _hil_frames;
 	uint64_t _old_timestamp;
+	uint64_t _hil_last_frame;
 	bool _hil_local_proj_inited;
 	float _hil_local_alt0;
+	float _hil_prev_gyro[3];
+	float _hil_prev_accel[3];
 	struct map_projection_reference_s _hil_local_proj_ref;
 	struct offboard_control_mode_s _offboard_control_mode;
 	struct vehicle_attitude_setpoint_s _att_sp;

src/modules/mavlink/mavlink_tests/CMakeLists.txt

@@ -0,0 +1,53 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__mavlink__mavlink_tests
+	MAIN mavlink_tests
+	STACK 5000
+	COMPILE_FLAGS
+		-Weffc++
+		-DMAVLINK_FTP_UNIT_TEST
+		-Wno-attributes
+		-Wno-packed
+		-Wno-packed
+		-Os
+	SRCS
+		mavlink_tests.cpp
+		mavlink_ftp_test.cpp
+		../mavlink_stream.cpp
+		../mavlink_ftp.cpp
+		../mavlink.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/mavlink/module.mk

@@ -1,59 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012-2014 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.
-#
-############################################################################
-
-#
-# MAVLink protocol to uORB interface process
-#
-
-MODULE_COMMAND  = mavlink
-
-SRCS 		 +=	mavlink.c \
-		  	mavlink_main.cpp \
-			mavlink_mission.cpp \
-			mavlink_parameters.cpp \
-			mavlink_orb_subscription.cpp \
-			mavlink_messages.cpp \
-			mavlink_stream.cpp \
-			mavlink_rate_limiter.cpp \
-			mavlink_receiver.cpp \
-			mavlink_ftp.cpp 
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
-
-MAXOPTIMIZATION	 = -Os
-
-MODULE_STACKSIZE = 1200
-
-EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed -DMAVLINK_COMM_NUM_BUFFERS=3
-
-EXTRACFLAGS	= -Wno-packed -DMAVLINK_COMM_NUM_BUFFERS=3

src/modules/mc_att_control/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+if (${OS} STREQUAL "nuttx")
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=3500)
+endif()
+px4_add_module(
+	MODULE modules__mc_att_control
+	MAIN mc_att_control
+	STACK 1200
+	COMPILE_FLAGS ${MODULE_CFLAGS}
+	SRCS
+		mc_att_control_main.cpp
+		mc_att_control_params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/mc_att_control/module.mk

@@ -1,45 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2013, 2014 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.
-#
-############################################################################
-
-#
-# Multirotor attitude controller (vector based, no Euler singularities)
-#
-
-MODULE_COMMAND	= mc_att_control
-
-SRCS		= mc_att_control_main.cpp \
-			  mc_att_control_params.c
-
-# Startup handler, the actual app stack size is
-# in the task_spawn command
-MODULE_STACKSIZE = 1200

src/modules/mc_att_control_multiplatform/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__mc_att_control_multiplatform
+	MAIN mc_att_control_m
+	SRCS
+		mc_att_control_main.cpp
+		mc_att_control_start_nuttx.cpp
+		mc_att_control.cpp
+		mc_att_control_base.cpp
+		mc_att_control_params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/mc_att_control_multiplatform/mc_att_control.cpp

@@ -64,11 +64,9 @@ static const int ERROR = -1;
 
 MulticopterAttitudeControlMultiplatform::MulticopterAttitudeControlMultiplatform() :
 	MulticopterAttitudeControlBase(),
-	_task_should_exit(false),
 	_actuators_0_circuit_breaker_enabled(false),
 
 	/* publications */
-	_att_sp_pub(nullptr),
 	_v_rates_sp_pub(nullptr),
 	_actuators_0_pub(nullptr),
 	_n(_appState),

src/modules/mc_att_control_multiplatform/mc_att_control.h

@@ -85,10 +85,8 @@ public:
 	void spin() { _n.spin(); }
 
 private:
-	bool		_task_should_exit;			/**< if true, sensor task should exit */
 	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
 
-	px4::Publisher<px4_vehicle_attitude_setpoint> 	*_att_sp_pub;			/**< attitude setpoint publication */
 	px4::Publisher<px4_vehicle_rates_setpoint> 	*_v_rates_sp_pub;		/**< rate setpoint publication */
 	px4::Publisher<px4_actuator_controls_0> 	*_actuators_0_pub;		/**< attitude actuator controls publication */
 

src/modules/mc_att_control_multiplatform/module.mk

@@ -1,44 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2013, 2014 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.
-#
-############################################################################
-
-#
-# Multirotor attitude controller (vector based, no Euler singularities)
-#
-
-MODULE_COMMAND	= mc_att_control_m
-
-SRCS		= mc_att_control_main.cpp \
-		  mc_att_control_start_nuttx.cpp \
-		  mc_att_control.cpp \
-		  mc_att_control_base.cpp \
-		  mc_att_control_params.c

src/modules/mc_pos_control/CMakeLists.txt

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__mc_pos_control
+	MAIN mc_pos_control
+	STACK 1200
+	SRCS
+		mc_pos_control_main.cpp
+		mc_pos_control_params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -803,7 +803,7 @@ void MulticopterPositionControl::control_auto(float dt)
 				       &curr_sp.data[0], &curr_sp.data[1]);
 		curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
 
-		/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
+		/* scaled space: 1 == position error resulting max allowed speed */
 		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
 
 		/* convert current setpoint to scaled space */
@@ -822,14 +822,14 @@ void MulticopterPositionControl::control_auto(float dt)
 
 			if ((curr_sp - prev_sp).length() > MIN_DIST) {
 
-				/* find X - cross point of L1 sphere and trajectory */
+				/* find X - cross point of unit sphere and trajectory */
 				math::Vector<3> pos_s = _pos.emult(scale);
 				math::Vector<3> prev_sp_s = prev_sp.emult(scale);
 				math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
 				math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
 				float curr_pos_s_len = curr_pos_s.length();
 				if (curr_pos_s_len < 1.0f) {
-					/* copter is closer to waypoint than L1 radius */
+					/* copter is closer to waypoint than unit radius */
 					/* check next waypoint and use it to avoid slowing down when passing via waypoint */
 					if (_pos_sp_triplet.next.valid) {
 						math::Vector<3> next_sp;
@@ -853,7 +853,7 @@ void MulticopterPositionControl::control_auto(float dt)
 
 							if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
 								float curr_next_s_len = curr_next_s.length();
-								/* if curr - next distance is larger than L1 radius, limit it */
+								/* if curr - next distance is larger than unit radius, limit it */
 								if (curr_next_s_len > 1.0f) {
 									cos_a_curr_next /= curr_next_s_len;
 								}
@@ -870,7 +870,7 @@ void MulticopterPositionControl::control_auto(float dt)
 				} else {
 					bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
 					if (near) {
-						/* L1 sphere crosses trajectory */
+						/* unit sphere crosses trajectory */
 
 					} else {
 						/* copter is too far from trajectory */

src/modules/mc_pos_control_multiplatform/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__mc_pos_control_multiplatform
+	MAIN mc_pos_control_m
+	SRCS
+		mc_pos_control_main.cpp
+		mc_pos_control_start_nuttx.cpp
+		mc_pos_control.cpp
+		mc_pos_control_params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/mc_pos_control_multiplatform/module.mk

@@ -1,48 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2013-2015 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.
-#
-############################################################################
-
-#
-# Build multicopter position controller
-#
-
-MODULE_COMMAND	= mc_pos_control_m
-
-SRCS		= mc_pos_control_main.cpp \
-		  mc_pos_control_start_nuttx.cpp \
-		  mc_pos_control.cpp \
-		  mc_pos_control_params.c
-
-# Only NuttX has a frame size limit
-ifeq ($(PX4_TARGET_OS),nuttx)
-EXTRACXXFLAGS = -Wframe-larger-than=1200
-endif

src/modules/muorb/adsp/CMakeLists.txt

@@ -0,0 +1,45 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+include_directories("../../uORB")
+
+px4_add_module(
+	MODULE modules__muorb__adsp
+	COMPILE_FLAGS
+		-Os
+	SRCS
+		px4muorb.cpp
+		uORBFastRpcChannel.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/muorb/adsp/px4muorb.cpp

@@ -31,39 +31,30 @@
  *
  ****************************************************************************/
 #include "px4muorb.hpp"
-#include "qurt.h"
 #include "uORBFastRpcChannel.hpp"
 #include "uORBManager.hpp"
 
 #include <px4_middleware.h>
 #include <px4_tasks.h>
 #include <px4_posix.h>
-#include <map>
-#include <string>
+#include <dspal_platform.h>
 #include "px4_log.h"
 #include "uORB/topics/sensor_combined.h"
 #include "uORB.h"
 
-#include "HAP_power.h"
-
-#define _ENABLE_MUORB 1
-
-extern "C" {
-int dspal_main(int argc, const char *argv[]);
-};
-
+__BEGIN_DECLS
+extern int dspal_main(int argc, char *argv[]);
+__END_DECLS
 
 int px4muorb_orb_initialize()
 {
-	int rc = 0;
-	PX4_WARN("Before calling dspal_entry() method...");
 	HAP_power_request(100, 100, 1000);
 	// register the fastrpc muorb with uORBManager.
 	uORB::Manager::get_instance()->set_uorb_communicator(uORB::FastRpcChannel::GetInstance());
-	const char *argv[2] = { "dspal", "start" };
+	const char *argv[] = { "dspal", "start" };
 	int argc = 2;
-	dspal_main(argc, argv);
-	PX4_WARN("After calling dspal_entry");
+	int rc;
+	rc = dspal_main(argc, (char **)argv);
 	return rc;
 }
 
@@ -103,7 +94,6 @@ int px4muorb_remove_subscriber(const char *name)
 	}
 
 	return rc;
-
 }
 
 int px4muorb_send_topic_data(const char *name, const uint8_t *data, int data_len_in_bytes)

src/modules/muorb/krait-stubs/px4muorb_KraitRpcWrapper.hpp

@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2015 Mark Charlebois. 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.
+ *
+ ****************************************************************************/
+#ifndef _px4muorb_KraitRpcWrapper_hpp_
+#define _px4muorb_KraitRpcWrapper_hpp_
+#include <stdint.h>
+
+namespace px4muorb
+{
+   class KraitRpcWrapper;
+}
+
+class px4muorb::KraitRpcWrapper
+{
+public:
+   /**
+    * Constructor
+    */
+   KraitRpcWrapper() {}
+   
+   /**
+    * destructor
+    */
+   ~KraitRpcWrapper() {}
+
+   /**
+    * Initiatizes the rpc channel px4 muorb
+    */
+   bool Initialize() { return true; }
+
+   /**
+    * Terminate to clean up the resources.  This should be called at program exit
+    */
+   bool Terminate() { return true; }
+
+   /**
+    * Muorb related functions to pub/sub of orb topic from krait to adsp
+    */
+   int32_t AddSubscriber( const char* topic ) { return 1; }
+   int32_t RemoveSubscriber( const char* topic ) { return 1; }
+   int32_t SendData( const char* topic, int32_t length_in_bytes, const uint8_t* data ) { return 1; }
+   int32_t ReceiveData( int32_t* msg_type, char** topic, int32_t* length_in_bytes, uint8_t** data ) { return 1; }
+   int32_t IsSubscriberPresent( const char* topic, int32_t* status ) { return 1; }
+   int32_t ReceiveBulkData( uint8_t** bulk_data, int32_t* length_in_bytes, int32_t* topic_count ) { return 1; }
+   int32_t UnblockReceiveData() { return 1; }
+};
+#endif // _px4muorb_KraitWrapper_hpp_

src/modules/muorb/krait/CMakeLists.txt

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+if("${DSPAL_STUBS_ENABLE}" STREQUAL "1")
+	include_directories(../krait-stubs)
+endif()
+
+px4_add_module(
+	MODULE modules__muorb__krait
+	MAIN muorb
+	SRCS
+		uORBKraitFastRpcChannel.cpp
+		muorb_main.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/muorb/krait/muorb_main.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 #include <string.h>
-#include "uORBManager.hpp"
+#include "modules/uORB/uORBManager.hpp"
 #include "uORBKraitFastRpcChannel.hpp"
 
 extern "C" { __EXPORT int muorb_main(int argc, char *argv[]); }

src/modules/muorb/krait/uORBKraitFastRpcChannel.hpp

@@ -38,7 +38,7 @@
 #include <string>
 #include <pthread.h>
 #include "uORB/uORBCommunicator.hpp"
-#include <px4muorb_KraitRpcWrapper.hpp>
+#include "px4muorb_KraitRpcWrapper.hpp"
 #include <map>
 #include "drivers/drv_hrt.h"
 

src/modules/navigator/CMakeLists.txt

@@ -0,0 +1,63 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+px4_add_module(
+	MODULE modules__navigator
+	MAIN navigator
+	STACK 1200
+	COMPILE_FLAGS
+		-Wno-sign-compare
+		-Os
+	SRCS
+		navigator_main.cpp
+		navigator_params.c
+		navigator_mode.cpp
+		mission_block.cpp
+		mission.cpp
+		mission_params.c
+		loiter.cpp
+		rtl.cpp
+		rtl_params.c
+		mission_feasibility_checker.cpp
+		geofence.cpp
+		geofence_params.c
+		datalinkloss.cpp
+		datalinkloss_params.c
+		rcloss.cpp
+		rcloss_params.c
+		enginefailure.cpp
+		gpsfailure.cpp
+		gpsfailure_params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/navigator/mission_feasibility_checker.cpp

@@ -51,12 +51,6 @@
 #include <errno.h>
 #include <uORB/topics/fence.h>
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 MissionFeasibilityChecker::MissionFeasibilityChecker() :
 	_mavlink_fd(-1),
 	_capabilities_sub(-1),

src/modules/navigator/module.mk

@@ -1,66 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2013-2014 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 Navigation Controller
-#
-
-MODULE_COMMAND	= navigator
-
-SRCS		= navigator_main.cpp \
-		  navigator_params.c \
-		  navigator_mode.cpp \
-		  mission_block.cpp \
-		  mission.cpp \
-		  mission_params.c \
-		  loiter.cpp \
-		  rtl.cpp \
-		  rtl_params.c \
-		  mission_feasibility_checker.cpp \
-		  geofence.cpp \
-		  geofence_params.c \
-		  datalinkloss.cpp \
-		  datalinkloss_params.c \
-		  rcloss.cpp \
-		  rcloss_params.c \
-		  enginefailure.cpp \
-		  gpsfailure.cpp \
-		  gpsfailure_params.c
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
-
-MODULE_STACKSIZE = 1200
-
-MAXOPTIMIZATION = -Os
-
-EXTRACXXFLAGS	= -Wno-sign-compare

src/modules/navigator/navigator_main.cpp

@@ -332,8 +332,8 @@ Navigator::task_main()
 
 	while (!_task_should_exit) {
 
-		/* wait for up to 100ms for data */
-		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+		/* wait for up to 200ms for data */
+		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
 
 		if (pret == 0) {
 			/* timed out - periodic check for _task_should_exit, etc. */

src/modules/param/CMakeLists.txt

@@ -0,0 +1,49 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+
+px4_generate_parameters_source(OUT param_files
+	XML ${CMAKE_BINARY_DIR}/parameters.xml
+	DEPS xml_gen
+	)
+
+px4_add_module(
+	MODULE modules__param
+	COMPILE_FLAGS
+		-Os
+	SRCS ${param_files}
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/position_estimator_inav/CMakeLists.txt

@@ -0,0 +1,48 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+if(${OS} STREQUAL "nuttx")
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=3800)
+endif()
+px4_add_module(
+	MODULE modules__position_estimator_inav
+	MAIN position_estimator_inav
+	STACK 1200
+	COMPILE_FLAGS ${MODULE_CFLAGS}
+	SRCS
+		position_estimator_inav_main.c
+		position_estimator_inav_params.c
+		inertial_filter.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -39,6 +39,7 @@
  * @author Nuno Marques <n.marques21@hotmail.com>
  */
 
+#include <px4_posix.h>
 #include <unistd.h>
 #include <stdlib.h>
 #include <stdio.h>

src/modules/px4iofirmware/CMakeLists.txt

@@ -0,0 +1,140 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+
+# kill all flags above us, this is a different board (io)
+set_directory_properties(PROPERTIES
+	INCLUDE_DIRECTORIES ""
+	LINK_DIRECTORIES ""
+	COMPILE_DEFINITIONS ""
+	)
+set(c_flags)
+set(exe_linker_flags)
+set(cxx_flags)
+set(include_dirs)
+set(link_dirs)
+set(definitions)
+
+px4_os_prebuild_targets(OUT io_prebuild_targets
+	BOARD ${config_io_board}
+	THREADS ${THREADS})
+
+px4_os_add_flags(
+	BOARD ${config_io_board}
+	C_FLAGS c_flags
+	CXX_FLAGS cxx_flags
+	EXE_LINKER_FLAGS exe_linker_flags
+	INCLUDE_DIRS include_dirs
+	LINK_DIRS link_dirs
+	DEFINITIONS definitions)
+
+px4_join(OUT CMAKE_EXE_LINKER_FLAGS LIST "${exe_linker_flags}" GLUE " ")
+px4_join(OUT CMAKE_C_FLAGS LIST "${c_flags}" GLUE " ")
+px4_join(OUT CMAKE_CXX_FLAGS LIST "${cxx_flags}" GLUE " ")
+
+include_directories(
+	${include_dirs}
+	${CMAKE_BINARY_DIR}/src/modules/systemlib/mixer
+	)
+link_directories(${link_dirs})
+add_definitions(${definitions})
+
+set(srcs
+	adc.c
+	controls.c
+	dsm.c
+	px4io.c
+	registers.c
+	safety.c
+	sbus.c
+	../systemlib/up_cxxinitialize.c
+	../systemlib/perf_counter.c
+	mixer.cpp
+	../systemlib/mixer/mixer.cpp
+	../systemlib/mixer/mixer_group.cpp
+	../systemlib/mixer/mixer_multirotor.cpp
+	../systemlib/mixer/mixer_simple.cpp
+	../systemlib/pwm_limit/pwm_limit.c
+	../../lib/rc/st24.c
+	../../lib/rc/sumd.c
+	../../drivers/stm32/drv_hrt.c
+	../../drivers/stm32/drv_pwm_servo.c
+	../../drivers/boards/${config_io_board}/px4io_init.c
+	../../drivers/boards/${config_io_board}/px4io_pwm_servo.c
+	)
+
+if(${config_io_board} STREQUAL "px4io-v1")
+	list(APPEND srcs
+		i2c.c
+		)
+elseif(${config_io_board} STREQUAL "px4io-v2")
+	list(APPEND srcs
+		serial.c
+		../systemlib/hx_stream.c
+		)
+endif()
+
+set(fw_io_name ${config_io_board}_${LABEL})
+
+add_executable(${fw_io_name}
+	${srcs})
+
+add_dependencies(${fw_io_name}
+	nuttx_export_${config_io_board}
+	msg_gen
+	io_prebuild_targets
+	mixer_gen
+	)
+
+set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${config_io_board}/NuttX/nuttx-export)
+set(main_link_flags
+	"-T${nuttx_export_dir}/build/ld.script"
+	"-Wl,-Map=${CMAKE_BINARY_DIR}/${config_io_board}/main.map"
+	)
+px4_join(OUT main_link_flags LIST ${main_link_flags} GLUE " ")
+set_target_properties(${fw_io_name} PROPERTIES LINK_FLAGS ${main_link_flags})
+
+target_link_libraries(${fw_io_name}
+	-Wl,--start-group
+	apps nuttx nosys m gcc
+	${config_io_extra_libs}
+	-Wl,--end-group)
+
+px4_nuttx_create_bin(OUT ${CMAKE_CURRENT_BINARY_DIR}/${fw_io_name}.bin
+	EXE ${fw_io_name}
+	)
+
+add_custom_target(fw_io
+	DEPENDS ${CMAKE_CURRENT_BINARY_DIR}/${fw_io_name}.bin)
+
+
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/px4iofirmware/module.mk

@@ -1,30 +0,0 @@
-
-SRCS		= adc.c \
-		  controls.c \
-		  dsm.c \
-		  px4io.c \
-		  registers.c \
-		  safety.c \
-		  sbus.c \
-		  ../systemlib/up_cxxinitialize.c \
-		  ../systemlib/perf_counter.c \
-		  mixer.cpp \
-		  ../systemlib/mixer/mixer.cpp \
-		  ../systemlib/mixer/mixer_group.cpp \
-		  ../systemlib/mixer/mixer_multirotor.cpp \
-		  ../systemlib/mixer/mixer_simple.cpp \
-		  ../systemlib/pwm_limit/pwm_limit.c \
-		  ../../lib/rc/st24.c \
-		  ../../lib/rc/sumd.c
-
-ifeq ($(BOARD),px4io-v1)
-SRCS		+= i2c.c
-endif
-ifeq ($(BOARD),px4io-v2)
-SRCS		+= serial.c \
-		   ../systemlib/hx_stream.c
-endif
-
-SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST)))
-include $(SELF_DIR)../systemlib/mixer/multi_tables.mk
-	

src/modules/px4iofirmware/protocol.h

@@ -33,6 +33,7 @@
 
 #pragma once
 
+#define __STDC_FORMAT_MACROS
 #include <inttypes.h>
 
 /**

src/modules/sdlog2/CMakeLists.txt

@@ -0,0 +1,51 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+if (${OS} STREQUAL "nuttx")
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=1500)
+endif()
+px4_add_module(
+	MODULE modules__sdlog2
+	MAIN sdlog2
+	PRIORITY "SCHED_PRIORITY_MAX-30"
+	STACK 1200
+	COMPILE_FLAGS
+		${MODULE_CFLAGS}
+		-Os
+	SRCS
+		sdlog2.c
+		logbuffer.c
+		params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/sdlog2/params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2015 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
@@ -31,51 +31,47 @@
  *
  ****************************************************************************/
 
-#include <uavcan_stm32/uavcan_stm32.hpp>
-#include <drivers/drv_hrt.h>
+#include <systemlib/param/param.h>
 
 /**
- * @file uavcan_clock.cpp
+ * Logging rate.
  *
- * Implements a clock for the CAN node.
+ * A value of -1 indicates the commandline argument
+ * should be obeyed. A value of 0 sets the minimum rate,
+ * any other value is interpreted as rate in Hertz. This
+ * parameter is only read out before logging starts (which
+ * commonly is before arming).
  *
- * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @min -1
+ * @max  1
+ * @group SD Logging
  */
+PARAM_DEFINE_INT32(SDLOG_RATE, -1);
 
-namespace uavcan_stm32
-{
-namespace clock
-{
-
-uavcan::MonotonicTime getMonotonic()
-{
-	return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
-}
-
-uavcan::UtcTime getUtc()
-{
-	return uavcan::UtcTime();
-}
-
-void adjustUtc(uavcan::UtcDuration adjustment)
-{
-	(void)adjustment;
-}
-
-uavcan::uint64_t getUtcUSecFromCanInterrupt();
-
-uavcan::uint64_t getUtcUSecFromCanInterrupt()
-{
-	return 0;
-}
-
-} // namespace clock
-
-SystemClock &SystemClock::instance()
-{
-	static SystemClock inst;
-	return inst;
-}
-
-}
+/**
+ * Enable extended logging mode.
+ *
+ * A value of -1 indicates the commandline argument
+ * should be obeyed. A value of 0 disables extended
+ * logging mode, a value of 1 enables it. This
+ * parameter is only read out before logging starts
+ * (which commonly is before arming).
+ *
+ * @min -1
+ * @max  1
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_EXT, -1);
 
+/**
+ * Use timestamps only if GPS 3D fix is available
+ *
+ * A value of 1 constrains the log folder creation
+ * to only use the time stamp if a 3D GPS lock is
+ * present.
+ *
+ * @min 0
+ * @max  1
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1);

src/modules/sdlog2/sdlog2.c

@@ -45,10 +45,17 @@
 #include <px4_config.h>
 #include <px4_defines.h>
 #include <px4_getopt.h>
+#include <px4_tasks.h>
+#include <px4_time.h>
+#include <px4_posix.h>
 #include <sys/types.h>
 #include <sys/stat.h>
-#include <sys/prctl.h>
+#ifdef __PX4_DARWIN
+#include <sys/param.h>
+#include <sys/mount.h>
+#else
 #include <sys/statfs.h>
+#endif
 #include <fcntl.h>
 #include <errno.h>
 #include <unistd.h>
@@ -118,56 +125,13 @@
 
 #define PX4_EPOCH_SECS 1234567890L
 
-/**
- * Logging rate.
- *
- * A value of -1 indicates the commandline argument
- * should be obeyed. A value of 0 sets the minimum rate,
- * any other value is interpreted as rate in Hertz. This
- * parameter is only read out before logging starts (which
- * commonly is before arming).
- *
- * @min -1
- * @max  1
- * @group SD Logging
- */
-PARAM_DEFINE_INT32(SDLOG_RATE, -1);
-
-/**
- * Enable extended logging mode.
- *
- * A value of -1 indicates the commandline argument
- * should be obeyed. A value of 0 disables extended
- * logging mode, a value of 1 enables it. This
- * parameter is only read out before logging starts
- * (which commonly is before arming).
- *
- * @min -1
- * @max  1
- * @group SD Logging
- */
-PARAM_DEFINE_INT32(SDLOG_EXT, -1);
-
-/**
- * Use timestamps only if GPS 3D fix is available
- *
- * A value of 1 constrains the log folder creation
- * to only use the time stamp if a 3D GPS lock is
- * present.
- *
- * @min 0
- * @max  1
- * @group SD Logging
- */
-PARAM_DEFINE_INT32(SDLOG_GPSTIME, 1);
-
 #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
 		log_msgs_written++; \
 	} else { \
 		log_msgs_skipped++; \
 	}
 
-#define MIN(X,Y) ((X) < (Y) ? (X) : (Y))
+#define SDLOG_MIN(X,Y) ((X) < (Y) ? (X) : (Y))
 
 static bool main_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;			/**< Deamon status flag */
@@ -276,8 +240,6 @@ static int write_parameters(int fd);
 
 static bool file_exist(const char *filename);
 
-static int file_copy(const char *file_old, const char *file_new);
-
 /**
  * Check if there is still free space available
  */
@@ -411,7 +373,7 @@ int sdlog2_main(int argc, char *argv[])
 
 bool get_log_time_utc_tt(struct tm *tt, bool boot_time) {
 	struct timespec ts;
-	clock_gettime(CLOCK_REALTIME, &ts);
+	px4_clock_gettime(CLOCK_REALTIME, &ts);
 	/* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */
 	time_t utc_time_sec;
 
@@ -1077,17 +1039,6 @@ int sdlog2_thread_main(int argc, char *argv[])
 		return 1;
 	}
 
-	/* copy conversion scripts */
-	const char *converter_in = "/etc/logging/conv.zip";
-	char *converter_out = malloc(64);
-	snprintf(converter_out, 64, "%s/conv.zip", log_root);
-
-	if (file_copy(converter_in, converter_out) != OK) {
-		warn("unable to copy conversion scripts");
-	}
-
-	free(converter_out);
-
 	/* initialize log buffer with specified size */
 	warnx("log buffer size: %i bytes", log_buffer_size);
 
@@ -1400,7 +1351,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (copy_if_updated(ORB_ID(satellite_info), &subs.sat_info_sub, &buf.sat_info)) {
 
 				/* log the SNR of each satellite for a detailed view of signal quality */
-				unsigned sat_info_count = MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
+				unsigned sat_info_count = SDLOG_MIN(buf.sat_info.count, sizeof(buf.sat_info.snr) / sizeof(buf.sat_info.snr[0]));
 				unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
 
 				log_msg.msg_type = LOG_GS0A_MSG;
@@ -1878,11 +1829,11 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
 			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
 			log_msg.body.log_TECS.totalEnergyError = buf.tecs_status.totalEnergyError;
-			log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError;
 			log_msg.body.log_TECS.totalEnergyRateError = buf.tecs_status.totalEnergyRateError;
+			log_msg.body.log_TECS.energyDistributionError = buf.tecs_status.energyDistributionError;
 			log_msg.body.log_TECS.energyDistributionRateError = buf.tecs_status.energyDistributionRateError;
-			log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ;
 			log_msg.body.log_TECS.pitch_integ = buf.tecs_status.pitch_integ;
+			log_msg.body.log_TECS.throttle_integ = buf.tecs_status.throttle_integ;
 			log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode;
 			LOGBUFFER_WRITE_AND_COUNT(TECS);
 		}
@@ -1973,46 +1924,6 @@ bool file_exist(const char *filename)
 	return stat(filename, &buffer) == 0;
 }
 
-int file_copy(const char *file_old, const char *file_new)
-{
-	FILE *source, *target;
-	source = fopen(file_old, "r");
-	int ret = 0;
-
-	if (source == NULL) {
-		warnx("ERR: open in");
-		return PX4_ERROR;
-	}
-
-	target = fopen(file_new, "w");
-
-	if (target == NULL) {
-		fclose(source);
-		warnx("ERR: open out");
-		return PX4_ERROR;
-	}
-
-	char buf[128];
-	int nread;
-
-	while ((nread = fread(buf, 1, sizeof(buf), source)) > 0) {
-		ret = fwrite(buf, 1, nread, target);
-
-		if (ret <= 0) {
-			warnx("error writing file");
-			ret = PX4_ERROR;
-			break;
-		}
-	}
-
-	fsync(fileno(target));
-
-	fclose(source);
-	fclose(target);
-
-	return PX4_OK;
-}
-
 int check_free_space()
 {
 	/* use statfs to determine the number of blocks left */

src/modules/sdlog2/sdlog2_messages.h

@@ -558,7 +558,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "ffffffffffffffB",	"ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,EDE,ERE,EDRE,PtchI,ThrI,M"),
+	LOG_FORMAT(TECS, "ffffffffffffffB",	"ASP,AF,FSP,F,AsSP,AsF,AsDSP,AsD,EE,ERE,EDE,EDRE,PtchI,ThrI,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 	LOG_FORMAT(ENCD, "qfqf",	"cnt0,vel0,cnt1,vel1"),
 	LOG_FORMAT(TSYN, "Q", 		"TimeOffset"),

src/modules/segway/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015 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
@@ -30,11 +30,14 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-
-#
-# Makefile to build the unit test library.
-#
-
-SRCS		 = unit_test.cpp
-
-MAXOPTIMIZATION	 = -Os
+px4_add_module(
+	MODULE modules__segway
+	MAIN segway
+	SRCS
+		segway_main.cpp
+		BlockSegwayController.cpp
+		params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/segway/module.mk

@@ -1,42 +0,0 @@
-############################################################################
-#
-#   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.
-#
-############################################################################
-
-#
-# segway controller
-#
-
-MODULE_COMMAND	= segway
-
-SRCS		= segway_main.cpp \
-		  BlockSegwayController.cpp \
-		  params.c

src/modules/sensors/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015 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
@@ -30,16 +30,18 @@
 # POSSIBILITY OF SUCH DAMAGE.
 #
 ############################################################################
-
-#
-# Build multicopter position controller
-#
-
-MODULE_COMMAND	= mc_pos_control
-
-SRCS		= mc_pos_control_main.cpp \
-			  mc_pos_control_params.c
-
-# Startup handler, the actual app stack size is
-# in the task_spawn command
-MODULE_STACKSIZE = 1200
+px4_add_module(
+	MODULE modules__sensors
+	MAIN sensors
+	PRIORITY "SCHED_PRIORITY_MAX-5"
+	STACK 1200
+	COMPILE_FLAGS
+		-Wno-type-limits
+		-O3
+	SRCS
+		sensors.cpp
+		sensor_params.c
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/sensors/module.mk

@@ -1,48 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2012-2014 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.
-#
-############################################################################
-
-#
-# Makefile to build the sensor data collector
-#
-
-MODULE_COMMAND	= sensors
-MODULE_PRIORITY	= "SCHED_PRIORITY_MAX-5"
-
-SRCS =		  sensors.cpp \
-		  sensor_params.c
-
-MODULE_STACKSIZE = 1200
-
-MAXOPTIMIZATION	 = -Os
-
-EXTRACXXFLAGS	= -Wno-type-limits

src/modules/sensors/sensor_params.c

@@ -1807,36 +1807,12 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, -1);
  */
 PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
 
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
 /**
  * Scaling factor for battery voltage sensor on FMU v2.
  *
- * @board CONFIG_ARCH_BOARD_PX4FMU_V2
  * @group Battery Calibration
  */
-PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
-#elif CONFIG_ARCH_BOARD_AEROCORE
-/**
- * Scaling factor for battery voltage sensor on AeroCore.
- *
- * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063
- *
- * @board CONFIG_ARCH_BOARD_AEROCORE
- * @group Battery Calibration
- */
-PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f);
-#else
-/**
- * Scaling factor for battery voltage sensor on FMU v1.
- *
- * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659
- * FMUv1 with PX4IO: 0.00459340659
- * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238
- *
- * @group Battery Calibration
- */
-PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f);
-#endif
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f);
 
 /**
  * Scaling factor for battery current sensor.

src/modules/sensors/sensors.cpp

@@ -212,7 +212,7 @@ private:
 	unsigned	_accel_count;			/**< raw accel data count */
 	unsigned	_mag_count;			/**< raw mag data count */
 	unsigned	_baro_count;			/**< raw baro data count */
-	
+
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vcontrol_mode_sub;		/**< vehicle control mode subscription */
@@ -366,7 +366,7 @@ private:
 
 
 	int		init_sensor_class(const struct orb_metadata *meta, int *subs,
-				unsigned *priorities, unsigned *errcount);
+				uint32_t *priorities, uint32_t *errcount);
 
 	/**
 	 * Update our local parameter cache.
@@ -825,6 +825,19 @@ Sensors::parameters_update()
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
 		warnx("%s", paramerr);
+	} else if (_parameters.battery_voltage_scaling < 0.0f) {
+		/* apply scaling according to defaults if set to default */
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+		_parameters.battery_voltage_scaling = 0.0082f;
+		#elif CONFIG_ARCH_BOARD_AEROCORE
+		_parameters.battery_voltage_scaling = 0.0063f;
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+		_parameters.battery_voltage_scaling = 0.00459340659f;
+		#else
+		/* ensure a missing default trips a low voltage lockdown */
+		_parameters.battery_voltage_scaling = 0.00001f;
+		#endif
 	}
 
 	/* scaling of ADC ticks to battery current */
@@ -1949,7 +1962,7 @@ Sensors::task_main_trampoline(int argc, char *argv[])
 
 int
 Sensors::init_sensor_class(const struct orb_metadata *meta, int *subs,
-	unsigned *priorities, unsigned *errcount)
+	uint32_t *priorities, uint32_t *errcount)
 {
 	unsigned group_count = orb_group_count(meta);
 
@@ -1988,7 +2001,7 @@ Sensors::task_main()
 	} while (0);
 
 	if (ret) {
-		warnx("Sensor initialization failed");
+		warnx("sensor initialization failed");
 		_sensors_task = -1;
 		if (_fd_adc >=0) {
 			px4_close(_fd_adc);
@@ -2068,6 +2081,8 @@ Sensors::task_main()
 
 	raw.timestamp = 0;
 
+	uint64_t _last_config_update = hrt_absolute_time();
+
 	while (!_task_should_exit) {
 
 		/* wait for up to 50ms for data */
@@ -2077,7 +2092,7 @@ Sensors::task_main()
 
 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
-			warn("poll error %d, %d", pret, errno);
+			warnx("poll error %d, %d", pret, errno);
 			continue;
 		}
 
@@ -2086,21 +2101,6 @@ Sensors::task_main()
 		/* check vehicle status for changes to publication state */
 		vehicle_control_mode_poll();
 
-		/* keep adding sensors as long as we are not armed */
-		if (!_armed) {
-			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
-				&raw.gyro_priority[0], &raw.gyro_errcount[0]);
-
-			_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
-				&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
-
-			_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
-				&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
-
-			_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
-				&raw.baro_priority[0], &raw.baro_errcount[0]);
-		}
-
 		/* the timestamp of the raw struct is updated by the gyro_poll() method */
 		/* copy most recent sensor data */
 		gyro_poll(raw);
@@ -2129,11 +2129,32 @@ Sensors::task_main()
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
 		}
 
-		/* check parameters for updates */
-		parameter_update_poll();
+		/* keep adding sensors as long as we are not armed,
+		 * when not adding sensors poll for param updates
+		 */
+		if (!_armed && hrt_elapsed_time(&_last_config_update) > 500 * 1000) {
+			_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
+				&raw.gyro_priority[0], &raw.gyro_errcount[0]);
 
-		/* check rc parameter map for updates */
-		rc_parameter_map_poll();
+			_mag_count = init_sensor_class(ORB_ID(sensor_mag), &_mag_sub[0],
+				&raw.magnetometer_priority[0], &raw.magnetometer_errcount[0]);
+
+			_accel_count = init_sensor_class(ORB_ID(sensor_accel), &_accel_sub[0],
+				&raw.accelerometer_priority[0], &raw.accelerometer_errcount[0]);
+
+			_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
+				&raw.baro_priority[0], &raw.baro_errcount[0]);
+
+			_last_config_update = hrt_absolute_time();
+
+		} else {
+
+			/* check parameters for updates */
+			parameter_update_poll();
+
+			/* check rc parameter map for updates */
+			rc_parameter_map_poll();
+		}
 
 		/* Look for new r/c input data */
 		rc_poll();
@@ -2152,7 +2173,7 @@ Sensors::start()
 	ASSERT(_sensors_task == -1);
 
 	/* start the task */
-	_sensors_task = px4_task_spawn_cmd("sensors_task",
+	_sensors_task = px4_task_spawn_cmd("sensors",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
 				       2000,

src/modules/simulator/CMakeLists.txt

@@ -0,0 +1,52 @@
+############################################################################
+#
+#   Copyright (c) 2015 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.
+#
+############################################################################
+set(SIMULATOR_SRCS simulator.cpp)
+if (NOT ${OS} STREQUAL "qurt")
+	list(APPEND SIMULATOR_SRCS
+		simulator_mavlink.cpp)
+endif()
+
+px4_add_module(
+	MODULE modules__simulator
+	MAIN simulator
+	COMPILE_FLAGS
+		-Wno-attributes
+		-Wno-packed
+		-Wno-packed
+	SRCS
+		${SIMULATOR_SRCS}
+	DEPENDS
+		platforms__common
+	)
+
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/simulator/module.mk

@@ -1,50 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015 MArk Charlebois. 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.
-#
-############################################################################
-
-#
-# Makefile to build simulator 
-#
-
-MODULE_COMMAND		= simulator
-
-SRCS			= simulator.cpp
-
-ifneq ($(PX4_TARGET_OS), qurt)
-SRCS			+= simulator_mavlink.cpp
-endif
-
-INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
-
-EXTRACXXFLAGS	= -Weffc++ -Wno-attributes -Wno-packed
-
-EXTRACFLAGS	= -Wno-packed

src/modules/simulator/simulator.cpp

@@ -45,7 +45,7 @@
 #include <errno.h>
 #include <stdio.h>
 #include <string.h>
-#include <sys/types.h> 
+#include <sys/types.h>
 #include <drivers/drv_led.h>
 
 #include "simulator.h"
@@ -91,27 +91,33 @@ bool Simulator::getAirspeedSample(uint8_t *buf, int len)
 	return _airspeed.copyData(buf, len);
 }
 
-void Simulator::write_MPU_data(void *buf) {
+void Simulator::write_MPU_data(void *buf)
+{
 	_mpu.writeData(buf);
 }
 
-void Simulator::write_accel_data(void *buf) {
+void Simulator::write_accel_data(void *buf)
+{
 	_accel.writeData(buf);
 }
 
-void Simulator::write_mag_data(void *buf) {
+void Simulator::write_mag_data(void *buf)
+{
 	_mag.writeData(buf);
 }
 
-void Simulator::write_baro_data(void *buf) {
+void Simulator::write_baro_data(void *buf)
+{
 	_baro.writeData(buf);
 }
 
-void Simulator::write_gps_data(void *buf) {
+void Simulator::write_gps_data(void *buf)
+{
 	_gps.writeData(buf);
 }
 
-void Simulator::write_airspeed_data(void *buf) {
+void Simulator::write_airspeed_data(void *buf)
+{
 	_airspeed.writeData(buf);
 }
 
@@ -119,21 +125,23 @@ int Simulator::start(int argc, char *argv[])
 {
 	int ret = 0;
 	_instance = new Simulator();
+
 	if (_instance) {
-		PX4_INFO("Simulator started");
 		drv_led_start();
+
 		if (argv[2][1] == 's') {
 			_instance->initializeSensorData();
 #ifndef __PX4_QURT
 			// Update sensor data
 			_instance->pollForMAVLinkMessages(false);
 #endif
+
 		} else {
 			// Update sensor data
 			_instance->pollForMAVLinkMessages(true);
 		}
-	}
-	else {
+
+	} else {
 		PX4_WARN("Simulator creation failed");
 		ret = 1;
 	}
@@ -154,52 +162,54 @@ __END_DECLS
 
 extern "C" {
 
-int simulator_main(int argc, char *argv[])
-{
-	int ret = 0;
-	if (argc == 3 && strcmp(argv[1], "start") == 0) {
-		if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) {
-			if (g_sim_task >= 0) {
-				warnx("Simulator already started");
-				return 0;
-			}
-			g_sim_task = px4_task_spawn_cmd("Simulator",
-				SCHED_DEFAULT,
-				SCHED_PRIORITY_MAX - 5,
-				1500,
-				Simulator::start,
-				argv);
+	int simulator_main(int argc, char *argv[])
+	{
+		int ret = 0;
 
-			// now wait for the command to complete
-			while(true) {
-				if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) {
-					break;
-				} else {
-					usleep(100000);
+		if (argc == 3 && strcmp(argv[1], "start") == 0) {
+			if (strcmp(argv[2], "-s") == 0 || strcmp(argv[2], "-p") == 0) {
+				if (g_sim_task >= 0) {
+					warnx("Simulator already started");
+					return 0;
 				}
+
+				g_sim_task = px4_task_spawn_cmd("Simulator",
+								SCHED_DEFAULT,
+								SCHED_PRIORITY_MAX - 5,
+								1500,
+								Simulator::start,
+								argv);
+
+				// now wait for the command to complete
+				while (true) {
+					if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) {
+						break;
+
+					} else {
+						usleep(100000);
+					}
+				}
+
+			} else {
+				usage();
+				ret = -EINVAL;
 			}
-		}
-		else
-		{
+
+		} else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
+			if (g_sim_task < 0) {
+				PX4_WARN("Simulator not running");
+
+			} else {
+				px4_task_delete(g_sim_task);
+				g_sim_task = -1;
+			}
+
+		} else {
 			usage();
 			ret = -EINVAL;
 		}
-	}
-	else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
-		if (g_sim_task < 0) {
-			PX4_WARN("Simulator not running");
-		}
-		else {
-			px4_task_delete(g_sim_task);
-			g_sim_task = -1;
-		}
-	}
-	else {
-		usage();
-		ret = -EINVAL;
+
+		return ret;
 	}
 
-	return ret;
-}
-
 }

src/modules/simulator/simulator.h

@@ -38,7 +38,7 @@
 
 #pragma once
 
-#include <semaphore.h>
+#include <px4_posix.h>
 #include <uORB/topics/hil_sensor.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -51,26 +51,28 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/optical_flow.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
-namespace simulator {
+namespace simulator
+{
 
 // FIXME - what is the endianness of these on actual device?
 #pragma pack(push, 1)
 struct RawAccelData {
-		float temperature;
-        float x;
-        float y;
-        float z;
+	float temperature;
+	float x;
+	float y;
+	float z;
 };
 #pragma pack(pop)
 
 #pragma pack(push, 1)
 struct RawMagData {
-		float temperature;
-        float x;
-        float y;
-        float z;
+	float temperature;
+	float x;
+	float y;
+	float z;
 };
 #pragma pack(pop)
 
@@ -103,29 +105,30 @@ struct RawAirspeedData {
 
 #pragma pack(push, 1)
 struct RawGPSData {
- int32_t lat;
- int32_t lon;
- int32_t alt;
- uint16_t eph;
- uint16_t epv;
- uint16_t vel;
- int16_t vn;
- int16_t ve;
- int16_t vd;
- uint16_t cog;
- uint8_t fix_type;
- uint8_t satellites_visible;
+	int32_t lat;
+	int32_t lon;
+	int32_t alt;
+	uint16_t eph;
+	uint16_t epv;
+	uint16_t vel;
+	int16_t vn;
+	int16_t ve;
+	int16_t vd;
+	uint16_t cog;
+	uint8_t fix_type;
+	uint8_t satellites_visible;
 };
 #pragma pack(pop)
 
-template <typename RType> class Report {
+template <typename RType> class Report
+{
 public:
 	Report(int readers) :
 		_readidx(0),
 		_max_readers(readers),
 		_report_len(sizeof(RType))
 	{
-		sem_init(&_lock, 0, _max_readers);
+		px4_sem_init(&_lock, 0, _max_readers);
 	}
 
 	~Report() {};
@@ -135,6 +138,7 @@ public:
 		if (len != _report_len) {
 			return false;
 		}
+
 		read_lock();
 		memcpy(outbuf, &_buf[_readidx], _report_len);
 		read_unlock();
@@ -149,23 +153,23 @@ public:
 	}
 
 protected:
-	void read_lock() { sem_wait(&_lock); }
-	void read_unlock() { sem_post(&_lock); }
+	void read_lock() { px4_sem_wait(&_lock); }
+	void read_unlock() { px4_sem_post(&_lock); }
 	void write_lock()
 	{
-		for (int i=0; i<_max_readers; i++) {
-			sem_wait(&_lock);
+		for (int i = 0; i < _max_readers; i++) {
+			px4_sem_wait(&_lock);
 		}
 	}
 	void write_unlock()
 	{
-		for (int i=0; i<_max_readers; i++) {
-			sem_post(&_lock);
+		for (int i = 0; i < _max_readers; i++) {
+			px4_sem_post(&_lock);
 		}
 	}
 
 	int _readidx;
-	sem_t _lock;
+	px4_sem_t _lock;
 	const int _max_readers;
 	const int _report_len;
 	RType _buf[2];
@@ -173,7 +177,8 @@ protected:
 
 };
 
-class Simulator {
+class Simulator
+{
 public:
 	static Simulator *getInstance();
 
@@ -211,32 +216,32 @@ public:
 
 private:
 	Simulator() :
-	_accel(1),
-	_mpu(1),
-	_baro(1),
-	_mag(1),
-	_gps(1),
-	_airspeed(1),
-	_accel_pub(nullptr),
-	_baro_pub(nullptr),
-	_gyro_pub(nullptr),
-	_mag_pub(nullptr),
-	_initialized(false)
+		_accel(1),
+		_mpu(1),
+		_baro(1),
+		_mag(1),
+		_gps(1),
+		_airspeed(1),
+		_accel_pub(nullptr),
+		_baro_pub(nullptr),
+		_gyro_pub(nullptr),
+		_mag_pub(nullptr),
+		_initialized(false)
 #ifndef __PX4_QURT
-	,
-	_rc_channels_pub(nullptr),
-	_actuator_outputs_sub(-1),
-	_vehicle_attitude_sub(-1),
-	_manual_sub(-1),
-	_vehicle_status_sub(-1),
-	_rc_input{},
-	_actuators{},
-	_attitude{},
-	_manual{},
-	_vehicle_status{}
+		,
+		_rc_channels_pub(nullptr),
+		_actuator_outputs_sub(-1),
+		_vehicle_attitude_sub(-1),
+		_manual_sub(-1),
+		_vehicle_status_sub(-1),
+		_rc_input{},
+		_actuators{},
+		_attitude{},
+		_manual{},
+		_vehicle_status{}
 #endif
 	{}
-	~Simulator() { _instance=NULL; }
+	~Simulator() { _instance = NULL; }
 
 	void initializeSensorData();
 
@@ -255,11 +260,13 @@ private:
 	orb_advert_t _baro_pub;
 	orb_advert_t _gyro_pub;
 	orb_advert_t _mag_pub;
+	orb_advert_t _flow_pub;
 
 	bool _initialized;
 
 	// class methods
 	int publish_sensor_topics(mavlink_hil_sensor_t *imu);
+	int publish_flow_topic(mavlink_hil_optical_flow_t *flow);
 
 #ifndef __PX4_QURT
 	// uORB publisher handlers

src/modules/simulator/simulator_mavlink.cpp

@@ -46,6 +46,14 @@ extern "C" __EXPORT hrt_abstime hrt_reset(void);
 #define UDP_PORT 	14560
 #define PIXHAWK_DEVICE "/dev/ttyACM0"
 
+#ifndef B460800
+#define B460800 460800
+#endif
+
+#ifndef B921600
+#define B921600 921600
+#endif
+
 #define PRESS_GROUND 101325.0f
 #define DENSITY 1.2041f
 #define GRAVITY 9.81f
@@ -63,7 +71,8 @@ static socklen_t _addrlen = sizeof(_srcaddr);
 
 using namespace simulator;
 
-void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
+void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg)
+{
 	float out[8] = {};
 
 	const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
@@ -88,6 +97,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
 				out[i] = 0.0f;
 			}
 		}
+
 	} else {
 		// convert back to range [-1, 1]
 		for (unsigned i = 0; i < 8; i++) {
@@ -114,29 +124,30 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
 	actuator_msg.nav_mode = 0;
 }
 
-void Simulator::send_controls() {
+void Simulator::send_controls()
+{
 	mavlink_hil_controls_t msg;
 	pack_actuator_message(msg);
-	//PX4_WARN("Sending HIL_CONTROLS msg");
 	send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
 }
 
-static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels) {
+static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t *rc_channels)
+{
 	rc->timestamp_publication = hrt_absolute_time();
 	rc->timestamp_last_signal = hrt_absolute_time();
 	rc->channel_count = rc_channels->chancount;
 	rc->rssi = rc_channels->rssi;
 
-/*	PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d",
-		 rc_channels->chan1_raw,
-		 rc_channels->chan2_raw,
-		 rc_channels->chan3_raw,
-		 rc_channels->chan4_raw,
-		 rc_channels->chan5_raw,
-		 rc_channels->chan6_raw,
-		 rc_channels->chan7_raw,
-		 rc_channels->chan8_raw);
-*/
+	/*	PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d",
+			 rc_channels->chan1_raw,
+			 rc_channels->chan2_raw,
+			 rc_channels->chan3_raw,
+			 rc_channels->chan4_raw,
+			 rc_channels->chan5_raw,
+			 rc_channels->chan6_raw,
+			 rc_channels->chan7_raw,
+			 rc_channels->chan8_raw);
+	*/
 
 	rc->values[0] = rc_channels->chan1_raw;
 	rc->values[1] = rc_channels->chan2_raw;
@@ -158,9 +169,10 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t
 	rc->values[17] = rc_channels->chan18_raw;
 }
 
-void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
+void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
+{
 	// write sensor data to memory so that drivers can copy data from there
-	RawMPUData mpu;
+	RawMPUData mpu = {};
 	mpu.accel_x = imu->xacc;
 	mpu.accel_y = imu->yacc;
 	mpu.accel_z = imu->zacc;
@@ -171,21 +183,21 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
 
 	write_MPU_data((void *)&mpu);
 
-	RawAccelData accel;
+	RawAccelData accel = {};
 	accel.x = imu->xacc;
 	accel.y = imu->yacc;
 	accel.z = imu->zacc;
 
 	write_accel_data((void *)&accel);
 
-	RawMagData mag;
+	RawMagData mag = {};
 	mag.x = imu->xmag;
 	mag.y = imu->ymag;
 	mag.z = imu->zmag;
 
 	write_mag_data((void *)&mag);
 
-	RawBaroData baro;
+	RawBaroData baro = {};
 	// calculate air pressure from altitude (valid for low altitude)
 	baro.pressure = (PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt) / 100.0f; // convert from Pa to mbar
 	baro.altitude = imu->pressure_alt;
@@ -193,18 +205,19 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
 
 	write_baro_data((void *)&baro);
 
-	RawAirspeedData airspeed;
+	RawAirspeedData airspeed = {};
 	airspeed.temperature = imu->temperature;
 	airspeed.diff_pressure = imu->diff_pressure;
 
 	write_airspeed_data((void *)&airspeed);
 }
 
-void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
+void Simulator::update_gps(mavlink_hil_gps_t *gps_sim)
+{
 	RawGPSData gps;
-	gps.lat = gps_sim->lat; 
- 	gps.lon = gps_sim->lon;
- 	gps.alt = gps_sim->alt;
+	gps.lat = gps_sim->lat;
+	gps.lon = gps_sim->lon;
+	gps.alt = gps_sim->alt;
 	gps.eph = gps_sim->eph;
 	gps.epv = gps_sim->epv;
 	gps.vel = gps_sim->vel;
@@ -218,23 +231,34 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
 	write_gps_data((void *)&gps);
 }
 
-void Simulator::handle_message(mavlink_message_t *msg, bool publish) {
-	switch(msg->msgid) {
+void Simulator::handle_message(mavlink_message_t *msg, bool publish)
+{
+	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_HIL_SENSOR:
 		mavlink_hil_sensor_t imu;
 		mavlink_msg_hil_sensor_decode(msg, &imu);
+
 		if (publish) {
 			publish_sensor_topics(&imu);
 		}
+
 		update_sensors(&imu);
 		break;
 
+	case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
+		mavlink_hil_optical_flow_t flow;
+		mavlink_msg_hil_optical_flow_decode(msg, &flow);
+		publish_flow_topic(&flow);
+		break;
+
 	case MAVLINK_MSG_ID_HIL_GPS:
 		mavlink_hil_gps_t gps_sim;
 		mavlink_msg_hil_gps_decode(msg, &gps_sim);
+
 		if (publish) {
 			//PX4_WARN("FIXME:  Need to publish GPS topic.  Not done yet.");
 		}
+
 		update_gps(&gps_sim);
 		break;
 
@@ -245,17 +269,20 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) {
 
 		// publish message
 		if (publish) {
-			if(_rc_channels_pub == nullptr) {
+			if (_rc_channels_pub == nullptr) {
 				_rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input);
+
 			} else {
 				orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input);
 			}
 		}
+
 		break;
 	}
 }
 
-void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID) {
+void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
+{
 	component_ID = 0;
 	uint8_t payload_len = mavlink_message_lengths[msgid];
 	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -272,7 +299,7 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
 	buf[5] = msgid;
 
 	/* payload */
-	memcpy(&buf[MAVLINK_NUM_HEADER_BYTES],msg, payload_len);
+	memcpy(&buf[MAVLINK_NUM_HEADER_BYTES], msg, payload_len);
 
 	/* checksum */
 	uint16_t checksum;
@@ -284,44 +311,51 @@ void Simulator::send_mavlink_message(const uint8_t msgid, const void *msg, uint8
 	buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
 
 	ssize_t len = sendto(_fd, buf, packet_len, 0, (struct sockaddr *)&_srcaddr, _addrlen);
+
 	if (len <= 0) {
 		PX4_WARN("Failed sending mavlink message");
 	}
 }
 
-void Simulator::poll_topics() {
+void Simulator::poll_topics()
+{
 	// copy new actuator data if available
 	bool updated;
 	orb_check(_actuator_outputs_sub, &updated);
+
 	if (updated) {
 		orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
 	}
 
 	orb_check(_vehicle_status_sub, &updated);
+
 	if (updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 	}
 }
 
-void *Simulator::sending_trampoline(void *) {
+void *Simulator::sending_trampoline(void *)
+{
 	_instance->send();
 	return 0;	// why do I have to put this???
 }
 
-void Simulator::send() {
+void Simulator::send()
+{
 	px4_pollfd_struct_t fds[1];
 	fds[0].fd = _actuator_outputs_sub;
 	fds[0].events = POLLIN;
 
 	int pret;
 
-	while(true) {
+	while (true) {
 		// wait for up to 100ms for data
 		pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
-		//timed out
-		if (pret == 0)
+		// timed out
+		if (pret == 0) {
 			continue;
+		}
 
 		// this is undesirable but not much we can do
 		if (pret < 0) {
@@ -341,23 +375,35 @@ void Simulator::send() {
 
 void Simulator::initializeSensorData()
 {
-	struct baro_report baro;
-	memset(&baro,0,sizeof(baro));
-	baro.pressure = 120000.0f;
+	// write sensor data to memory so that drivers can copy data from there
+	RawMPUData mpu = {};
+	mpu.accel_z = 9.81f;
 
-	// acceleration report
-	struct accel_report accel;
-	memset(&accel,0,sizeof(accel));
+	write_MPU_data((void *)&mpu);
+
+	RawAccelData accel = {};
 	accel.z = 9.81f;
-	accel.range_m_s2 = 80.0f;
 
-	// gyro report
-	struct gyro_report gyro;
-	memset(&gyro, 0 ,sizeof(gyro));
+	write_accel_data((void *)&accel);
 
-	// mag report
-	struct mag_report mag;
-	memset(&mag, 0 ,sizeof(mag));
+	RawMagData mag = {};
+	mag.x = 0.4f;
+	mag.y = 0.0f;
+	mag.z = 0.6f;
+
+	write_mag_data((void *)&mag);
+
+	RawBaroData baro = {};
+	// calculate air pressure from altitude (valid for low altitude)
+	baro.pressure = 120000.0f;
+	baro.altitude = 0.0f;
+	baro.temperature = 25.0f;
+
+	write_baro_data((void *)&baro);
+
+	RawAirspeedData airspeed {};
+
+	write_airspeed_data((void *)&airspeed);
 }
 
 void Simulator::pollForMAVLinkMessages(bool publish)
@@ -401,42 +447,51 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	int serial_fd = openUart(PIXHAWK_DEVICE, 115200);
 
 	if (serial_fd < 0) {
-		PX4_WARN("failed to open %s", PIXHAWK_DEVICE);
-	}
+		PX4_INFO("Not using %s for radio control input. Assuming joystick input via MAVLink.", PIXHAWK_DEVICE);
 
-	// tell the device to stream some messages
-	char command[] = "\nsh /etc/init.d/rc.usb\n";
-	int w = ::write(serial_fd, command, sizeof(command));
+	} else {
 
-	if (w <= 0) {
-		PX4_WARN("failed to send streaming command to %s", PIXHAWK_DEVICE);
+		// tell the device to stream some messages
+		char command[] = "\nsh /etc/init.d/rc.usb\n";
+		int w = ::write(serial_fd, command, sizeof(command));
+
+		if (w <= 0) {
+			PX4_WARN("failed to send streaming command to %s", PIXHAWK_DEVICE);
+		}
 	}
 
 	char serial_buf[1024];
 
 	struct pollfd fds[2];
+	unsigned fd_count = 1;
 	fds[0].fd = _fd;
 	fds[0].events = POLLIN;
-	fds[1].fd = serial_fd;
-	fds[1].events = POLLIN;
+
+	if (serial_fd >= 0) {
+		fds[1].fd = serial_fd;
+		fds[1].events = POLLIN;
+		fd_count++;
+	}
 
 	int len = 0;
 
 	// wait for first data from simulator and respond with first controls
 	// this is important for the UDP communication to work
 	int pret = -1;
-	PX4_WARN("Waiting for initial data on UDP.. Please connect the simulator first.");
+	PX4_INFO("Waiting for initial data on UDP. Please start the flight simulator to proceed..");
+
 	while (pret <= 0) {
-		pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100);
+		pret = ::poll(&fds[0], (sizeof(fds[0]) / sizeof(fds[0])), 100);
 	}
-	PX4_WARN("Found initial message, pret = %d",pret);
+
+	PX4_INFO("Found initial message, pret = %d", pret);
 	_initialized = true;
 	// reset system time
 	(void)hrt_reset();
 
 	if (fds[0].revents & POLLIN) {
 		len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
-		PX4_WARN("Sending initial controls message to jMAVSim.");
+		PX4_INFO("Sending initial controls message to jMAVSim.");
 		send_controls();
 	}
 
@@ -451,11 +506,12 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	// wait for new mavlink messages to arrive
 	while (true) {
 
-		pret = ::poll(&fds[0], (sizeof(fds)/sizeof(fds[0])), 100);
+		pret = ::poll(&fds[0], fd_count, 100);
 
 		//timed out
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}
 
 		// this is undesirable but not much we can do
 		if (pret < 0) {
@@ -468,13 +524,13 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 		// got data from simulator
 		if (fds[0].revents & POLLIN) {
 			len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
+
 			if (len > 0) {
 				mavlink_message_t msg;
 				mavlink_status_t status;
-				for (int i = 0; i < len; ++i)
-				{
-					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status))
-					{
+
+				for (int i = 0; i < len; ++i) {
+					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) {
 						// have a message, handle it
 						handle_message(&msg, publish);
 					}
@@ -485,15 +541,15 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 		// got data from PIXHAWK
 		if (fds[1].revents & POLLIN) {
 			len = ::read(serial_fd, serial_buf, sizeof(serial_buf));
+
 			if (len > 0) {
 				mavlink_message_t msg;
 				mavlink_status_t status;
-				for (int i = 0; i < len; ++i)
-				{
-					if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status))
-					{
+
+				for (int i = 0; i < len; ++i) {
+					if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status)) {
 						// have a message, handle it
-						handle_message(&msg, publish);
+						handle_message(&msg, true);
 					}
 				}
 			}
@@ -603,15 +659,17 @@ int openUart(const char *uart_name, int baud)
 	return uart_fd;
 }
 
-int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) {
+int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
+{
 
 
-        //uint64_t timestamp = imu->time_usec;
-        uint64_t timestamp = hrt_absolute_time();
+	//uint64_t timestamp = imu->time_usec;
+	uint64_t timestamp = hrt_absolute_time();
+
+	if ((imu->fields_updated & 0x1FFF) != 0x1FFF) {
+		PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated.  Got %08x", imu->fields_updated);
+	}
 
-        if((imu->fields_updated & 0x1FFF)!=0x1FFF) {
-	   	        PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated.  Got %08x",imu->fields_updated);
-        }
 	/*
 	  static int count=0;
 	  static uint64_t last_timestamp=0;
@@ -708,5 +766,40 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) {
 		}
 	}
 
-        return OK;
+	return OK;
+}
+
+int Simulator::publish_flow_topic(mavlink_hil_optical_flow_t* flow_mavlink)
+{
+	uint64_t timestamp = hrt_absolute_time();
+
+	/* flow */
+	{
+		struct optical_flow_s flow;
+		memset(&flow, 0, sizeof(flow));
+
+		flow.sensor_id = flow_mavlink->sensor_id;
+		flow.timestamp = timestamp;
+		flow.time_since_last_sonar_update = 0;
+		flow.frame_count_since_last_readout = 0; // ?
+		flow.integration_timespan = flow_mavlink->integration_time_us;
+
+		flow.ground_distance_m = flow_mavlink->distance;
+		flow.gyro_temperature = flow_mavlink->temperature;
+		flow.gyro_x_rate_integral = flow_mavlink->integrated_xgyro;
+		flow.gyro_y_rate_integral = flow_mavlink->integrated_ygyro;
+		flow.gyro_z_rate_integral = flow_mavlink->integrated_zgyro;
+		flow.pixel_flow_x_integral = flow_mavlink->integrated_x;
+		flow.pixel_flow_x_integral = flow_mavlink->integrated_y;
+		flow.quality = flow_mavlink->quality;
+
+		if (_flow_pub == nullptr) {
+		_flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
+
+		} else {
+		orb_publish(ORB_ID(optical_flow), _flow_pub, &flow);
+		}
+	}
+
+	return OK;
 }

src/modules/systemlib/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2015 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
@@ -31,18 +31,52 @@
 #
 ############################################################################
 
-#
-# Makefile to build position_estimator_inav
-#
+# for generated files
+include_directories(${CMAKE_BINARY_DIR}/src/modules/param)
 
-MODULE_COMMAND	 	= position_estimator_inav
-SRCS		 	= position_estimator_inav_main.c \
-			position_estimator_inav_params.c \
-			inertial_filter.c
+set(SRCS
+	perf_counter.c
+	param/param.c
+	conversions.c
+	cpuload.c
+	pid/pid.c
+	airspeed.c
+	mavlink_log.c
+	rc_check.c
+	otp.c
+	board_serial.c
+	pwm_limit/pwm_limit.c
+	mcu_version.c
+	bson/tinybson.c
+	circuit_breaker.cpp
+	)
 
-MODULE_STACKSIZE = 1200
+if(${OS} STREQUAL "nuttx")
+	list(APPEND SRCS
+		err.c
+		printload.c
+		up_cxxinitialize.c 
+		)
+else()
+	list(APPEND SRCS
+		print_load_posix.c
+		)	
+endif()
 
-ifeq ($(PX4_TARGEGT_OS),nuttx)
-EXTRACFLAGS = -Wframe-larger-than=3800
-endif
+if(NOT ${OS} STREQUAL "qurt")
+	list(APPEND SRCS
+		hx_stream.c
+		)
+endif()
 
+px4_add_module(
+	MODULE modules__systemlib
+	COMPILE_FLAGS
+		-Wno-sign-compare
+		-Os
+	SRCS ${SRCS}
+	DEPENDS
+		platforms__common
+		modules__param
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/systemlib/circuit_breaker_params.c

@@ -42,8 +42,6 @@
  * parameter needs to set to the key (magic).
  */
 
-#include <px4.h>
-
 /**
  * Circuit breaker for power supply check
  *

src/modules/systemlib/err.c

@@ -40,6 +40,9 @@
 
 #include <px4_config.h>
 
+#define __STDC_FORMAT_MACROS
+#include <inttypes.h>
+
 #include <stdlib.h>
 #include <errno.h>
 #include <string.h>

src/modules/systemlib/mcu_version.c

@@ -62,7 +62,7 @@
 /** Copy the 96bit MCU Unique ID into the provided pointer */
 void mcu_unique_id(uint32_t *uid_96_bit)
 {
-#ifdef __PX4_NUTTX
+#ifdef CONFIG_ARCH_CHIP_STM32
 	uid_96_bit[0] = getreg32(UNIQUE_ID);
 	uid_96_bit[1] = getreg32(UNIQUE_ID + 4);
 	uid_96_bit[2] = getreg32(UNIQUE_ID + 8);