Merge branch 'dev_ros' into dev_ros_mcatt

Conflicts:
	CMakeLists.txt

.catkin_workspace

@@ -0,0 +1 @@
+# This file currently only serves to mark the location of a catkin workspace for tool integration

.gitmodules

@@ -7,3 +7,9 @@
 [submodule "uavcan"]
 	path = uavcan
 	url = git://github.com/pavel-kirienko/uavcan.git
+[submodule "Tools/genmsg"]
+	path = Tools/genmsg
+	url = https://github.com/ros/genmsg.git
+[submodule "Tools/gencpp"]
+	path = Tools/gencpp
+	url = https://github.com/ros/gencpp.git

CMakeLists.txt

@@ -1,5 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(px4)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -111,7 +112,9 @@ target_link_libraries(px4
 )
 
 ## Declare a test publisher
-add_executable(publisher src/examples/publisher/publisher.cpp)
+add_executable(publisher
+	src/examples/publisher/publisher_main.cpp
+	src/examples/publisher/publisher_example.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
@@ -124,7 +127,9 @@ target_link_libraries(publisher
 )
 
 ## Declare a test subscriber
-add_executable(subscriber src/examples/subscriber/subscriber.cpp)
+add_executable(subscriber
+	src/examples/subscriber/subscriber_main.cpp
+	src/examples/subscriber/subscriber_example.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
@@ -136,13 +141,15 @@ target_link_libraries(subscriber
   px4
 )
 
-## mc_att_control
-add_executable(mc_att_control src/module/mc_att_control_main.cpp)
-add_dependencies(mc_att_control px4_generate_messages_cpp)
-target_link_libraries(mc_att_control
-  ${catkin_LIBRARIES}
-  px4
-)
+# add_executable(mc_att_control
+	# src/modules/mc_att_control/mc_att_control_main.cpp
+	# src/moudles/mc_att_control/mc_att_control_base.cpp)
+# add_dependencies(mc_att_control px4_generate_messages_cpp)
+# target_link_libraries(mc_att_control
+  # ${catkin_LIBRARIES}
+  # px4
+# )
+
 
 #############
 ## Install ##

Makefile

@@ -227,12 +227,18 @@ updatesubmodules:
 MSG_DIR = $(PX4_BASE)msg/px4_msgs
 MSG_TEMPLATE_DIR = $(PX4_BASE)msg/templates
 TOPICS_DIR = $(PX4_BASE)src/modules/uORB/topics
+TOPICS_TEMPORARY_DIR = $(BUILD_DIR)topics_temporary
+GENMSG_PYTHONPATH = $(PX4_BASE)/Tools/genmsg/src
+GENCPP_PYTHONPATH = $(PX4_BASE)/Tools/gencpp/src
 
 .PHONY: generateuorbtopicheaders
 generateuorbtopicheaders:
 	@$(ECHO) "Generating uORB topic headers"
-	$(Q) ($(PX4_BASE)/Tools/px_generate_uorb_topic_headers.py -d $(MSG_DIR) \
-		-o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR))
+	$(Q) (PYTHONPATH=$(GENMSG_PYTHONPATH):$(GENCPP_PYTHONPATH) $(PYTHON) \
+		$(PX4_BASE)Tools/px_generate_uorb_topic_headers.py \
+		-d $(MSG_DIR) -o $(TOPICS_DIR) -e $(MSG_TEMPLATE_DIR) -t $(TOPICS_TEMPORARY_DIR))
+# clean up temporary files
+	$(Q) (rm -r $(TOPICS_TEMPORARY_DIR))
 
 #
 # Testing targets

NuttX

@@ -1 +1 @@
-Subproject commit ae4b05e2c51d07369b5d131052099ac346b0841c
+Subproject commit 89a6776fc1b31d242c637d3f19072609bb98ec6c

Tools/gencpp

@@ -0,0 +1 @@
+Subproject commit 26a86f04bcec0018af6652b3ddd3f680e6e3fa2a

Tools/genmsg

@@ -0,0 +1 @@
+Subproject commit 72f0383f0e6a489214c51802ae12d6e271b1e454

Tools/px_generate_uorb_topic_headers.py

@@ -39,6 +39,8 @@ message files
 """
 from __future__ import print_function
 import os
+import shutil
+import filecmp
 import argparse
 import genmsg.template_tools
 
@@ -55,44 +57,93 @@ package = 'px4'
 
 
 def convert_file(filename, outputdir, templatedir, includepath):
-    """
-    Converts a single .msg file to a uorb header
-    """
-    print("Generating uORB headers from {0}".format(filename))
-    genmsg.template_tools.generate_from_file(filename,
-                                             package,
-                                             outputdir,
-                                             templatedir,
-                                             includepath,
-                                             msg_template_map,
-                                             srv_template_map)
+        """
+        Converts a single .msg file to a uorb header
+        """
+        print("Generating uORB headers from {0}".format(filename))
+        genmsg.template_tools.generate_from_file(filename,
+                                                 package,
+                                                 outputdir,
+                                                 templatedir,
+                                                 includepath,
+                                                 msg_template_map,
+                                                 srv_template_map)
 
 
 def convert_dir(inputdir, outputdir, templatedir):
-    """
-    Converts all .msg files in inputdir to uORB header files
-    """
-    includepath = incl_default + [':'.join([package, inputdir])]
-    for f in os.listdir(inputdir):
-        fn = os.path.join(inputdir, f)
-        if os.path.isfile(fn):
-            convert_file(fn, outputdir, templatedir, includepath)
+        """
+        Converts all .msg files in inputdir to uORB header files
+        """
+        includepath = incl_default + [':'.join([package, inputdir])]
+        for f in os.listdir(inputdir):
+                fn = os.path.join(inputdir, f)
+                if os.path.isfile(fn):
+                        convert_file(
+                            fn,
+                            outputdir,
+                            templatedir,
+                            includepath)
+
+
+def copy_changed(inputdir, outputdir):
+        """
+        Copies files from inputdir to outputdir if they don't exist in
+        ouputdir or if their content changed
+        """
+        for f in os.listdir(inputdir):
+                fni = os.path.join(inputdir, f)
+                if os.path.isfile(fni):
+                        # Check if f exists in outpoutdir, copy the file if not
+                        fno = os.path.join(outputdir, f)
+                        if not os.path.isfile(fno):
+                                shutil.copy(fni, fno)
+                                print("{0}: new header file".format(f))
+                                continue
+                        # The file exists in inputdir and outputdir
+                        # only copy if contents do not match
+                        if not filecmp.cmp(fni, fno):
+                                shutil.copy(fni, fno)
+                                print("{0}: updated".format(f))
+                                continue
+
+                        print("{0}: unchanged".format(f))
+
+def convert_dir_save(inputdir, outputdir, templatedir, temporarydir):
+        """
+        Converts all .msg files in inputdir to uORB header files
+        Unchanged existing files are not overwritten.
+        """
+        # Create new headers in temporary output directory
+        convert_dir(inputdir, temporarydir, templatedir)
+
+        # Copy changed headers from temporary dir to output dir
+        copy_changed(temporarydir, outputdir)
 
 if __name__ == "__main__":
-    parser = argparse.ArgumentParser(
-        description='Convert msg files to uorb headers')
-    parser.add_argument('-d', dest='dir', help='directory with msg files')
-    parser.add_argument('-f', dest='file',
-                        help="files to convert (use only without -d)",
-                        nargs="+")
-    parser.add_argument('-e', dest='templatedir',
-                        help='directory with template files',)
-    parser.add_argument('-o', dest='outputdir',
-                        help='output directory for header files')
-    args = parser.parse_args()
+        parser = argparse.ArgumentParser(
+            description='Convert msg files to uorb headers')
+        parser.add_argument('-d', dest='dir', help='directory with msg files')
+        parser.add_argument('-f', dest='file',
+                            help="files to convert (use only without -d)",
+                            nargs="+")
+        parser.add_argument('-e', dest='templatedir',
+                            help='directory with template files',)
+        parser.add_argument('-o', dest='outputdir',
+                            help='output directory for header files')
+        parser.add_argument('-t', dest='temporarydir',
+                            help='temporary directory')
+        args = parser.parse_args()
 
-    if args.file is not None:
-        for f in args.file:
-            convert_file(f, args.outputdir, args.templatedir, incl_default)
-    elif args.dir is not None:
-            convert_dir(args.dir, args.outputdir, args.templatedir)
+        if args.file is not None:
+                for f in args.file:
+                        convert_file(
+                            f,
+                            args.outputdir,
+                            args.templatedir,
+                            incl_default)
+        elif args.dir is not None:
+                convert_dir_save(
+                    args.dir,
+                    args.outputdir,
+                    args.templatedir,
+                    args.temporarydir)

makefiles/config_px4fmu-v2_default.mk

@@ -113,6 +113,7 @@ MODULES		+= lib/geo
 MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection
+MODULES		+= platforms/nuttx
 
 #
 # OBC challenge

makefiles/setup.mk

@@ -37,13 +37,14 @@
 # Some useful paths.
 #
 # Note that in general we always keep directory paths with the separator
-# at the end, and join paths without explicit separators. This reduces 
+# at the end, and join paths without explicit separators. This reduces
 # the number of duplicate slashes we have lying around in paths,
 # and is consistent with joining the results of $(dir) and $(notdir).
 #
 export PX4_INCLUDE_DIR	 = $(abspath $(PX4_BASE)/src/include)/
 export PX4_MODULE_SRC	 = $(abspath $(PX4_BASE)/src)/
 export PX4_LIB_DIR	 = $(abspath $(PX4_BASE)/src/lib)/
+export PX4_PLATFORMS_DIR = $(abspath $(PX4_BASE)/src/platforms)/
 export PX4_MK_DIR	 = $(abspath $(PX4_BASE)/makefiles)/
 export NUTTX_SRC	 = $(abspath $(PX4_BASE)/NuttX/nuttx)/
 export MAVLINK_SRC	 = $(abspath $(PX4_BASE)/mavlink/include/mavlink/v1.0)/
@@ -61,7 +62,8 @@ export ARCHIVE_DIR	 = $(abspath $(PX4_BASE)/Archives)/
 export INCLUDE_DIRS	:= $(PX4_MODULE_SRC) \
 			   $(PX4_MODULE_SRC)/modules/ \
 			   $(PX4_INCLUDE_DIR) \
-			   $(PX4_LIB_DIR)
+			   $(PX4_LIB_DIR) \
+			   $(PX4_PLATFORMS_DIR)
 
 #
 # Tools

makefiles/toolchain_gnu-arm-eabi.mk

@@ -121,7 +121,7 @@ INSTRUMENTATIONDEFINES	 = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
 # Language-specific flags
 #
 ARCHCFLAGS		 = -std=gnu99
-ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics
+ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__
 
 # Generic warnings
 #

msg/px4_msgs/actuator_armed.msg

@@ -0,0 +1,6 @@
+
+uint64 timestamp	# Microseconds since system boot
+bool armed		# Set to true if system is armed
+bool ready_to_arm	# Set to true if system is ready to be armed
+bool lockdown		# Set to true if actuators are forced to being disabled (due to emergency or HIL)
+bool force_failsafe	# Set to true if the actuators are forced to the failsafe position

msg/px4_msgs/actuator_controls.msg

@@ -0,0 +1,4 @@
+uint8 NUM_ACTUATOR_CONTROLS = 8
+uint8 NUM_ACTUATOR_CONTROL_GROUPS = 4
+uint64 timestamp
+float32[8] control

msg/px4_msgs/manual_control_setpoint.msg

@@ -0,0 +1,44 @@
+
+uint8 SWITCH_POS_NONE = 0		# switch is not mapped
+uint8 SWITCH_POS_ON = 1			# switch activated (value = 1)
+uint8 SWITCH_POS_MIDDLE = 2		# middle position (value = 0)
+uint8 SWITCH_POS_OFF = 3		# switch not activated (value = -1)
+uint64 timestamp
+
+# Any of the channels may not be available and be set to NaN
+# to indicate that it does not contain valid data.
+# The variable names follow the definition of the
+# MANUAL_CONTROL mavlink message.
+# The default range is from -1 to 1 (mavlink message -1000 to 1000)
+# The range for the z variable is defined from 0 to 1. (The z field of
+# the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
+
+float32 x			 # stick position in x direction -1..1
+				 # in general corresponds to forward/back motion or pitch of vehicle,
+				 # in general a positive value means forward or negative pitch and
+				 # a negative value means backward or positive pitch
+float32 y			 # stick position in y direction -1..1
+				 # in general corresponds to right/left motion or roll of vehicle,
+				 # in general a positive value means right or positive roll and
+				 # a negative value means left or negative roll
+float32 z			 # throttle stick position 0..1
+				 # in general corresponds to up/down motion or thrust of vehicle,
+				 # in general the value corresponds to the demanded throttle by the user,
+				 # if the input is used for setting the setpoint of a vertical position
+				 # controller any value > 0.5 means up and any value < 0.5 means down
+float32 r			 # yaw stick/twist positon, -1..1
+				 # in general corresponds to the righthand rotation around the vertical
+				 # (downwards) axis of the vehicle
+float32 flaps			 # flap position
+float32 aux1			 # default function: camera yaw / azimuth
+float32 aux2			 # default function: camera pitch / tilt
+float32 aux3			 # default function: camera trigger
+float32 aux4			 # default function: camera roll
+float32 aux5			 # default function: payload drop
+
+uint8 mode_switch		 # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO
+uint8 return_switch		 # return to launch 2 position switch (mandatory): _NORMAL_, RTL
+uint8 posctl_switch		 # position control 2 position switch (optional): _ALTCTL_, POSCTL
+uint8 loiter_switch		 # loiter 2 position switch (optional): _MISSION_, LOITER
+uint8 acro_switch		 # acro 2 position switch (optional): _MANUAL_, ACRO
+uint8 offboard_switch		 # offboard 2 position switch (optional): _NORMAL_, OFFBOARD

msg/px4_msgs/parameter_update.msg

@@ -0,0 +1 @@
+uint64 timestamp	# time at which the latest parameter was updated

msg/px4_msgs/unused/actuator_armed.msg

@@ -1,6 +0,0 @@
-
-uint64 timestamp	# Microseconds since system boot
-bool armed          # Set to true if system is armed
-bool ready_to_arm	# Set to true if system is ready to be armed
-bool lockdown	    # Set to true if actuators are forced to being disabled (due to emergency or HIL)
-bool force_failsafe # Set to true if the actuators are forced to the failsafe position

msg/px4_msgs/vehicle_attitude_setpoint.msg

@@ -0,0 +1,21 @@
+
+uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
+
+float32 roll_body				# body angle in NED frame
+float32 pitch_body				# body angle in NED frame
+float32 yaw_body					# body angle in NED frame
+
+float32[9] R_body				# Rotation matrix describing the setpoint as rotation from the current body frame
+bool R_valid					# Set to true if rotation matrix is valid
+
+# For quaternion-based attitude control
+float32[4] q_d					# Desired quaternion for quaternion control
+bool q_d_valid					# Set to true if quaternion vector is valid
+float32[4] q_e					# Attitude error in quaternion
+bool q_e_valid					# Set to true if quaternion error vector is valid
+
+float32 thrust					# Thrust in Newton the power system should generate
+
+bool roll_reset_integral			# Reset roll integral part (navigation logic change)
+bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
+bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)

msg/px4_msgs/vehicle_control_mode.msg

@@ -0,0 +1,22 @@
+
+uint64 timestamp				# in microseconds since system start
+						# is set whenever the writing thread stores new data
+
+bool flag_armed
+
+bool flag_external_manual_override_ok		# external override non-fatal for system. Only true for fixed wing
+
+# XXX needs yet to be set by state machine helper
+bool flag_system_hil_enabled
+
+bool flag_control_manual_enabled		# true if manual input is mixed in
+bool flag_control_auto_enabled			# true if onboard autopilot should act
+bool flag_control_offboard_enabled		# true if offboard control should be used
+bool flag_control_rates_enabled			# true if rates are stabilized
+bool flag_control_attitude_enabled		# true if attitude stabilization is mixed in
+bool flag_control_force_enabled			# true if force control is mixed in
+bool flag_control_velocity_enabled		# true if horizontal velocity (implies direction) is controlled
+bool flag_control_position_enabled		# true if position is controlled
+bool flag_control_altitude_enabled		# true if altitude is controlled
+bool flag_control_climb_rate_enabled		# true if climb rate is controlled
+bool flag_control_termination_enabled		# true if flighttermination is enabled

msg/px4_msgs/vehicle_rates_setpoint.msg

@@ -0,0 +1,6 @@
+uint64 timestamp    # in microseconds since system start
+
+float32 roll	    # body angular rates in NED frame
+float32 pitch	    # body angular rates in NED frame
+float32 yaw	    # body angular rates in NED frame
+float32 thrust	    # thrust normalized to 0..1

src/examples/flow_position_estimator/flow_position_estimator_main.c

@@ -67,7 +67,7 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <poll.h>
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 #include "flow_position_estimator_params.h"
 

src/examples/publisher/module.mk

@@ -37,6 +37,7 @@
 
 MODULE_COMMAND	= publisher
 
-SRCS		= publisher.cpp
+SRCS		= publisher_main.cpp \
+		  publisher_example.cpp
 
 MODULE_STACKSIZE = 1200

src/examples/publisher/publisher.cpp

@@ -1,100 +0,0 @@
-/*
- * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *   * Redistributions of source code must retain the above copyright notice,
- *     this list of conditions and the following disclaimer.
- *   * 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.
- *   * Neither the names of Stanford University or Willow Garage, Inc. 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 <px4.h>
-
-using namespace px4;
-
-/**
- * This tutorial demonstrates simple sending of messages over the PX4 middleware system.
- */
-
-namespace px4
-{
-bool task_should_exit = false;
-}
-
-PX4_MAIN_FUNCTION(publisher)
-{
-
-	px4::init(argc, argv, "px4_publisher");
-
-	px4::NodeHandle n;
-
-	/**
-	 * The advertise() function is how you tell ROS that you want to
-	 * publish on a given topic name. This invokes a call to the ROS
-	 * master node, which keeps a registry of who is publishing and who
-	 * is subscribing. After this advertise() call is made, the master
-	 * node will notify anyone who is trying to subscribe to this topic name,
-	 * and they will in turn negotiate a peer-to-peer connection with this
-	 * node.  advertise() returns a Publisher object which allows you to
-	 * publish messages on that topic through a call to publish().  Once
-	 * all copies of the returned Publisher object are destroyed, the topic
-	 * will be automatically unadvertised.
-	 *
-	 * The second parameter to advertise() is the size of the message queue
-	 * used for publishing messages.  If messages are published more quickly
-	 * than we can send them, the number here specifies how many messages to
-	 * buffer up before throwing some away.
-	 */
-	px4::Publisher * rc_channels_pub = PX4_ADVERTISE(n, rc_channels);
-
-
-	px4::Rate loop_rate(10);
-
-	/**
-	 * A count of how many messages we have sent. This is used to create
-	 * a unique string for each message.
-	 */
-	int count = 0;
-
-	while (px4::ok()) {
-		/**
-		 * This is a message object. You stuff it with data, and then publish it.
-		 */
-		PX4_TOPIC_T(rc_channels) msg;
-
-		msg.timestamp_last_valid = px4::get_time_micros();
-		PX4_INFO("%llu", msg.timestamp_last_valid);
-
-		/**
-		 * The publish() function is how you send messages. The parameter
-		 * is the message object. The type of this object must agree with the type
-		 * given as a template parameter to the advertise<>() call, as was done
-		 * in the constructor above.
-		 */
-		rc_channels_pub->publish(msg);
-
-		n.spinOnce();
-
-		loop_rate.sleep();
-		++count;
-	}
-
-	return 0;
-}

src/examples/publisher/publisher_example.cpp

@@ -0,0 +1,69 @@
+
+/****************************************************************************
+ *
+ *   Copyright (C) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_example.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "publisher_example.h"
+
+using namespace px4;
+
+PublisherExample::PublisherExample() :
+	_n(),
+	_rc_channels_pub(PX4_ADVERTISE(_n, rc_channels))
+{
+
+}
+
+int PublisherExample::main()
+{
+	px4::Rate loop_rate(10);
+
+	while (px4::ok()) {
+		PX4_TOPIC_T(rc_channels) msg;
+		msg.timestamp_last_valid = px4::get_time_micros();
+		PX4_INFO("%llu", msg.timestamp_last_valid);
+
+		_rc_channels_pub->publish(msg);
+
+		_n.spinOnce();
+		loop_rate.sleep();
+	}
+
+	return 0;
+}

src/examples/publisher/publisher_example.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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
@@ -32,30 +32,21 @@
  ****************************************************************************/
 
 /**
- * @file parameter_update.h
- * Notification about a parameter update.
+ * @file publisher.h
+ * Example publisher for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
+#include <px4.h>
 
-#ifndef TOPIC_PARAMETER_UPDATE_H
-#define TOPIC_PARAMETER_UPDATE_H
+class PublisherExample {
+public:
+	PublisherExample();
 
-#include <stdint.h>
-#include "../uORB.h"
+	~PublisherExample() {};
 
-/**
- * @addtogroup topics
- * @{
- */
-
-struct parameter_update_s {
-	/** time at which the latest parameter was updated */
-	uint64_t	timestamp;
+	int main();
+protected:
+	px4::NodeHandle _n;
+	px4::Publisher * _rc_channels_pub;
 };
-
-/**
- * @}
- */
-
-ORB_DECLARE(parameter_update);
-
-#endif

src/examples/publisher/publisher_main.cpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_main.cpp
+ * Example publisher for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include "publisher_example.h"
+
+static bool thread_running = false;     /**< Deamon status flag */
+static int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+PX4_MAIN_FUNCTION(publisher);
+
+#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
+extern "C" __EXPORT int publisher_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: publisher {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("publisher",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       publisher_task_main,
+					(argv) ? (const char **)&argv[2] : (const char **)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
+#endif
+
+PX4_MAIN_FUNCTION(publisher)
+{
+	px4::init(argc, argv, "publisher");
+
+	PX4_INFO("starting");
+	PublisherExample p;
+	thread_running = true;
+	p.main();
+
+	PX4_INFO("exiting.");
+	thread_running = false;
+	return 0;
+}

src/examples/subscriber/module.mk

@@ -37,6 +37,8 @@
 
 MODULE_COMMAND	= subscriber
 
-SRCS		= subscriber.cpp
+SRCS		= subscriber_main.cpp \
+		  subscriber_example.cpp \
+		  subscriber_params.c
 
 MODULE_STACKSIZE = 2400

src/examples/subscriber/subscriber.cpp

@@ -1,106 +0,0 @@
-/*
- * Copyright (C) 2008, Morgan Quigley and Willow Garage, Inc.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *   * Redistributions of source code must retain the above copyright notice,
- *     this list of conditions and the following disclaimer.
- *   * 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.
- *   * Neither the names of Stanford University or Willow Garage, Inc. 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 <px4.h>
-
-using namespace px4;
-
-/**
- * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
- */
-void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
-}
-void rc_channels_callback2(const PX4_TOPIC_T(rc_channels) &msg) {
-	PX4_INFO("I heard (2): [%llu]", msg.timestamp_last_valid);
-}
-
-class RCHandler {
-public:
-	void  callback(const PX4_TOPIC_T(rc_channels) &msg) {
-		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp_last_valid);
-	}
-};
-
-RCHandler rchandler;
-
-
-namespace px4
-{
-bool task_should_exit = false;
-}
-
-PX4_MAIN_FUNCTION(subscriber) {
-	/**
-	 * The ros::init() function needs to see argc and argv so that it can perform
-	 * any ROS arguments and name remapping that were provided at the command line. For programmatic
-	 * remappings you can use a different version of init() which takes remappings
-	 * directly, but for most command-line programs, passing argc and argv is the easiest
-	 * way to do it.  The third argument to init() is the name of the node.
-	 *
-	 * You must call one of the versions of px4::init() before using any other
-	 * part of the PX4/ ROS system.
-	 */
-	px4::init(argc, argv, "listener");
-
-	/**
-	 * NodeHandle is the main access point to communications with the ROS system.
-	 * The first NodeHandle constructed will fully initialize this node, and the last
-	 * NodeHandle destructed will close down the node.
-	 */
-	px4::NodeHandle n;
-
-	/**
-	 * The subscribe() call is how you tell ROS that you want to receive messages
-	 * on a given topic.  This invokes a call to the ROS
-	 * master node, which keeps a registry of who is publishing and who
-	 * is subscribing.  Messages are passed to a callback function, here
-	 * called chatterCallback.  subscribe() returns a Subscriber object that you
-	 * must hold on to until you want to unsubscribe.  When all copies of the Subscriber
-	 * object go out of scope, this callback will automatically be unsubscribed from
-	 * this topic.
-	 *
-	 * The second parameter to the subscribe() function is the size of the message
-	 * queue.  If messages are arriving faster than they are being processed, this
-	 * is the number of messages that will be buffered up before beginning to throw
-	 * away the oldest ones.
-	 */
-	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback, 100);
-	PX4_SUBSCRIBE(n, rc_channels, rc_channels_callback2, 1000);
-	PX4_SUBSCRIBE(n, rc_channels, RCHandler::callback, rchandler, 1000);
-	PX4_INFO("subscribed");
-
-	/**
-	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
-	 * callbacks will be called from within this thread (the main one).  px4::spin()
-	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
-	 */
-	n.spin();
-	PX4_INFO("finished, returning");
-
-	return 0;
-}

src/examples/subscriber/subscriber_example.cpp

@@ -0,0 +1,82 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_example.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "subscriber_params.h"
+#include "subscriber_example.h"
+
+using namespace px4;
+
+void rc_channels_callback_function(const PX4_TOPIC_T(rc_channels) &msg) {
+	PX4_INFO("I heard: [%llu]", msg.timestamp_last_valid);
+}
+
+SubscriberExample::SubscriberExample() :
+	_n(),
+	_p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)),
+	_interval(0),
+	_p_test_float(PX4_PARAM_INIT(SUB_TESTF)),
+	_test_float(0.0f)
+{
+	/* Read the parameter back as example */
+	PX4_PARAM_GET(_p_sub_interv, &_interval);
+	PX4_INFO("Param SUB_INTERV = %d", _interval);
+	PX4_PARAM_GET(_p_test_float, &_test_float);
+	PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float);
+
+	/* Do some subscriptions */
+	/* Function */
+	PX4_SUBSCRIBE(_n, rc_channels, rc_channels_callback_function, _interval);
+	/* Class Method */
+	PX4_SUBSCRIBE(_n, rc_channels, SubscriberExample::rc_channels_callback, this, 1000);
+	/* No callback */
+	_sub_rc_chan = PX4_SUBSCRIBE(_n, rc_channels, 500);
+
+	PX4_INFO("subscribed");
+}
+
+/**
+ * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
+ * Also the current value of the _sub_rc_chan subscription is printed
+ */
+void SubscriberExample::rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg) {
+	PX4_INFO("Subscriber callback: [%llu], value of _sub_rc_chan: [%llu]",
+			msg.timestamp_last_valid,
+			_sub_rc_chan->get_msg().timestamp_last_valid);
+}

src/examples/subscriber/subscriber_example.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 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
@@ -32,38 +32,31 @@
  ****************************************************************************/
 
 /**
- * @file actuator_armed.h
- *
- * Actuator armed topic
+ * @file subscriber_example.h
+ * Example subscriber for ros and px4
  *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
+#include <px4.h>
 
-#ifndef TOPIC_ACTUATOR_ARMED_H
-#define TOPIC_ACTUATOR_ARMED_H
+using namespace px4;
 
-#include <stdint.h>
-#include "../uORB.h"
+class SubscriberExample {
+public:
+	SubscriberExample();
 
-/**
- * @addtogroup topics
- * @{
- */
+	~SubscriberExample() {};
+
+	void spin() {_n.spin();}
+protected:
+	px4::NodeHandle _n;
+	px4_param_t _p_sub_interv;
+	int32_t _interval;
+	px4_param_t _p_test_float;
+	float _test_float;
+	px4::PX4_SUBSCRIBER(rc_channels) * _sub_rc_chan;
+
+	void rc_channels_callback(const PX4_TOPIC_T(rc_channels) &msg);
 
-/** global 'actuator output is live' control. */
-struct actuator_armed_s {
 
-	uint64_t	timestamp;	/**< Microseconds since system boot */
-	bool		armed;		/**< Set to true if system is armed */
-	bool		ready_to_arm;	/**< Set to true if system is ready to be armed */
-	bool		lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
-	bool		force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */
 };
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(actuator_armed);
-
-#endif

src/examples/subscriber/subscriber_main.cpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_main.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include "subscriber_example.h"
+
+static bool thread_running = false;     /**< Deamon status flag */
+static int daemon_task;             /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+PX4_MAIN_FUNCTION(subscriber);
+
+#if !defined(__linux) && !(defined(__APPLE__) && defined(__MACH__))
+extern "C" __EXPORT int subscriber_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: subscriber {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
+		}
+
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("subscriber",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       subscriber_task_main,
+					(argv) ? (const char **)&argv[2] : (const char **)NULL);
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		task_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("is running");
+
+		} else {
+			warnx("not started");
+		}
+
+		exit(0);
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}
+#endif
+
+PX4_MAIN_FUNCTION(subscriber)
+{
+	px4::init(argc, argv, "subscriber");
+
+	PX4_INFO("starting");
+	SubscriberExample s;
+	thread_running = true;
+	s.spin();
+
+	PX4_INFO("exiting.");
+	thread_running = false;
+	return 0;
+}

src/examples/subscriber/subscriber_params.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   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
@@ -33,35 +32,25 @@
  ****************************************************************************/
 
 /**
- * @file vehicle_rates_setpoint.h
- * Definition of the vehicle rates setpoint topic
+ * @file subscriber_params.c
+ * Parameters for the subscriber example
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_
-#define TOPIC_VEHICLE_RATES_SETPOINT_H_
-
-#include <stdint.h>
-#include "../uORB.h"
+#include <px4_defines.h>
+#include "subscriber_params.h"
 
 /**
- * @addtogroup topics
- * @{
+ * Interval of one subscriber in the example in ms
+ *
+ * @group Subscriber Example
  */
-struct vehicle_rates_setpoint_s {
-	uint64_t timestamp; /**< in microseconds since system start */
-
-	float roll;	/**< body angular rates in NED frame		*/
-	float pitch;	/**< body angular rates in NED frame		*/
-	float yaw;	/**< body angular rates in NED frame		*/
-	float thrust;	/**< thrust normalized to 0..1			*/
-
-}; /**< vehicle_rates_setpoint */
+PX4_PARAM_DEFINE_INT32(SUB_INTERV);
 
 /**
-* @}
-*/
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_rates_setpoint);
-
-#endif
+ * Float Demonstration Parameter in the Example
+ *
+ * @group Subscriber Example
+ */
+PX4_PARAM_DEFINE_FLOAT(SUB_TESTF);

src/examples/subscriber/subscriber_params.h

@@ -0,0 +1,43 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_params.h
+ * Default parameters for the subscriber example
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#pragma once
+
+#define PARAM_SUB_INTERV_DEFAULT 100
+#define PARAM_SUB_TESTF_DEFAULT 3.14f

src/include/px4.h

@@ -39,27 +39,8 @@
 
 #pragma once
 
-#include <stdbool.h>
-
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
-/*
- * Building for running within the ROS environment
- */
-#include "ros/ros.h"
-#include "px4/rc_channels.h"
-
-#else
-/*
- * Building for NuttX
- */
-#include <nuttx/config.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/rc_channels.h>
-#include <systemlib/err.h>
-
-#endif
-
-#include <px4_defines.h>
+#include "../platforms/px4_includes.h"
+#include "../platforms/px4_defines.h"
 #include "../platforms/px4_middleware.h"
 #include "../platforms/px4_nodehandle.h"
 #include "../platforms/px4_subscriber.h"

src/include/px4_defines.h

@@ -1,77 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 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.
- *
- ****************************************************************************/
-
-/**
- * @file px4_defines.h
- *
- * Generally used magic defines
- */
-
-#pragma once
-
-#if defined(__linux) || (defined(__APPLE__) && defined(__MACH__))
-/*
- * Building for running within the ROS environment
- */
-#define __EXPORT
-#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
-#define PX4_WARN ROS_WARN
-#define PX4_INFO ROS_INFO
-#define PX4_TOPIC(_name) #_name
-#define PX4_TOPIC_T(_name) _name
-#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, &_obj);
-#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
-#else
-/*
- * Building for NuttX
- */
-#define PX4_MAIN_FUNCTION(_prefix) extern "C" __EXPORT int _prefix##_main(int argc, char *argv[])
-#define PX4_WARN warnx
-#define PX4_WARN warnx
-#define PX4_INFO warnx
-#define PX4_TOPIC(_name) ORB_ID(_name)
-#define PX4_TOPIC_T(_name) _name##_s
-#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _obj, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _obj, std::placeholders::_1), _interval)
-#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
-#endif
-
-/* Overload the PX4_SUBSCRIBE macro to suppport methods and pure functions as callback */
-#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
-#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC)(__VA_ARGS__)
-#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name))
-
-/* wrapper for 2d matrices */
-#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y])
-
-/* wrapper for rotation matrices stored in arrays */
-#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)

src/lib/mathlib/math/Limits.cpp

@@ -46,6 +46,9 @@
 
 namespace math {
 
+#ifndef CONFIG_ARCH_ARM	
+#define M_PI_F 3.14159265358979323846f
+#endif
 
 float __EXPORT min(float val1, float val2)
 {
@@ -143,4 +146,4 @@ double __EXPORT degrees(double radians)
 	return (radians / M_PI) * 180.0;
 }
 
-}
+}

src/lib/mathlib/math/Limits.hpp

@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <nuttx/config.h>
+#include <platforms/px4_defines.h>
 #include <stdint.h>
 
 namespace math {
@@ -84,4 +84,4 @@ float __EXPORT degrees(float radians);
 
 double __EXPORT degrees(double radians);
 
-}
+}

src/lib/mathlib/math/Matrix.hpp

@@ -44,12 +44,20 @@
 #define MATRIX_HPP
 
 #include <stdio.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <math/eigen_math.h>
+#include <Eigen/Eigen>
+#define M_PI_2_F 1.5707963267948966192f
+#endif
 
 namespace math
 {
 
-template <unsigned int M, unsigned int N>
+template<unsigned int M, unsigned int N>
 class __EXPORT Matrix;
 
 // MxN matrix with float elements
@@ -65,16 +73,19 @@ public:
 	/**
 	 * struct for using arm_math functions
 	 */
+#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_mat;
+#else
+	eigen_matrix_instance arm_mat;
+#endif
 
 	/**
 	 * trivial ctor
 	 * Initializes the elements to zero.
 	 */
-	MatrixBase() : 
-		data{},
-		arm_mat{M, N, &data[0][0]}
-	{
+	MatrixBase() :
+		data {},
+	arm_mat {M, N, &data[0][0]} {
 	}
 
 	virtual ~MatrixBase() {};
@@ -83,20 +94,17 @@ public:
 	 * copyt ctor
 	 */
 	MatrixBase(const MatrixBase<M, N> &m) :
-		arm_mat{M, N, &data[0][0]}
-	{
+		arm_mat {M, N, &data[0][0]} {
 		memcpy(data, m.data, sizeof(data));
 	}
 
 	MatrixBase(const float *d) :
-		arm_mat{M, N, &data[0][0]}
-	{
+		arm_mat {M, N, &data[0][0]} {
 		memcpy(data, d, sizeof(data));
 	}
 
-	MatrixBase(const float d[M][N]) : 
-		arm_mat{M, N, &data[0][0]}
-	{
+	MatrixBase(const float d[M][N]) :
+		arm_mat {M, N, &data[0][0]} {
 		memcpy(data, d, sizeof(data));
 	}
 
@@ -148,8 +156,9 @@ public:
 	bool operator ==(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
-				if (data[i][j] != m.data[i][j])
+				if (data[i][j] != m.data[i][j]) {
 					return false;
+				}
 
 		return true;
 	}
@@ -160,8 +169,9 @@ public:
 	bool operator !=(const Matrix<M, N> &m) const {
 		for (unsigned int i = 0; i < M; i++)
 			for (unsigned int j = 0; j < N; j++)
-				if (data[i][j] != m.data[i][j])
+				if (data[i][j] != m.data[i][j]) {
 					return true;
+				}
 
 		return false;
 	}
@@ -181,8 +191,9 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
+			for (unsigned int j = 0; j < M; j++) {
 				res.data[i][j] = -data[i][j];
+			}
 
 		return res;
 	}
@@ -194,16 +205,18 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
+			for (unsigned int j = 0; j < M; j++) {
 				res.data[i][j] = data[i][j] + m.data[i][j];
+			}
 
 		return res;
 	}
 
 	Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
+			for (unsigned int j = 0; j < M; j++) {
 				data[i][j] += m.data[i][j];
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -215,16 +228,18 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				res.data[i][j] = data[i][j] - m.data[i][j];
+			}
 
 		return res;
 	}
 
 	Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
 		for (unsigned int i = 0; i < N; i++)
-			for (unsigned int j = 0; j < M; j++)
+			for (unsigned int j = 0; j < M; j++) {
 				data[i][j] -= m.data[i][j];
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -236,16 +251,19 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				res.data[i][j] = data[i][j] * num;
+			}
 
 		return res;
+
 	}
 
 	Matrix<M, N> &operator *=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				data[i][j] *= num;
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -254,16 +272,18 @@ public:
 		Matrix<M, N> res;
 
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				res[i][j] = data[i][j] / num;
+			}
 
 		return res;
 	}
 
 	Matrix<M, N> &operator /=(const float num) {
 		for (unsigned int i = 0; i < M; i++)
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				data[i][j] /= num;
+			}
 
 		return *static_cast<Matrix<M, N>*>(this);
 	}
@@ -273,27 +293,53 @@ public:
 	 */
 	template <unsigned int P>
 	Matrix<M, P> operator *(const Matrix<N, P> &m) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<M, P> res;
 		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::Matrix<float, N, P, Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float, N, P, Eigen::RowMajor> >
+				(m.arm_mat.pData);
+		Eigen::Matrix<float, M, P, Eigen::RowMajor> Product = Me * Him;
+		Matrix<M, P> res(Product.data());
+		return res;
+#endif
 	}
 
 	/**
 	 * transpose the matrix
 	 */
 	Matrix<N, M> transposed(void) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<N, M> res;
 		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Me.transposeInPlace();
+		Matrix<N, M> res(Me.data());
+		return res;
+#endif
 	}
 
 	/**
 	 * invert the matrix
 	 */
 	Matrix<M, N> inversed(void) const {
+#ifdef CONFIG_ARCH_ARM
 		Matrix<M, N> res;
 		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
 		return res;
+#else
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
+		Matrix<M, N> res(MyInverse.data());
+		return res;
+#endif
 	}
 
 	/**
@@ -310,16 +356,18 @@ public:
 		memset(data, 0, sizeof(data));
 		unsigned int n = (M < N) ? M : N;
 
-		for (unsigned int i = 0; i < n; i++)
+		for (unsigned int i = 0; i < n; i++) {
 			data[i][i] = 1;
+		}
 	}
 
 	void print(void) {
 		for (unsigned int i = 0; i < M; i++) {
 			printf("[ ");
 
-			for (unsigned int j = 0; j < N; j++)
+			for (unsigned int j = 0; j < N; j++) {
 				printf("%.3f\t", data[i][j]);
+			}
 
 			printf(" ]\n");
 		}
@@ -352,8 +400,17 @@ public:
 	 * multiplication by a vector
 	 */
 	Vector<M> operator *(const Vector<N> &v) const {
+#ifdef CONFIG_ARCH_ARM
 		Vector<M> res;
 		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
+#else
+		//probably nicer if this could go into a function like "eigen_mat_mult" or so
+		Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+				(this->arm_mat.pData);
+		Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData, N);
+		Eigen::VectorXf Product = Me * Vec;
+		Vector<M> res(Product.data());
+#endif
 		return res;
 	}
 };

src/lib/mathlib/math/Quaternion.hpp

@@ -44,7 +44,7 @@
 #define QUATERNION_HPP
 
 #include <math.h>
-#include "../CMSIS/Include/arm_math.h"
+
 #include "Vector.hpp"
 #include "Matrix.hpp"
 

src/lib/mathlib/math/Vector.hpp

@@ -45,7 +45,12 @@
 
 #include <stdio.h>
 #include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
 #include "../CMSIS/Include/arm_math.h"
+#else
+#include <math/eigen_math.h>
+#endif
 
 namespace math
 {
@@ -65,7 +70,12 @@ public:
 	/**
 	 * struct for using arm_math functions, represents column vector
 	 */
+	#ifdef CONFIG_ARCH_ARM
 	arm_matrix_instance_f32 arm_col;
+	#else
+	eigen_matrix_instance arm_col;
+	#endif
+
 
 	/**
 	 * trivial ctor

src/modules/attitude_estimator_ekf/AttitudeEKF.m

@@ -0,0 +1,298 @@
+function [xa_apo,Pa_apo,Rot_matrix,eulerAngles,debugOutput]...
+    = AttitudeEKF(approx_prediction,use_inertia_matrix,zFlag,dt,z,q_rotSpeed,q_rotAcc,q_acc,q_mag,r_gyro,r_accel,r_mag,J)
+
+
+%LQG Postion Estimator and Controller
+% Observer:
+%        x[n|n]   = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])
+%        x[n+1|n] = Ax[n|n] + Bu[n]
+%
+% $Author: Tobias Naegeli $    $Date: 2014 $    $Revision: 3 $
+%
+%
+% Arguments:
+% approx_prediction: if 1 then the exponential map is approximated with a
+% first order taylor approximation. has at the moment not a big influence
+% (just 1st or 2nd order approximation) we should change it to rodriquez
+% approximation.
+% use_inertia_matrix: set to true if you have the inertia matrix J for your
+% quadrotor
+% xa_apo_k: old state vectotr
+% zFlag: if sensor measurement is available [gyro, acc, mag]
+% dt: dt in s
+% z: measurements [gyro, acc, mag]
+% q_rotSpeed: process noise gyro
+% q_rotAcc: process noise gyro acceleration
+% q_acc: process noise acceleration
+% q_mag: process noise magnetometer
+% r_gyro: measurement noise gyro
+% r_accel: measurement noise accel
+% r_mag: measurement noise mag
+% J: moment of inertia matrix
+
+
+% Output:
+% xa_apo: updated state vectotr
+% Pa_apo: updated state covariance matrix
+% Rot_matrix: rotation matrix
+% eulerAngles: euler angles
+% debugOutput: not used
+
+
+%% model specific parameters
+
+
+
+% compute once the inverse of the Inertia
+persistent Ji;
+if isempty(Ji)
+    Ji=single(inv(J));
+end
+
+%% init
+persistent x_apo
+if(isempty(x_apo))
+    gyro_init=single([0;0;0]);
+    gyro_acc_init=single([0;0;0]);
+    acc_init=single([0;0;-9.81]);
+    mag_init=single([1;0;0]);
+    x_apo=single([gyro_init;gyro_acc_init;acc_init;mag_init]);
+    
+end
+
+persistent P_apo
+if(isempty(P_apo))
+    %     P_apo = single(eye(NSTATES) * 1000);
+    P_apo = single(200*ones(12));
+end
+
+debugOutput = single(zeros(4,1));
+
+%% copy the states
+wx=  x_apo(1);   % x  body angular rate
+wy=  x_apo(2);   % y  body angular rate
+wz=  x_apo(3);   % z  body angular rate
+
+wax=  x_apo(4);  % x  body angular acceleration
+way=  x_apo(5);  % y  body angular acceleration
+waz=  x_apo(6);  % z  body angular acceleration
+
+zex=  x_apo(7);  % x  component gravity vector
+zey=  x_apo(8);  % y  component gravity vector
+zez=  x_apo(9);  % z  component gravity vector
+
+mux=  x_apo(10); % x  component magnetic field vector
+muy=  x_apo(11); % y  component magnetic field vector
+muz=  x_apo(12); % z  component magnetic field vector
+
+
+
+
+%% prediction section
+% compute the apriori state estimate from the previous aposteriori estimate
+%body angular accelerations
+if (use_inertia_matrix==1)
+    wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt;
+else
+    wak =[wax;way;waz];
+end
+
+%body angular rates
+wk =[wx;  wy; wz] + dt*wak;
+
+%derivative of the prediction rotation matrix
+O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]';
+
+%prediction of the earth z vector
+if (approx_prediction==1)
+    %e^(Odt)=I+dt*O+dt^2/2!O^2
+    % so we do a first order approximation of the exponential map
+    zek =(O*dt+single(eye(3)))*[zex;zey;zez];
+    
+else
+    zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez];
+    %zek =expm2(O*dt)*[zex;zey;zez]; not working because use double
+    %precision
+end
+
+
+
+%prediction of the magnetic vector
+if (approx_prediction==1)
+    %e^(Odt)=I+dt*O+dt^2/2!O^2
+    % so we do a first order approximation of the exponential map
+    muk =(O*dt+single(eye(3)))*[mux;muy;muz];
+else
+     muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz];
+    %muk =expm2(O*dt)*[mux;muy;muz]; not working because use double
+    %precision
+end
+
+x_apr=[wk;wak;zek;muk];
+
+% compute the apriori error covariance estimate from the previous
+%aposteriori estimate
+
+EZ=[0,zez,-zey;
+    -zez,0,zex;
+    zey,-zex,0]';
+MA=[0,muz,-muy;
+    -muz,0,mux;
+    muy,-mux,0]';
+
+E=single(eye(3));
+Z=single(zeros(3));
+
+A_lin=[ Z,  E,  Z,  Z
+    Z,  Z,  Z,  Z
+    EZ, Z,  O,  Z
+    MA, Z,  Z,  O];
+
+A_lin=eye(12)+A_lin*dt;
+
+%process covariance matrix
+
+persistent Q
+if (isempty(Q))
+    Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,...
+        q_rotAcc,q_rotAcc,q_rotAcc,...
+        q_acc,q_acc,q_acc,...
+        q_mag,q_mag,q_mag]);
+end
+
+P_apr=A_lin*P_apo*A_lin'+Q;
+
+
+%% update
+if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1
+    
+%     R=[r_gyro,0,0,0,0,0,0,0,0;
+%         0,r_gyro,0,0,0,0,0,0,0;
+%         0,0,r_gyro,0,0,0,0,0,0;
+%         0,0,0,r_accel,0,0,0,0,0;
+%         0,0,0,0,r_accel,0,0,0,0;
+%         0,0,0,0,0,r_accel,0,0,0;
+%         0,0,0,0,0,0,r_mag,0,0;
+%         0,0,0,0,0,0,0,r_mag,0;
+%         0,0,0,0,0,0,0,0,r_mag];
+     R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag];
+    %observation matrix
+    %[zw;ze;zmk];
+    H_k=[  E,     Z,      Z,    Z;
+        Z,     Z,      E,    Z;
+        Z,     Z,      Z,    E];
+    
+    y_k=z(1:9)-H_k*x_apr;
+    
+    
+    %S_k=H_k*P_apr*H_k'+R;
+     S_k=H_k*P_apr*H_k';
+     S_k(1:9+1:end) = S_k(1:9+1:end) + R_v;
+    K_k=(P_apr*H_k'/(S_k));
+    
+    
+    x_apo=x_apr+K_k*y_k;
+    P_apo=(eye(12)-K_k*H_k)*P_apr;
+else
+    if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0
+        
+        R=[r_gyro,0,0;
+            0,r_gyro,0;
+            0,0,r_gyro];
+        R_v=[r_gyro,r_gyro,r_gyro];
+        %observation matrix
+        
+        H_k=[  E,     Z,      Z,    Z];
+        
+        y_k=z(1:3)-H_k(1:3,1:12)*x_apr;
+        
+       % S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3);
+        S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)';
+        S_k(1:3+1:end) = S_k(1:3+1:end) + R_v;
+        K_k=(P_apr*H_k(1:3,1:12)'/(S_k));
+        
+        
+        x_apo=x_apr+K_k*y_k;
+        P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr;
+    else
+        if  zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0
+            
+%             R=[r_gyro,0,0,0,0,0;
+%                 0,r_gyro,0,0,0,0;
+%                 0,0,r_gyro,0,0,0;
+%                 0,0,0,r_accel,0,0;
+%                 0,0,0,0,r_accel,0;
+%                 0,0,0,0,0,r_accel];
+            
+            R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel];
+            %observation matrix
+            
+            H_k=[  E,     Z,      Z,    Z;
+                Z,     Z,      E,    Z];
+            
+            y_k=z(1:6)-H_k(1:6,1:12)*x_apr;
+            
+           % S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
+            S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
+            S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
+            K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
+            
+            
+            x_apo=x_apr+K_k*y_k;
+            P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
+        else
+            if  zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1
+%                 R=[r_gyro,0,0,0,0,0;
+%                     0,r_gyro,0,0,0,0;
+%                     0,0,r_gyro,0,0,0;
+%                     0,0,0,r_mag,0,0;
+%                     0,0,0,0,r_mag,0;
+%                     0,0,0,0,0,r_mag];
+                  R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag];
+                %observation matrix
+                
+                H_k=[  E,     Z,      Z,    Z;
+                    Z,     Z,      Z,    E];
+                
+                y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr;
+                
+                %S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6);
+                S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)';
+                S_k(1:6+1:end) = S_k(1:6+1:end) + R_v;
+                K_k=(P_apr*H_k(1:6,1:12)'/(S_k));
+                
+                
+                x_apo=x_apr+K_k*y_k;
+                P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr;
+            else
+                x_apo=x_apr;
+                P_apo=P_apr;
+            end
+        end
+    end
+end
+
+
+
+%% euler anglels extraction
+z_n_b = -x_apo(7:9)./norm(x_apo(7:9));
+m_n_b = x_apo(10:12)./norm(x_apo(10:12));
+
+y_n_b=cross(z_n_b,m_n_b);
+y_n_b=y_n_b./norm(y_n_b);
+
+x_n_b=(cross(y_n_b,z_n_b));
+x_n_b=x_n_b./norm(x_n_b);
+
+
+xa_apo=x_apo;
+Pa_apo=P_apo;
+% rotation matrix from earth to body system
+Rot_matrix=[x_n_b,y_n_b,z_n_b];
+
+
+phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3));
+theta=-asin(Rot_matrix(1,3));
+psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1));
+eulerAngles=[phi;theta;psi];
+

src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj

@@ -0,0 +1,502 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<deployment-project plugin="plugin.matlabcoder" plugin-version="R2013a">
+  <configuration target="target.matlab.coder" target-name="MEX, C, and C++ Code Generation" name="attitudeKalmanfilter" location="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf" file="/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/attitudeKalmanfilter.prj" build-checksum="1213478164">
+    <profile key="profile.mex">
+      <param.MergeInstrumentationResults>false</param.MergeInstrumentationResults>
+      <param.BuiltInstrumentedMex>false</param.BuiltInstrumentedMex>
+      <param.RanInstrumentedMex>false</param.RanInstrumentedMex>
+      <param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
+      <param.SpecifiedWorkingFolder />
+      <param.BuildFolder>option.BuildFolder.Project</param.BuildFolder>
+      <param.SpecifiedBuildFolder />
+      <param.SearchPaths />
+      <param.ResponsivenessChecks>true</param.ResponsivenessChecks>
+      <param.ExtrinsicCalls>true</param.ExtrinsicCalls>
+      <param.IntegrityChecks>true</param.IntegrityChecks>
+      <param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
+      <param.GlobalDataSyncMethod>option.GlobalDataSyncMethod.SyncAlways</param.GlobalDataSyncMethod>
+      <param.EnableVariableSizing>true</param.EnableVariableSizing>
+      <param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
+      <param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
+      <param.StackUsageMax>200000</param.StackUsageMax>
+      <param.FilePartitionMethod>option.FilePartitionMethod.MapMFileToCFile</param.FilePartitionMethod>
+      <param.GenerateComments>true</param.GenerateComments>
+      <param.MATLABSourceComments>false</param.MATLABSourceComments>
+      <param.ReservedNameArray />
+      <param.EnableScreener>true</param.EnableScreener>
+      <param.EnableDebugging>false</param.EnableDebugging>
+      <param.GenerateReport>true</param.GenerateReport>
+      <param.LaunchReport>false</param.LaunchReport>
+      <param.CustomSourceCode />
+      <param.CustomHeaderCode />
+      <param.CustomInitializer />
+      <param.CustomTerminator />
+      <param.CustomInclude />
+      <param.CustomSource />
+      <param.CustomLibrary />
+      <param.PostCodeGenCommand />
+      <param.ProposeFixedPointDataTypes>true</param.ProposeFixedPointDataTypes>
+      <param.mex.GenCodeOnly>false</param.mex.GenCodeOnly>
+      <param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
+      <param.RecursionLimit>100</param.RecursionLimit>
+      <param.TargetLang>option.TargetLang.C</param.TargetLang>
+      <param.EchoExpressions>true</param.EchoExpressions>
+      <param.InlineThreshold>10</param.InlineThreshold>
+      <param.InlineThresholdMax>200</param.InlineThresholdMax>
+      <param.InlineStackLimit>4000</param.InlineStackLimit>
+      <param.EnableMemcpy>true</param.EnableMemcpy>
+      <param.MemcpyThreshold>64</param.MemcpyThreshold>
+      <param.EnableOpenMP>true</param.EnableOpenMP>
+      <param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
+      <param.ConstantInputs>option.ConstantInputs.CheckValues</param.ConstantInputs>
+      <unset>
+        <param.MergeInstrumentationResults />
+        <param.BuiltInstrumentedMex />
+        <param.RanInstrumentedMex />
+        <param.WorkingFolder />
+        <param.SpecifiedWorkingFolder />
+        <param.BuildFolder />
+        <param.SpecifiedBuildFolder />
+        <param.SearchPaths />
+        <param.ResponsivenessChecks />
+        <param.ExtrinsicCalls />
+        <param.IntegrityChecks />
+        <param.SaturateOnIntegerOverflow />
+        <param.GlobalDataSyncMethod />
+        <param.EnableVariableSizing />
+        <param.DynamicMemoryAllocation />
+        <param.DynamicMemoryAllocationThreshold />
+        <param.StackUsageMax />
+        <param.FilePartitionMethod />
+        <param.GenerateComments />
+        <param.MATLABSourceComments />
+        <param.ReservedNameArray />
+        <param.EnableScreener />
+        <param.EnableDebugging />
+        <param.GenerateReport />
+        <param.LaunchReport />
+        <param.CustomSourceCode />
+        <param.CustomHeaderCode />
+        <param.CustomInitializer />
+        <param.CustomTerminator />
+        <param.CustomInclude />
+        <param.CustomSource />
+        <param.CustomLibrary />
+        <param.PostCodeGenCommand />
+        <param.ProposeFixedPointDataTypes />
+        <param.mex.GenCodeOnly />
+        <param.ConstantFoldingTimeout />
+        <param.RecursionLimit />
+        <param.TargetLang />
+        <param.EchoExpressions />
+        <param.InlineThreshold />
+        <param.InlineThresholdMax />
+        <param.InlineStackLimit />
+        <param.EnableMemcpy />
+        <param.MemcpyThreshold />
+        <param.EnableOpenMP />
+        <param.InitFltsAndDblsToZero />
+        <param.ConstantInputs />
+      </unset>
+    </profile>
+    <profile key="profile.c">
+      <param.grt.GenCodeOnly>true</param.grt.GenCodeOnly>
+      <param.WorkingFolder>option.WorkingFolder.Project</param.WorkingFolder>
+      <param.SpecifiedWorkingFolder />
+      <param.BuildFolder>option.BuildFolder.Specified</param.BuildFolder>
+      <param.SpecifiedBuildFolder>codegen</param.SpecifiedBuildFolder>
+      <param.SearchPaths />
+      <param.SaturateOnIntegerOverflow>true</param.SaturateOnIntegerOverflow>
+      <param.PurelyIntegerCode>false</param.PurelyIntegerCode>
+      <param.SupportNonFinite>false</param.SupportNonFinite>
+      <param.EnableVariableSizing>false</param.EnableVariableSizing>
+      <param.DynamicMemoryAllocation>option.DynamicMemoryAllocation.Threshold</param.DynamicMemoryAllocation>
+      <param.DynamicMemoryAllocationThreshold>65536</param.DynamicMemoryAllocationThreshold>
+      <param.StackUsageMax>4000</param.StackUsageMax>
+      <param.MultiInstanceCode>false</param.MultiInstanceCode>
+      <param.FilePartitionMethod>option.FilePartitionMethod.SingleFile</param.FilePartitionMethod>
+      <param.GenerateComments>true</param.GenerateComments>
+      <param.MATLABSourceComments>true</param.MATLABSourceComments>
+      <param.MATLABFcnDesc>false</param.MATLABFcnDesc>
+      <param.DataTypeReplacement>option.DataTypeReplacement.CBuiltIn</param.DataTypeReplacement>
+      <param.ConvertIfToSwitch>false</param.ConvertIfToSwitch>
+      <param.PreserveExternInFcnDecls>true</param.PreserveExternInFcnDecls>
+      <param.ParenthesesLevel>option.ParenthesesLevel.Nominal</param.ParenthesesLevel>
+      <param.MaxIdLength>31</param.MaxIdLength>
+      <param.CustomSymbolStrGlobalVar>$M$N</param.CustomSymbolStrGlobalVar>
+      <param.CustomSymbolStrType>$M$N</param.CustomSymbolStrType>
+      <param.CustomSymbolStrField>$M$N</param.CustomSymbolStrField>
+      <param.CustomSymbolStrFcn>$M$N</param.CustomSymbolStrFcn>
+      <param.CustomSymbolStrTmpVar>$M$N</param.CustomSymbolStrTmpVar>
+      <param.CustomSymbolStrMacro>$M$N</param.CustomSymbolStrMacro>
+      <param.CustomSymbolStrEMXArray>emxArray_$M$N</param.CustomSymbolStrEMXArray>
+      <param.CustomSymbolStrEMXArrayFcn>emx$M$N</param.CustomSymbolStrEMXArrayFcn>
+      <param.ReservedNameArray />
+      <param.EnableScreener>true</param.EnableScreener>
+      <param.Verbose>false</param.Verbose>
+      <param.GenerateReport>true</param.GenerateReport>
+      <param.GenerateCodeMetricsReport>true</param.GenerateCodeMetricsReport>
+      <param.GenerateCodeReplacementReport>false</param.GenerateCodeReplacementReport>
+      <param.LaunchReport>true</param.LaunchReport>
+      <param.CustomSourceCode />
+      <param.CustomHeaderCode />
+      <param.CustomInitializer />
+      <param.CustomTerminator />
+      <param.CustomInclude />
+      <param.CustomSource />
+      <param.CustomLibrary />
+      <param.PostCodeGenCommand />
+      <param.CodeReplacementLibrary>C89/C90 (ANSI)</param.CodeReplacementLibrary>
+      <param.SameHardware>true</param.SameHardware>
+      <param.HardwareVendor.Production>ARM Compatible</param.HardwareVendor.Production>
+      <param.HardwareType.Production>ARM Cortex</param.HardwareType.Production>
+      <var.instance.enabled.Production>true</var.instance.enabled.Production>
+      <param.HardwareSizeChar.Production>8</param.HardwareSizeChar.Production>
+      <param.HardwareSizeShort.Production>16</param.HardwareSizeShort.Production>
+      <param.HardwareSizeInt.Production>32</param.HardwareSizeInt.Production>
+      <param.HardwareSizeLong.Production>32</param.HardwareSizeLong.Production>
+      <param.HardwareSizeLongLong.Production>64</param.HardwareSizeLongLong.Production>
+      <param.HardwareSizeFloat.Production>32</param.HardwareSizeFloat.Production>
+      <param.HardwareSizeDouble.Production>64</param.HardwareSizeDouble.Production>
+      <param.HardwareSizeWord.Production>32</param.HardwareSizeWord.Production>
+      <param.HardwareSizePointer.Production>32</param.HardwareSizePointer.Production>
+      <param.HardwareEndianness.Production>option.HardwareEndianness.Little</param.HardwareEndianness.Production>
+      <param.HardwareArithmeticRightShift.Production>true</param.HardwareArithmeticRightShift.Production>
+      <param.HardwareLongLongMode.Production>false</param.HardwareLongLongMode.Production>
+      <param.HardwareAtomicIntegerSize.Production>option.HardwareAtomicIntegerSize.Long</param.HardwareAtomicIntegerSize.Production>
+      <param.HardwareAtomicFloatSize.Production>option.HardwareAtomicFloatSize.Double</param.HardwareAtomicFloatSize.Production>
+      <param.HardwareDivisionRounding.Production>option.HardwareDivisionRounding.Undefined</param.HardwareDivisionRounding.Production>
+      <param.HardwareVendor.Target>Generic</param.HardwareVendor.Target>
+      <param.HardwareType.Target>MATLAB Host Computer</param.HardwareType.Target>
+      <var.instance.enabled.Target>false</var.instance.enabled.Target>
+      <param.HardwareSizeChar.Target>8</param.HardwareSizeChar.Target>
+      <param.HardwareSizeShort.Target>16</param.HardwareSizeShort.Target>
+      <param.HardwareSizeInt.Target>32</param.HardwareSizeInt.Target>
+      <param.HardwareSizeLong.Target>64</param.HardwareSizeLong.Target>
+      <param.HardwareSizeLongLong.Target>64</param.HardwareSizeLongLong.Target>
+      <param.HardwareSizeFloat.Target>32</param.HardwareSizeFloat.Target>
+      <param.HardwareSizeDouble.Target>64</param.HardwareSizeDouble.Target>
+      <param.HardwareSizeWord.Target>64</param.HardwareSizeWord.Target>
+      <param.HardwareSizePointer.Target>64</param.HardwareSizePointer.Target>
+      <param.HardwareEndianness.Target>option.HardwareEndianness.Little</param.HardwareEndianness.Target>
+      <param.HardwareArithmeticRightShift.Target>true</param.HardwareArithmeticRightShift.Target>
+      <param.HardwareLongLongMode.Target>true</param.HardwareLongLongMode.Target>
+      <param.HardwareAtomicIntegerSize.Target>option.HardwareAtomicIntegerSize.Char</param.HardwareAtomicIntegerSize.Target>
+      <param.HardwareAtomicFloatSize.Target>option.HardwareAtomicFloatSize.None</param.HardwareAtomicFloatSize.Target>
+      <param.HardwareDivisionRounding.Target>option.HardwareDivisionRounding.Zero</param.HardwareDivisionRounding.Target>
+      <param.Toolchain>Automatically locate an installed toolchain</param.Toolchain>
+      <param.BuildConfiguration>Faster Builds</param.BuildConfiguration>
+      <param.CustomToolchainOptions />
+      <param.ConstantFoldingTimeout>40000</param.ConstantFoldingTimeout>
+      <param.RecursionLimit>100</param.RecursionLimit>
+      <param.IncludeTerminateFcn>true</param.IncludeTerminateFcn>
+      <param.TargetLang>option.TargetLang.C</param.TargetLang>
+      <param.CCompilerOptimization>option.CCompilerOptimization.On</param.CCompilerOptimization>
+      <param.CCompilerCustomOptimizations />
+      <param.GenerateMakefile>true</param.GenerateMakefile>
+      <param.BuildToolEnable>false</param.BuildToolEnable>
+      <param.MakeCommand>make_rtw</param.MakeCommand>
+      <param.TemplateMakefile>default_tmf</param.TemplateMakefile>
+      <param.BuildToolConfiguration />
+      <param.InlineThreshold>10</param.InlineThreshold>
+      <param.InlineThresholdMax>200</param.InlineThresholdMax>
+      <param.InlineStackLimit>4000</param.InlineStackLimit>
+      <param.EnableMemcpy>true</param.EnableMemcpy>
+      <param.MemcpyThreshold>64</param.MemcpyThreshold>
+      <param.EnableOpenMP>true</param.EnableOpenMP>
+      <param.InitFltsAndDblsToZero>true</param.InitFltsAndDblsToZero>
+      <param.PassStructByReference>false</param.PassStructByReference>
+      <param.UseECoderFeatures>true</param.UseECoderFeatures>
+      <unset>
+        <param.WorkingFolder />
+        <param.SpecifiedWorkingFolder />
+        <param.SearchPaths />
+        <param.SaturateOnIntegerOverflow />
+        <param.PurelyIntegerCode />
+        <param.DynamicMemoryAllocation />
+        <param.DynamicMemoryAllocationThreshold />
+        <param.MultiInstanceCode />
+        <param.GenerateComments />
+        <param.MATLABFcnDesc />
+        <param.DataTypeReplacement />
+        <param.ConvertIfToSwitch />
+        <param.PreserveExternInFcnDecls />
+        <param.ParenthesesLevel />
+        <param.MaxIdLength />
+        <param.CustomSymbolStrGlobalVar />
+        <param.CustomSymbolStrType />
+        <param.CustomSymbolStrField />
+        <param.CustomSymbolStrFcn />
+        <param.CustomSymbolStrTmpVar />
+        <param.CustomSymbolStrMacro />
+        <param.CustomSymbolStrEMXArray />
+        <param.CustomSymbolStrEMXArrayFcn />
+        <param.ReservedNameArray />
+        <param.EnableScreener />
+        <param.Verbose />
+        <param.GenerateReport />
+        <param.GenerateCodeMetricsReport />
+        <param.GenerateCodeReplacementReport />
+        <param.CustomInclude />
+        <param.CustomSource />
+        <param.CustomLibrary />
+        <param.SameHardware />
+        <var.instance.enabled.Production />
+        <param.HardwareSizeChar.Production />
+        <param.HardwareSizeShort.Production />
+        <param.HardwareSizeInt.Production />
+        <param.HardwareSizeLong.Production />
+        <param.HardwareSizeLongLong.Production />
+        <param.HardwareSizeFloat.Production />
+        <param.HardwareSizeDouble.Production />
+        <param.HardwareSizeWord.Production />
+        <param.HardwareSizePointer.Production />
+        <param.HardwareEndianness.Production />
+        <param.HardwareLongLongMode.Production />
+        <param.HardwareDivisionRounding.Production />
+        <var.instance.enabled.Target />
+        <param.HardwareSizeChar.Target />
+        <param.HardwareSizeShort.Target />
+        <param.HardwareSizeInt.Target />
+        <param.HardwareSizeLongLong.Target />
+        <param.HardwareSizeFloat.Target />
+        <param.HardwareSizeDouble.Target />
+        <param.HardwareEndianness.Target />
+        <param.HardwareAtomicFloatSize.Target />
+        <param.CustomToolchainOptions />
+        <param.ConstantFoldingTimeout />
+        <param.RecursionLimit />
+        <param.IncludeTerminateFcn />
+        <param.TargetLang />
+        <param.CCompilerCustomOptimizations />
+        <param.GenerateMakefile />
+        <param.BuildToolEnable />
+        <param.MakeCommand />
+        <param.TemplateMakefile />
+        <param.BuildToolConfiguration />
+        <param.InlineThreshold />
+        <param.InlineThresholdMax />
+        <param.InlineStackLimit />
+        <param.EnableMemcpy />
+        <param.MemcpyThreshold />
+        <param.EnableOpenMP />
+        <param.InitFltsAndDblsToZero />
+        <param.UseECoderFeatures />
+      </unset>
+    </profile>
+    <param.outputfile>/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</param.outputfile>
+    <param.version>R2012a</param.version>
+    <param.HasECoderFeatures>true</param.HasECoderFeatures>
+    <param.mex.mainhtml>t:\private\desktop-dinfk-xp\Attitude_Kalmanfilter\codegen\mex\attitudeKalmanfilter\html\index.html</param.mex.mainhtml>
+    <param.grt.mainhtml>/home/thomasgubler/src/Firmware/src/modules/attitude_estimator_ekf/codegen/html/index.html</param.grt.mainhtml>
+    <param.CallGeneratedCodeFromTest>true</param.CallGeneratedCodeFromTest>
+    <param.VerificationMode>option.VerificationMode.None</param.VerificationMode>
+    <param.SILDebugging>false</param.SILDebugging>
+    <param.DefaultTestFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.DefaultTestFile>
+    <param.AutoInferDefaultFile>${PROJECT_ROOT}/AttitudeEKF_Test.m</param.AutoInferDefaultFile>
+    <param.AutoInferUseVariableSize>false</param.AutoInferUseVariableSize>
+    <param.AutoInferUseUnboundedSize>false</param.AutoInferUseUnboundedSize>
+    <param.AutoInferVariableSizeThreshold>1024</param.AutoInferVariableSizeThreshold>
+    <param.AutoInferUnboundedSizeThreshold>2048</param.AutoInferUnboundedSizeThreshold>
+    <param.mex.outputfile>AttitudeEKF_mex</param.mex.outputfile>
+    <param.grt.outputfile>AttitudeEKF</param.grt.outputfile>
+    <param.artifact>option.target.artifact.lib</param.artifact>
+    <param.FixedPointTypeProposalMode>option.FixedPointTypeProposalMode.ProposeFractionLengths</param.FixedPointTypeProposalMode>
+    <param.DefaultProposedFixedPointType>numerictype([],16,12)</param.DefaultProposedFixedPointType>
+    <param.MinMaxSafetyMargin>0</param.MinMaxSafetyMargin>
+    <param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
+    <param.LaunchInstrumentationReport>false</param.LaunchInstrumentationReport>
+    <param.OpenInstrumentationReportInBrowser>false</param.OpenInstrumentationReportInBrowser>
+    <param.CreatePrintableInstrumentationReport>false</param.CreatePrintableInstrumentationReport>
+    <param.EnableAutoExtrinsicCalls>true</param.EnableAutoExtrinsicCalls>
+    <param.UsePreconditions>false</param.UsePreconditions>
+    <param.FeatureFlags />
+    <param.FixedPointMode>option.FixedPointMode.None</param.FixedPointMode>
+    <param.AutoScaleLoopIndexVariables>false</param.AutoScaleLoopIndexVariables>
+    <param.ComputedFixedPointData />
+    <param.UserFixedPointData />
+    <param.DefaultWordLength>16</param.DefaultWordLength>
+    <param.DefaultFractionLength>4</param.DefaultFractionLength>
+    <param.FixedPointSafetyMargin>0</param.FixedPointSafetyMargin>
+    <param.FixedPointFimath>fimath('RoundingMethod', 'Floor', 'OverflowAction', 'Wrap', 'ProductMode', 'FullPrecision', 'MaxProductWordLength', 128, 'SumMode', 'FullPrecision', 'MaxSumWordLength', 128)</param.FixedPointFimath>
+    <param.FixedPointTypeSource>option.FixedPointTypeSource.SimAndDerived</param.FixedPointTypeSource>
+    <param.StaticAnalysisTimeout />
+    <param.StaticAnalysisGlobalRangesOnly>false</param.StaticAnalysisGlobalRangesOnly>
+    <param.LogAllIOValues>false</param.LogAllIOValues>
+    <param.LogHistogram>false</param.LogHistogram>
+    <param.ShowCoverage>true</param.ShowCoverage>
+    <param.ExcludedFixedPointVerificationFiles />
+    <param.ExcludedFixedPointSimulationFiles />
+    <param.InstrumentedBuildChecksum />
+    <param.FixedPointStaticAnalysisChecksum />
+    <param.InstrumentedMexFile />
+    <param.FixedPointValidationChecksum />
+    <param.FixedPointSourceCodeChecksum />
+    <param.FixedPointFunctionReplacements />
+    <param.OptimizeWholeNumbers>true</param.OptimizeWholeNumbers>
+    <param.GeneratedFixedPointFileSuffix>_fixpt</param.GeneratedFixedPointFileSuffix>
+    <param.DefaultFixedPointSignedness>option.DefaultFixedPointSignedness.Automatic</param.DefaultFixedPointSignedness>
+    <unset>
+      <param.outputfile />
+      <param.version />
+      <param.HasECoderFeatures />
+      <param.CallGeneratedCodeFromTest />
+      <param.VerificationMode />
+      <param.SILDebugging />
+      <param.AutoInferUseVariableSize />
+      <param.AutoInferUseUnboundedSize />
+      <param.AutoInferVariableSizeThreshold />
+      <param.AutoInferUnboundedSizeThreshold />
+      <param.mex.outputfile />
+      <param.grt.outputfile />
+      <param.FixedPointTypeProposalMode />
+      <param.DefaultProposedFixedPointType />
+      <param.MinMaxSafetyMargin />
+      <param.OptimizeWholeNumbers />
+      <param.LaunchInstrumentationReport />
+      <param.OpenInstrumentationReportInBrowser />
+      <param.CreatePrintableInstrumentationReport />
+      <param.EnableAutoExtrinsicCalls />
+      <param.UsePreconditions />
+      <param.FeatureFlags />
+      <param.FixedPointMode />
+      <param.AutoScaleLoopIndexVariables />
+      <param.ComputedFixedPointData />
+      <param.UserFixedPointData />
+      <param.DefaultWordLength />
+      <param.DefaultFractionLength />
+      <param.FixedPointSafetyMargin />
+      <param.FixedPointFimath />
+      <param.FixedPointTypeSource />
+      <param.StaticAnalysisTimeout />
+      <param.StaticAnalysisGlobalRangesOnly />
+      <param.LogAllIOValues />
+      <param.LogHistogram />
+      <param.ShowCoverage />
+      <param.ExcludedFixedPointVerificationFiles />
+      <param.ExcludedFixedPointSimulationFiles />
+      <param.InstrumentedBuildChecksum />
+      <param.FixedPointStaticAnalysisChecksum />
+      <param.InstrumentedMexFile />
+      <param.FixedPointValidationChecksum />
+      <param.FixedPointSourceCodeChecksum />
+      <param.FixedPointFunctionReplacements />
+      <param.GeneratedFixedPointFileSuffix />
+      <param.DefaultFixedPointSignedness />
+    </unset>
+    <fileset.entrypoints>
+      <file value="${PROJECT_ROOT}/AttitudeEKF.m" custom-data-expanded="true">
+        <Inputs fileName="AttitudeEKF.m" functionName="AttitudeEKF">
+          <Input Name="approx_prediction">
+            <Class>uint8</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="use_inertia_matrix">
+            <Class>uint8</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="zFlag">
+            <Class>uint8</Class>
+            <UserDefined>false</UserDefined>
+            <Size>3 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="dt">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="z">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>9 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="q_rotSpeed">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="q_rotAcc">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="q_acc">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="q_mag">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="r_gyro">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="r_accel">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="r_mag">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>1 x 1</Size>
+            <Complex>false</Complex>
+          </Input>
+          <Input Name="J">
+            <Class>single</Class>
+            <UserDefined>false</UserDefined>
+            <Size>3 x 3</Size>
+            <Complex>false</Complex>
+          </Input>
+        </Inputs>
+      </file>
+    </fileset.entrypoints>
+    <fileset.testbench>
+      <file>${PROJECT_ROOT}/AttitudeEKF_Test.m</file>
+    </fileset.testbench>
+    <fileset.package />
+    <build-deliverables>
+      <file name="AttitudeEKF.a" location="${MATLAB_ROOT}/bin/codegen/codegen/lib/AttitudeEKF" optional="false">/opt/matlab/r2013b/bin/codegen/codegen/lib/AttitudeEKF/AttitudeEKF.a</file>
+    </build-deliverables>
+    <workflow />
+    <matlab>
+      <root>/opt/matlab/r2013b</root>
+      <toolboxes>
+        <toolbox name="fixedpoint" />
+      </toolboxes>
+    </matlab>
+    <platform>
+      <unix>true</unix>
+      <mac>false</mac>
+      <windows>false</windows>
+      <win2k>false</win2k>
+      <winxp>false</winxp>
+      <vista>false</vista>
+      <linux>true</linux>
+      <solaris>false</solaris>
+      <osver>3.16.1-1-ARCH</osver>
+      <os32>false</os32>
+      <os64>true</os64>
+      <arch>glnxa64</arch>
+      <matlab>true</matlab>
+    </platform>
+  </configuration>
+</deployment-project>
+

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp

@@ -38,6 +38,7 @@
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
 #include <nuttx/config.h>
@@ -74,8 +75,7 @@
 #ifdef __cplusplus
 extern "C" {
 #endif
-#include "codegen/attitudeKalmanfilter_initialize.h"
-#include "codegen/attitudeKalmanfilter.h"
+#include "codegen/AttitudeEKF.h"
 #include "attitude_estimator_ekf_params.h"
 #ifdef __cplusplus
 }
@@ -132,7 +132,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
-					      14000,
+					      7200,
 					      attitude_estimator_ekf_thread_main,
 					      (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
@@ -206,10 +206,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			      0,  0,  1.f
 			     };		/**< init: identity matrix */
 
+	float debugOutput[4] = { 0.0f };
+
 	int overloadcounter = 19;
 
 	/* Initialize filter */
-	attitudeKalmanfilter_initialize();
+	AttitudeEKF_initialize();
 
 	/* store start time to guard against too slow update rates */
 	uint64_t last_run = hrt_absolute_time();
@@ -273,9 +275,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* keep track of sensor updates */
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
-	struct attitude_estimator_ekf_params ekf_params;
+	struct attitude_estimator_ekf_params ekf_params = { 0 };
 
-	struct attitude_estimator_ekf_param_handles ekf_param_handles;
+	struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 };
 
 	/* initialize parameter handles */
 	parameters_init(&ekf_param_handles);
@@ -504,8 +506,25 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
-							     euler, Rot_matrix, x_aposteriori, P_aposteriori);
+					/* Call the estimator */
+					AttitudeEKF(false, // approx_prediction
+							(unsigned char)ekf_params.use_moment_inertia,
+							update_vect,
+							dt,
+							z_k,
+							ekf_params.q[0], // q_rotSpeed,
+							ekf_params.q[1], // q_rotAcc
+							ekf_params.q[2], // q_acc
+							ekf_params.q[3], // q_mag
+							ekf_params.r[0], // r_gyro
+							ekf_params.r[1], // r_accel
+							ekf_params.r[2], // r_mag
+							ekf_params.moment_inertia_J,
+							x_aposteriori,
+							P_aposteriori,
+							Rot_matrix,
+							euler,
+							debugOutput);
 
 					/* swap values for next iteration, check for fatal inputs */
 					if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) {

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -44,28 +44,96 @@
 
 /* Extended Kalman Filter covariances */
 
-/* gyro process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
-/* gyro offsets process noise */
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
-PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f);
 
-/* gyro measurement noise */
+/**
+ * Body angular rate process noise
+ *
+ * @group attitude_ekf
+ */
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f);
+
+/**
+ * Body angular acceleration process noise
+ *
+ * @group attitude_ekf
+ */
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f);
+
+/**
+ * Acceleration process noise
+ *
+ * @group attitude_ekf
+ */
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f);
+
+/**
+ * Magnet field vector process noise
+ *
+ * @group attitude_ekf
+ */
+PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f);
+
+/**
+ * Gyro measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R0, 0.0008f);
-/* accel measurement noise */
+
+/**
+ * Accel measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
-/* mag measurement noise */
+
+/**
+ * Mag measurement noise
+ *
+ * @group attitude_ekf
+ */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
-/* offset estimation - UNUSED */
-PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f);
 
 /* magnetic declination, in degrees */
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
 
 PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);
 
+/**
+ * Moment of inertia matrix diagonal entry (1, 1)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J11, 0.0018);
+
+/**
+ * Moment of inertia matrix diagonal entry (2, 2)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J22, 0.0018);
+
+/**
+ * Moment of inertia matrix diagonal entry (3, 3)
+ *
+ * @group attitude_ekf
+ * @unit kg*m^2
+ */
+PARAM_DEFINE_FLOAT(ATT_J33, 0.0037);
+
+/**
+ * Moment of inertia enabled in estimator
+ *
+ * If set to != 0 the moment of inertia will be used in the estimator
+ *
+ * @group attitude_ekf
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(ATT_J_EN, 0);
+
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
 	/* PID parameters */
@@ -73,17 +141,20 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	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->q4 	=	param_find("EKF_ATT_V3_Q4");
 
 	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->r3 	=	param_find("EKF_ATT_V4_R3");
 
 	h->mag_decl   =	param_find("ATT_MAG_DECL");
 
 	h->acc_comp   =	param_find("ATT_ACC_COMP");
 
+	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;
 }
 
@@ -93,17 +164,20 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->q1, &(p->q[1]));
 	param_get(h->q2, &(p->q[2]));
 	param_get(h->q3, &(p->q[3]));
-	param_get(h->q4, &(p->q[4]));
 
 	param_get(h->r0, &(p->r[0]));
 	param_get(h->r1, &(p->r[1]));
 	param_get(h->r2, &(p->r[2]));
-	param_get(h->r3, &(p->r[3]));
 
 	param_get(h->mag_decl, &(p->mag_decl));
 	p->mag_decl *= M_PI_F / 180.0f;
 
 	param_get(h->acc_comp, &(p->acc_comp));
 
+	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/attitude_estimator_ekf_params.h

@@ -42,8 +42,10 @@
 #include <systemlib/param/param.h>
 
 struct attitude_estimator_ekf_params {
-	float r[9];
-	float q[12];
+	float r[3];
+	float q[4];
+	float moment_inertia_J[9];
+	int32_t use_moment_inertia;
 	float roll_off;
 	float pitch_off;
 	float yaw_off;
@@ -52,8 +54,10 @@ struct attitude_estimator_ekf_params {
 };
 
 struct attitude_estimator_ekf_param_handles {
-	param_t r0, r1, r2, r3;
-	param_t q0, q1, q2, q3, q4;
+	param_t r0, r1, r2;
+	param_t q0, q1, q2, q3;
+	param_t moment_inertia_J[3]; /**< diagonal entries of the matrix */
+	param_t use_moment_inertia;
 	param_t mag_decl;
 	param_t acc_comp;
 };

src/modules/attitude_estimator_ekf/codegen/AttitudeEKF.h

@@ -0,0 +1,26 @@
+/*
+ * AttitudeEKF.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __ATTITUDEEKF_H__
+#define __ATTITUDEEKF_H__
+/* Include files */
+#include <math.h>
+#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "rtwtypes.h"
+#include "AttitudeEKF_types.h"
+
+/* Function Declarations */
+extern void AttitudeEKF(unsigned char approx_prediction, unsigned char use_inertia_matrix, const unsigned char zFlag[3], float dt, const float z[9], float q_rotSpeed, float q_rotAcc, float q_acc, float q_mag, float r_gyro, float r_accel, float r_mag, const float J[9], float xa_apo[12], float Pa_apo[144], float Rot_matrix[9], float eulerAngles[3], float debugOutput[4]);
+extern void AttitudeEKF_initialize(void);
+extern void AttitudeEKF_terminate(void);
+#endif
+/* End of code generation (AttitudeEKF.h) */

src/modules/attitude_estimator_ekf/codegen/AttitudeEKF_types.h

@@ -0,0 +1,17 @@
+/*
+ * AttitudeEKF_types.h
+ *
+ * Code generation for function 'AttitudeEKF'
+ *
+ * C source code generated on: Thu Aug 21 11:17:28 2014
+ *
+ */
+
+#ifndef __ATTITUDEEKF_TYPES_H__
+#define __ATTITUDEEKF_TYPES_H__
+
+/* Include files */
+#include "rtwtypes.h"
+
+#endif
+/* End of code generation (AttitudeEKF_types.h) */

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h

@@ -1,34 +0,0 @@
-/*
- * attitudeKalmanfilter.h
- *
- * Code generation for function 'attitudeKalmanfilter'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_H__
-#define __ATTITUDEKALMANFILTER_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]);
-#endif
-/* End of code generation (attitudeKalmanfilter.h) */

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c

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

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h

@@ -1,34 +0,0 @@
-/*
- * attitudeKalmanfilter_initialize.h
- *
- * Code generation for function 'attitudeKalmanfilter_initialize'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_INITIALIZE_H__
-#define __ATTITUDEKALMANFILTER_INITIALIZE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void attitudeKalmanfilter_initialize(void);
-#endif
-/* End of code generation (attitudeKalmanfilter_initialize.h) */

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c

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

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h

@@ -1,34 +0,0 @@
-/*
- * attitudeKalmanfilter_terminate.h
- *
- * Code generation for function 'attitudeKalmanfilter_terminate'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __ATTITUDEKALMANFILTER_TERMINATE_H__
-#define __ATTITUDEKALMANFILTER_TERMINATE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void attitudeKalmanfilter_terminate(void);
-#endif
-/* End of code generation (attitudeKalmanfilter_terminate.h) */

src/modules/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h

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

src/modules/attitude_estimator_ekf/codegen/cross.c

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

src/modules/attitude_estimator_ekf/codegen/cross.h

@@ -1,34 +0,0 @@
-/*
- * cross.h
- *
- * Code generation for function 'cross'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __CROSS_H__
-#define __CROSS_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void cross(const real32_T a[3], const real32_T b[3], real32_T c[3]);
-#endif
-/* End of code generation (cross.h) */

src/modules/attitude_estimator_ekf/codegen/eye.c

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

src/modules/attitude_estimator_ekf/codegen/eye.h

@@ -1,35 +0,0 @@
-/*
- * eye.h
- *
- * Code generation for function 'eye'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __EYE_H__
-#define __EYE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_eye(real_T I[144]);
-extern void eye(real_T I[9]);
-#endif
-/* End of code generation (eye.h) */

src/modules/attitude_estimator_ekf/codegen/mrdivide.c

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

src/modules/attitude_estimator_ekf/codegen/mrdivide.h

@@ -1,36 +0,0 @@
-/*
- * mrdivide.h
- *
- * Code generation for function 'mrdivide'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __MRDIVIDE_H__
-#define __MRDIVIDE_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern void b_mrdivide(const real32_T A[36], const real32_T B[9], real32_T y[36]);
-extern void c_mrdivide(const real32_T A[72], const real32_T B[36], real32_T y[72]);
-extern void mrdivide(const real32_T A[108], const real32_T B[81], real32_T y[108]);
-#endif
-/* End of code generation (mrdivide.h) */

src/modules/attitude_estimator_ekf/codegen/norm.c

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

src/modules/attitude_estimator_ekf/codegen/norm.h

@@ -1,34 +0,0 @@
-/*
- * norm.h
- *
- * Code generation for function 'norm'
- *
- * C source code generated on: Sat Jan 19 15:25:29 2013
- *
- */
-
-#ifndef __NORM_H__
-#define __NORM_H__
-/* Include files */
-#include <math.h>
-#include <stddef.h>
-#include <stdlib.h>
-#include <string.h>
-#include "rt_defines.h"
-#include "rt_nonfinite.h"
-
-#include "rtwtypes.h"
-#include "attitudeKalmanfilter_types.h"
-
-/* Type Definitions */
-
-/* Named Constants */
-
-/* Variable Declarations */
-
-/* Variable Definitions */
-
-/* Function Declarations */
-extern real32_T norm(const real32_T x[3]);
-#endif
-/* End of code generation (norm.h) */

src/modules/attitude_estimator_ekf/codegen/rdivide.c

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

src/modules/attitude_estimator_ekf/codegen/rdivide.h

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

src/modules/attitude_estimator_ekf/codegen/rtGetInf.c

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

src/modules/attitude_estimator_ekf/codegen/rtGetInf.h

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

src/modules/attitude_estimator_ekf/codegen/rtGetNaN.c

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

src/modules/attitude_estimator_ekf/codegen/rtGetNaN.h

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

src/modules/attitude_estimator_ekf/codegen/rt_defines.h

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

src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.c

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

src/modules/attitude_estimator_ekf/codegen/rt_nonfinite.h

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

src/modules/attitude_estimator_ekf/codegen/rtwtypes.h

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

src/modules/attitude_estimator_ekf/module.mk

@@ -39,16 +39,6 @@ MODULE_COMMAND	 = attitude_estimator_ekf
 
 SRCS		 = attitude_estimator_ekf_main.cpp \
 		   attitude_estimator_ekf_params.c \
-		   codegen/eye.c \
-		   codegen/attitudeKalmanfilter.c \
-		   codegen/mrdivide.c \
-		   codegen/rdivide.c \
-		   codegen/attitudeKalmanfilter_initialize.c \
-		   codegen/attitudeKalmanfilter_terminate.c \
-		   codegen/rt_nonfinite.c \
-		   codegen/rtGetInf.c \
-		   codegen/rtGetNaN.c \
-		   codegen/norm.c \
-		   codegen/cross.c
+		   codegen/AttitudeEKF.c
 
 MODULE_STACKSIZE = 1200

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -83,7 +83,7 @@
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 #include <mavlink/mavlink_log.h>
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 #include "estimator_23states.h"
 

src/modules/fw_att_control/fw_att_control_base.cpp

@@ -0,0 +1,325 @@
+/* Copyright (c) 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.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.cpp
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "fw_att_control_base.h"
+#include <math.h>
+#include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
+
+using namespace std;
+
+FixedwingAttitudeControlBase::FixedwingAttitudeControlBase() :
+
+		_task_should_exit(false), _task_running(false), _control_task(-1),
+
+		/* performance counters */
+		_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")), _nonfinite_input_perf(
+				perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(
+				perf_alloc(PC_COUNT, "fw att control nonfinite output")),
+		/* states */
+		_setpoint_valid(false), _debug(false) {
+	/* safely initialize structs */
+	_att = {};
+	_att_sp = {};
+	_manual = {};
+	_airspeed = {};
+	_vcontrol_mode = {};
+	_actuators = {};
+	_actuators_airframe = {};
+	_global_pos = {};
+	_vehicle_status = {};
+
+}
+
+FixedwingAttitudeControlBase::~FixedwingAttitudeControlBase() {
+
+}
+
+void FixedwingAttitudeControlBase::control_attitude() {
+	bool lock_integrator = false;
+	static int loop_counter = 0;
+	/* scale around tuning airspeed */
+
+	float airspeed;
+
+	/* if airspeed is not updating, we assume the normal average speed */
+	if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s)
+			|| hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
+		airspeed = _parameters.airspeed_trim;
+		if (nonfinite) {
+			perf_count(_nonfinite_input_perf);
+		}
+	} else {
+		/* prevent numerical drama by requiring 0.5 m/s minimal speed */
+		airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
+	}
+
+	/*
+	 * For scaling our actuators using anything less than the min (close to stall)
+	 * speed doesn't make any sense - its the strongest reasonable deflection we
+	 * want to do in flight and its the baseline a human pilot would choose.
+	 *
+	 * Forcing the scaling to this value allows reasonable handheld tests.
+	 */
+
+	float airspeed_scaling = _parameters.airspeed_trim
+			/ ((airspeed < _parameters.airspeed_min) ?
+					_parameters.airspeed_min : airspeed);
+
+	float roll_sp = _parameters.rollsp_offset_rad;
+	float pitch_sp = _parameters.pitchsp_offset_rad;
+	float throttle_sp = 0.0f;
+
+	if (_vcontrol_mode.flag_control_velocity_enabled
+			|| _vcontrol_mode.flag_control_position_enabled) {
+		/* read in attitude setpoint from attitude setpoint uorb topic */
+		roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
+		pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+		throttle_sp = _att_sp.thrust;
+
+		/* reset integrals where needed */
+		if (_att_sp.roll_reset_integral) {
+			_roll_ctrl.reset_integrator();
+		}
+		if (_att_sp.pitch_reset_integral) {
+			_pitch_ctrl.reset_integrator();
+		}
+		if (_att_sp.yaw_reset_integral) {
+			_yaw_ctrl.reset_integrator();
+		}
+	} else {
+		/*
+		 * Scale down roll and pitch as the setpoints are radians
+		 * and a typical remote can only do around 45 degrees, the mapping is
+		 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
+		 *
+		 * With this mapping the stick angle is a 1:1 representation of
+		 * the commanded attitude.
+		 *
+		 * The trim gets subtracted here from the manual setpoint to get
+		 * the intended attitude setpoint. Later, after the rate control step the
+		 * trim is added again to the control signal.
+		 */
+		roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+				+ _parameters.rollsp_offset_rad;
+		pitch_sp = -(_manual.x * _parameters.man_pitch_max
+				- _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
+		throttle_sp = _manual.z;
+		_actuators.control[4] = _manual.flaps;
+
+		/*
+		 * in manual mode no external source should / does emit attitude setpoints.
+		 * emit the manual setpoint here to allow attitude controller tuning
+		 * in attitude control mode.
+		 */
+		struct vehicle_attitude_setpoint_s att_sp;
+		att_sp.timestamp = hrt_absolute_time();
+		att_sp.roll_body = roll_sp;
+		att_sp.pitch_body = pitch_sp;
+		att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
+		att_sp.thrust = throttle_sp;
+
+	}
+
+	/* If the aircraft is on ground reset the integrators */
+	if (_vehicle_status.condition_landed) {
+		_roll_ctrl.reset_integrator();
+		_pitch_ctrl.reset_integrator();
+		_yaw_ctrl.reset_integrator();
+	}
+
+	/* Prepare speed_body_u and speed_body_w */
+	float speed_body_u = 0.0f;
+	float speed_body_v = 0.0f;
+	float speed_body_w = 0.0f;
+	if (_att.R_valid) {
+		speed_body_u = _att.R[0][0] * _global_pos.vel_n
+				+ _att.R[1][0] * _global_pos.vel_e
+				+ _att.R[2][0] * _global_pos.vel_d;
+		speed_body_v = _att.R[0][1] * _global_pos.vel_n
+				+ _att.R[1][1] * _global_pos.vel_e
+				+ _att.R[2][1] * _global_pos.vel_d;
+		speed_body_w = _att.R[0][2] * _global_pos.vel_n
+				+ _att.R[1][2] * _global_pos.vel_e
+				+ _att.R[2][2] * _global_pos.vel_d;
+	} else {
+		if (_debug && loop_counter % 10 == 0) {
+			warnx("Did not get a valid R\n");
+		}
+	}
+
+	/* Run attitude controllers */
+	if (isfinite(roll_sp) && isfinite(pitch_sp)) {
+		_roll_ctrl.control_attitude(roll_sp, _att.roll);
+		_pitch_ctrl.control_attitude(pitch_sp, _att.roll, _att.pitch, airspeed);
+		_yaw_ctrl.control_attitude(_att.roll, _att.pitch, speed_body_u,
+				speed_body_v, speed_body_w, _roll_ctrl.get_desired_rate(),
+				_pitch_ctrl.get_desired_rate()); //runs last, because is depending on output of roll and pitch attitude
+
+		/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
+		float roll_u = _roll_ctrl.control_bodyrate(_att.pitch, _att.rollspeed,
+				_att.yawspeed, _yaw_ctrl.get_desired_rate(),
+				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
+				airspeed_scaling, lock_integrator);
+		_actuators.control[0] =
+				(isfinite(roll_u)) ?
+						roll_u + _parameters.trim_roll : _parameters.trim_roll;
+		if (!isfinite(roll_u)) {
+			_roll_ctrl.reset_integrator();
+			perf_count(_nonfinite_output_perf);
+
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("roll_u %.4f", (double) roll_u);
+			}
+		}
+
+		float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
+				_att.pitchspeed, _att.yawspeed, _yaw_ctrl.get_desired_rate(),
+				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
+				airspeed_scaling, lock_integrator);
+		_actuators.control[1] =
+				(isfinite(pitch_u)) ?
+						pitch_u + _parameters.trim_pitch :
+						_parameters.trim_pitch;
+		if (!isfinite(pitch_u)) {
+			_pitch_ctrl.reset_integrator();
+			perf_count(_nonfinite_output_perf);
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
+						" airspeed %.4f, airspeed_scaling %.4f,"
+						" roll_sp %.4f, pitch_sp %.4f,"
+						" _roll_ctrl.get_desired_rate() %.4f,"
+						" _pitch_ctrl.get_desired_rate() %.4f"
+						" att_sp.roll_body %.4f", (double) pitch_u,
+						(double) _yaw_ctrl.get_desired_rate(),
+						(double) airspeed, (double) airspeed_scaling,
+						(double) roll_sp, (double) pitch_sp,
+						(double) _roll_ctrl.get_desired_rate(),
+						(double) _pitch_ctrl.get_desired_rate(),
+						(double) _att_sp.roll_body);
+			}
+		}
+
+		float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
+				_att.pitchspeed, _att.yawspeed, _pitch_ctrl.get_desired_rate(),
+				_parameters.airspeed_min, _parameters.airspeed_max, airspeed,
+				airspeed_scaling, lock_integrator);
+		_actuators.control[2] =
+				(isfinite(yaw_u)) ?
+						yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+		if (!isfinite(yaw_u)) {
+			_yaw_ctrl.reset_integrator();
+			perf_count(_nonfinite_output_perf);
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("yaw_u %.4f", (double) yaw_u);
+			}
+		}
+
+		/* throttle passed through */
+		_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+		if (!isfinite(throttle_sp)) {
+			if (_debug && loop_counter % 10 == 0) {
+				warnx("throttle_sp %.4f", (double) throttle_sp);
+			}
+		}
+	} else {
+		perf_count(_nonfinite_input_perf);
+		if (_debug && loop_counter % 10 == 0) {
+			warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f",
+					(double) roll_sp, (double) pitch_sp);
+		}
+	}
+
+}
+
+void FixedwingAttitudeControlBase::set_attitude(const Eigen::Quaternion<double> attitude) {
+	// watch out, still need to see where we modify attitude for the tailsitter case
+	math::Quaternion quat;
+	quat(0) = (float)attitude.w();
+	quat(1) = (float)attitude.x();
+	quat(2) = (float)attitude.y();
+	quat(3) = (float)attitude.z();
+
+	_att.q[0] = quat(0);
+	_att.q[1] = quat(1);
+	_att.q[2] = quat(2);
+	_att.q[3] = quat(3);
+
+	math::Matrix<3,3> Rot = quat.to_dcm();
+	_att.R[0][0] = Rot(0,0);
+	_att.R[1][0] = Rot(1,0);
+	_att.R[2][0] = Rot(2,0);
+	_att.R[0][1] = Rot(0,1);
+	_att.R[1][1] = Rot(1,1);
+	_att.R[2][1] = Rot(2,1);
+	_att.R[0][2] = Rot(0,2);
+	_att.R[1][2] = Rot(1,2);
+	_att.R[2][2] = Rot(2,2);
+
+	_att.R_valid = true;
+}
+void FixedwingAttitudeControlBase::set_attitude_rates(const Eigen::Vector3d& angular_rate) {
+	_att.rollspeed  = angular_rate(0);
+	_att.pitchspeed = angular_rate(1);
+	_att.yawspeed   = angular_rate(2);
+}
+void FixedwingAttitudeControlBase::set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference) {
+	_att_sp.roll_body  = control_attitude_thrust_reference(0);
+	_att_sp.pitch_body = control_attitude_thrust_reference(1);
+	_att_sp.yaw_body   = control_attitude_thrust_reference(2);
+	_att_sp.thrust     = (control_attitude_thrust_reference(3) -30)*(-1)/30;
+
+	// setup rotation matrix
+	math::Matrix<3,3> Rot_sp;
+	Rot_sp.from_euler(_att_sp.roll_body,_att_sp.pitch_body,_att_sp.yaw_body);
+	_att_sp.R_body[0][0] = Rot_sp(0,0);
+	_att_sp.R_body[1][0] = Rot_sp(1,0);
+	_att_sp.R_body[2][0] = Rot_sp(2,0);
+	_att_sp.R_body[0][1] = Rot_sp(0,1);
+	_att_sp.R_body[1][1] = Rot_sp(1,1);
+	_att_sp.R_body[2][1] = Rot_sp(2,1);
+	_att_sp.R_body[0][2] = Rot_sp(0,2);
+	_att_sp.R_body[1][2] = Rot_sp(1,2);
+	_att_sp.R_body[2][2] = Rot_sp(2,2);
+}
+void FixedwingAttitudeControlBase::get_mixer_input(Eigen::Vector4d& motor_inputs) {
+	motor_inputs(0) = _actuators.control[0];
+	motor_inputs(1) = _actuators.control[1];
+	motor_inputs(2) = _actuators.control[2];
+	motor_inputs(3) = _actuators.control[3];
+}

src/modules/fw_att_control/fw_att_control_base.h

@@ -0,0 +1,151 @@
+#ifndef FW_ATT_CONTROL_BASE_H_
+#define FW_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 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.
+*
+****************************************************************************/
+
+/**
+ * @file fw_att_control_base.h
+ * 
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include <ecl/attitude_fw/ecl_pitch_controller.h>
+#include <ecl/attitude_fw/ecl_roll_controller.h>
+#include <ecl/attitude_fw/ecl_yaw_controller.h>
+
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_status.h>
+
+#include <systemlib/perf_counter.h>
+
+class FixedwingAttitudeControlBase
+{
+public:
+	/**
+	 * Constructor
+	 */
+	FixedwingAttitudeControlBase();
+
+	/**
+	 * Destructor
+	 */
+	~FixedwingAttitudeControlBase();
+
+
+protected:
+
+	bool		_task_should_exit;		/**< if true, sensor task should exit */
+	bool		_task_running;			/**< if true, task is running in its mainloop */
+	int		_control_task;			/**< task handle for sensor task */
+
+	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
+	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
+	struct airspeed_s				_airspeed;		/**< airspeed */
+	struct vehicle_control_mode_s			_vcontrol_mode;		/**< vehicle control mode */
+	struct actuator_controls_s			_actuators;		/**< actuator control inputs */
+	struct actuator_controls_s			_actuators_airframe;	/**< actuator control inputs */
+	struct vehicle_global_position_s		_global_pos;		/**< global position */
+	struct vehicle_status_s				_vehicle_status;	/**< vehicle status */
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
+	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
+
+	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
+	bool		_debug;				/**< if set to true, print debug output */
+
+	struct {
+		float tconst;
+		float p_p;
+		float p_d;
+		float p_i;
+		float p_ff;
+		float p_rmax_pos;
+		float p_rmax_neg;
+		float p_integrator_max;
+		float p_roll_feedforward;
+		float r_p;
+		float r_d;
+		float r_i;
+		float r_ff;
+		float r_integrator_max;
+		float r_rmax;
+		float y_p;
+		float y_i;
+		float y_d;
+		float y_ff;
+		float y_roll_feedforward;
+		float y_integrator_max;
+		float y_coordinated_min_speed;
+		float y_rmax;
+
+		float airspeed_min;
+		float airspeed_trim;
+		float airspeed_max;
+
+		float trim_roll;
+		float trim_pitch;
+		float trim_yaw;
+		float rollsp_offset_deg;			/**< Roll Setpoint Offset in deg */
+		float pitchsp_offset_deg;			/**< Pitch Setpoint Offset in deg */
+		float rollsp_offset_rad;			/**< Roll Setpoint Offset in rad */
+		float pitchsp_offset_rad;			/**< Pitch Setpoint Offset in rad */
+		float man_roll_max;						/**< Max Roll in rad */
+		float man_pitch_max;					/**< Max Pitch in rad */
+
+	}		_parameters;			/**< local copies of interesting parameters */
+
+	ECL_RollController				_roll_ctrl;
+	ECL_PitchController				_pitch_ctrl;
+	ECL_YawController				_yaw_ctrl;
+
+	void control_attitude();
+
+	// setters and getters for interface with euroc-gazebo simulator
+	void set_attitude(const Eigen::Quaternion<double> attitude);
+	void set_attitude_rates(const Eigen::Vector3d& angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d& control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d& motor_inputs);
+	
+};
+
+#endif /* FW_ATT_CONTROL_BASE_H_ */

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -75,7 +75,7 @@
 #include <ecl/attitude_fw/ecl_pitch_controller.h>
 #include <ecl/attitude_fw/ecl_roll_controller.h>
 #include <ecl/attitude_fw/ecl_yaw_controller.h>
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 /**
  * Fixedwing attitude control app start / stop handling function

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -77,6 +77,7 @@
 #include <uORB/topics/navigation_capabilities.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>
@@ -90,7 +91,7 @@
 #include <external_lgpl/tecs/tecs.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 static int	_control_task = -1;			/**< task handle for sensor task */
 

src/modules/mavlink/mavlink_receiver.cpp

@@ -778,7 +778,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
 					att_sp.timestamp = hrt_absolute_time();
 					mavlink_quaternion_to_euler(set_attitude_target.q,
 							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
-					mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body);
+					mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
 					att_sp.R_valid = true;
 					att_sp.thrust = set_attitude_target.thrust;
 					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));

src/modules/mc_att_control/mc_att_control_base.cpp

@@ -0,0 +1,311 @@
+/* Copyright (c) 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.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.cpp
+ *
+ * MC Attitude Controller
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "mc_att_control_base.h"
+#include <geo/geo.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
+	_publish_att_sp(false)
+
+{
+	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_actuators, 0, sizeof(_actuators));
+	memset(&_armed, 0, sizeof(_armed));
+
+	_params.att_p.zero();
+	_params.rate_p.zero();
+	_params.rate_i.zero();
+	_params.rate_d.zero();
+	_params.yaw_ff = 0.0f;
+	_params.yaw_rate_max = 0.0f;
+	_params.man_roll_max = 0.0f;
+	_params.man_pitch_max = 0.0f;
+	_params.man_yaw_max = 0.0f;
+	_params.acro_rate_max.zero();
+
+	_rates_prev.zero();
+	_rates_sp.zero();
+	_rates_int.zero();
+	_thrust_sp = 0.0f;
+	_att_control.zero();
+
+	_I.identity();
+}
+
+MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()
+{
+}
+
+void MulticopterAttitudeControlBase::control_attitude(float dt)
+{
+	float yaw_sp_move_rate = 0.0f;
+	_publish_att_sp = false;
+
+	if (_v_control_mode.flag_control_manual_enabled) {
+		/* manual input, set or modify attitude setpoint */
+
+		if (_v_control_mode.flag_control_velocity_enabled
+		    || _v_control_mode.flag_control_climb_rate_enabled) {
+			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
+			vehicle_attitude_setpoint_poll();
+		}
+
+		if (!_v_control_mode.flag_control_climb_rate_enabled) {
+			/* pass throttle directly if not in altitude stabilized mode */
+			_v_att_sp.thrust = _manual_control_sp.z;
+			_publish_att_sp = true;
+		}
+
+		if (!_armed.armed) {
+			/* reset yaw setpoint when disarmed */
+			_reset_yaw_sp = true;
+		}
+
+		/* move yaw setpoint in all modes */
+		if (_v_att_sp.thrust < 0.1f) {
+			// TODO
+			//if (_status.condition_landed) {
+			/* reset yaw setpoint if on ground */
+			//	reset_yaw_sp = true;
+			//}
+		} else {
+			/* move yaw setpoint */
+			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
+			_v_att_sp.yaw_body = _wrap_pi(
+						     _v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
+			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+
+			if (yaw_offs < -yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
+
+			} else if (yaw_offs > yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
+			}
+
+			_v_att_sp.R_valid = false;
+			_publish_att_sp = true;
+		}
+
+		/* reset yaw setpint to current position if needed */
+		if (_reset_yaw_sp) {
+			_reset_yaw_sp = false;
+			_v_att_sp.yaw_body = _v_att.yaw;
+			_v_att_sp.R_valid = false;
+			_publish_att_sp = true;
+		}
+
+		if (!_v_control_mode.flag_control_velocity_enabled) {
+			/* update attitude setpoint if not in position control mode */
+			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp.x
+					       * _params.man_pitch_max;
+			_v_att_sp.R_valid = false;
+			_publish_att_sp = true;
+		}
+
+	} else {
+		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
+		vehicle_attitude_setpoint_poll();
+
+		/* reset yaw setpoint after non-manual control mode */
+		_reset_yaw_sp = true;
+	}
+
+	_thrust_sp = _v_att_sp.thrust;
+
+	/* construct attitude setpoint rotation matrix */
+	math::Matrix<3, 3> R_sp;
+
+	if (_v_att_sp.R_valid) {
+		/* rotation matrix in _att_sp is valid, use it */
+		R_sp.set(&_v_att_sp.R_body[0]);
+
+	} else {
+		/* rotation matrix in _att_sp is not valid, use euler angles instead */
+		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body,
+				_v_att_sp.yaw_body);
+
+		/* copy rotation matrix back to setpoint struct */
+		memcpy(&_v_att_sp.R_body[0], &R_sp.data[0][0],
+		       sizeof(_v_att_sp.R_body));
+		_v_att_sp.R_valid = true;
+	}
+
+	/* rotation matrix for current state */
+	math::Matrix<3, 3> R;
+	R.set(_v_att.R);
+
+	/* all input data is ready, run controller itself */
+
+	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
+	math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
+	math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+
+	/* axis and sin(angle) of desired rotation */
+	math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);
+
+	/* calculate angle error */
+	float e_R_z_sin = e_R.length();
+	float e_R_z_cos = R_z * R_sp_z;
+
+	/* calculate weight for yaw control */
+	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
+
+	/* calculate rotation matrix after roll/pitch only rotation */
+	math::Matrix<3, 3> R_rp;
+
+	if (e_R_z_sin > 0.0f) {
+		/* get axis-angle representation */
+		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
+		math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;
+
+		e_R = e_R_z_axis * e_R_z_angle;
+
+		/* cross product matrix for e_R_axis */
+		math::Matrix<3, 3> e_R_cp;
+		e_R_cp.zero();
+		e_R_cp(0, 1) = -e_R_z_axis(2);
+		e_R_cp(0, 2) = e_R_z_axis(1);
+		e_R_cp(1, 0) = e_R_z_axis(2);
+		e_R_cp(1, 2) = -e_R_z_axis(0);
+		e_R_cp(2, 0) = -e_R_z_axis(1);
+		e_R_cp(2, 1) = e_R_z_axis(0);
+
+		/* rotation matrix for roll/pitch only rotation */
+		R_rp = R
+		       * (_I + e_R_cp * e_R_z_sin
+			  + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+
+	} else {
+		/* zero roll/pitch rotation */
+		R_rp = R;
+	}
+
+	/* R_rp and R_sp has the same Z axis, calculate yaw error */
+	math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
+	math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
+
+	if (e_R_z_cos < 0.0f) {
+		/* for large thrust vector rotations use another rotation method:
+		 * calculate angle and axis for R -> R_sp rotation directly */
+		math::Quaternion q;
+		q.from_dcm(R.transposed() * R_sp);
+		math::Vector < 3 > e_R_d = q.imag();
+		e_R_d.normalize();
+		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
+
+		/* use fusion of Z axis based rotation and direct rotation */
+		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
+		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
+	}
+
+	/* calculate angular rates setpoint */
+	_rates_sp = _params.att_p.emult(e_R);
+
+	/* limit yaw rate */
+	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
+				       _params.yaw_rate_max);
+
+	/* feed forward yaw setpoint rate */
+	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+}
+
+void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
+{
+	/* reset integral if disarmed */
+	if (!_armed.armed) {
+		_rates_int.zero();
+	}
+
+	/* current body angular rates */
+	math::Vector < 3 > rates;
+	rates(0) = _v_att.rollspeed;
+	rates(1) = _v_att.pitchspeed;
+	rates(2) = _v_att.yawspeed;
+
+	/* angular rates error */
+	math::Vector < 3 > rates_err = _rates_sp - rates;
+	_att_control = _params.rate_p.emult(rates_err)
+		       + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+	_rates_prev = rates;
+
+	/* update integral only if not saturated on low limit */
+	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
+		for (int i = 0; i < 3; i++) {
+			if (fabsf(_att_control(i)) < _thrust_sp) {
+				float rate_i = _rates_int(i)
+					       + _params.rate_i(i) * rates_err(i) * dt;
+
+				if (isfinite(
+					    rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+					_rates_int(i) = rate_i;
+				}
+			}
+		}
+	}
+
+}
+
+void MulticopterAttitudeControlBase::set_actuator_controls()
+{
+	_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+	_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+	_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+	_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+}

src/modules/mc_att_control/mc_att_control_base.h

@@ -0,0 +1,127 @@
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 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.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.h
+ *
+ * MC Attitude Controller
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+#include <px4.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+
+#include <systemlib/perf_counter.h>
+#include <lib/mathlib/mathlib.h>
+
+
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+class MulticopterAttitudeControlBase
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControlBase();
+
+	/**
+	 * Destructor
+	 */
+	~MulticopterAttitudeControlBase();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+	void control_attitude(float dt);
+	void control_attitude_rates(float dt);
+
+	void set_actuator_controls();
+
+protected:
+	struct vehicle_attitude_s _v_att; /**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
+	struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
+	struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
+	struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
+	struct actuator_controls_s _actuators; /**< actuator controls */
+	struct actuator_armed_s _armed; /**< actuator arming status */
+
+	math::Vector<3> _rates_prev; /**< angular rates on previous step */
+	math::Vector<3> _rates_sp; /**< angular rates setpoint */
+	math::Vector<3> _rates_int; /**< angular rates integral error */
+	float _thrust_sp; /**< thrust setpoint */
+	math::Vector<3> _att_control; /**< attitude control vector */
+
+	math::Matrix<3, 3> _I; /**< identity matrix */
+
+	bool _reset_yaw_sp; /**< reset yaw setpoint flag */
+
+	struct {
+		math::Vector<3> att_p; /**< P gain for angular error */
+		math::Vector<3> rate_p; /**< P gain for angular rate error */
+		math::Vector<3> rate_i; /**< I gain for angular rate error */
+		math::Vector<3> rate_d; /**< D gain for angular rate error */
+		float yaw_ff; /**< yaw control feed-forward */
+		float yaw_rate_max; /**< max yaw rate */
+
+		float man_roll_max;
+		float man_pitch_max;
+		float man_yaw_max;
+		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
+	} _params;
+
+	bool _publish_att_sp;
+
+	virtual void vehicle_attitude_setpoint_poll() = 0;
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -38,6 +38,9 @@
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
  * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
  *
  * The controller has two loops: P loop for angular error and PD loop for angular rate error.
  * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
@@ -49,8 +52,7 @@
  * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
  */
 
-#include <nuttx/config.h>
-#include <stdio.h>
+#include <px4.h>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
@@ -59,15 +61,6 @@
 #include <poll.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_control_mode.h>
-#include <uORB/topics/actuator_armed.h>
-#include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
@@ -76,18 +69,56 @@
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 
+#include "mc_att_control_base.h"
+
+static bool thread_running = false;     /**< Deamon status flag */
+static int daemon_task;             /**< Handle of deamon task / thread */
+
+using namespace px4;
+
 /**
  * Multicopter attitude control app start / stop handling function
  *
  * @ingroup apps
  */
-extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
+
+PX4_MAIN_FUNCTION(mc_att_control);
+
+int mc_attitude_thread_main(int argc, char *argv[]);
 
 #define YAW_DEADZONE	0.05f
 #define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
 
-class MulticopterAttitudeControl
+void handle_vehicle_attitude2(const PX4_TOPIC_T(rc_channels) &msg) {
+		PX4_INFO("RCHandler class heard: [%llu]", msg.timestamp);
+}
+
+
+namespace px4
+{
+bool task_should_exit = false;
+}
+
+// PX4_MAIN_FUNCTION(mc_att_control) { px4::init(argc, argv, "listener");
+
+	// px4::NodeHandle n;
+
+	// PX4_SUBSCRIBE(n, rc_channels, handle_vehicle_attitude2, 1000);
+
+	/**
+	 * px4::spin() will enter a loop, pumping callbacks.  With this version, all
+	 * callbacks will be called from within this thread (the main one).  px4::spin()
+	 * will exit when Ctrl-C is pressed, or the node is shutdown by the master.
+	 */
+	// n.spin();
+	// PX4_INFO("finished, returning");
+
+	// return 0;
+// }
+
+class MulticopterAttitudeControl :
+	public MulticopterAttitudeControlBase
 {
 public:
 	/**
@@ -100,17 +131,15 @@ public:
 	 */
 	~MulticopterAttitudeControl();
 
-	/**
-	 * Start the sensors task.
-	 *
-	 * @return		OK on success.
-	 */
-	int		start();
+	void  handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg);
+
+	void spin() { n.spin(); }
 
 private:
+	bool		_task_should_exit;			/**< if true, sensor task should exit */
+	int		_control_task;				/**< task handle for sensor task */
+	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
 
-	bool	_task_should_exit;		/**< if true, sensor task should exit */
-	int		_control_task;			/**< task handle for sensor task */
 
 	int		_v_att_sub;				/**< vehicle attitude subscription */
 	int		_v_att_sp_sub;			/**< vehicle attitude setpoint subscription */
@@ -124,27 +153,7 @@ private:
 	orb_advert_t	_v_rates_sp_pub;		/**< rate setpoint publication */
 	orb_advert_t	_actuators_0_pub;		/**< attitude actuator controls publication */
 
-	bool		_actuators_0_circuit_breaker_enabled;	/**< circuit breaker to suppress output */
-
-	struct vehicle_attitude_s			_v_att;				/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s	_v_att_sp;			/**< vehicle attitude setpoint */
-	struct vehicle_rates_setpoint_s		_v_rates_sp;		/**< vehicle rates setpoint */
-	struct manual_control_setpoint_s	_manual_control_sp;	/**< manual control setpoint */
-	struct vehicle_control_mode_s		_v_control_mode;	/**< vehicle control mode */
-	struct actuator_controls_s			_actuators;			/**< actuator controls */
-	struct actuator_armed_s				_armed;				/**< actuator arming status */
-
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
-	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
-	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
-	math::Vector<3>		_rates_int;		/**< angular rates integral error */
-	float				_thrust_sp;		/**< thrust setpoint */
-	math::Vector<3>		_att_control;	/**< attitude control vector */
-
-	math::Matrix<3, 3>  _I;				/**< identity matrix */
-
-	bool	_reset_yaw_sp;			/**< reset yaw setpoint flag */
+	px4::NodeHandle n;
 
 	struct {
 		param_t roll_p;
@@ -170,19 +179,7 @@ private:
 		param_t acro_yaw_max;
 	}		_params_handles;		/**< handles for interesting parameters */
 
-	struct {
-		math::Vector<3> att_p;					/**< P gain for angular error */
-		math::Vector<3> rate_p;				/**< P gain for angular rate error */
-		math::Vector<3> rate_i;				/**< I gain for angular rate error */
-		math::Vector<3> rate_d;				/**< D gain for angular rate error */
-		float yaw_ff;						/**< yaw control feed-forward */
-		float yaw_rate_max;					/**< max yaw rate */
-
-		float man_roll_max;
-		float man_pitch_max;
-		float man_yaw_max;
-		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
-	}		_params;
+	perf_counter_t _loop_perf; /**< loop performance counter */
 
 	/**
 	 * Update our local parameter cache.
@@ -219,25 +216,6 @@ private:
 	 */
 	void		arming_status_poll();
 
-	/**
-	 * Attitude controller.
-	 */
-	void		control_attitude(float dt);
-
-	/**
-	 * Attitude rates controller.
-	 */
-	void		control_attitude_rates(float dt);
-
-	/**
-	 * Shim for calling task_main from task_create.
-	 */
-	static void	task_main_trampoline(int argc, char *argv[]);
-
-	/**
-	 * Main attitude control task.
-	 */
-	void		task_main();
 };
 
 namespace mc_att_control
@@ -249,15 +227,14 @@ namespace mc_att_control
 #endif
 static const int ERROR = -1;
 
-MulticopterAttitudeControl	*g_control;
 }
 
 MulticopterAttitudeControl::MulticopterAttitudeControl() :
-
+	MulticopterAttitudeControlBase(),
 	_task_should_exit(false),
 	_control_task(-1),
-
-/* subscriptions */
+	_actuators_0_circuit_breaker_enabled(false),
+	/* subscriptions */
 	_v_att_sub(-1),
 	_v_att_sp_sub(-1),
 	_v_control_mode_sub(-1),
@@ -265,44 +242,16 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_manual_control_sp_sub(-1),
 	_armed_sub(-1),
 
-/* publications */
+	/* publications */
 	_att_sp_pub(-1),
 	_v_rates_sp_pub(-1),
 	_actuators_0_pub(-1),
+	n(),
 
-	_actuators_0_circuit_breaker_enabled(false),
-
-/* performance counters */
+	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
 
 {
-	memset(&_v_att, 0, sizeof(_v_att));
-	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
-	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
-	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
-	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
-	memset(&_actuators, 0, sizeof(_actuators));
-	memset(&_armed, 0, sizeof(_armed));
-
-	_params.att_p.zero();
-	_params.rate_p.zero();
-	_params.rate_i.zero();
-	_params.rate_d.zero();
-	_params.yaw_ff = 0.0f;
-	_params.yaw_rate_max = 0.0f;
-	_params.man_roll_max = 0.0f;
-	_params.man_pitch_max = 0.0f;
-	_params.man_yaw_max = 0.0f;
-	_params.acro_rate_max.zero();
-
-	_rates_prev.zero();
-	_rates_sp.zero();
-	_rates_int.zero();
-	_thrust_sp = 0.0f;
-	_att_control.zero();
-
-	_I.identity();
-
 	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
 	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
 	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
@@ -326,6 +275,25 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 
 	/* fetch initial parameter values */
 	parameters_update();
+
+	/*
+	 * do subscriptions
+	 */
+	// _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	PX4_SUBSCRIBE(n, vehicle_attitude, MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
+	// _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	PX4_SUBSCRIBE(n, vehicle_attitude_setpoint, 0);
+	// _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	PX4_SUBSCRIBE(n, vehicle_rates_setpoint, 0);
+	// _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+	PX4_SUBSCRIBE(n, vehicle_control_mode, 0);
+	// _params_sub = orb_subscribe(ORB_ID(parameter_update));
+	PX4_SUBSCRIBE(n, parameter_update, 0);
+	// _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	PX4_SUBSCRIBE(n, manual_control_setpoint, 0);
+	// _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	PX4_SUBSCRIBE(n, actuator_armed, 0);
+
 }
 
 MulticopterAttitudeControl::~MulticopterAttitudeControl()
@@ -349,7 +317,7 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl()
 		} while (_control_task != -1);
 	}
 
-	mc_att_control::g_control = nullptr;
+	// mc_att_control::g_control = nullptr;
 }
 
 int
@@ -489,445 +457,213 @@ MulticopterAttitudeControl::arming_status_poll()
 	}
 }
 
-/*
- * Attitude controller.
- * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
- * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes)
- */
-void
-MulticopterAttitudeControl::control_attitude(float dt)
-{
-	float yaw_sp_move_rate = 0.0f;
-	bool publish_att_sp = false;
+// void
+// MulticopterAttitudeControl::task_main()
+// {
 
-	if (_v_control_mode.flag_control_manual_enabled) {
-		/* manual input, set or modify attitude setpoint */
 
-		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
-			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			vehicle_attitude_setpoint_poll();
-		}
+	// [> wakeup source: vehicle attitude <]
+	// struct pollfd fds[1];
 
-		if (!_v_control_mode.flag_control_climb_rate_enabled) {
-			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp.thrust = _manual_control_sp.z;
-			publish_att_sp = true;
-		}
+	// fds[0].fd = _v_att_sub;
+	// fds[0].events = POLLIN;
 
-		if (!_armed.armed) {
-			/* reset yaw setpoint when disarmed */
-			_reset_yaw_sp = true;
-		}
+	// while (!_task_should_exit) {
 
-		/* move yaw setpoint in all modes */
-		if (_v_att_sp.thrust < 0.1f) {
-			// TODO
-			//if (_status.condition_landed) {
-			/* reset yaw setpoint if on ground */
-			//	reset_yaw_sp = true;
-			//}
-		} else {
-			/* move yaw setpoint */
-			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
-			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
-			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
-			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
-			if (yaw_offs < - yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
 
-			} else if (yaw_offs > yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
-			}
-			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
-		}
+		// perf_end(_loop_perf);
+	// }
 
-		/* reset yaw setpint to current position if needed */
-		if (_reset_yaw_sp) {
-			_reset_yaw_sp = false;
-			_v_att_sp.yaw_body = _v_att.yaw;
-			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
-		}
+	// warnx("exit");
 
-		if (!_v_control_mode.flag_control_velocity_enabled) {
-			/* update attitude setpoint if not in position control mode */
-			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
-			_v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max;
-			_v_att_sp.R_valid = false;
-			publish_att_sp = true;
-		}
+	// _control_task = -1;
+	// _exit(0);
+// }
 
-	} else {
-		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		vehicle_attitude_setpoint_poll();
+void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehicle_attitude) &msg) {
 
-		/* reset yaw setpoint after non-manual control mode */
-		_reset_yaw_sp = true;
+	perf_begin(_loop_perf);
+
+	/* run controller on attitude changes */
+	static uint64_t last_run = 0;
+	float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
+	last_run = hrt_absolute_time();
+
+	/* guard against too small (< 2ms) and too large (> 20ms) dt's */
+	if (dt < 0.002f) {
+		dt = 0.002f;
+
+	} else if (dt > 0.02f) {
+		dt = 0.02f;
 	}
 
-	_thrust_sp = _v_att_sp.thrust;
+	/* copy attitude topic */
+	orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
 
-	/* construct attitude setpoint rotation matrix */
-	math::Matrix<3, 3> R_sp;
+	/* check for updates in other topics */
+	parameter_update_poll();
+	vehicle_control_mode_poll();
+	arming_status_poll();
+	vehicle_manual_poll();
 
-	if (_v_att_sp.R_valid) {
-		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp.R_body[0][0]);
+	if (_v_control_mode.flag_control_attitude_enabled) {
+		control_attitude(dt);
 
-	} else {
-		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+		/* publish the attitude setpoint if needed */
+		if (_publish_att_sp) {
+			_v_att_sp.timestamp = hrt_absolute_time();
 
-		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
-		_v_att_sp.R_valid = true;
-	}
-
-	/* publish the attitude setpoint if needed */
-	if (publish_att_sp) {
-		_v_att_sp.timestamp = hrt_absolute_time();
-
-		if (_att_sp_pub > 0) {
-			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_v_att_sp);
-
-		} else {
-			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_v_att_sp);
-		}
-	}
-
-	/* rotation matrix for current state */
-	math::Matrix<3, 3> R;
-	R.set(_v_att.R);
-
-	/* all input data is ready, run controller itself */
-
-	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
-	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
-	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
-
-	/* axis and sin(angle) of desired rotation */
-	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
-
-	/* calculate angle error */
-	float e_R_z_sin = e_R.length();
-	float e_R_z_cos = R_z * R_sp_z;
-
-	/* calculate weight for yaw control */
-	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
-
-	/* calculate rotation matrix after roll/pitch only rotation */
-	math::Matrix<3, 3> R_rp;
-
-	if (e_R_z_sin > 0.0f) {
-		/* get axis-angle representation */
-		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
-		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
-
-		e_R = e_R_z_axis * e_R_z_angle;
-
-		/* cross product matrix for e_R_axis */
-		math::Matrix<3, 3> e_R_cp;
-		e_R_cp.zero();
-		e_R_cp(0, 1) = -e_R_z_axis(2);
-		e_R_cp(0, 2) = e_R_z_axis(1);
-		e_R_cp(1, 0) = e_R_z_axis(2);
-		e_R_cp(1, 2) = -e_R_z_axis(0);
-		e_R_cp(2, 0) = -e_R_z_axis(1);
-		e_R_cp(2, 1) = e_R_z_axis(0);
-
-		/* rotation matrix for roll/pitch only rotation */
-		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
-
-	} else {
-		/* zero roll/pitch rotation */
-		R_rp = R;
-	}
-
-	/* R_rp and R_sp has the same Z axis, calculate yaw error */
-	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
-	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
-	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
-
-	if (e_R_z_cos < 0.0f) {
-		/* for large thrust vector rotations use another rotation method:
-		 * calculate angle and axis for R -> R_sp rotation directly */
-		math::Quaternion q;
-		q.from_dcm(R.transposed() * R_sp);
-		math::Vector<3> e_R_d = q.imag();
-		e_R_d.normalize();
-		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
-
-		/* use fusion of Z axis based rotation and direct rotation */
-		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
-		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
-	}
-
-	/* calculate angular rates setpoint */
-	_rates_sp = _params.att_p.emult(e_R);
-
-	/* limit yaw rate */
-	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
-
-	/* feed forward yaw setpoint rate */
-	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
-}
-
-/*
- * Attitude rates controller.
- * Input: '_rates_sp' vector, '_thrust_sp'
- * Output: '_att_control' vector
- */
-void
-MulticopterAttitudeControl::control_attitude_rates(float dt)
-{
-	/* reset integral if disarmed */
-	if (!_armed.armed) {
-		_rates_int.zero();
-	}
-
-	/* current body angular rates */
-	math::Vector<3> rates;
-	rates(0) = _v_att.rollspeed;
-	rates(1) = _v_att.pitchspeed;
-	rates(2) = _v_att.yawspeed;
-
-	/* angular rates error */
-	math::Vector<3> rates_err = _rates_sp - rates;
-	_att_control = _params.rate_p.emult(rates_err) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
-	_rates_prev = rates;
-
-	/* update integral only if not saturated on low limit */
-	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
-		for (int i = 0; i < 3; i++) {
-			if (fabsf(_att_control(i)) < _thrust_sp) {
-				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
-
-				if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
-				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
-					_rates_int(i) = rate_i;
-				}
-			}
-		}
-	}
-}
-
-void
-MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
-{
-	mc_att_control::g_control->task_main();
-}
-
-void
-MulticopterAttitudeControl::task_main()
-{
-
-	/*
-	 * do subscriptions
-	 */
-	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
-	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
-
-	/* initialize parameters cache */
-	parameters_update();
-
-	/* wakeup source: vehicle attitude */
-	struct pollfd fds[1];
-
-	fds[0].fd = _v_att_sub;
-	fds[0].events = POLLIN;
-
-	while (!_task_should_exit) {
-
-		/* wait for up to 100ms for data */
-		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
-
-		/* timed out - periodic check for _task_should_exit */
-		if (pret == 0)
-			continue;
-
-		/* 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);
-			/* sleep a bit before next try */
-			usleep(100000);
-			continue;
-		}
-
-		perf_begin(_loop_perf);
-
-		/* run controller on attitude changes */
-		if (fds[0].revents & POLLIN) {
-			static uint64_t last_run = 0;
-			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
-			last_run = hrt_absolute_time();
-
-			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
-			if (dt < 0.002f) {
-				dt = 0.002f;
-
-			} else if (dt > 0.02f) {
-				dt = 0.02f;
-			}
-
-			/* copy attitude topic */
-			orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
-
-			/* check for updates in other topics */
-			parameter_update_poll();
-			vehicle_control_mode_poll();
-			arming_status_poll();
-			vehicle_manual_poll();
-
-			if (_v_control_mode.flag_control_attitude_enabled) {
-				control_attitude(dt);
-
-				/* publish attitude rates setpoint */
-				_v_rates_sp.roll = _rates_sp(0);
-				_v_rates_sp.pitch = _rates_sp(1);
-				_v_rates_sp.yaw = _rates_sp(2);
-				_v_rates_sp.thrust = _thrust_sp;
-				_v_rates_sp.timestamp = hrt_absolute_time();
-
-				if (_v_rates_sp_pub > 0) {
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
-
-				} else {
-					_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
-				}
+			if (_att_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
+					    &_v_att_sp);
 
 			} else {
-				/* attitude controller disabled, poll rates setpoint topic */
-				if (_v_control_mode.flag_control_manual_enabled) {
-					/* manual rates control - ACRO mode */
-					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
-					_thrust_sp = _manual_control_sp.z;
-
-					/* reset yaw setpoint after ACRO */
-					_reset_yaw_sp = true;
-
-					/* publish attitude rates setpoint */
-					_v_rates_sp.roll = _rates_sp(0);
-					_v_rates_sp.pitch = _rates_sp(1);
-					_v_rates_sp.yaw = _rates_sp(2);
-					_v_rates_sp.thrust = _thrust_sp;
-					_v_rates_sp.timestamp = hrt_absolute_time();
-
-					if (_v_rates_sp_pub > 0) {
-						orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
-
-					} else {
-						_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
-					}
-
-				} else {
-					/* attitude controller disabled, poll rates setpoint topic */
-					vehicle_rates_setpoint_poll();
-					_rates_sp(0) = _v_rates_sp.roll;
-					_rates_sp(1) = _v_rates_sp.pitch;
-					_rates_sp(2) = _v_rates_sp.yaw;
-					_thrust_sp = _v_rates_sp.thrust;
-				}
-			}
-
-			if (_v_control_mode.flag_control_rates_enabled) {
-				control_attitude_rates(dt);
-
-				/* publish actuator controls */
-				_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
-				_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
-				_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
-				_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
-				_actuators.timestamp = hrt_absolute_time();
-
-				if (!_actuators_0_circuit_breaker_enabled) {
-					if (_actuators_0_pub > 0) {
-						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
-
-					} else {
-						_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
-					}
-				}
+				_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
+							    &_v_att_sp);
 			}
 		}
 
-		perf_end(_loop_perf);
+		/* publish attitude rates setpoint */
+		_v_rates_sp.roll = _rates_sp(0);
+		_v_rates_sp.pitch = _rates_sp(1);
+		_v_rates_sp.yaw = _rates_sp(2);
+		_v_rates_sp.thrust = _thrust_sp;
+		_v_rates_sp.timestamp = hrt_absolute_time();
+
+		if (_v_rates_sp_pub > 0) {
+			orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+		} else {
+			_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+		}
+
+	} else {
+		/* attitude controller disabled, poll rates setpoint topic */
+		if (_v_control_mode.flag_control_manual_enabled) {
+			/* manual rates control - ACRO mode */
+			_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
+						    _manual_control_sp.r).emult(_params.acro_rate_max);
+			_thrust_sp = _manual_control_sp.z;
+
+			/* reset yaw setpoint after ACRO */
+			_reset_yaw_sp = true;
+
+			/* publish attitude rates setpoint */
+			_v_rates_sp.roll = _rates_sp(0);
+			_v_rates_sp.pitch = _rates_sp(1);
+			_v_rates_sp.yaw = _rates_sp(2);
+			_v_rates_sp.thrust = _thrust_sp;
+			_v_rates_sp.timestamp = hrt_absolute_time();
+
+			if (_v_rates_sp_pub > 0) {
+				orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+			} else {
+				_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+			}
+
+		} else {
+			/* attitude controller disabled, poll rates setpoint topic */
+			vehicle_rates_setpoint_poll();
+			_rates_sp(0) = _v_rates_sp.roll;
+			_rates_sp(1) = _v_rates_sp.pitch;
+			_rates_sp(2) = _v_rates_sp.yaw;
+			_thrust_sp = _v_rates_sp.thrust;
+		}
 	}
 
-	warnx("exit");
+	if (_v_control_mode.flag_control_rates_enabled) {
+		control_attitude_rates(dt);
 
-	_control_task = -1;
-	_exit(0);
-}
+		/* publish actuator controls */
+		_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+		_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+		_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+		_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+		_actuators.timestamp = hrt_absolute_time();
 
-int
-MulticopterAttitudeControl::start()
-{
-	ASSERT(_control_task == -1);
+		if (!_actuators_0_circuit_breaker_enabled) {
+			if (_actuators_0_pub > 0) {
+				orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators);
 
-	/* start the task */
-	_control_task = task_spawn_cmd("mc_att_control",
-				       SCHED_DEFAULT,
-				       SCHED_PRIORITY_MAX - 5,
-				       2000,
-				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
-				       nullptr);
-
-	if (_control_task < 0) {
-		warn("task start failed");
-		return -errno;
+			} else {
+				_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators);
+			}
+		}
 	}
-
-	return OK;
 }
 
-int mc_att_control_main(int argc, char *argv[])
+PX4_MAIN_FUNCTION(mc_att_control)
 {
-	if (argc < 1)
+	px4::init(argc, argv, "mc_att_control");
+
+	if (argc < 1) {
 		errx(1, "usage: mc_att_control {start|stop|status}");
+	}
 
 	if (!strcmp(argv[1], "start")) {
 
-		if (mc_att_control::g_control != nullptr)
-			errx(1, "already running");
-
-		mc_att_control::g_control = new MulticopterAttitudeControl;
-
-		if (mc_att_control::g_control == nullptr)
-			errx(1, "alloc failed");
-
-		if (OK != mc_att_control::g_control->start()) {
-			delete mc_att_control::g_control;
-			mc_att_control::g_control = nullptr;
-			err(1, "start failed");
+		if (thread_running) {
+			warnx("already running");
+			/* this is not an error */
+			exit(0);
 		}
 
+		task_should_exit = false;
+
+		daemon_task = task_spawn_cmd("mc_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2000,
+				       mc_attitude_thread_main,
+					(argv) ? (const char **)&argv[2] : (const char **)NULL);
+
 		exit(0);
 	}
 
-	if (!strcmp(argv[1], "stop")) {
-		if (mc_att_control::g_control == nullptr)
-			errx(1, "not running");
+	// if (!strcmp(argv[1], "stop")) {
+		// if (mc_att_control::g_control == nullptr) {
+			// errx(1, "not running");
+		// }
 
-		delete mc_att_control::g_control;
-		mc_att_control::g_control = nullptr;
-		exit(0);
-	}
+		// delete mc_att_control::g_control;
+		// mc_att_control::g_control = nullptr;
+		// exit(0);
+	// }
 
-	if (!strcmp(argv[1], "status")) {
-		if (mc_att_control::g_control) {
-			errx(0, "running");
+	// if (!strcmp(argv[1], "status")) {
+		// if (mc_att_control::g_control) {
+			// errx(0, "running");
 
-		} else {
-			errx(1, "not running");
-		}
-	}
+		// } else {
+			// errx(1, "not running");
+		// }
+	// }
 
 	warnx("unrecognized command");
 	return 1;
 }
+
+int mc_attitude_thread_main(int argc, char *argv[])
+{
+
+	warnx("starting");
+
+	MulticopterAttitudeControl attctl;
+
+	thread_running = true;
+
+	attctl.spin();
+
+	// while (!task_should_exit) {
+		// attctl.update();
+	// }
+
+	warnx("exiting.");
+
+	thread_running = false;
+
+	return 0;
+}
+
+

src/modules/mc_att_control/mc_att_control_sim.cpp

@@ -0,0 +1,142 @@
+/* Copyright (c) 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.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_sim.cpp
+ *
+ * MC Attitude Controller Interface for usage in a simulator
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "mc_att_control_sim.h"
+#include <geo/geo.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+MulticopterAttitudeControlSim::MulticopterAttitudeControlSim()
+{
+	/* setup standard gains */
+	//XXX: make these configurable
+	_params.att_p(0)      = 5.0;
+	_params.rate_p(0)     = 0.05;
+	_params.rate_i(0)     = 0.0;
+	_params.rate_d(0)     = 0.003;
+	/* pitch gains */
+	_params.att_p(1)      = 5.0;
+	_params.rate_p(1)     = 0.05;
+	_params.rate_i(1)     = 0.0;
+	_params.rate_d(1)     = 0.003;
+	/* yaw gains */
+	_params.att_p(2)      = 2.8;
+	_params.rate_p(2)     = 0.2;
+	_params.rate_i(2)     = 0.1;
+	_params.rate_d(2)     = 0.0;
+	_params.yaw_rate_max  = 0.5;
+	_params.yaw_ff        = 0.5;
+	_params.man_roll_max  = 0.6;
+	_params.man_pitch_max = 0.6;
+	_params.man_yaw_max   = 0.6;
+}
+
+MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim()
+{
+}
+
+void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude)
+{
+	math::Quaternion quat;
+	quat(0) = (float)attitude.w();
+	quat(1) = (float)attitude.x();
+	quat(2) = (float)attitude.y();
+	quat(3) = (float)attitude.z();
+
+	_v_att.q[0] = quat(0);
+	_v_att.q[1] = quat(1);
+	_v_att.q[2] = quat(2);
+	_v_att.q[3] = quat(3);
+
+	math::Matrix<3, 3> Rot = quat.to_dcm();
+	_v_att.R[0][0] = Rot(0, 0);
+	_v_att.R[1][0] = Rot(1, 0);
+	_v_att.R[2][0] = Rot(2, 0);
+	_v_att.R[0][1] = Rot(0, 1);
+	_v_att.R[1][1] = Rot(1, 1);
+	_v_att.R[2][1] = Rot(2, 1);
+	_v_att.R[0][2] = Rot(0, 2);
+	_v_att.R[1][2] = Rot(1, 2);
+	_v_att.R[2][2] = Rot(2, 2);
+
+	_v_att.R_valid = true;
+}
+
+void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate)
+{
+	// check if this is consistent !!!
+	_v_att.rollspeed  = angular_rate(0);
+	_v_att.pitchspeed = angular_rate(1);
+	_v_att.yawspeed   = angular_rate(2);
+}
+
+void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference)
+{
+	_v_att_sp.roll_body  = control_attitude_thrust_reference(0);
+	_v_att_sp.pitch_body = control_attitude_thrust_reference(1);
+	_v_att_sp.yaw_body   = control_attitude_thrust_reference(2);
+	_v_att_sp.thrust     = (control_attitude_thrust_reference(3) - 30) * (-1) / 30;
+
+	// setup rotation matrix
+	math::Matrix<3, 3> Rot_sp;
+	Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+	_v_att_sp.R_body[0][0] = Rot_sp(0, 0);
+	_v_att_sp.R_body[1][0] = Rot_sp(1, 0);
+	_v_att_sp.R_body[2][0] = Rot_sp(2, 0);
+	_v_att_sp.R_body[0][1] = Rot_sp(0, 1);
+	_v_att_sp.R_body[1][1] = Rot_sp(1, 1);
+	_v_att_sp.R_body[2][1] = Rot_sp(2, 1);
+	_v_att_sp.R_body[0][2] = Rot_sp(0, 2);
+	_v_att_sp.R_body[1][2] = Rot_sp(1, 2);
+	_v_att_sp.R_body[2][2] = Rot_sp(2, 2);
+}
+
+void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs)
+{
+	motor_inputs(0) = _actuators.control[0];
+	motor_inputs(1) = _actuators.control[1];
+	motor_inputs(2) = _actuators.control[2];
+	motor_inputs(3) = _actuators.control[3];
+}

src/modules/mc_att_control/mc_att_control_sim.h

@@ -0,0 +1,97 @@
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 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.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_sim.h
+ *
+ * MC Attitude Controller Interface for usage in a simulator
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <lib/mathlib/mathlib.h>
+#inlcude "mc_att_control_base.h"
+
+
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+class MulticopterAttitudeControlSim :
+	public MulticopterAttitudeControlBase
+
+{
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControlSim();
+
+	/**
+	 * Destructor
+	 */
+	~MulticopterAttitudeControlSim();
+
+	/* setters and getters for interface with euroc-gazebo simulator */
+	void set_attitude(const Eigen::Quaternion<double> attitude);
+	void set_attitude_rates(const Eigen::Vector3d &angular_rate);
+	void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference);
+	void get_mixer_input(Eigen::Vector4d &motor_inputs);
+
+protected:
+	void vehicle_attitude_setpoint_poll() {};
+
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */

src/modules/mc_att_control/module.mk

@@ -38,4 +38,5 @@
 MODULE_COMMAND	= mc_att_control
 
 SRCS		= mc_att_control_main.cpp \
-			  mc_att_control_params.c
+		  mc_att_control_base.cpp \
+		  mc_att_control_params.c

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -43,8 +43,9 @@
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
-#include <nuttx/config.h>
-#include <stdio.h>
+#include <px4.h>
+#include <functional>
+#include <cstdio>
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
@@ -54,8 +55,7 @@
 #include <poll.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
+
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
@@ -67,13 +67,13 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
+// #include <systemlib/param/param.h>
+// #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <mavlink/mavlink_log.h>
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 #define TILT_COS_MAX	0.7f
 #define SIGMA			0.000001f
@@ -532,9 +532,9 @@ MulticopterPositionControl::reset_pos_sp()
 	if (_reset_pos_sp) {
 		_reset_pos_sp = false;
 		/* shift position setpoint to make attitude setpoint continuous */
-		_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
+		_pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
 				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
-		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
+		_pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
 				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
 	}
@@ -987,7 +987,7 @@ MulticopterPositionControl::task_main()
 			if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
 				/* idle state, don't run controller and set zero thrust */
 				R.identity();
-				memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+				memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 				_att_sp.R_valid = true;
 
 				_att_sp.roll_body = 0.0f;
@@ -1260,7 +1260,7 @@ MulticopterPositionControl::task_main()
 						}
 
 						/* copy rotation matrix to attitude setpoint topic */
-						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 						_att_sp.R_valid = true;
 
 						/* calculate euler angles, for logging only, must not be used for control */
@@ -1275,7 +1275,7 @@ MulticopterPositionControl::task_main()
 						R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
 
 						/* copy rotation matrix to attitude setpoint topic */
-						memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+						memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
 						_att_sp.R_valid = true;
 
 						_att_sp.roll_body = 0.0f;

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -68,7 +68,7 @@
 #include <geo/geo.h>
 #include <systemlib/systemlib.h>
 #include <drivers/drv_hrt.h>
-#include <px4_defines.h>
+#include <platforms/px4_defines.h>
 
 #include "position_estimator_inav_params.h"
 #include "inertial_filter.h"

src/modules/sensors/sensors.cpp

@@ -187,8 +187,8 @@ private:
 	/**
 	 * Get switch position for specified function.
 	 */
-	switch_pos_t	get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
-	switch_pos_t	get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
+	uint8_t	get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
+	uint8_t	get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
 
 	/**
 	 * Gather and publish RC input data.
@@ -1512,7 +1512,7 @@ Sensors::get_rc_value(uint8_t func, float min_value, float max_value)
 	}
 }
 
-switch_pos_t
+uint8_t
 Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv)
 {
 	if (_rc.function[func] >= 0) {
@@ -1533,7 +1533,7 @@ Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mi
 	}
 }
 
-switch_pos_t
+uint8_t
 Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
 {
 	if (_rc.function[func] >= 0) {

src/modules/systemlib/systemlib.h

@@ -41,6 +41,7 @@
 #define SYSTEMLIB_H_
 #include <float.h>
 #include <stdint.h>
+#include <sched.h>
 
 __BEGIN_DECLS
 

src/modules/uORB/Subscription.cpp

@@ -95,5 +95,7 @@ template class __EXPORT Subscription<vehicle_local_position_s>;
 template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
 template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
 template class __EXPORT Subscription<rc_channels_s>;
+template class __EXPORT Subscription<vehicle_control_mode_s>;
+template class __EXPORT Subscription<actuator_armed_s>;
 
 } // namespace uORB

src/modules/uORB/topics/airspeed.h

@@ -40,7 +40,7 @@
 #ifndef TOPIC_AIRSPEED_H_
 #define TOPIC_AIRSPEED_H_
 
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 #include <stdint.h>
 
 /**

src/modules/uORB/topics/fence.h

@@ -42,7 +42,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics

src/modules/uORB/topics/manual_control_setpoint.h

@@ -1,111 +0,0 @@
-/****************************************************************************
- *
- *	 Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *	 Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *		notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *		notice, this list of conditions and the following disclaimer in
- *		the documentation and/or other materials provided with the
- *		distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *		used to endorse or promote products derived from this software
- *		without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file manual_control_setpoint.h
- * Definition of the manual_control_setpoint uORB topic.
- */
-
-#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_
-#define TOPIC_MANUAL_CONTROL_SETPOINT_H_
-
-#include <stdint.h>
-#include "../uORB.h"
-
-/**
- * Switch position
- */
-typedef enum {
-	SWITCH_POS_NONE = 0,		/**< switch is not mapped */
-	SWITCH_POS_ON,			/**< switch activated (value = 1) */
-	SWITCH_POS_MIDDLE,		/**< middle position (value = 0) */
-	SWITCH_POS_OFF			/**< switch not activated (value = -1) */
-} switch_pos_t;
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct manual_control_setpoint_s {
-	uint64_t timestamp;
-
-	/**
-	 * Any of the channels may not be available and be set to NaN
-	 * to indicate that it does not contain valid data.
-	 * The variable names follow the definition of the
-	 * MANUAL_CONTROL mavlink message.
-	 * The default range is from -1 to 1 (mavlink message -1000 to 1000)
-	 * The range for the z variable is defined from 0 to 1. (The z field of
-	 * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
-	 */
-	float x;			/**< stick position in x direction -1..1
-					  in general corresponds to forward/back motion or pitch of vehicle,
-					  in general a positive value means forward or negative pitch and
-					  a negative value means backward or positive pitch */
-	float y;			/**< stick position in y direction -1..1
-					  in general corresponds to right/left motion or roll of vehicle,
-					  in general a positive value means right or positive roll and
-					  a negative value means left or negative roll */
-	float z;			/**< throttle stick position 0..1
-					  in general corresponds to up/down motion or thrust of vehicle,
-					  in general the value corresponds to the demanded throttle by the user,
-					  if the input is used for setting the setpoint of a vertical position
-					  controller any value > 0.5 means up and any value < 0.5 means down */
-	float r;			/**< yaw stick/twist positon, -1..1
-					  in general corresponds to the righthand rotation around the vertical
-					  (downwards) axis of the vehicle */
-	float flaps;			/**< flap position */
-	float aux1;			/**< default function: camera yaw / azimuth */
-	float aux2;			/**< default function: camera pitch / tilt */
-	float aux3;			/**< default function: camera trigger */
-	float aux4;			/**< default function: camera roll */
-	float aux5;			/**< default function: payload drop */
-
-	switch_pos_t mode_switch;	/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
-	switch_pos_t return_switch;	/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
-	switch_pos_t posctl_switch;	/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
-	switch_pos_t loiter_switch;	/**< loiter 2 position switch (optional): _MISSION_, LOITER */
-	switch_pos_t acro_switch;	/**< acro 2 position switch (optional): _MANUAL_, ACRO */
-	switch_pos_t offboard_switch;	/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(manual_control_setpoint);
-
-#endif

src/modules/uORB/topics/vehicle_attitude_setpoint.h

@@ -1,87 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file vehicle_attitude_setpoint.h
- * Definition of the vehicle attitude setpoint uORB topic.
- */
-
-#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
-#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * vehicle attitude setpoint.
- */
-struct vehicle_attitude_setpoint_s {
-	uint64_t timestamp;		/**< in microseconds since system start, is set whenever the writing thread stores new data */
-
-	float roll_body;				/**< body angle in NED frame		*/
-	float pitch_body;				/**< body angle in NED frame		*/
-	float yaw_body;					/**< body angle in NED frame		*/
-	//float body_valid;				/**< Set to true if body angles are valid */
-
-	float R_body[3][3];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
-	bool R_valid;					/**< Set to true if rotation matrix is valid */
-
-	//! For quaternion-based attitude control
-	float q_d[4];				/** Desired quaternion for quaternion control */
-	bool q_d_valid;					/**< Set to true if quaternion vector is valid */
-	float q_e[4];				/** Attitude error in quaternion */
-	bool q_e_valid;					/**< Set to true if quaternion error vector is valid */
-
-	float thrust;					/**< Thrust in Newton the power system should generate */
-
-	bool	roll_reset_integral;			/**< Reset roll integral part (navigation logic change) */
-	bool	pitch_reset_integral;			/**< Reset pitch integral part (navigation logic change) */
-	bool	yaw_reset_integral;			/**< Reset yaw integral part (navigation logic change) */
-
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_attitude_setpoint);
-
-#endif /* TOPIC_ARDRONE_CONTROL_H_ */

src/modules/uORB/topics/vehicle_control_mode.h

@@ -1,95 +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.
- *
- ****************************************************************************/
-
-/**
- * @file vehicle_control_mode.h
- * Definition of the vehicle_control_mode uORB topic.
- *
- * All control apps should depend their actions based on the flags set here.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- */
-
-#ifndef VEHICLE_CONTROL_MODE
-#define VEHICLE_CONTROL_MODE
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-#include "vehicle_status.h"
-
-/**
- * @addtogroup topics @{
- */
-
-
-/**
- * state machine / state of vehicle.
- *
- * Encodes the complete system state and is set by the commander app.
- */
-
-struct vehicle_control_mode_s {
-	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
-
-	bool flag_armed;
-
-	bool flag_external_manual_override_ok;	/**< external override non-fatal for system. Only true for fixed wing */
-
-	// XXX needs yet to be set by state machine helper
-	bool flag_system_hil_enabled;
-
-	bool flag_control_manual_enabled;		/**< true if manual input is mixed in */
-	bool flag_control_auto_enabled;			/**< true if onboard autopilot should act */
-	bool flag_control_offboard_enabled;		/**< true if offboard control should be used */
-	bool flag_control_rates_enabled;		/**< true if rates are stabilized */
-	bool flag_control_attitude_enabled;		/**< true if attitude stabilization is mixed in */
-	bool flag_control_force_enabled;		/**< true if force control is mixed in */
-	bool flag_control_velocity_enabled;		/**< true if horizontal velocity (implies direction) is controlled */
-	bool flag_control_position_enabled;		/**< true if position is controlled */
-	bool flag_control_altitude_enabled;		/**< true if altitude is controlled */
-	bool flag_control_climb_rate_enabled;	/**< true if climb rate is controlled */
-	bool flag_control_termination_enabled;	/**< true if flighttermination is enabled */
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_control_mode);
-
-#endif

src/modules/uORB/topics/vehicle_global_position.h

@@ -45,7 +45,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics

src/modules/uORB/topics/vehicle_gps_position.h

@@ -79,7 +79,7 @@ struct vehicle_gps_position_s {
 	bool vel_ned_valid;				/**< Flag to indicate if NED speed is valid */
 
 	uint64_t timestamp_time;			/**< Timestamp for time information */
-	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
+	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */
 
 	uint8_t satellites_used;			/**< Number of satellites used */
 };

src/modules/uORB/topics/vehicle_status.h

@@ -53,7 +53,7 @@
 
 #include <stdint.h>
 #include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
 
 /**
  * @addtogroup topics @{

src/modules/uORB/uORB.h

@@ -125,7 +125,7 @@ typedef intptr_t	orb_advert_t;
  * node in /obj if required and publishes the initial data.
  *
  * Any number of advertisers may publish to a topic; publications are atomic
- * but co-ordination between publishers is not provided by the ORB. 
+ * but co-ordination between publishers is not provided by the ORB.
  *
  * @param meta		The uORB metadata (usually from the ORB_ID() macro)
  *			for the topic.
@@ -261,4 +261,11 @@ extern int	orb_set_interval(int handle, unsigned interval) __EXPORT;
 
 __END_DECLS
 
+/* Diverse uORB header defines */ //XXX: move to better location
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS    ORB_ID(actuator_controls_0)
+ORB_DECLARE(actuator_controls_0);
+ORB_DECLARE(actuator_controls_1);
+ORB_DECLARE(actuator_controls_2);
+ORB_DECLARE(actuator_controls_3);
+
 #endif /* _UORB_UORB_H */

src/modules/uavcan/uavcan_main.cpp

@@ -46,6 +46,8 @@
 #include <arch/board/board.h>
 #include <arch/chip/chip.h>
 
+#include <uORB/topics/esc_status.h>
+
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_pwm_output.h>
 
@@ -612,10 +614,32 @@ UavcanNode::print_info()
 
 	if (_outputs.noutputs != 0) {
 		printf("ESC output: ");
+
 		for (uint8_t i=0; i<_outputs.noutputs; i++) {
 			printf("%d ", (int)(_outputs.output[i]*1000));
 		}
 		printf("\n");
+
+		// ESC status
+		int esc_sub = orb_subscribe(ORB_ID(esc_status));
+		struct esc_status_s esc;
+		memset(&esc, 0, sizeof(esc));
+		orb_copy(ORB_ID(esc_status), esc_sub, &esc);
+
+		printf("ESC Status:\n");
+		printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");
+		for (uint8_t i=0; i<_outputs.noutputs; i++) {
+			printf("%d\t",    esc.esc[i].esc_address);
+			printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
+			printf("%3.2f\t", (double)esc.esc[i].esc_current);
+			printf("%3.2f\t", (double)esc.esc[i].esc_temperature);
+			printf("%3.2f\t", (double)esc.esc[i].esc_setpoint);
+			printf("%d\t",    esc.esc[i].esc_rpm);
+			printf("%d",      esc.esc[i].esc_errorcount);
+			printf("\n");
+		}
+
+		orb_unsubscribe(esc_sub);
 	}
 
 	// Sensor bridges