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

.gitignore

@@ -38,3 +38,4 @@ Firmware.sublime-workspace
 nsh_romfsimg.h
 cscope.out
 .configX-e
+nuttx-export.zip

ROMFS/scripts/rc.FMU_quad_x

@@ -9,7 +9,7 @@ echo "[init] eeprom"
 eeprom start
 if [ -f /eeprom/parameters ]
 then
-	eeprom load_param /eeprom/parameters
+	param load
 fi
 
 echo "[init] sensors"

ROMFS/scripts/rc.PX4IOAR

@@ -19,7 +19,7 @@ echo "[init] eeprom"
 eeprom start
 if [ -f /eeprom/parameters ]
 then
-	eeprom load_param /eeprom/parameters
+	param load
 fi
  
 #

ROMFS/scripts/rc.standalone

@@ -10,6 +10,16 @@ echo "[init] doing standalone PX4FMU startup..."
 #
 uorb start
 
+#
+# Init the EEPROM
+#
+echo "[init] eeprom"
+eeprom start
+if [ -f /eeprom/parameters ]
+then
+	param load
+fi
+
 #
 # Start the sensors.
 #

ROMFS/scripts/rcS

@@ -33,6 +33,7 @@ fi
 
 usleep 500
 
+
 #
 # Look for an init script on the microSD card.
 #

apps/ChangeLog.txt

@@ -279,14 +279,14 @@
 	* apps/*/Make.defs:  Numerous fixes needed to use the automated
 	  configuration (from Richard Cochran).
 
-6.22 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
+6.22 2012-09-29 Gregory Nutt <gnutt@nuttx.org>
 
 	* apps/netutils/thttpd/thttpd_cgi.c:  Missing NULL in argv[]
 	  list (contributed by Kate).
 	* apps/nshlib/nsh_parse.c: CONFIG_NSH_DISABLE_WGET not CONFIG_NSH_DISABLE_GET
 	  in one location (found by Kate).
 	* apps/examples/ostest/prioinherit.c:  Limit the number of test
-	  threds to no more than 3 of each priority.  Bad things happen
+	  threads to no more than 3 of each priority.  Bad things happen
 	  when the existing logic tried to created several hundred test
 	  treads!
 	* apps/nshlib/nsh.h:  Both CONFIG_LIBC_STRERROR and CONFIG_NSH_STRERROR
@@ -341,7 +341,30 @@
 	* apps/netutils/webserver/httpd.c:  Add support for Keep-alive connections
 	  (from Kate).
 	* apps/NxWidget/Kconfig:  This is a kludge.  I created this NxWidgets
-	  directory that ONLY contains Kconfig.  NxWidgets does not like in
+	  directory that ONLY contains Kconfig.  NxWidgets does not live in
 	  either the nuttx/ or the apps/ source trees.  This kludge makes it
 	  possible to configure NxWidgets/NxWM without too much trouble (with
 	  the tradeoff being a kind ugly structure and some maintenance issues).
+	* apps/examples/Make.defs: Missing support for apps/examples/watchdog.
+	* apps/NxWidgets/Kconfig:  Add option to turn on the memory monitor
+	  feature of the NxWidgets/NxWM unit tests.
+
+6.23 2012-xx-xx Gregory Nutt <gnutt@nuttx.org>
+
+	* vsn: Moved all NSH commands from vsn/ to system/.  Deleted the vsn/
+	  directory.
+	* Makefile:  Change order of includes when CONFIG_NEWCONFIG=y.  In
+	  that case, namedapp must be included first so that the namedapp
+	  context is established first.  If the namedapp context is established
+	  later, it will overwrite any existing namedapp_list.h and nameapp_proto.h
+	  files.
+	* CONFIG_EXAMPLES_*: To make things consistent, changed all occurrences
+	  of CONFIG_EXAMPLE_* to CONFIG_EXAMPLES_*.
+	* Kconfig:  Fleshed out apps/examples/adc/Kconfig and apps/examples/wget/Kconfig.
+	  There are still a LOT of empty, stub Kconfig files.
+	* Kconfig:  Fleshed out apps/examples/buttons/Kconfig. There are still a LOT
+	  of empty, stub Kconfig files.
+	* apps/netutils/webserver/httpd.c:  Fix a bug that I introduced in
+	  recent check-ins (Darcy Gong).
+	* apps/netutils/webclient/webclient.c:  Fix another but that I introduced
+	  when I was trying to add correct handling for loss of connection (Darcy Gong)

apps/Kconfig

@@ -34,7 +34,3 @@ endmenu
 menu "System NSH Add-Ons"
 source "$APPSDIR/system/Kconfig"
 endmenu
-
-menu "VSN board Add-Ons"
-source "$APPSDIR/vsn/Kconfig"
-endmenu

apps/Makefile

@@ -46,10 +46,10 @@ APPDIR = ${shell pwd}
 #   appears in this directory as .config)
 # SUBDIRS is the list of all directories containing Makefiles.  It is used
 #   only for cleaning. namedapp must always be the first in the list.  This
-#   list can be extended by the .config file as well
+#   list can be extended by the .config file as well.
 
 CONFIGURED_APPS =
-#SUBDIRS = examples graphics interpreters modbus namedapp nshlib netutils system vsn
+#SUBDIRS = examples graphics interpreters modbus namedapp nshlib netutils system
 ALL_SUBDIRS = $(dir $(shell /usr/bin/find . -name Makefile))
 SUBDIRS = namedapp/ $(filter-out ./ ./namedapp/ ./examples/,$(ALL_SUBDIRS))
 
@@ -73,15 +73,16 @@ SUBDIRS = namedapp/ $(filter-out ./ ./namedapp/ ./examples/,$(ALL_SUBDIRS))
 
 ifeq ($(CONFIG_NUTTX_NEWCONFIG),y)
 
+# namedapp/Make.defs must be included first
+
+-include namedapp/Make.defs
 -include examples/Make.defs
 -include graphics/Make.defs
 -include interpreters/Make.defs
 -include modbus/Make.defs
--include namedapp/Make.defs
 -include netutils/Make.defs
 -include nshlib/Make.defs
 -include system/Make.defs
--include vsn/Make.defs
 
 # INSTALLED_APPS is the list of currently available application directories.  It
 # is the same as CONFIGURED_APPS, but filtered to exclude any non-existent

apps/README.txt

@@ -107,7 +107,7 @@ NuttX is configured.  .config is included in the toplevel apps/Makefile.
 As a minimum, this configuration file must define files to add to the
 CONFIGURED_APPS list like:
 
-  CONFIGURED_APPS  += examples/hello vsn/poweroff
+  CONFIGURED_APPS  += examples/hello system/poweroff
 
 Named Start-Up main() function
 ------------------------------

apps/ardrone_interface/ardrone_interface.c

@@ -115,7 +115,7 @@ int ardrone_interface_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		ardrone_interface_task = task_spawn("ardrone_interface",
-						    SCHED_RR,
+						    SCHED_DEFAULT,
 						    SCHED_PRIORITY_MAX - 15,
 						    4096,
 						    ardrone_interface_thread_main,

apps/attitude_estimator_ekf/Makefile

@@ -36,6 +36,7 @@ PRIORITY	 = SCHED_PRIORITY_DEFAULT
 STACKSIZE	 = 2048
 
 CSRCS		 = attitude_estimator_ekf_main.c \
+		   attitude_estimator_ekf_params.c \
 		   codegen/eye.c \
 		   codegen/attitudeKalmanfilter.c \
 		   codegen/mrdivide.c \

apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c

@@ -58,25 +58,20 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/parameter_update.h>
 #include <arch/board/up_hrt.h>
 
 #include <systemlib/systemlib.h>
 
 #include "codegen/attitudeKalmanfilter_initialize.h"
 #include "codegen/attitudeKalmanfilter.h"
+#include "attitude_estimator_ekf_params.h"
 
 __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
 
-
-// #define N_STATES 6
-
-// #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000
-// #define REPROJECTION_COUNTER_LIMIT 125
-
 static unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 static float dt = 1.0f;
-/* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */
 /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
 static float z_k[9];					/**< Measurement vector */
 static float x_aposteriori_k[12];		/**< */
@@ -94,6 +89,7 @@ static float P_aposteriori_k[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0
 				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
 				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
 				  };
+
 static float P_aposteriori[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
 				   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,   0,
 				   0,   0, 100.f,   0,   0,   0,   0,   0,   0,   0,   0,   0,
@@ -107,11 +103,14 @@ static float P_aposteriori[144] = {100.f, 0,   0,   0,   0,   0,   0,   0,   0,
 				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   100.0f,   0,
 				   0,   0,   0,   0,   0,   0,   0,   0,  0.0f,   0,   0,   100.0f,
 				  };	/**< init: diagonal matrix with big values */
-// static float knownConst[15] = {1, 1, 1, 1, 1, 0.04, 4, 0.1, 70, 70, -2000, 9.81, 1, 4, 1};			/**< knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */
+
+/* output euler angles */
+static float euler[3] = {0.0f, 0.0f, 0.0f};
+
 static float Rot_matrix[9] = {1.f,  0,  0,
-			      0,  1.f,  0,
+		      0,  1.f,  0,
-			      0,  0,  1.f
+		      0,  0,  1.f
-			     };		/**< init: identity matrix */
+		     };		/**< init: identity matrix */
 
 
 static bool thread_should_exit = false;		/**< Deamon exit flag */
@@ -160,8 +159,8 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
-							 SCHED_RR,
+							 SCHED_DEFAULT,
-							 SCHED_PRIORITY_DEFAULT,
+							 SCHED_PRIORITY_MAX - 5,
 							 20000,
 							 attitude_estimator_ekf_thread_main,
 							 (argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -221,7 +220,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	/* subscribe to raw data */
 	int sub_raw = orb_subscribe(ORB_ID(sensor_combined));
 	/* rate-limit raw data updates to 200Hz */
-	orb_set_interval(sub_raw, 5);
+	orb_set_interval(sub_raw, 4);
+
+	/* subscribe to param changes */
+	int sub_params = orb_subscribe(ORB_ID(parameter_update));
 
 	/* advertise attitude */
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
@@ -236,21 +238,26 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 	struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
 	orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg);
 
+	/* keep track of sensor updates */
+	uint32_t sensor_last_count[3] = {0, 0, 0};
+	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
+	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
 
-	/* process noise covariance */
+	struct attitude_estimator_ekf_params ekf_params;
-	float q[12];
+
-	/* measurement noise covariance */
+	struct attitude_estimator_ekf_param_handles ekf_param_handles;
-	float r[9];
+
-	/* output euler angles */
+	/* initialize parameter handles */
-	float euler[3] = {0.0f, 0.0f, 0.0f};
+	parameters_init(&ekf_param_handles);
 
 	/* Main loop*/
 	while (!thread_should_exit) {
 
-		struct pollfd fds[1] = {
+		struct pollfd fds[2] = {
 			{ .fd = sub_raw,   .events = POLLIN },
+			{ .fd = sub_params, .events = POLLIN }
 		};
-		int ret = poll(fds, 1, 1000);
+		int ret = poll(fds, 2, 1000);
 
 		if (ret < 0) {
 			/* XXX this is seriously bad - should be an emergency */
@@ -259,151 +266,173 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 			printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n");
 		} else {
 	
-			orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
+			/* only update parameters if they changed */
+			if (fds[1].revents & POLLIN) {
+				/* read from param to clear updated flag */
+				struct parameter_update_s update;
+				orb_copy(ORB_ID(parameter_update), sub_params, &update);
 
-			/* Calculate data time difference in seconds */
+				/* update parameters */
-			dt = (raw.timestamp - last_measurement) / 1000000.0f;
+				parameters_update(&ekf_param_handles, &ekf_params);
-			last_measurement = raw.timestamp;
+			}
 
-			/* Fill in gyro measurements */
+			/* only run filter if sensor values changed */
-			z_k[0] =  raw.gyro_rad_s[0];
+			if (fds[0].revents & POLLIN) {
-			z_k[1] =  raw.gyro_rad_s[1];
-			z_k[2] =  raw.gyro_rad_s[2];
 
-			/* scale from 14 bit to m/s2 */
+				/* get latest measurements */
-			z_k[3] = raw.accelerometer_m_s2[0];
+				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
-			z_k[4] = raw.accelerometer_m_s2[1];
-			z_k[5] = raw.accelerometer_m_s2[2];
 
-			z_k[6] = raw.magnetometer_ga[0];
+				/* Calculate data time difference in seconds */
-			z_k[7] = raw.magnetometer_ga[1];
+				dt = (raw.timestamp - last_measurement) / 1000000.0f;
-			z_k[8] = raw.magnetometer_ga[2];
+				last_measurement = raw.timestamp;
+				uint8_t update_vect[3] = {0, 0, 0};
 
-			uint64_t now = hrt_absolute_time();
+				/* Fill in gyro measurements */
-			unsigned int time_elapsed = now - last_run;
+				if (sensor_last_count[0] != raw.gyro_counter) {
-			last_run = now;
+					update_vect[0] = 1;
-
+					sensor_last_count[0] = raw.gyro_counter;
-			if (time_elapsed > loop_interval_alarm) {
+					sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
-				//TODO: add warning, cpu overload here
+					sensor_last_timestamp[0] = raw.timestamp;
-				if (overloadcounter == 20) {
-					printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
-					overloadcounter = 0;
 				}
 
-				overloadcounter++;
+				z_k[0] =  raw.gyro_rad_s[0];
+				z_k[1] =  raw.gyro_rad_s[1];
+				z_k[2] =  raw.gyro_rad_s[2];
+
+				/* update accelerometer measurements */
+				if (sensor_last_count[1] != raw.accelerometer_counter) {
+					update_vect[1] = 1;
+					sensor_last_count[1] = raw.accelerometer_counter;
+					sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
+					sensor_last_timestamp[1] = raw.timestamp;
+				}
+				z_k[3] = raw.accelerometer_m_s2[0];
+				z_k[4] = raw.accelerometer_m_s2[1];
+				z_k[5] = raw.accelerometer_m_s2[2];
+
+				/* update magnetometer measurements */
+				if (sensor_last_count[2] != raw.magnetometer_counter) {
+					update_vect[2] = 1;
+					sensor_last_count[2] = raw.magnetometer_counter;
+					sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
+					sensor_last_timestamp[2] = raw.timestamp;
+				}
+				z_k[6] = raw.magnetometer_ga[0];
+				z_k[7] = raw.magnetometer_ga[1];
+				z_k[8] = raw.magnetometer_ga[2];
+
+				uint64_t now = hrt_absolute_time();
+				unsigned int time_elapsed = now - last_run;
+				last_run = now;
+
+				if (time_elapsed > loop_interval_alarm) {
+					//TODO: add warning, cpu overload here
+					// if (overloadcounter == 20) {
+					// 	printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm);
+					// 	overloadcounter = 0;
+					// }
+
+					overloadcounter++;
+				}
+
+				int32_t z_k_sizes = 9;
+				// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
+
+				static bool const_initialized = false;
+
+				/* initialize with good values once we have a reasonable dt estimate */
+				if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) 
+				{
+					dt = 0.005f;
+					parameters_update(&ekf_param_handles, &ekf_params);
+
+					x_aposteriori_k[0] = z_k[0];
+					x_aposteriori_k[1] = z_k[1];
+					x_aposteriori_k[2] = z_k[2];
+					x_aposteriori_k[3] = 0.0f;
+					x_aposteriori_k[4] = 0.0f;
+					x_aposteriori_k[5] = 0.0f;
+					x_aposteriori_k[6] = z_k[3];
+					x_aposteriori_k[7] = z_k[4];
+					x_aposteriori_k[8] = z_k[5];
+					x_aposteriori_k[9] = z_k[6];
+					x_aposteriori_k[10] = z_k[7];
+					x_aposteriori_k[11] = z_k[8];
+
+					const_initialized = true;
+				}
+
+				/* do not execute the filter if not initialized */
+				if (!const_initialized) {
+					continue;
+				}
+
+				dt = 0.004f;
+
+				uint64_t timing_start = hrt_absolute_time();
+				// attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
+				// 	Rot_matrix, x_aposteriori, P_aposteriori);
+				attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
+					euler, Rot_matrix, x_aposteriori, P_aposteriori);
+				/* swap values for next iteration */
+				memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
+				memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
+				uint64_t timing_diff = hrt_absolute_time() - timing_start;
+
+				// /* print rotation matrix every 200th time */
+				if (printcounter % 200 == 0) {
+					// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
+					// 	x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
+					// 	x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
+					// 	x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
+
+
+					// }
+
+					//printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
+					//printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
+					//printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]);
+				// 	printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
+				// 	       (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
+				// 	       (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
+				}
+
+				// 				int i = printcounter % 9;
+
+				// 	// for (int i = 0; i < 9; i++) {
+				// 		char name[10];
+				// 		sprintf(name, "xapo #%d", i);
+				// 		memcpy(dbg.key, name, sizeof(dbg.key));
+				// 		dbg.value = x_aposteriori[i];
+				// 		orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
+
+				printcounter++;
+
+				if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
+
+				last_data = raw.timestamp;
+
+				/* send out */
+				att.timestamp = raw.timestamp;
+				att.roll = euler[0];
+				att.pitch = euler[1];
+				att.yaw = euler[2];
+
+				// XXX replace with x_apo after fix to filter
+				att.rollspeed = raw.gyro_rad_s[0]; //x_aposteriori[0];
+				att.pitchspeed = raw.gyro_rad_s[1]; //x_aposteriori[1];
+				att.yawspeed = raw.gyro_rad_s[2]; //x_aposteriori[2];
+
+				/* copy offsets */
+				memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
+
+				/* copy rotation matrix */
+				memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
+				att.R_valid = true;
+
+				// Broadcast
+				orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
 			}
-
-			uint8_t update_vect[3] = {1, 1, 1};
-			int32_t z_k_sizes = 9;
-			// float u[4] = {0.0f, 0.0f, 0.0f, 0.0f};
-
-			static bool const_initialized = false;
-
-			/* initialize with good values once we have a reasonable dt estimate */
-			if (!const_initialized /*&& dt < 0.05 && dt > 0.005*/) 
-			{
-				dt = 0.005f;
-				q[0] =  1e1f;
-				q[1] =  1e1f;
-				q[2] =  1e1f;
-				/* process noise gyro offset covariance */
-				q[3] =  1e-4f;
-				q[4] =  1e-4f;
-				q[5] =  1e-4f;
-				q[6] =  1e-1f;
-				q[7] =  1e-1f;
-				q[8] =  1e-1f;
-				q[9] =  1e-1f;
-				q[10] = 1e-1f;
-				q[11] = 1e-1f;
-
-				r[0]= 1e-2f;
-				r[1]= 1e-2f;
-				r[2]= 1e-2f;
-				r[3]= 1e-1f;
-				r[4]= 1e-1f;
-				r[5]= 1e-1f;
-				r[6]= 1e-1f;
-				r[7]= 1e-1f;
-				r[8]= 1e-1f;
-
-				x_aposteriori_k[0] = z_k[0];
-				x_aposteriori_k[1] = z_k[1];
-				x_aposteriori_k[2] = z_k[2];
-				x_aposteriori_k[3] = 0.0f;
-				x_aposteriori_k[4] = 0.0f;
-				x_aposteriori_k[5] = 0.0f;
-				x_aposteriori_k[6] = z_k[3];
-				x_aposteriori_k[7] = z_k[4];
-				x_aposteriori_k[8] = z_k[5];
-				x_aposteriori_k[9] = z_k[6];
-				x_aposteriori_k[10] = z_k[7];
-				x_aposteriori_k[11] = z_k[8];
-
-				const_initialized = true;
-			}
-
-			/* do not execute the filter if not initialized */
-			if (!const_initialized) {
-				continue;
-			}
-
-			dt = 0.004f;
-
-			uint64_t timing_start = hrt_absolute_time();
-			// attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler,
-			// 	Rot_matrix, x_aposteriori, P_aposteriori);
-			attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, q, r,
-				euler, Rot_matrix, x_aposteriori, P_aposteriori);
-			/* swap values for next iteration */
-			memcpy(P_aposteriori_k, P_aposteriori, sizeof(P_aposteriori_k));
-			memcpy(x_aposteriori_k, x_aposteriori, sizeof(x_aposteriori_k));
-			uint64_t timing_diff = hrt_absolute_time() - timing_start;
-
-			// /* print rotation matrix every 200th time */
-			// if (printcounter % 200 == 0) {
-			// 	// printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n",
-			// 	// 	x_aposteriori[0], x_aposteriori[1], x_aposteriori[2],
-			// 	// 	x_aposteriori[3], x_aposteriori[4], x_aposteriori[5],
-			// 	// 	x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]);
-
-
-			// 	// }
-
-			// 	printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt));
-			// 	printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]);
-			// // 	printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100),
-			// // 	       (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100),
-			// // 	       (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100));
-			// }
-
-			// 				int i = printcounter % 9;
-
-			// 	// for (int i = 0; i < 9; i++) {
-			// 		char name[10];
-			// 		sprintf(name, "xapo #%d", i);
-			// 		memcpy(dbg.key, name, sizeof(dbg.key));
-			// 		dbg.value = x_aposteriori[i];
-			// 		orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg);
-
-			printcounter++;
-
-			if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data);
-
-			last_data = raw.timestamp;
-
-			/* send out */
-			att.timestamp = raw.timestamp;
-			att.roll = euler[0];
-			att.pitch = euler[1];
-			att.yaw = euler[2];
-
-			att.rollspeed = x_aposteriori[0];
-			att.pitchspeed = x_aposteriori[1];
-			att.yawspeed = x_aposteriori[2];
-
-			// Broadcast
-			orb_publish(ORB_ID(vehicle_attitude), pub_att, &att);
 		}
 
 		loopcounter++;

apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file attitude_estimator_ekf_params.c
+ * 
+ * Parameters for EKF filter
+ */
+
+#include "attitude_estimator_ekf_params.h"
+
+/* Extended Kalman Filter covariances */
+
+/* gyro process noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_Q0, 1e1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_Q1, 1e1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_Q2, 1e1f);
+/* gyro offsets process noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_Q3, 1e-4f);
+PARAM_DEFINE_FLOAT(EKF_ATT_Q4, 1e-4f);
+PARAM_DEFINE_FLOAT(EKF_ATT_Q5, 1e-4f);
+/* accelerometer process noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_Q6, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_Q7, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_Q8, 1e-1f);
+/* magnetometer process noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_Q9, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_Q10, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_Q11, 1e-1f);
+
+/* gyro measurement noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_R0, 1e-2f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R1, 1e-2f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R2, 1e-2f);
+/* accelerometer measurement noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_R3, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R4, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e-1f);
+/* magnetometer measurement noise */
+PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
+PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f);
+
+int parameters_init(struct attitude_estimator_ekf_param_handles *h)
+{
+	/* PID parameters */
+	h->q0 	=	param_find("EKF_ATT_Q0");
+	h->q1 	=	param_find("EKF_ATT_Q1");
+	h->q2 	=	param_find("EKF_ATT_Q2");
+	h->q3 	=	param_find("EKF_ATT_Q3");
+	h->q4 	=	param_find("EKF_ATT_Q4");
+	h->q5 	=	param_find("EKF_ATT_Q5");
+	h->q6 	=	param_find("EKF_ATT_Q6");
+	h->q7 	=	param_find("EKF_ATT_Q7");
+	h->q8 	=	param_find("EKF_ATT_Q8");
+	h->q9 	=	param_find("EKF_ATT_Q9");
+	h->q10 	=	param_find("EKF_ATT_Q10");
+	h->q11 	=	param_find("EKF_ATT_Q11");
+
+	h->r0 	=	param_find("EKF_ATT_R0");
+	h->r1 	=	param_find("EKF_ATT_R1");
+	h->r2 	=	param_find("EKF_ATT_R2");
+	h->r3 	=	param_find("EKF_ATT_R3");
+	h->r4 	=	param_find("EKF_ATT_R4");
+	h->r5 	=	param_find("EKF_ATT_R5");
+	h->r6 	=	param_find("EKF_ATT_R6");
+	h->r7 	=	param_find("EKF_ATT_R7");
+	h->r8 	=	param_find("EKF_ATT_R8");
+
+	return OK;
+}
+
+int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p)
+{
+	param_get(h->q0, &(p->q[0]));
+	param_get(h->q1, &(p->q[1]));
+	param_get(h->q2, &(p->q[2]));
+	param_get(h->q3, &(p->q[3]));
+	param_get(h->q4, &(p->q[4]));
+	param_get(h->q5, &(p->q[5]));
+	param_get(h->q6, &(p->q[6]));
+	param_get(h->q7, &(p->q[7]));
+	param_get(h->q8, &(p->q[8]));
+	param_get(h->q9, &(p->q[9]));
+	param_get(h->q10, &(p->q[10]));
+	param_get(h->q11, &(p->q[11]));
+
+	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->r4, &(p->r[4]));
+	param_get(h->r5, &(p->r[5]));
+	param_get(h->r6, &(p->r[6]));
+	param_get(h->r7, &(p->r[7]));
+	param_get(h->r8, &(p->r[8]));
+
+	return OK;
+}

apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h

@@ -1,5 +1,8 @@
-/*
+/****************************************************************************
- *   Copyright (C) 2012 Lorenz Meier. All rights reserved.
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -11,7 +14,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name of the author or the names of contributors may be
+ * 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.
  *
@@ -27,50 +30,35 @@
  * 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.
- */
-
-/*
- * Driver for the Meas Spec MS5611 barometric pressure sensor
- */
-
-#include <sys/ioctl.h>
-
-#define _MS5611BASE	0x6A00
-#define MS5611C(_x)	_IOC(_MS5611BASE, _x)
-
-/* 
- * Sets the sensor internal sampling rate, and if a buffer
- * has been configured, the rate at which entries will be
- * added to the buffer.
- */
-#define MS5611_SETRATE		MS5611C(1)
-
-/* set rate (configuration A register */
-#define MS5611_RATE_0_75HZ		(0 << 2) /* 0.75 Hz */
-
-/*
- * Sets the sensor internal range.
- */
-#define MS5611_SETRANGE		MS5611C(2)
-
-#define MS5611_RANGE_0_88GA			(0 << 5)
-
-/*
- * Sets the address of a shared MS5611_buffer
- * structure that is maintained by the driver.
  *
- * If zero is passed as the address, disables
+ ****************************************************************************/
- * the buffer updating.
- */
-#define MS5611_SETBUFFER	MS5611C(3)
 
-struct ms5611_buffer {
+/*
-	uint32_t	size;		/* number of entries in the samples[] array */
+ * @file attitude_estimator_ekf_params.h
-	uint32_t	next;		/* the next entry that will be populated */
+ * 
-	struct {
+ * Parameters for EKF filter
-		uint32_t	pressure;
+ */
-		uint16_t temperature;
+
-	} samples[];
+#include <systemlib/param/param.h>
+
+struct attitude_estimator_ekf_params {
+	float r[9];
+	float q[12];
 };
 
-extern int	ms5611_attach(struct i2c_dev_s *i2c);
+struct attitude_estimator_ekf_param_handles {
+	param_t r0, r1, r2, r3, r4, r5, r6, r7, r8;
+	param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct attitude_estimator_ekf_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct attitude_estimator_ekf_param_handles *h, struct attitude_estimator_ekf_params *p);

apps/commander/commander.c

@@ -293,7 +293,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 	struct sensor_combined_s raw;
 
 	/* 30 seconds */
-	const uint64_t calibration_interval_us = 45 * 1000000;
+	int calibration_interval_ms = 30 * 1000;
 	unsigned int calibration_counter = 0;
 
 	float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX};
@@ -312,10 +312,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 		warn("WARNING: failed to set scale / offsets for mag");
 	close(fd);
 
-	mavlink_log_info(mavlink_fd, "[commander] Please rotate around all axes.");
+	mavlink_log_info(mavlink_fd, "[commander] Please rotate around X");
 	
 	uint64_t calibration_start = hrt_absolute_time();
-	while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) {
+	while ((hrt_absolute_time() - calibration_start)/1000 < calibration_interval_ms) {
 
 		/* wait blocking for new data */
 		struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
@@ -348,11 +348,13 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
 			calibration_counter++;
 		} else {
 			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry.");
+			mavlink_log_info(mavlink_fd, "[commander] mag cal canceled");
 			break;
 		}
 	}
 
+	mavlink_log_info(mavlink_fd, "[commander] mag calibration done");
+
 	/* disable calibration mode */
 	status->flag_preflight_mag_calibration = false;
 	state_machine_publish(status_pub, status, mavlink_fd);
@@ -950,7 +952,7 @@ int commander_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		deamon_task = task_spawn("commander",
-					 SCHED_RR,
+					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 50,
 					 4096,
 					 commander_thread_main,
@@ -1307,18 +1309,12 @@ int commander_thread_main(int argc, char *argv[])
 				//printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count);
 
 				if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) {
-					current_status.flag_control_attitude_enabled = true;
-					current_status.flag_control_rates_enabled = false;
 					update_state_machine_mode_manual(stat_pub, &current_status, mavlink_fd);
 
 				} else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) {
-					current_status.flag_control_attitude_enabled = true;
-					current_status.flag_control_rates_enabled = false;
 					update_state_machine_mode_auto(stat_pub, &current_status, mavlink_fd);
 
 				} else {
-					current_status.flag_control_attitude_enabled = true;
-					current_status.flag_control_rates_enabled = false;
 					update_state_machine_mode_stabilized(stat_pub, &current_status, mavlink_fd);
 				}
 

apps/commander/state_machine_helper.c

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

apps/drivers/bma180/bma180.cpp

@@ -410,8 +410,8 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-				/* XXX 500Hz is just a wild guess */
+				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 500);
+				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {

apps/drivers/device/cdev.cpp

@@ -74,7 +74,7 @@ static int	cdev_poll(struct file *filp, struct pollfd *fds, bool setup);
  * Note that we use the GNU extension syntax here because we don't get designated
  * initialisers in gcc 4.6.
  */
-static const struct file_operations cdev_fops = {
+const struct file_operations CDev::fops = {
 open	: cdev_open,
 close	: cdev_close,
 read	: cdev_read,
@@ -118,7 +118,7 @@ CDev::init()
 		goto out;
 
 	// now register the driver
-	ret = register_driver(_devname, &cdev_fops, 0666, (void *)this);
+	ret = register_driver(_devname, &fops, 0666, (void *)this);
 
 	if (ret != OK)
 		goto out;

apps/drivers/device/device.h

@@ -286,6 +286,12 @@ public:
        bool            is_open() { return _open_count > 0; }
 
 protected:
+	/**
+	 * Pointer to the default cdev file operations table; useful for
+	 * registering clone devices etc.
+	 */
+	static const struct file_operations	fops;
+
 	/**
 	 * Check the current state of the device for poll events from the
 	 * perspective of the file.

apps/drivers/l3gd20/l3gd20.cpp

@@ -420,8 +420,8 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg)
 				/* set default/max polling rate */
 			case SENSOR_POLLRATE_MAX:
 			case SENSOR_POLLRATE_DEFAULT:
-				/* XXX 500Hz is just a wild guess */
+				/* With internal low pass filters enabled, 250 Hz is sufficient */
-				return ioctl(filp, SENSORIOCSPOLLRATE, 500);
+				return ioctl(filp, SENSORIOCSPOLLRATE, 250);
 
 				/* adjust to a legal polling interval in Hz */
 			default: {

apps/examples/Make.defs

@@ -58,7 +58,7 @@ ifeq ($(CONFIG_EXAMPLES_DHCPD),y)
 CONFIGURED_APPS += examples/dhcpd
 endif
 
-ifeq ($(CONFIG_EXAMPLE_DISCOVER),y)
+ifeq ($(CONFIG_EXAMPLES_DISCOVER),y)
 CONFIGURED_APPS += examples/discover
 endif
 
@@ -218,6 +218,10 @@ ifeq ($(CONFIG_EXAMPLES_USBTERM),y)
 CONFIGURED_APPS += examples/usbterm
 endif
 
+ifeq ($(CONFIG_EXAMPLES_WATCHDOG),y)
+CONFIGURED_APPS += examples/watchdog
+endif
+
 ifeq ($(CONFIG_EXAMPLES_WGET),y)
 CONFIGURED_APPS += examples/wget
 endif

apps/examples/README.txt

@@ -60,20 +60,20 @@ examples/buttons
   This is a simple configuration that may be used to test the board-
   specific button interfaces.  Configuration options:
 
-  CONFIG_ARCH_BUTTONS                - Must be defined for button support
+  CONFIG_ARCH_BUTTONS                 - Must be defined for button support
-  CONFIG_EXAMPLE_BUTTONS_MIN         - Lowest button number (MIN=0)
+  CONFIG_EXAMPLES_BUTTONS_MIN         - Lowest button number (MIN=0)
-  CONFIG_EXAMPLE_BUTTONS_MAX         - Highest button number (MAX=7)
+  CONFIG_EXAMPLES_BUTTONS_MAX         - Highest button number (MAX=7)
 
-  CONFIG_ARCH_IRQBUTTONS             - Must be defined for interrupting button support
+  CONFIG_ARCH_IRQBUTTONS              - Must be defined for interrupting button support
-  CONFIG_EXAMPLE_IRQBUTTONS_MIN      - Lowest interrupting button number (MIN=0)
+  CONFIG_EXAMPLES_IRQBUTTONS_MIN      - Lowest interrupting button number (MIN=0)
-  CONFIG_EXAMPLE_IRQBUTTONS_MAX      - Highest interrupting button number (MAX=7)
+  CONFIG_EXAMPLES_IRQBUTTONS_MAX      - Highest interrupting button number (MAX=7)
 
   Name strings for buttons:
   
-    CONFIG_EXAMPLE_BUTTONS_NAME0, CONFIG_EXAMPLE_BUTTONS_NAME1,
+    CONFIG_EXAMPLES_BUTTONS_NAME0, CONFIG_EXAMPLES_BUTTONS_NAME1,
-    CONFIG_EXAMPLE_BUTTONS_NAME2, CONFIG_EXAMPLE_BUTTONS_NAME3,
+    CONFIG_EXAMPLES_BUTTONS_NAME2, CONFIG_EXAMPLES_BUTTONS_NAME3,
-    CONFIG_EXAMPLE_BUTTONS_NAME4, CONFIG_EXAMPLE_BUTTONS_NAME5,
+    CONFIG_EXAMPLES_BUTTONS_NAME4, CONFIG_EXAMPLES_BUTTONS_NAME5,
-    CONFIG_EXAMPLE_BUTTONS_NAME6, CONFIG_EXAMPLE_BUTTONS_NAME7,
+    CONFIG_EXAMPLES_BUTTONS_NAME6, CONFIG_EXAMPLES_BUTTONS_NAME7,
 
   Additional architecture-/board- specific configuration settings may also
   be required.
@@ -260,10 +260,10 @@ examples/dhcpd
                                      configuration settings)
     CONFIG_NET_BROADCAST=y         - UDP broadcast support is needed.
 
-    CONFIG_EXAMPLE_DHCPD_NOMAC     - (May be defined to use software assigned MAC)
+    CONFIG_EXAMPLES_DHCPD_NOMAC     - (May be defined to use software assigned MAC)
-    CONFIG_EXAMPLE_DHCPD_IPADDR    - Target IP address
+    CONFIG_EXAMPLES_DHCPD_IPADDR    - Target IP address
-    CONFIG_EXAMPLE_DHCPD_DRIPADDR  - Default router IP addess
+    CONFIG_EXAMPLES_DHCPD_DRIPADDR  - Default router IP addess
-    CONFIG_EXAMPLE_DHCPD_NETMASK   - Network mask
+    CONFIG_EXAMPLES_DHCPD_NETMASK   - Network mask
 
   See also CONFIG_NETUTILS_DHCPD_* settings described elsewhere
   and used in netutils/dhcpd/dhcpd.c. These settings are required
@@ -291,11 +291,11 @@ examples/discover
 
   NuttX configuration settings:
 
-    CONFIG_EXAMPLE_DISCOVER_DHCPC - DHCP Client
+    CONFIG_EXAMPLES_DISCOVER_DHCPC - DHCP Client
-    CONFIG_EXAMPLE_DISCOVER_NOMAC - Use canned MAC address
+    CONFIG_EXAMPLES_DISCOVER_NOMAC - Use canned MAC address
-    CONFIG_EXAMPLE_DISCOVER_IPADDR - Target IP address
+    CONFIG_EXAMPLES_DISCOVER_IPADDR - Target IP address
-    CONFIG_EXAMPLE_DISCOVER_DRIPADDR - Router IP address
+    CONFIG_EXAMPLES_DISCOVER_DRIPADDR - Router IP address
-    CONFIG_EXAMPLE_DISCOVER_NETMASK - Network Mask
+    CONFIG_EXAMPLES_DISCOVER_NETMASK - Network Mask
 
 examples/ftpc
 ^^^^^^^^^^^^^
@@ -367,12 +367,12 @@ examples/ftpd
   If CONFIG_EXAMPLES_FTPD_NONETINIT is not defined, then the following may
   be specified to customized the network configuration:
 
-    CONFIG_EXAMPLE_FTPD_NOMAC - If the hardware has no MAC address of its
+    CONFIG_EXAMPLES_FTPD_NOMAC - If the hardware has no MAC address of its
       own, define this =y to provide a bogus address for testing.
-    CONFIG_EXAMPLE_FTPD_IPADDR - The target IP address.  Default 10.0.0.2
+    CONFIG_EXAMPLES_FTPD_IPADDR - The target IP address.  Default 10.0.0.2
-    CONFIG_EXAMPLE_FTPD_DRIPADDR - The default router address. Default
+    CONFIG_EXAMPLES_FTPD_DRIPADDR - The default router address. Default
       10.0.0.1
-    CONFIG_EXAMPLE_FTPD_NETMASK - The network mask.  Default: 255.255.255.0
+    CONFIG_EXAMPLES_FTPD_NETMASK - The network mask.  Default: 255.255.255.0
 
   Other required configuration settings:  Of course TCP networking support
   is required.  But here are a couple that are less obvious:
@@ -465,15 +465,15 @@ examples/igmp
   does not do much of value -- Much more is needed in order to verify
   the IGMP features!
 
-  * CONFIG_EXAMPLE_IGMP_NOMAC
+  * CONFIG_EXAMPLES_IGMP_NOMAC
       Set if the hardware has no MAC address; one will be assigned
-  * CONFIG_EXAMPLE_IGMP_IPADDR
+  * CONFIG_EXAMPLES_IGMP_IPADDR
       Target board IP address
-  * CONFIG_EXAMPLE_IGMP_DRIPADDR
+  * CONFIG_EXAMPLES_IGMP_DRIPADDR
       Default router address
-  * CONFIG_EXAMPLE_IGMP_NETMASK
+  * CONFIG_EXAMPLES_IGMP_NETMASK
       Network mask
-  * CONFIG_EXAMPLE_IGMP_GRPADDR
+  * CONFIG_EXAMPLES_IGMP_GRPADDR
       Multicast group address
 
   Applications using this example will need to provide an appconfig
@@ -1023,10 +1023,10 @@ examples/poll
   CONFIG_NSOCKET_DESCRIPTORS        - Defined to be greater than 0
   CONFIG_NET_NTCP_READAHEAD_BUFFERS - Defined to be greater than zero
 
-  CONFIG_EXAMPLE_POLL_NOMAC         - (May be defined to use software assigned MAC)
+  CONFIG_EXAMPLES_POLL_NOMAC         - (May be defined to use software assigned MAC)
-  CONFIG_EXAMPLE_POLL_IPADDR        - Target IP address
+  CONFIG_EXAMPLES_POLL_IPADDR        - Target IP address
-  CONFIG_EXAMPLE_POLL_DRIPADDR      - Default router IP addess
+  CONFIG_EXAMPLES_POLL_DRIPADDR      - Default router IP addess
-  CONFIG_EXAMPLE_POLL_NETMASK       - Network mask
+  CONFIG_EXAMPLES_POLL_NETMASK       - Network mask
 
   In order to for select to work with incoming connections, you
   must also select:
@@ -1163,14 +1163,14 @@ examples/sendmail
 
  Settings unique to this example include:
 
-    CONFIG_EXAMPLE_SENDMAIL_NOMAC     - May be defined to use software assigned MAC (optional)
+    CONFIG_EXAMPLES_SENDMAIL_NOMAC     - May be defined to use software assigned MAC (optional)
-    CONFIG_EXAMPLE_SENDMAIL_IPADDR    - Target IP address (required)
+    CONFIG_EXAMPLES_SENDMAIL_IPADDR    - Target IP address (required)
-    CONFIG_EXAMPLE_SENDMAIL_DRIPADDR  - Default router IP addess (required)
+    CONFIG_EXAMPLES_SENDMAIL_DRIPADDR  - Default router IP addess (required)
-    CONFIG_EXAMPLE_SENDMAILT_NETMASK  - Network mask (required)
+    CONFIG_EXAMPLES_SENDMAILT_NETMASK  - Network mask (required)
-    CONFIG_EXAMPLE_SENDMAIL_RECIPIENT - The recipient of the email (required)
+    CONFIG_EXAMPLES_SENDMAIL_RECIPIENT - The recipient of the email (required)
-    CONFIG_EXAMPLE_SENDMAIL_SENDER    - Optional. Default: "nuttx-testing@example.com"
+    CONFIG_EXAMPLES_SENDMAIL_SENDER    - Optional. Default: "nuttx-testing@example.com"
-    CONFIG_EXAMPLE_SENDMAIL_SUBJECT   - Optional. Default: "Testing SMTP from NuttX"
+    CONFIG_EXAMPLES_SENDMAIL_SUBJECT   - Optional. Default: "Testing SMTP from NuttX"
-    CONFIG_EXAMPLE_SENDMAIL_BODY   -    Optional. Default: "Test message sent by NuttX"
+    CONFIG_EXAMPLES_SENDMAIL_BODY   -    Optional. Default: "Test message sent by NuttX"
 
   NOTE: This test has not been verified on the NuttX target environment.
   As of this writing, unit-tested in the Cygwin/Linux host environment.
@@ -1213,12 +1213,12 @@ examples/telnetd
       Default: SCHED_PRIORITY_DEFAULT
     CONFIG_EXAMPLES_TELNETD_CLIENTSTACKSIZE - Stack size allocated for the
       Telnet client. Default: 2048
-    CONFIG_EXAMPLE_TELNETD_NOMAC - If the hardware has no MAC address of its
+    CONFIG_EXAMPLES_TELNETD_NOMAC - If the hardware has no MAC address of its
       own, define this =y to provide a bogus address for testing.
-    CONFIG_EXAMPLE_TELNETD_IPADDR - The target IP address.  Default 10.0.0.2
+    CONFIG_EXAMPLES_TELNETD_IPADDR - The target IP address.  Default 10.0.0.2
-    CONFIG_EXAMPLE_TELNETD_DRIPADDR - The default router address. Default
+    CONFIG_EXAMPLES_TELNETD_DRIPADDR - The default router address. Default
       10.0.0.1
-    CONFIG_EXAMPLE_TELNETD_NETMASK - The network mask.  Default: 255.255.255.0
+    CONFIG_EXAMPLES_TELNETD_NETMASK - The network mask.  Default: 255.255.255.0
  
   The appconfig file (apps/.config) should include:
 
@@ -1240,9 +1240,9 @@ examples/thttpd
   CGI programs.  see configs/README.txt for most THTTPD settings.
   In addition to those, this example accepts:
 
-    CONFIG_EXAMPLE_THTTPD_NOMAC    - (May be defined to use software assigned MAC)
+    CONFIG_EXAMPLES_THTTPD_NOMAC    - (May be defined to use software assigned MAC)
-    CONFIG_EXAMPLE_THTTPD_DRIPADDR - Default router IP addess
+    CONFIG_EXAMPLES_THTTPD_DRIPADDR - Default router IP addess
-    CONFIG_EXAMPLE_THTTPD_NETMASK  - Network mask
+    CONFIG_EXAMPLES_THTTPD_NETMASK  - Network mask
 
   Applications using this example will need to provide an appconfig
   file in the configuration directory with instruction to build applications
@@ -1335,11 +1335,11 @@ examples/uip
   This is a port of uIP tiny webserver example application.  Settings
   specific to this example include:
 
-    CONFIG_EXAMPLE_UIP_NOMAC     - (May be defined to use software assigned MAC)
+    CONFIG_EXAMPLES_UIP_NOMAC     - (May be defined to use software assigned MAC)
-    CONFIG_EXAMPLE_UIP_IPADDR    - Target IP address
+    CONFIG_EXAMPLES_UIP_IPADDR    - Target IP address
-    CONFIG_EXAMPLE_UIP_DRIPADDR  - Default router IP addess
+    CONFIG_EXAMPLES_UIP_DRIPADDR  - Default router IP addess
-    CONFIG_EXAMPLE_UIP_NETMASK   - Network mask
+    CONFIG_EXAMPLES_UIP_NETMASK   - Network mask
-    CONFIG_EXAMPLE_UIP_DHCPC     - Select to get IP address via DHCP
+    CONFIG_EXAMPLES_UIP_DHCPC     - Select to get IP address via DHCP
 
   If you use DHCPC, then some special configuration network options are
   required.  These include:
@@ -1637,11 +1637,11 @@ examples/wget
   A simple web client example.  It will obtain a file from a server using the HTTP
   protocol.  Settings unique to this example include:
 
-    CONFIG_EXAMPLE_WGET_URL       - The URL of the file to get
+    CONFIG_EXAMPLES_WGET_URL       - The URL of the file to get
-    CONFIG_EXAMPLE_WGET_NOMAC     - (May be defined to use software assigned MAC)
+    CONFIG_EXAMPLES_WGET_NOMAC     - (May be defined to use software assigned MAC)
-    CONFIG_EXAMPLE_WGET_IPADDR    - Target IP address
+    CONFIG_EXAMPLES_WGET_IPADDR    - Target IP address
-    CONFIG_EXAMPLE_WGET_DRIPADDR  - Default router IP addess
+    CONFIG_EXAMPLES_WGET_DRIPADDR  - Default router IP addess
-    CONFIG_EXAMPLE_WGET_NETMASK   - Network mask
+    CONFIG_EXAMPLES_WGET_NETMASK   - Network mask
 
   This example uses netutils/webclient.  Additional configuration settings apply
   to that code as follows (but built-in defaults are probably OK):

apps/examples/adc/Kconfig

@@ -6,8 +6,32 @@
 config EXAMPLES_ADC
 	bool "ADC example"
 	default n
+	depends on ADC
 	---help---
 		Enable the ADC example
 
 if EXAMPLES_ADC
+
+config EXAMPLES_ADC_DEVPATH
+	string "ADC device path"
+	default "/dev/adc0"
+	---help---
+		The default path to the ADC device. Default: /dev/adc0
+
+config EXAMPLES_ADC_NSAMPLES
+	int "Number of Sample Groups"
+	default 0
+	depends on !NSH_BUILTIN_APPS
+	---help---
+		If NSH_BUILTIN_APPS is defined, then the number of samples is provided
+		on the command line and this value is ignored.  Otherwise, this number
+		of samples is collected and the program terminates.  Default:  0 (samples
+		are collected indefinitely).
+
+config EXAMPLES_ADC_GROUPSIZE
+	int "Number of Samples per Group"
+	default 4
+	---help---
+		The number of samples to read at once. Default: 4
+
 endif

apps/examples/adc/adc_main.c

@@ -57,6 +57,14 @@
  * Pre-processor Definitions
  ****************************************************************************/
 
+/* Use CONFIG_EXAMPLES_ADC_NSAMPLES == 0 to mean to collect samples
+ * indefinitely.
+ */
+
+#ifndef CONFIG_EXAMPLES_ADC_NSAMPLES
+#  define CONFIG_EXAMPLES_ADC_NSAMPLES 0
+#endif
+
 /****************************************************************************
  * Private Types
  ****************************************************************************/
@@ -249,7 +257,7 @@ int adc_main(int argc, char *argv[])
 
       adc_devpath(&g_adcstate, CONFIG_EXAMPLES_ADC_DEVPATH);
 
-#ifdef CONFIG_EXAMPLES_ADC_NSAMPLES
+#if CONFIG_EXAMPLES_ADC_NSAMPLES > 0
       g_adcstate.count = CONFIG_EXAMPLES_ADC_NSAMPLES;
 #else
       g_adcstate.count = 1;
@@ -267,7 +275,7 @@ int adc_main(int argc, char *argv[])
    * samples that we collect before returning.  Otherwise, we never return
    */
 
-#if defined(CONFIG_NSH_BUILTIN_APPS) || defined(CONFIG_EXAMPLES_ADC_NSAMPLES)
+#if defined(CONFIG_NSH_BUILTIN_APPS) || CONFIG_EXAMPLES_ADC_NSAMPLES > 0
   message("adc_main: g_adcstate.count: %d\n", g_adcstate.count);
 #endif
 
@@ -290,7 +298,7 @@ int adc_main(int argc, char *argv[])
 
 #if defined(CONFIG_NSH_BUILTIN_APPS)
   for (; g_adcstate.count > 0; g_adcstate.count--)
-#elif defined(CONFIG_EXAMPLES_ADC_NSAMPLES)
+#elif CONFIG_EXAMPLES_ADC_NSAMPLES > 0
   for (g_adcstate.count = 0; g_adcstate.count < CONFIG_EXAMPLES_ADC_NSAMPLES; g_adcstate.count++)
 #else
   for (;;)

apps/examples/buttons/Kconfig

@@ -7,7 +7,57 @@ config EXAMPLES_BUTTONS
 	bool "Buttons example"
 	default n
 	---help---
-		Enable the buttons example
+		Enable the buttons example.  May require ARCH_BUTTONS on some boards.
 
 if EXAMPLES_BUTTONS
+config EXAMPLES_BUTTONS_MIN
+int "Lowest Button Number"
+default 0
+
+config EXAMPLES_BUTTONS_MAX
+int "Highest Button Number"
+default 7
+
+if ARCH_IRQBUTTONS 
+config EXAMPLES_IRQBUTTONS_MIN
+int "Lowest Interrupting Button Number"
+default 0
+
+config EXAMPLES_IRQBUTTONS_MAX
+int "Highest Interrupting Button Number"
+default 7
+
+config EXAMPLES_BUTTONS_NAME0
+string "Button 0 Name"
+default "Button 0"
+
+config EXAMPLES_BUTTONS_NAME1
+string "Button 1 Name"
+default "Button 1"
+
+config EXAMPLES_BUTTONS_NAME2
+string "Button 2 Name"
+default "Button 2"
+
+config EXAMPLES_BUTTONS_NAME3
+string "Button 3 Name"
+default "Button 3"
+
+config EXAMPLES_BUTTONS_NAME4
+string "Button 4 Name"
+default "Button 4"
+
+config EXAMPLES_BUTTONS_NAME5
+string "Button 5 Name"
+default "Button 5"
+
+config EXAMPLES_BUTTONS_NAME6
+string "Button 6 Name"
+default "Button 6"
+
+config EXAMPLES_BUTTONS_NAME7
+string "Button 7 Name"
+default "Button 7"
+
+endif
 endif

apps/examples/buttons/buttons_main.c

@@ -61,60 +61,60 @@
 #  error "CONFIG_ARCH_BUTTONS is not defined in the configuration"
 #endif
 
-#ifndef CONFIG_EXAMPLE_BUTTONS_NAME0
+#ifndef CONFIG_EXAMPLES_BUTTONS_NAME0
-#  define CONFIG_EXAMPLE_BUTTONS_NAME0 "BUTTON0"
+#  define CONFIG_EXAMPLES_BUTTONS_NAME0 "BUTTON0"
 #endif
-#ifndef CONFIG_EXAMPLE_BUTTONS_NAME1
+#ifndef CONFIG_EXAMPLES_BUTTONS_NAME1
-#  define CONFIG_EXAMPLE_BUTTONS_NAME1 "BUTTON1"
+#  define CONFIG_EXAMPLES_BUTTONS_NAME1 "BUTTON1"
 #endif
-#ifndef CONFIG_EXAMPLE_BUTTONS_NAME2
+#ifndef CONFIG_EXAMPLES_BUTTONS_NAME2
-#  define CONFIG_EXAMPLE_BUTTONS_NAME2 "BUTTON2"
+#  define CONFIG_EXAMPLES_BUTTONS_NAME2 "BUTTON2"
 #endif
-#ifndef CONFIG_EXAMPLE_BUTTONS_NAME3
+#ifndef CONFIG_EXAMPLES_BUTTONS_NAME3
-#  define CONFIG_EXAMPLE_BUTTONS_NAME3 "BUTTON3"
+#  define CONFIG_EXAMPLES_BUTTONS_NAME3 "BUTTON3"
 #endif
-#ifndef CONFIG_EXAMPLE_BUTTONS_NAME4
+#ifndef CONFIG_EXAMPLES_BUTTONS_NAME4
-#  define CONFIG_EXAMPLE_BUTTONS_NAME4 "BUTTON4"
+#  define CONFIG_EXAMPLES_BUTTONS_NAME4 "BUTTON4"
 #endif
-#ifndef CONFIG_EXAMPLE_BUTTONS_NAME5
+#ifndef CONFIG_EXAMPLES_BUTTONS_NAME5
-#  define CONFIG_EXAMPLE_BUTTONS_NAME5 "BUTTON5"
+#  define CONFIG_EXAMPLES_BUTTONS_NAME5 "BUTTON5"
 #endif
-#ifndef CONFIG_EXAMPLE_BUTTONS_NAME6
+#ifndef CONFIG_EXAMPLES_BUTTONS_NAME6
-#  define CONFIG_EXAMPLE_BUTTONS_NAME6 "BUTTON6"
+#  define CONFIG_EXAMPLES_BUTTONS_NAME6 "BUTTON6"
 #endif
-#ifndef CONFIG_EXAMPLE_BUTTONS_NAME7
+#ifndef CONFIG_EXAMPLES_BUTTONS_NAME7
-#  define CONFIG_EXAMPLE_BUTTONS_NAME7 "BUTTON7"
+#  define CONFIG_EXAMPLES_BUTTONS_NAME7 "BUTTON7"
 #endif
 
 #define BUTTON_MIN 0
 #define BUTTON_MAX 7
 
-#ifndef CONFIG_EXAMPLE_BUTTONS_MIN
+#ifndef CONFIG_EXAMPLES_BUTTONS_MIN
-#  define CONFIG_EXAMPLE_BUTTONS_MIN BUTTON_MIN
+#  define CONFIG_EXAMPLES_BUTTONS_MIN BUTTON_MIN
 #endif
-#ifndef CONFIG_EXAMPLE_BUTTONS_MAX
+#ifndef CONFIG_EXAMPLES_BUTTONS_MAX
-#  define CONFIG_EXAMPLE_BUTTONS_MAX BUTTON_MAX
+#  define CONFIG_EXAMPLES_BUTTONS_MAX BUTTON_MAX
 #endif
 
-#if CONFIG_EXAMPLE_BUTTONS_MIN > CONFIG_EXAMPLE_BUTTONS_MAX
+#if CONFIG_EXAMPLES_BUTTONS_MIN > CONFIG_EXAMPLES_BUTTONS_MAX
-#  error "CONFIG_EXAMPLE_BUTTONS_MIN > CONFIG_EXAMPLE_BUTTONS_MAX"
+#  error "CONFIG_EXAMPLES_BUTTONS_MIN > CONFIG_EXAMPLES_BUTTONS_MAX"
 #endif
-#if CONFIG_EXAMPLE_BUTTONS_MAX > 7
+#if CONFIG_EXAMPLES_BUTTONS_MAX > 7
-#  error "CONFIG_EXAMPLE_BUTTONS_MAX > 7"
+#  error "CONFIG_EXAMPLES_BUTTONS_MAX > 7"
 #endif
 
-#ifndef CONFIG_EXAMPLE_IRQBUTTONS_MIN
+#ifndef CONFIG_EXAMPLES_IRQBUTTONS_MIN
-#  define CONFIG_EXAMPLE_IRQBUTTONS_MIN CONFIG_EXAMPLE_BUTTONS_MIN
+#  define CONFIG_EXAMPLES_IRQBUTTONS_MIN CONFIG_EXAMPLES_BUTTONS_MIN
 #endif
-#ifndef CONFIG_EXAMPLE_IRQBUTTONS_MAX
+#ifndef CONFIG_EXAMPLES_IRQBUTTONS_MAX
-#  define CONFIG_EXAMPLE_IRQBUTTONS_MAX CONFIG_EXAMPLE_BUTTONS_MAX
+#  define CONFIG_EXAMPLES_IRQBUTTONS_MAX CONFIG_EXAMPLES_BUTTONS_MAX
 #endif
 
-#if CONFIG_EXAMPLE_IRQBUTTONS_MIN > CONFIG_EXAMPLE_IRQBUTTONS_MAX
+#if CONFIG_EXAMPLES_IRQBUTTONS_MIN > CONFIG_EXAMPLES_IRQBUTTONS_MAX
-#  error "CONFIG_EXAMPLE_IRQBUTTONS_MIN > CONFIG_EXAMPLE_IRQBUTTONS_MAX"
+#  error "CONFIG_EXAMPLES_IRQBUTTONS_MIN > CONFIG_EXAMPLES_IRQBUTTONS_MAX"
 #endif
-#if CONFIG_EXAMPLE_IRQBUTTONS_MAX > 7
+#if CONFIG_EXAMPLES_IRQBUTTONS_MAX > 7
-#  error "CONFIG_EXAMPLE_IRQBUTTONS_MAX > 7"
+#  error "CONFIG_EXAMPLES_IRQBUTTONS_MAX > 7"
 #endif
 
 #ifndef MIN
@@ -124,8 +124,8 @@
 #  define MAX(a,b) (a > b ? a : b)
 #endif
 
-#define MIN_BUTTON MIN(CONFIG_EXAMPLE_BUTTONS_MIN, CONFIG_EXAMPLE_IRQBUTTONS_MIN)
+#define MIN_BUTTON MIN(CONFIG_EXAMPLES_BUTTONS_MIN, CONFIG_EXAMPLES_IRQBUTTONS_MIN)
-#define MAX_BUTTON MAX(CONFIG_EXAMPLE_BUTTONS_MAX, CONFIG_EXAMPLE_IRQBUTTONS_MAX)
+#define MAX_BUTTON MAX(CONFIG_EXAMPLES_BUTTONS_MAX, CONFIG_EXAMPLES_IRQBUTTONS_MAX)
 
 #define NUM_BUTTONS     (MAX_BUTTON - MIN_BUTTON + 1)
 #define BUTTON_INDEX(b) ((b)-MIN_BUTTON)
@@ -187,7 +187,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
 {
 #if MIN_BUTTON < 1
   {
-    CONFIG_EXAMPLE_BUTTONS_NAME0,
+    CONFIG_EXAMPLES_BUTTONS_NAME0,
 #ifdef CONFIG_ARCH_IRQBUTTONS
     button0_handler
 #endif
@@ -195,7 +195,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
 #endif
 #if MIN_BUTTON < 2 && MAX_BUTTON > 0
   {
-    CONFIG_EXAMPLE_BUTTONS_NAME1,
+    CONFIG_EXAMPLES_BUTTONS_NAME1,
 #ifdef CONFIG_ARCH_IRQBUTTONS
     button1_handler
 #endif
@@ -203,7 +203,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
 #endif
 #if MIN_BUTTON < 3 && MAX_BUTTON > 1
   {
-    CONFIG_EXAMPLE_BUTTONS_NAME2,
+    CONFIG_EXAMPLES_BUTTONS_NAME2,
 #ifdef CONFIG_ARCH_IRQBUTTONS
     button2_handler
 #endif
@@ -211,7 +211,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
 #endif
 #if MIN_BUTTON < 4 && MAX_BUTTON > 2
   {
-    CONFIG_EXAMPLE_BUTTONS_NAME3,
+    CONFIG_EXAMPLES_BUTTONS_NAME3,
 #ifdef CONFIG_ARCH_IRQBUTTONS
     button3_handler
 #endif
@@ -219,7 +219,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
 #endif
 #if MIN_BUTTON < 5 && MAX_BUTTON > 3
   {
-    CONFIG_EXAMPLE_BUTTONS_NAME4,
+    CONFIG_EXAMPLES_BUTTONS_NAME4,
 #ifdef CONFIG_ARCH_IRQBUTTONS
     button4_handler
 #endif
@@ -227,7 +227,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
 #endif
 #if MIN_BUTTON < 6 && MAX_BUTTON > 4
   {
-    CONFIG_EXAMPLE_BUTTONS_NAME5,
+    CONFIG_EXAMPLES_BUTTONS_NAME5,
 #ifdef CONFIG_ARCH_IRQBUTTONS
     button5_handler
 #endif
@@ -235,7 +235,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
 #endif
 #if MIN_BUTTON < 7 && MAX_BUTTON > 5
   {
-    CONFIG_EXAMPLE_BUTTONS_NAME6,
+    CONFIG_EXAMPLES_BUTTONS_NAME6,
 #ifdef CONFIG_ARCH_IRQBUTTONS
     button6_handler
 #endif
@@ -243,7 +243,7 @@ static const struct button_info_s g_buttoninfo[NUM_BUTTONS] =
 #endif
 #if MAX_BUTTON > 6
   {
-    CONFIG_EXAMPLE_BUTTONS_NAME7,
+    CONFIG_EXAMPLES_BUTTONS_NAME7,
 #ifdef CONFIG_ARCH_IRQBUTTONS
     button7_handler
 #endif
@@ -419,7 +419,7 @@ int buttons_main(int argc, char *argv[])
   /* Register to recieve button interrupts */
 
 #ifdef CONFIG_ARCH_IRQBUTTONS
-  for (i = CONFIG_EXAMPLE_IRQBUTTONS_MIN; i <= CONFIG_EXAMPLE_IRQBUTTONS_MAX; i++)
+  for (i = CONFIG_EXAMPLES_IRQBUTTONS_MIN; i <= CONFIG_EXAMPLES_IRQBUTTONS_MAX; i++)
     {
       xcpt_t oldhandler = up_irqbutton(i, g_buttoninfo[BUTTON_INDEX(i)].handler);
 
@@ -488,7 +488,7 @@ int buttons_main(int argc, char *argv[])
   /* Un-register button handlers */
 
 #if defined(CONFIG_ARCH_IRQBUTTONS) && defined(CONFIG_NSH_BUILTIN_APPS)
-  for (i = CONFIG_EXAMPLE_IRQBUTTONS_MIN; i <= CONFIG_EXAMPLE_IRQBUTTONS_MAX; i++)
+  for (i = CONFIG_EXAMPLES_IRQBUTTONS_MIN; i <= CONFIG_EXAMPLES_IRQBUTTONS_MAX; i++)
     {
       (void)up_irqbutton(i, NULL);
     }

apps/examples/poll/net_listener.c

@@ -290,14 +290,14 @@ static inline bool net_mksocket(struct net_listener_s *nls)
 static void net_configure(void)
 {
   struct in_addr addr;
-#if defined(CONFIG_EXAMPLE_POLL_NOMAC)
+#if defined(CONFIG_EXAMPLES_POLL_NOMAC)
   uint8_t mac[IFHWADDRLEN];
 #endif
 
   /* Configure uIP */
   /* Many embedded network interfaces must have a software assigned MAC */
 
-#ifdef CONFIG_EXAMPLE_POLL_NOMAC
+#ifdef CONFIG_EXAMPLES_POLL_NOMAC
   mac[0] = 0x00;
   mac[1] = 0xe0;
   mac[2] = 0xde;
@@ -309,17 +309,17 @@ static void net_configure(void)
 
   /* Set up our host address */
 
-  addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_IPADDR);
+  addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_IPADDR);
   uip_sethostaddr("eth0", &addr);
 
   /* Set up the default router address */
 
-  addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_DRIPADDR);
+  addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_DRIPADDR);
   uip_setdraddr("eth0", &addr);
 
   /* Setup the subnet mask */
 
-  addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_NETMASK);
+  addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_NETMASK);
   uip_setnetmask("eth0", &addr);
 }
 

apps/examples/poll/net_reader.c

@@ -83,14 +83,14 @@
 static void net_configure(void)
 {
   struct in_addr addr;
-#if defined(CONFIG_EXAMPLE_POLL_NOMAC)
+#if defined(CONFIG_EXAMPLES_POLL_NOMAC)
   uint8_t mac[IFHWADDRLEN];
 #endif
 
   /* Configure uIP */
   /* Many embedded network interfaces must have a software assigned MAC */
 
-#ifdef CONFIG_EXAMPLE_POLL_NOMAC
+#ifdef CONFIG_EXAMPLES_POLL_NOMAC
   mac[0] = 0x00;
   mac[1] = 0xe0;
   mac[2] = 0xde;
@@ -102,17 +102,17 @@ static void net_configure(void)
 
   /* Set up our host address */
 
-  addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_IPADDR);
+  addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_IPADDR);
   uip_sethostaddr("eth0", &addr);
 
   /* Set up the default router address */
 
-  addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_DRIPADDR);
+  addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_DRIPADDR);
   uip_setdraddr("eth0", &addr);
 
   /* Setup the subnet mask */
 
-  addr.s_addr = HTONL(CONFIG_EXAMPLE_POLL_NETMASK);
+  addr.s_addr = HTONL(CONFIG_EXAMPLES_POLL_NETMASK);
   uip_setnetmask("eth0", &addr);
 }
 

apps/examples/px4_deamon_app/px4_deamon_app.c

@@ -92,12 +92,11 @@ int px4_deamon_app_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		deamon_task = task_spawn("deamon",
-					 SCHED_RR,
+					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT,
 					 4096,
 					 px4_deamon_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
-		thread_running = true;
 		exit(0);
 	}
 
@@ -123,6 +122,8 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
 
 	printf("[deamon] starting\n");
 
+	thread_running = true;
+
 	while (!thread_should_exit) {
 		printf("Hello Deamon!\n");
 		sleep(10);
@@ -130,5 +131,7 @@ int px4_deamon_thread_main(int argc, char *argv[]) {
 
 	printf("[deamon] exiting.\n");
 
+	thread_running = false;
+
 	return 0;
 }

apps/fixedwing_control/fixedwing_control.c

@@ -31,7 +31,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  *
  ****************************************************************************/
-
+// Workflow test comment - DEW
 /**
  * @file fixedwing_control.c
  * Implementation of a fixed wing attitude and position controller.
@@ -416,7 +416,7 @@ int fixedwing_control_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		deamon_task = task_spawn("fixedwing_control",
-					 SCHED_RR,
+					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
 					 4096,
 					 fixedwing_control_thread_main,

apps/gps/gps.c

@@ -143,7 +143,7 @@ int gps_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		deamon_task = task_spawn("gps",
-					 SCHED_RR,
+					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT,
 					 4096,
 					 gps_thread_main,

apps/gps/ubx.c

@@ -781,6 +781,8 @@ void *ubx_watchdog_loop(void *args)
 		} else {
 			/* gps healthy */
 			ubx_success_count++;
+			ubx_healthy = true;
+			ubx_fail_count = 0;
 
 			if (!ubx_healthy && ubx_success_count == UBX_HEALTH_SUCCESS_COUNTER_LIMIT) {
 				//printf("[gps] ublox UBX module status ok (baud=%d)\r\n", current_gps_speed);

apps/mavlink/mavlink.c

@@ -68,6 +68,7 @@
 #include <uORB/topics/offboard_control_setpoint.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -134,6 +135,8 @@ static struct vehicle_command_s vcmd;
 
 static struct actuator_armed_s armed;
 
+static struct vehicle_vicon_position_s vicon_position;
+
 static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
@@ -191,8 +194,10 @@ static struct mavlink_subscriptions {
 
 static struct mavlink_publications {
 	orb_advert_t offboard_control_sp_pub;
+	orb_advert_t vicon_position_pub;
 } mavlink_pubs = {
-	.offboard_control_sp_pub = -1
+	.offboard_control_sp_pub = -1,
+	.vicon_position_pub = -1
 };
 
 
@@ -884,7 +889,7 @@ static void *uorb_receiveloop(void *arg)
 				/* copy rc channels into local buffer */
 				orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
 				/* Channels are sent in MAVLink main loop at a fixed interval */
-				mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw,
+			mavlink_msg_rc_channels_raw_send(chan, rc.timestamp / 1000, 0, rc.chan[0].raw, rc.chan[1].raw, rc.chan[2].raw, rc.chan[3].raw,
 							 rc.chan[4].raw, rc.chan[5].raw, rc.chan[6].raw, rc.chan[7].raw, rc.rssi);
 			}
 
@@ -1240,9 +1245,26 @@ void handleMessage(mavlink_message_t *msg)
 		/* check if topic is advertised */
 		if (cmd_pub <= 0) {
 			cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+		} else {
+			/* create command */
+			orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
+		}
+	}
+
+	/* Handle Vicon position estimates */
+	if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) {
+		mavlink_vicon_position_estimate_t pos;
+		mavlink_msg_vicon_position_estimate_decode(msg, &pos);
+
+		vicon_position.x = pos.x;
+		vicon_position.y = pos.y;
+		vicon_position.z = pos.z;
+
+		if (mavlink_pubs.vicon_position_pub <= 0) {
+			mavlink_pubs.vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position);
+		} else {
+			orb_publish(ORB_ID(vehicle_vicon_position), mavlink_pubs.vicon_position_pub, &vicon_position);
 		}
-		/* create command */
-		orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
 	}
 
 	/* Handle quadrotor motor setpoints */
@@ -1250,7 +1272,8 @@ void handleMessage(mavlink_message_t *msg)
 	if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) {
 		mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint;
 		mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint);
-//		printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", quad_motors_setpoint.target_system, mavlink_system.sysid);
+		//printf("got message\n");
+		//printf("got MAVLINK_MSG_ID_SET_QUAD_MOTORS_SETPOINT target_system=%u, sysid = %u\n", mavlink_system.sysid, quad_motors_setpoint.mode);
 
 		if (mavlink_system.sysid < 4) {
 			/* 
@@ -1260,19 +1283,28 @@ void handleMessage(mavlink_message_t *msg)
 			uint8_t ml_mode = 0;
 			bool ml_armed = false;
 
-			if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) {
+//			if (quad_motors_setpoint.mode & MAVLINK_OFFBOARD_CONTROL_FLAG_ARMED) {
-				ml_armed = true;
+//				ml_armed = true;
-			}
+//			}
 
 			switch (quad_motors_setpoint.mode) {
 				case 0:
+					ml_armed = false;
+
 					break;
 				case 1:
+
 					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES;
-					break;
+					ml_armed = true;
+
+				break;
 				case 2:
+
+
 					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE;
-					break;
+					ml_armed = true;
+
+				break;
 				case 3:
 					ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY;
 					break;
@@ -1284,7 +1316,14 @@ void handleMessage(mavlink_message_t *msg)
 			offboard_control_sp.p1 = quad_motors_setpoint.roll[mavlink_system.sysid]   / (float)INT16_MAX;
 			offboard_control_sp.p2 = quad_motors_setpoint.pitch[mavlink_system.sysid]  / (float)INT16_MAX;
 			offboard_control_sp.p3= quad_motors_setpoint.yaw[mavlink_system.sysid]    / (float)INT16_MAX;
-			offboard_control_sp.p4 = quad_motors_setpoint.thrust[mavlink_system.sysid] / (float)UINT16_MAX;
+			offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid]/(float)UINT16_MAX;
+			//offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid] ;
+
+			if (quad_motors_setpoint.thrust[mavlink_system.sysid] ==0){
+				ml_armed = false;
+
+			}
+
 			offboard_control_sp.armed = ml_armed;
 			offboard_control_sp.mode = ml_mode;
 
@@ -1418,6 +1457,8 @@ void handleMessage(mavlink_message_t *msg)
 			memset(&rc_hil, 0, sizeof(rc_hil));
 			static orb_advert_t rc_pub = 0;
 
+			rc_hil.timestamp = hrt_absolute_time();
+			rc_hil.chan_count = 4;
 			rc_hil.chan[0].raw = 1500 + man.x / 2;
 			rc_hil.chan[1].raw = 1500 + man.y / 2;
 			rc_hil.chan[2].raw = 1500 + man.r / 2;
@@ -1431,6 +1472,7 @@ void handleMessage(mavlink_message_t *msg)
 			struct manual_control_setpoint_s mc;
 			static orb_advert_t mc_pub = 0;
 
+			mc.timestamp = rc_hil.timestamp;
 			mc.roll = man.x / 1000.0f;
 			mc.pitch = man.y / 1000.0f;
 			mc.yaw = man.r / 1000.0f;
@@ -1695,14 +1737,6 @@ int mavlink_thread_main(int argc, char *argv[])
 	/* all subscriptions are now active, set up initial guess about rate limits */
 	if (baudrate >= 460800) {
 		/* 200 Hz / 5 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 5);
-		/* 200 Hz / 5 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 3);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
-		/* 5 Hz */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
 	} else if (baudrate >= 230400) {
 		/* 200 Hz / 5 ms */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
@@ -1716,13 +1750,13 @@ int mavlink_thread_main(int argc, char *argv[])
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);
 	} else if (baudrate >= 115200) {
 		/* 50 Hz / 20 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 200);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 200);
 		/* 20 Hz / 50 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
 		/* 10 Hz / 100 ms */
-		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100);
+		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
 		/* 1 Hz */
 		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000);
 	} else if (baudrate >= 57600) {
@@ -1799,7 +1833,9 @@ int mavlink_thread_main(int argc, char *argv[])
 		mavlink_pm_queued_send();
 		/* sleep quarter the time */
 		usleep(25000);
-		mavlink_pm_queued_send();
+		if (baudrate > 57600) {
+			mavlink_pm_queued_send();
+		}
 
 		/* sleep 10 ms */
 		usleep(10000);
@@ -1869,7 +1905,7 @@ int mavlink_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		mavlink_task = task_spawn("mavlink",
-					  SCHED_RR,
+					  SCHED_DEFAULT,
 					  SCHED_PRIORITY_DEFAULT,
 					  6000,
 					  mavlink_thread_main,

apps/multirotor_att_control/multirotor_att_control_main.c

@@ -82,63 +82,6 @@ static orb_advert_t actuator_pub;
 
 static struct vehicle_status_s state;
 
-/**
- * Perform rate control right after gyro reading
- */
-static void *rate_control_thread_main(void *arg)
-{
-	prctl(PR_SET_NAME, "mc rate control", getpid());
-
-	struct actuator_controls_s actuators;
-
-	int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
-	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-
-	struct pollfd fds = { .fd = gyro_sub, .events = POLLIN };
-
-	struct gyro_report gyro_report;
-	struct vehicle_rates_setpoint_s rates_sp;
-	memset(&rates_sp, 0, sizeof(rates_sp));
-	float gyro_lp[3] = {0.0f, 0.0f, 0.0f};
-
-	while (!thread_should_exit) {
-		/* rate control at maximum rate */
-		/* wait for a sensor update, check for exit condition every 1000 ms */
-		int ret = poll(&fds, 1, 1000);
-
-		if (ret < 0) {
-			/* XXX this is seriously bad - should be an emergency */
-		} else if (ret == 0) {
-			/* XXX this means no sensor data - should be critical or emergency */
-			printf("[mc att control] WARNING: Not getting gyro data, no rate control\n");
-		} else {
-			/* get data */
-			orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report);
-			bool rates_sp_valid = false;
-			orb_check(rates_sp_sub, &rates_sp_valid);
-			if (rates_sp_valid) {
-				orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
-			}
-
-			/* perform local lowpass */
-
-			/* apply controller */
-			if (state.flag_control_rates_enabled) {
-				/* lowpass gyros */
-				// XXX
-				gyro_lp[0] = gyro_report.x;
-				gyro_lp[1] = gyro_report.y;
-				gyro_lp[2] = gyro_report.z;
-
-				multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
-				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-			}
-		}
-	}
-
-	return NULL;
-}
-
 static int
 mc_thread_main(int argc, char *argv[])
 {
@@ -185,6 +128,7 @@ mc_thread_main(int argc, char *argv[])
 	actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 	orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
 	/* register the perf counter */
 	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
@@ -192,13 +136,6 @@ mc_thread_main(int argc, char *argv[])
 	/* welcome user */
 	printf("[multirotor_att_control] starting\n");
 
-	/* ready, spawn pthread */
-	pthread_attr_t rate_control_attr;
-	pthread_attr_init(&rate_control_attr);
-	pthread_attr_setstacksize(&rate_control_attr, 2048);
-	pthread_t rate_control_thread;
-	pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL);
-
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
@@ -228,16 +165,49 @@ mc_thread_main(int argc, char *argv[])
 
 
 		/** STEP 1: Define which input is the dominating control input */
+		if (state.flag_control_offboard_enabled) {
+					/* offboard inputs */
+					if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
+						rates_sp.roll = offboard_sp.p1;
+						rates_sp.pitch = offboard_sp.p2;
+						rates_sp.yaw = offboard_sp.p3;
+						rates_sp.thrust = offboard_sp.p4;
+						printf("thrust_rate=%8.4f\n",offboard_sp.p4);
+						rates_sp.timestamp = hrt_absolute_time();
+						orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+					} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
+						att_sp.roll_body = offboard_sp.p1;
+						att_sp.pitch_body = offboard_sp.p2;
+						att_sp.yaw_body = offboard_sp.p3;
+						att_sp.thrust = offboard_sp.p4;
+						printf("thrust_att=%8.4f\n",offboard_sp.p4);
+						att_sp.timestamp = hrt_absolute_time();
+						/* STEP 2: publish the result to the vehicle actuators */
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
+					}
 
-		if (state.flag_control_manual_enabled) {
+					/* decide wether we want rate or position input */
+				}
+		else if (state.flag_control_manual_enabled) {
 			/* manual inputs, from RC control or joystick */
-			att_sp.roll_body = manual.roll;
+
-			att_sp.pitch_body = manual.pitch;
+			if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
-			att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
+				rates_sp.roll = manual.roll;
-			/* set yaw rate */
+				rates_sp.pitch = manual.pitch;
-			rates_sp.yaw = manual.yaw;
+				rates_sp.yaw = manual.yaw;
-			att_sp.thrust = manual.throttle;
+				rates_sp.thrust = manual.throttle;
-			att_sp.timestamp = hrt_absolute_time();
+				rates_sp.timestamp = hrt_absolute_time();
+			}
+
+			if (state.flag_control_attitude_enabled) {
+				att_sp.roll_body = manual.roll;
+				att_sp.pitch_body = manual.pitch;
+				att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
+				/* set yaw rate */
+				rates_sp.yaw = manual.yaw;
+				att_sp.thrust = manual.throttle;
+				att_sp.timestamp = hrt_absolute_time();
+			}
 			/* STEP 2: publish the result to the vehicle actuators */
 			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
@@ -251,39 +221,40 @@ mc_thread_main(int argc, char *argv[])
 				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 			}
 
-		} else if (state.flag_control_offboard_enabled) {
-			/* offboard inputs */
-			if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
-				rates_sp.roll = offboard_sp.p1;
-				rates_sp.pitch = offboard_sp.p2;
-				rates_sp.yaw = offboard_sp.p3;
-				rates_sp.thrust = offboard_sp.p4;
-				rates_sp.timestamp = hrt_absolute_time();
-				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-			} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
-				att_sp.roll_body = offboard_sp.p1;
-				att_sp.pitch_body = offboard_sp.p2;
-				att_sp.yaw_body = offboard_sp.p3;
-				att_sp.thrust = offboard_sp.p4;
-				att_sp.timestamp = hrt_absolute_time();
-				/* STEP 2: publish the result to the vehicle actuators */
-				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-			}
-
-			/* decide wether we want rate or position input */
 		}
 
 		/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
 
-		/* run attitude controller */
-		if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
-			multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
-			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-		} else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
-			multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
-			orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
-		}
 
+		/* run attitude controller */
+		 if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
+		 	multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
+		 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+		 } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
+		 	multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
+		 	orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
+		 }
+
+
+		if (state.flag_control_rates_enabled) {
+
+			float gyro[3] = {0.0f, 0.0f, 0.0f};
+
+			/* get current rate setpoint */
+			bool rates_sp_valid = false;
+			orb_check(rates_sp_sub, &rates_sp_valid);
+			if (rates_sp_valid) {
+				orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+			}
+
+			/* apply controller */
+			gyro[0] = att.rollspeed;
+			gyro[1] = att.pitchspeed;
+			gyro[2] = att.yawspeed;
+
+			multirotor_control_rates(&rates_sp, gyro, &actuators);
+			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+		}
 
 		perf_end(mc_loop_perf);
 	}
@@ -305,8 +276,6 @@ mc_thread_main(int argc, char *argv[])
 	perf_print_counter(mc_loop_perf);
 	perf_free(mc_loop_perf);
 
-	pthread_join(rate_control_thread, NULL);
-
 	fflush(stdout);
 	exit(0);
 }
@@ -352,9 +321,9 @@ int multirotor_att_control_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		mc_task = task_spawn("multirotor_att_control",
-				     SCHED_RR,
+				     SCHED_DEFAULT,
 				     SCHED_PRIORITY_MAX - 15,
-				     2048,
+				     6000,
 				     mc_thread_main,
 				     NULL);
 		exit(0);

apps/multirotor_att_control/multirotor_rate_control.c

@@ -56,18 +56,21 @@
 // PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 0.1f);
 
 PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.2f); /* 0.15 F405 Flamewheel */
-PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
+PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.0f);
-PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
+//PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
+//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 8.0f);	/**< roughly < 500 deg/s limit */
 
 struct mc_rate_control_params {
 
 	float yawrate_p;
+	float yawrate_d;
 	float yawrate_i;
 	float yawrate_awu;
 	float yawrate_lim;
 
 	float attrate_p;
+	float attrate_d;
 	float attrate_i;
 	float attrate_awu;
 	float attrate_lim;
@@ -79,11 +82,13 @@ struct mc_rate_control_param_handles {
 
 	param_t yawrate_p;
 	param_t yawrate_i;
+	param_t yawrate_d;
 	param_t yawrate_awu;
 	param_t yawrate_lim;
 
 	param_t attrate_p;
 	param_t attrate_i;
+	param_t attrate_d;
 	param_t attrate_awu;
 	param_t attrate_lim;
 };
@@ -106,11 +111,13 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
 	/* PID parameters */
 	h->yawrate_p 	=	param_find("MC_YAWRATE_P");
 	h->yawrate_i 	=	param_find("MC_YAWRATE_I");
+	h->yawrate_d 	=	param_find("MC_YAWRATE_D");
 	h->yawrate_awu 	=	param_find("MC_YAWRATE_AWU");
 	h->yawrate_lim 	=	param_find("MC_YAWRATE_LIM");
 
 	h->attrate_p 	= 	param_find("MC_ATTRATE_P");
 	h->attrate_i 	= 	param_find("MC_ATTRATE_I");
+	h->attrate_d 	= 	param_find("MC_ATTRATE_D");
 	h->attrate_awu 	= 	param_find("MC_ATTRATE_AWU");
 	h->attrate_lim 	= 	param_find("MC_ATTRATE_LIM");
 
@@ -121,11 +128,13 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 {
 	param_get(h->yawrate_p, &(p->yawrate_p));
 	param_get(h->yawrate_i, &(p->yawrate_i));
+	param_get(h->yawrate_d, &(p->yawrate_d));
 	param_get(h->yawrate_awu, &(p->yawrate_awu));
 	param_get(h->yawrate_lim, &(p->yawrate_lim));
 
 	param_get(h->attrate_p, &(p->attrate_p));
 	param_get(h->attrate_i, &(p->attrate_i));
+	param_get(h->attrate_d, &(p->attrate_d));
 	param_get(h->attrate_awu, &(p->attrate_awu));
 	param_get(h->attrate_lim, &(p->attrate_lim));
 
@@ -135,17 +144,14 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	const float rates[], struct actuator_controls_s *actuators)
 {
+	static float roll_control_last=0;
+	static float pitch_control_last=0;
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 	last_run = hrt_absolute_time();
 
 	static int motor_skip_counter = 0;
 
-	// static PID_t yaw_pos_controller;
-	static PID_t yaw_speed_controller;
-	static PID_t pitch_controller;
-	static PID_t roll_controller;
-
 	static struct mc_rate_control_params p;
 	static struct mc_rate_control_param_handles h;
 
@@ -155,102 +161,33 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	if (initialized == false) {
 		parameters_init(&h);
 		parameters_update(&h, &p);
-
-		pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu,
-			PID_MODE_DERIVATIV_SET);
-		pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
-			PID_MODE_DERIVATIV_SET);
-		pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
-			PID_MODE_DERIVATIV_SET);
-
 		initialized = true;
 	}
 
 	/* load new parameters with lower rate */
-	if (motor_skip_counter % 250 == 0) {
+	if (motor_skip_counter % 2500 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		/* apply parameters */
+		printf("p.yawrate_p: %8.4f\n", (double)p.yawrate_p);
-		pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu);
-		pid_set_parameters(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu);
-		pid_set_parameters(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu);
 	}
 
 	/* calculate current control outputs */
 	
 	/* control pitch (forward) output */
-	float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch,
+
-					rates[1], 0.0f, deltaT);
+	float pitch_control = p.attrate_p * deltaT *(rate_sp->pitch-rates[1])-p.attrate_d*(pitch_control_last);
+	pitch_control_last=pitch_control;
 	/* control roll (left/right) output */
-	float roll_control = pid_calculate(&roll_controller, rate_sp->roll,
+
-					rates[0], 0.0f, deltaT);
+	float roll_control = p.attrate_p * deltaT * (rate_sp->roll-rates[0])-p.attrate_d*(roll_control_last);
+	roll_control_last=roll_control;
 	/* control yaw rate */
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
+	float yaw_rate_control = p.yawrate_p * deltaT * (rate_sp->yaw-rates[2]  );
-
-	/*
-	 * compensate the vertical loss of thrust
-	 * when thrust plane has an angle.
-	 * start with a factor of 1.0 (no change)
-	 */
-	float zcompensation = 1.0f;
-
-	// if (fabsf(att->roll) > 0.3f) {
-	// 	zcompensation *= 1.04675160154f;
-
-	// } else {
-	// 	zcompensation *= 1.0f / cosf(att->roll);
-	// }
-
-	// if (fabsf(att->pitch) > 0.3f) {
-	// 	zcompensation *= 1.04675160154f;
-
-	// } else {
-	// 	zcompensation *= 1.0f / cosf(att->pitch);
-	// }
-
-	float motor_thrust = 0.0f;
-
-	motor_thrust = rate_sp->thrust;
-
-	/* compensate thrust vector for roll / pitch contributions */
-	motor_thrust *= zcompensation;
-
-	/* limit yaw rate output */
-	if (yaw_rate_control > p.yawrate_lim) {
-		yaw_rate_control = p.yawrate_lim;
-		yaw_speed_controller.saturated = 1;
-	}
-
-	if (yaw_rate_control < -p.yawrate_lim) {
-		yaw_rate_control = -p.yawrate_lim;
-		yaw_speed_controller.saturated = 1;
-	}
-
-	if (pitch_control > p.attrate_lim) {
-		pitch_control = p.attrate_lim;
-		pitch_controller.saturated = 1;
-	}
-
-	if (pitch_control < -p.attrate_lim) {
-		pitch_control = -p.attrate_lim;
-		pitch_controller.saturated = 1;
-	}
-
-
-	if (roll_control > p.attrate_lim) {
-		roll_control = p.attrate_lim;
-		roll_controller.saturated = 1;
-	}
-
-	if (roll_control < -p.attrate_lim) {
-		roll_control = -p.attrate_lim;
-		roll_controller.saturated = 1;
-	}
 
 	actuators->control[0] = roll_control;
 	actuators->control[1] = pitch_control;
 	actuators->control[2] = yaw_rate_control;
-	actuators->control[3] = motor_thrust;
+	actuators->control[3] = rate_sp->thrust;
 
 	motor_skip_counter++;
 }

apps/multirotor_pos_control/multirotor_pos_control.c

@@ -49,24 +49,94 @@
 #include <time.h>
 #include <sys/prctl.h>
 #include <arch/board/up_hrt.h>
-#include "ardrone_control.h"
-#include "attitude_control.h"
-#include "rate_control.h"
-#include "ardrone_motor_control.h"
-#include "position_control.h"
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_local_position_setpoint.h>
+#include <uORB/topics/vehicle_vicon_position.h>
+#include <systemlib/systemlib.h>
+
+#include "multirotor_pos_control_params.h"
+
+
+static bool thread_should_exit = false;		/**< Deamon exit flag */
+static bool thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
 
 __EXPORT int multirotor_pos_control_main(int argc, char *argv[]);
 
-static bool thread_should_exit;
+/**
-static bool thread_running = false;
+ * Mainloop of position controller.
-static int mpc_task;
+ */
+static int multirotor_pos_control_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+	if (reason)
+		fprintf(stderr, "%s\n", reason);
+	fprintf(stderr, "usage: deamon {start|stop|status} [-p <additional params>]\n\n");
+	exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_spawn().
+ */
+int multirotor_pos_control_main(int argc, char *argv[])
+{
+	if (argc < 1)
+		usage("missing command");
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("multirotor pos control already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn("multirotor pos control",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 60,
+					 4096,
+					 multirotor_pos_control_thread_main,
+					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			printf("\tmultirotor pos control app is running\n");
+		} else {
+			printf("\tmultirotor pos control app not started\n");
+		}
+		exit(0);
+	}
+
+	usage("unrecognized command");
+	exit(1);
+}
 
 static int
-mpc_thread_main(int argc, char *argv[])
+multirotor_pos_control_thread_main(int argc, char *argv[])
 {
 	/* welcome user */
 	printf("[multirotor pos control] Control started, taking over position control\n");
@@ -76,7 +146,7 @@ mpc_thread_main(int argc, char *argv[])
 	struct vehicle_attitude_s att;
 	//struct vehicle_global_position_setpoint_s global_pos_sp;
 	struct vehicle_local_position_setpoint_s local_pos_sp;
-	struct vehicle_local_position_s local_pos;
+	struct vehicle_vicon_position_s local_pos;
 	struct manual_control_setpoint_s manual;
 	struct vehicle_attitude_setpoint_s att_sp;
 
@@ -84,13 +154,23 @@ mpc_thread_main(int argc, char *argv[])
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
+	int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
 	//int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
 	int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 
 	/* publish attitude setpoint */
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
 
+	thread_running = true;
+
+	int loopcounter = 0;
+
+	struct multirotor_position_control_params p;
+	struct multirotor_position_control_param_handles h;
+	parameters_init(&h);
+	parameters_update(&h, &p);
+
+
 	while (1) {
 		/* get a local copy of the vehicle state */
 		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
@@ -99,15 +179,34 @@ mpc_thread_main(int argc, char *argv[])
 		/* get a local copy of attitude */
 		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 		/* get a local copy of local position */
-		orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos);
+		orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos);
 		/* get a local copy of local position setpoint */
 		orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp);
 
-		if (state.state_machine == SYSTEM_STATE_AUTO) {
+		if (loopcounter == 500) {
-			position_control(&state, &manual, &att, &local_pos, &local_pos_sp, &att_sp);
+			parameters_update(&h, &p);
+			loopcounter = 0;
+		}
+
+		// if (state.state_machine == SYSTEM_STATE_AUTO) {
+			
+			// XXX IMPLEMENT POSITION CONTROL HERE
+
+			float dT = 1.0f / 50.0f;
+
+			float x_setpoint = 0.0f;
+
+			/* local pos is the Vicon position */
+
+			att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p * dT;
+			att_sp.roll_body = 0.0f;
+			att_sp.yaw_body = 0.0f;
+			att_sp.thrust = 0.4f;
+			att_sp.timestamp = hrt_absolute_time();
+
 			/* publish new attitude setpoint */
 			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
-		} else if (state.state_machine == SYSTEM_STATE_STABILIZE) {
+		// } else if (state.state_machine == SYSTEM_STATE_STABILIZED) {
 			/* set setpoint to current position */
 			// XXX select pos reset channel on remote
 			/* reset setpoint to current position  (position hold) */
@@ -117,19 +216,18 @@ mpc_thread_main(int argc, char *argv[])
 			// 	local_pos_sp.z = local_pos.z;
 			// 	local_pos_sp.yaw = att.yaw;
 			// }
-		}
+		// }
 
 		/* run at approximately 50 Hz */
 		usleep(20000);
+		loopcounter++;
 
-		counter++;
 	}
 
-	/* close uarts */
+	printf("[multirotor pos control] ending now...\n");
-	close(ardrone_write);
+
-	ar_multiplexing_deinit(gpios);
+	thread_running = false;
 
-	printf("[multirotor pos control] ending now...\r\n");
 	fflush(stdout);
 	return 0;
 }

apps/multirotor_pos_control/multirotor_pos_control_params.c

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file multirotor_position_control_params.c
+ * 
+ * Parameters for EKF filter
+ */
+
+#include "multirotor_pos_control_params.h"
+
+/* Extended Kalman Filter covariances */
+
+/* controller parameters */
+PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f);
+
+int parameters_init(struct multirotor_position_control_param_handles *h)
+{
+	/* PID parameters */
+	h->p 	=	param_find("MC_POS_P");
+
+	return OK;
+}
+
+int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
+{
+	param_get(h->p, &(p->p));
+
+	return OK;
+}

apps/multirotor_pos_control/multirotor_pos_control_params.h

@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file multirotor_position_control_params.h
+ * 
+ * Parameters for position controller
+ */
+
+#include <systemlib/param/param.h>
+
+struct multirotor_position_control_params {
+	float p;
+	float i;
+	float d;
+};
+
+struct multirotor_position_control_param_handles {
+	param_t p;
+	param_t i;
+	param_t d;
+};
+
+/**
+ * Initialize all parameter handles and values
+ *
+ */
+int parameters_init(struct multirotor_position_control_param_handles *h);
+
+/**
+ * Update all parameters
+ *
+ */
+int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p);

apps/multirotor_pos_control/position_control.c

@@ -1,235 +1,235 @@
-/****************************************************************************
+// /****************************************************************************
- *
+//  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+//  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+//  *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
+//  *           @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
+//  *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+//  *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
- *
+//  *
- * Redistribution and use in source and binary forms, with or without
+//  * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
+//  * modification, are permitted provided that the following conditions
- * are met:
+//  * are met:
- *
+//  *
- * 1. Redistributions of source code must retain the above copyright
+//  * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
+//  *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
+//  * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
+//  *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
+//  *    the documentation and/or other materials provided with the
- *    distribution.
+//  *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
+//  * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
+//  *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
+//  *    without specific prior written permission.
- *
+//  *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+//  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+//  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+//  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+//  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+//  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+//  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+//  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+//  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+//  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+//  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+//  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
+//  * POSSIBILITY OF SUCH DAMAGE.
- *
+//  *
- ****************************************************************************/
+//  ****************************************************************************/
 
-/**
+// /**
- * @file multirotor_position_control.c
+//  * @file multirotor_position_control.c
- * Implementation of the position control for a multirotor VTOL
+//  * Implementation of the position control for a multirotor VTOL
- */
+//  */
 
-#include <stdio.h>
+// #include <stdio.h>
-#include <stdlib.h>
+// #include <stdlib.h>
-#include <stdio.h>
+// #include <stdio.h>
-#include <stdint.h>
+// #include <stdint.h>
-#include <math.h>
+// #include <math.h>
-#include <stdbool.h>
+// #include <stdbool.h>
-#include <float.h>
+// #include <float.h>
-#include <systemlib/pid/pid.h>
+// #include <systemlib/pid/pid.h>
 
-#include "multirotor_position_control.h"
+// #include "multirotor_position_control.h"
 
-void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
+// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
- const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
+//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
- const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
+//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp)
-{
+// {
-	static PID_t distance_controller;
+// 	static PID_t distance_controller;
 
-	static int read_ret;
+// 	static int read_ret;
-	static global_data_position_t position_estimated;
+// 	static global_data_position_t position_estimated;
 
-	static uint16_t counter;
+// 	static uint16_t counter;
 
-	static bool initialized;
+// 	static bool initialized;
-	static uint16_t pm_counter;
+// 	static uint16_t pm_counter;
 
-	static float lat_next;
+// 	static float lat_next;
-	static float lon_next;
+// 	static float lon_next;
 
-	static float pitch_current;
+// 	static float pitch_current;
 
-	static float thrust_total;
+// 	static float thrust_total;
 
 
-	if (initialized == false) {
+// 	if (initialized == false) {
 
-		pid_init(&distance_controller,
+// 		pid_init(&distance_controller,
-			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
+// 			 global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU],
-			 PID_MODE_DERIVATIV_CALC, 150);//150
+// 			 PID_MODE_DERIVATIV_CALC, 150);//150
 
-//		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
+// //		pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-//		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
+// //		pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
 
-		thrust_total = 0.0f;
+// 		thrust_total = 0.0f;
 
-		/* Position initialization */
+// 		/* Position initialization */
-		/* Wait for new position estimate */
+// 		/* Wait for new position estimate */
-		do {
+// 		do {
-			read_ret = read_lock_position(&position_estimated);
+// 			read_ret = read_lock_position(&position_estimated);
-		} while (read_ret != 0);
+// 		} while (read_ret != 0);
 
-		lat_next = position_estimated.lat;
+// 		lat_next = position_estimated.lat;
-		lon_next = position_estimated.lon;
+// 		lon_next = position_estimated.lon;
 
-		/* attitude initialization */
+// 		/* attitude initialization */
-		global_data_lock(&global_data_attitude->access_conf);
+// 		global_data_lock(&global_data_attitude->access_conf);
-		pitch_current = global_data_attitude->pitch;
+// 		pitch_current = global_data_attitude->pitch;
-		global_data_unlock(&global_data_attitude->access_conf);
+// 		global_data_unlock(&global_data_attitude->access_conf);
 
-		initialized = true;
+// 		initialized = true;
-	}
+// 	}
 
-	/* load new parameters with 10Hz */
+// 	/* load new parameters with 10Hz */
-	if (counter % 50 == 0) {
+// 	if (counter % 50 == 0) {
-		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
+// 		if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) {
-			/* check whether new parameters are available */
+// 			/* check whether new parameters are available */
-			if (global_data_parameter_storage->counter > pm_counter) {
+// 			if (global_data_parameter_storage->counter > pm_counter) {
-				pid_set_parameters(&distance_controller,
+// 				pid_set_parameters(&distance_controller,
-						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P],
-						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I],
-						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D],
-						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
+// 						   global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]);
 
-//
+// //
-//				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
+// //				pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM];
-//				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
+// //				pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM];
 
-				pm_counter = global_data_parameter_storage->counter;
+// 				pm_counter = global_data_parameter_storage->counter;
-				printf("Position controller changed pid parameters\n");
+// 				printf("Position controller changed pid parameters\n");
-			}
+// 			}
-		}
+// 		}
 
-		global_data_unlock(&global_data_parameter_storage->access_conf);
+// 		global_data_unlock(&global_data_parameter_storage->access_conf);
-	}
+// 	}
 
 
-	/* Wait for new position estimate */
+// 	/* Wait for new position estimate */
-	do {
+// 	do {
-		read_ret = read_lock_position(&position_estimated);
+// 		read_ret = read_lock_position(&position_estimated);
-	} while (read_ret != 0);
+// 	} while (read_ret != 0);
 
-	/* Get next waypoint */ //TODO: add local copy
+// 	/* Get next waypoint */ //TODO: add local copy
 
-	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
+// 	if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) {
-		lat_next = global_data_position_setpoint->x;
+// 		lat_next = global_data_position_setpoint->x;
-		lon_next = global_data_position_setpoint->y;
+// 		lon_next = global_data_position_setpoint->y;
-		global_data_unlock(&global_data_position_setpoint->access_conf);
+// 		global_data_unlock(&global_data_position_setpoint->access_conf);
-	}
+// 	}
 
-	/* Get distance to waypoint */
+// 	/* Get distance to waypoint */
-	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
+// 	float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next);
-//	if(counter % 5 == 0)
+// //	if(counter % 5 == 0)
-//		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
+// //		printf("distance_to_waypoint: %.4f\n", distance_to_waypoint);
 
-	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
+// 	/* Get bearing to waypoint (direction on earth surface to next waypoint) */
-	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
+// 	float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next);
 
-	if (counter % 5 == 0)
+// 	if (counter % 5 == 0)
-		printf("bearing: %.4f\n", bearing);
+// 		printf("bearing: %.4f\n", bearing);
 
-	/* Calculate speed in direction of bearing (needed for controller) */
+// 	/* Calculate speed in direction of bearing (needed for controller) */
-	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
+// 	float speed_norm = sqrtf(position_estimated.vx  * position_estimated.vx + position_estimated.vy * position_estimated.vy);
-//	if(counter % 5 == 0)
+// //	if(counter % 5 == 0)
-//		printf("speed_norm: %.4f\n", speed_norm);
+// //		printf("speed_norm: %.4f\n", speed_norm);
-	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
+// 	float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller
 
-	/* Control Thrust in bearing direction  */
+// 	/* Control Thrust in bearing direction  */
-	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
+// 	float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint,
-				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
+// 				  CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else
 
-//	if(counter % 5 == 0)
+// //	if(counter % 5 == 0)
-//		printf("horizontal thrust: %.4f\n", horizontal_thrust);
+// //		printf("horizontal thrust: %.4f\n", horizontal_thrust);
 
-	/* Get total thrust (from remote for now) */
+// 	/* Get total thrust (from remote for now) */
-	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
+// 	if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
-		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
+// 		thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; 												//TODO: how should we use the RC_CHANNELS_FUNCTION enum?
-		global_data_unlock(&global_data_rc_channels->access_conf);
+// 		global_data_unlock(&global_data_rc_channels->access_conf);
-	}
+// 	}
 
-	const float max_gas = 500.0f;
+// 	const float max_gas = 500.0f;
-	thrust_total *= max_gas / 20000.0f; //TODO: check this
+// 	thrust_total *= max_gas / 20000.0f; //TODO: check this
-	thrust_total += max_gas / 2.0f;
+// 	thrust_total += max_gas / 2.0f;
 
 
-	if (horizontal_thrust > thrust_total) {
+// 	if (horizontal_thrust > thrust_total) {
-		horizontal_thrust = thrust_total;
+// 		horizontal_thrust = thrust_total;
 
-	} else if (horizontal_thrust < -thrust_total) {
+// 	} else if (horizontal_thrust < -thrust_total) {
-		horizontal_thrust = -thrust_total;
+// 		horizontal_thrust = -thrust_total;
-	}
+// 	}
 
 
 
-	//TODO: maybe we want to add a speed controller later...
+// 	//TODO: maybe we want to add a speed controller later...
 
-	/* Calclulate thrust in east and north direction */
+// 	/* Calclulate thrust in east and north direction */
-	float thrust_north = cosf(bearing) * horizontal_thrust;
+// 	float thrust_north = cosf(bearing) * horizontal_thrust;
-	float thrust_east = sinf(bearing) * horizontal_thrust;
+// 	float thrust_east = sinf(bearing) * horizontal_thrust;
 
-	if (counter % 10 == 0) {
+// 	if (counter % 10 == 0) {
-		printf("thrust north: %.4f\n", thrust_north);
+// 		printf("thrust north: %.4f\n", thrust_north);
-		printf("thrust east: %.4f\n", thrust_east);
+// 		printf("thrust east: %.4f\n", thrust_east);
-		fflush(stdout);
+// 		fflush(stdout);
-	}
+// 	}
 
-	/* Get current attitude */
+// 	/* Get current attitude */
-	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
+// 	if (0 == global_data_trylock(&global_data_attitude->access_conf)) {
-		pitch_current = global_data_attitude->pitch;
+// 		pitch_current = global_data_attitude->pitch;
-		global_data_unlock(&global_data_attitude->access_conf);
+// 		global_data_unlock(&global_data_attitude->access_conf);
-	}
+// 	}
 
-	/* Get desired pitch & roll */
+// 	/* Get desired pitch & roll */
-	float pitch_desired = 0.0f;
+// 	float pitch_desired = 0.0f;
-	float roll_desired = 0.0f;
+// 	float roll_desired = 0.0f;
 
-	if (thrust_total != 0) {
+// 	if (thrust_total != 0) {
-		float pitch_fraction = -thrust_north / thrust_total;
+// 		float pitch_fraction = -thrust_north / thrust_total;
-		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
+// 		float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total);
 
-		if (roll_fraction < -1) {
+// 		if (roll_fraction < -1) {
-			roll_fraction = -1;
+// 			roll_fraction = -1;
 
-		} else if (roll_fraction > 1) {
+// 		} else if (roll_fraction > 1) {
-			roll_fraction = 1;
+// 			roll_fraction = 1;
-		}
+// 		}
 
-//		if(counter % 5 == 0)
+// //		if(counter % 5 == 0)
-//		{
+// //		{
-//			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
+// //			printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction);
-//			fflush(stdout);
+// //			fflush(stdout);
-//		}
+// //		}
 
-		pitch_desired = asinf(pitch_fraction);
+// 		pitch_desired = asinf(pitch_fraction);
-		roll_desired = asinf(roll_fraction);
+// 		roll_desired = asinf(roll_fraction);
-	}
+// 	}
 
-	att_sp.roll = roll_desired;
+// 	att_sp.roll = roll_desired;
-	att_sp.pitch = pitch_desired;
+// 	att_sp.pitch = pitch_desired;
 
-	counter++;
+// 	counter++;
-}
+// }

apps/multirotor_pos_control/position_control.h

@@ -40,11 +40,11 @@
  * Definition of the position control for a multirotor VTOL
  */
 
-#ifndef POSITION_CONTROL_H_
+// #ifndef POSITION_CONTROL_H_
-#define POSITION_CONTROL_H_
+// #define POSITION_CONTROL_H_
 
-void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
+// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual,
- const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
+//  const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos,
- const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
+//  const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp);
 
-#endif /* POSITION_CONTROL_H_ */
+// #endif /* POSITION_CONTROL_H_ */

apps/nshlib/Kconfig

@@ -291,19 +291,18 @@ config NSH_CONSOLE
 		console front-end is selected (/dev/console).
 
 		Normally, the serial console device is a UART and RS-232
-		interface.  However, if CONFIG_USBDEV is defined, then a USB
+		interface.  However, if USBDEV is defined, then a USB
 		serial device may, instead, be used if the one of
 		the following are defined:
 
-		CONFIG_PL2303 and CONFIG_PL2303_CONSOLE - Sets up the
+		PL2303 and PL2303_CONSOLE - Set up the Prolifics PL2303
-		Prolifics PL2303 emulation as a console device at /dev/console.
+		emulation as a console device at /dev/console.
 
-		CONFIG_CDCACM and CONFIG_CDCACM_CONSOLE - Sets up the
+		CDCACM and CDCACM_CONSOLE - Set up the CDC/ACM serial
-		CDC/ACM serial device as a console device at dev/console.
+		device as a console device at dev/console.
 
-		CONFIG_NSH_USBCONSOLE and CONFIG_NSH_USBCONDEV - Sets up the
+		NSH_USBCONSOLE and NSH_USBCONDEV - Sets up some other USB
-		some other USB serial device as the NSH console (not necessarily
+		serial device as the NSH console (not necessarily dev/console).
-		dev/console).
 
 config NSH_USBCONSOLE
 	bool "Use a USB console"
@@ -311,20 +310,20 @@ config NSH_USBCONSOLE
 	depends on NSH_CONSOLE && USBDEV
 	---help---
 		If defined, then the an arbitrary USB device may be used
-		to as the NSH console.  In this case, CONFIG_NSH_USBCONDEV
+		to as the NSH console.  In this case, NSH_USBCONDEV must
-		must be defined to indicate which USB device to use as
+		be defined to indicate which USB device to use as the
-		the console.
+		console.
 
 config NSH_USBCONDEV
 	string "USB console device"
 	default "/dev/ttyACM0"
 	depends on NSH_USBCONSOLE
 	---help---
-		If CONFIG_NSH_USBCONSOLE is set to 'y', then CONFIG_NSH_USBCONDEV
+		If NSH_USBCONSOLE is set to 'y', then NSH_USBCONDEV must
-		must also be set to select the USB device used to support
+		also be set to select the USB device used to support the
-		the NSH console.   This should be set to the quoted name of a
+		NSH console.   This should be set to the quoted name of a
 		readable/write-able USB driver such as:
-		CONFIG_NSH_USBCONDEV="/dev/ttyACM0".
+		NSH_USBCONDEV="/dev/ttyACM0".
 
 config UBSDEV_MINOR
 	int "USB console device minor number"
@@ -448,6 +447,35 @@ config NSH_IOBUFFER_SIZE
 	---help---
 		Determines the size of the I/O buffer to use for sending/
 		receiving TELNET commands/reponses.  Default: 512
+
+config NSH_TELNET_LOGIN
+	bool "Telnet Login"
+	default n
+	---help---
+		If defined, then the Telnet user will be required to provide a
+		username and password to start the NSH shell.
+
+if NSH_TELNET_LOGIN
+
+config NSH_TELNET_USERNAME
+	string "Login Username"
+	default "admin"
+	---help---
+		Login user name.  Default: "admin"
+
+config NSH_TELNET_PASSWORD
+	string "Login Password"
+	default "nuttx"
+	---help---
+		Login password:  Default: "nuttx"
+
+config NSH_TELNET_FAILCOUNT
+	int "Login Retry Count"
+	default 3
+	---help---
+		Number of login retry attempts.
+
+endif
 endif
 
 config NSH_DHCPC

apps/nshlib/nsh.h

@@ -164,10 +164,19 @@
  *   Default: SCHED_PRIORITY_DEFAULT
  * CONFIG_NSH_TELNETD_DAEMONSTACKSIZE - Stack size allocated for the
  *   Telnet daemon. Default: 2048
- * CONFIG_NSH_TELNETD_CLIENTPRIO- Priority of the Telnet client.
+ * CONFIG_NSH_TELNETD_CLIENTPRIO - Priority of the Telnet client.
  *   Default: SCHED_PRIORITY_DEFAULT
  * CONFIG_NSH_TELNETD_CLIENTSTACKSIZE - Stack size allocated for the
  *   Telnet client. Default: 2048
+ * CONFIG_NSH_TELNET_LOGIN - Support a simple Telnet login.
+ *
+ * If CONFIG_NSH_TELNET_LOGIN is defined, then these additional
+ * options may be specified:
+ *
+ * CONFIG_NSH_TELNET_USERNAME - Login user name.  Default: "admin"
+ * CONFIG_NSH_TELNET_PASSWORD - Login password:  Default: "nuttx"
+ * CONFIG_NSH_TELNET_FAILCOUNT - Number of login retry attempts.
+ *   Default 3.
  */
 
 #ifndef CONFIG_NSH_TELNETD_PORT
@@ -190,6 +199,22 @@
 #  define CONFIG_NSH_TELNETD_CLIENTSTACKSIZE 2048
 #endif
 
+#ifdef CONFIG_NSH_TELNET_LOGIN
+
+#  ifndef CONFIG_NSH_TELNET_USERNAME
+#    define CONFIG_NSH_TELNET_USERNAME  "admin"
+#  endif
+
+#  ifndef CONFIG_NSH_TELNET_PASSWORD
+#    define CONFIG_NSH_TELNET_PASSWORD  "nuttx"
+#  endif
+
+#  ifndef CONFIG_NSH_TELNET_FAILCOUNT
+#    define CONFIG_NSH_TELNET_FAILCOUNT 3
+#  endif
+
+#endif /* CONFIG_NSH_TELNET_LOGIN */
+
 /* Verify support for ROMFS /etc directory support options */
 
 #ifdef CONFIG_NSH_ROMFSETC
@@ -364,6 +389,14 @@ typedef int (*cmd_t)(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
  ****************************************************************************/
 
 extern const char g_nshgreeting[];
+#if defined(CONFIG_NSH_TELNET_LOGIN) && defined(CONFIG_NSH_TELNET)
+extern const char g_telnetgreeting[];
+extern const char g_userprompt[];
+extern const char g_passwordprompt[];
+extern const char g_loginsuccess[];
+extern const char g_badcredentials[];
+extern const char g_loginfailure[];
+#endif
 extern const char g_nshprompt[];
 extern const char g_nshsyntax[];
 extern const char g_fmtargrequired[];

apps/nshlib/nsh_console.c

@@ -75,12 +75,17 @@ struct serialsave_s
 static FAR struct nsh_vtbl_s *nsh_consoleclone(FAR struct nsh_vtbl_s *vtbl);
 #endif
 static void nsh_consolerelease(FAR struct nsh_vtbl_s *vtbl);
-static ssize_t nsh_consolewrite(FAR struct nsh_vtbl_s *vtbl, FAR const void *buffer, size_t nbytes);
+static ssize_t nsh_consolewrite(FAR struct nsh_vtbl_s *vtbl,
-static int nsh_consoleoutput(FAR struct nsh_vtbl_s *vtbl, const char *fmt, ...);
+  FAR const void *buffer, size_t nbytes);
+static int nsh_consoleoutput(FAR struct nsh_vtbl_s *vtbl,
+  FAR const char *fmt, ...);
 static FAR char *nsh_consolelinebuffer(FAR struct nsh_vtbl_s *vtbl);
-static void nsh_consoleredirect(FAR struct nsh_vtbl_s *vtbl, int fd, FAR uint8_t *save);
+static void nsh_consoleredirect(FAR struct nsh_vtbl_s *vtbl, int fd,
-static void nsh_consoleundirect(FAR struct nsh_vtbl_s *vtbl, FAR uint8_t *save);
+  FAR uint8_t *save);
-static void nsh_consoleexit(FAR struct nsh_vtbl_s *vtbl, int exitstatus);
+static void nsh_consoleundirect(FAR struct nsh_vtbl_s *vtbl,
+  FAR uint8_t *save);
+static void nsh_consoleexit(FAR struct nsh_vtbl_s *vtbl, int exitstatus)
+  noreturn_function;
 
 /****************************************************************************
  * Private Data

apps/nshlib/nsh_console.h

@@ -109,7 +109,7 @@ struct nsh_vtbl_s
   FAR char *(*linebuffer)(FAR struct nsh_vtbl_s *vtbl);
   void (*redirect)(FAR struct nsh_vtbl_s *vtbl, int fd, FAR uint8_t *save);
   void (*undirect)(FAR struct nsh_vtbl_s *vtbl, FAR uint8_t *save);
-  void (*exit)(FAR struct nsh_vtbl_s *vtbl, int exitstatus);
+  void (*exit)(FAR struct nsh_vtbl_s *vtbl, int exitstatus) noreturn_function;
 
   /* Parser state data */
 

apps/nshlib/nsh_parse.c

@@ -395,6 +395,17 @@ const char g_nshgreeting[]       = "\nNuttShell (NSH) NuttX-" CONFIG_VERSION_STR
 const char g_nshgreeting[]       = "\nNuttShell (NSH)\n";
 #endif
 
+/* Telnet login prompts */
+
+#if defined(CONFIG_NSH_TELNET_LOGIN) && defined(CONFIG_NSH_TELNET)
+const char g_telnetgreeting[]    = "\nWelcome to NuttShell(NSH) Telnet Server...\n";
+const char g_userprompt[]        = "login: ";
+const char g_passwordprompt[]    = "password: ";
+const char g_loginsuccess[]      = "\nUser Logged-in!\n";
+const char g_badcredentials[]    = "\nInvalid username or password\n";
+const char g_loginfailure[]      = "Login failed!\n";
+#endif
+
 /* The NSH prompt */
 
 const char g_nshprompt[]         = "nsh> ";

apps/nshlib/nsh_telnetd.c

@@ -43,6 +43,7 @@
 #include <unistd.h>
 #include <assert.h>
 #include <debug.h>
+#include <string.h>
 
 #include <apps/netutils/telnetd.h>
 
@@ -55,6 +56,18 @@
  * Pre-processor Definitions
  ****************************************************************************/
 
+#ifdef CONFIG_NSH_TELNET_LOGIN
+
+#  define TELNET_IAC              255
+#  define TELNET_WILL             251
+#  define TELNET_WONT             252
+#  define TELNET_DO               253
+#  define TELNET_DONT             254
+#  define TELNET_USE_ECHO         1
+#  define TELNET_NOTUSE_ECHO      0
+
+#endif /* CONFIG_NSH_TELNET_LOGIN */
+
 /****************************************************************************
  * Private Types
  ****************************************************************************/
@@ -75,6 +88,91 @@
  * Private Functions
  ****************************************************************************/
 
+/****************************************************************************
+ * Name: nsh_telnetecho
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_TELNET_LOGIN
+void nsh_telnetecho(struct console_stdio_s *pstate, uint8_t is_use)
+{
+  uint8_t optbuf[4];
+  optbuf[0] = TELNET_IAC;
+  optbuf[1] = (is_use == TELNET_USE_ECHO) ? TELNET_WILL : TELNET_DO;
+  optbuf[2] = 1;
+  optbuf[3] = 0;
+  fputs((char *)optbuf, pstate->cn_outstream);
+  fflush(pstate->cn_outstream);
+}
+#endif
+
+/****************************************************************************
+ * Name: nsh_telnetlogin
+ ****************************************************************************/
+
+#ifdef CONFIG_NSH_TELNET_LOGIN
+int nsh_telnetlogin(struct console_stdio_s *pstate)
+{
+  char username[16];
+  char password[16];
+  uint8_t i;
+
+  /* Present the NSH Telnet greeting */
+
+  fputs(g_telnetgreeting, pstate->cn_outstream);
+  fflush(pstate->cn_outstream);
+
+  /* Loop for the configured number of retries */
+
+  for(i = 0; i < CONFIG_NSH_TELNET_FAILCOUNT; i++)
+    {
+      /* Ask for the login username */
+
+      fputs(g_userprompt, pstate->cn_outstream);
+      fflush(pstate->cn_outstream);
+      if (fgets(pstate->cn_line, CONFIG_NSH_LINELEN, INSTREAM(pstate)) != NULL)
+        {
+          strcpy(username, pstate->cn_line);
+          username[strlen(pstate->cn_line) - 1] = 0;
+        }
+
+      /* Ask for the login password */
+  
+      fputs(g_passwordprompt, pstate->cn_outstream);
+      fflush(pstate->cn_outstream);
+      nsh_telnetecho(pstate, TELNET_NOTUSE_ECHO);
+      if (fgets(pstate->cn_line, CONFIG_NSH_LINELEN, INSTREAM(pstate)) != NULL)
+        {
+          /* Verify the username and password */
+
+          strcpy(password,pstate->cn_line);
+          password[strlen(pstate->cn_line) - 1] = 0;
+
+          if (strcmp(password, CONFIG_NSH_TELNET_PASSWORD) == 0 &&
+              strcmp(username, CONFIG_NSH_TELNET_USERNAME) == 0)
+            {
+              fputs(g_loginsuccess, pstate->cn_outstream);
+              fflush(pstate->cn_outstream);
+              nsh_telnetecho(pstate, TELNET_USE_ECHO);
+              return OK;
+            }
+          else
+            {
+              fputs(g_badcredentials, pstate->cn_outstream);
+              fflush(pstate->cn_outstream);
+            }
+        }
+
+      nsh_telnetecho(pstate, TELNET_USE_ECHO);
+    }
+
+  /* Too many failed login attempts */
+
+  fputs(g_loginfailure, pstate->cn_outstream);
+  fflush(pstate->cn_outstream);
+  return -1;
+}
+#endif /* CONFIG_NSH_TELNET_LOGIN */
+
 /****************************************************************************
  * Public Functions
  ****************************************************************************/
@@ -90,7 +188,17 @@ int nsh_telnetmain(int argc, char *argv[])
 
   dbg("Session [%d] Started\n", getpid());
 
-  /* Present a greeting */
+  /* Login User and Password Check */
+
+#ifdef CONFIG_NSH_TELNET_LOGIN
+  if (nsh_telnetlogin(pstate) != OK)
+    {
+      nsh_exit(&pstate->cn_vtbl, 1);
+      return -1; /* nsh_exit does not return */
+    }
+#endif /* CONFIG_NSH_TELNET_LOGIN */
+
+  /* Present the NSH greeting */
 
   fputs(g_nshgreeting, pstate->cn_outstream);
   fflush(pstate->cn_outstream);

apps/position_estimator/Makefile

@@ -47,6 +47,4 @@ CSRCS		 = position_estimator_main.c \
 		   codegen/rtGetInf.c \
 		   codegen/rtGetNaN.c
 
-INCLUDES	 = $(TOPDIR)/../mavlink/include/mavlink
-
 include $(APPDIR)/mk/app.mk

apps/position_estimator/position_estimator_main.c

@@ -35,8 +35,9 @@
  *
  ****************************************************************************/
 
-/*
+/**
- * @file Model-identification based position estimator for multirotors
+ * @file position_estimator_main.c
+ * Model-identification based position estimator for multirotors
  */
 
 #include <nuttx/config.h>
@@ -45,7 +46,6 @@
 #include <stdio.h>
 #include <stdbool.h>
 #include <fcntl.h>
-#include <v1.0/common/mavlink.h>
 #include <float.h>
 #include <nuttx/sched.h>
 #include <sys/prctl.h>
@@ -58,6 +58,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <poll.h>
 
 #include "codegen/position_estimator.h"
@@ -251,14 +252,16 @@ int position_estimator_main(int argc, char *argv[])
 
 	bool new_initialization = true;
 
-	static double lat_current = 0;//[°]] --> 47.0
+	static double lat_current = 0.0d;//[°]] --> 47.0
-	static double lon_current = 0; //[°]] -->8.5
+	static double lon_current = 0.0d; //[°]] -->8.5
+	float alt_current = 0.0f;
 
 
 	//TODO: handle flight without gps but with estimator
 
 	/* subscribe to vehicle status, attitude, gps */
 	struct vehicle_gps_position_s gps;
+	gps.fix_type = 0;
 	struct vehicle_status_s vstatus;
 	struct vehicle_attitude_s att;
 
@@ -269,7 +272,7 @@ int position_estimator_main(int argc, char *argv[])
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 
 	/* wait until gps signal turns valid, only then can we initialize the projection */
-	while (!gps_valid) {
+	while (gps.fix_type < 3) {
 		struct pollfd fds[1] = { {.fd = vehicle_gps_sub, .events = POLLIN} };
 
 		/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
@@ -281,8 +284,8 @@ int position_estimator_main(int argc, char *argv[])
 			/* Wait for the GPS update to propagate (we have some time) */
 			usleep(5000);
 			/* Read wether the vehicle status changed */
-			orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus);
+			orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
-			gps_valid = vstatus.gps_valid;
+			gps_valid = (gps.fix_type > 2);
 		}
 	}
 
@@ -290,14 +293,18 @@ int position_estimator_main(int argc, char *argv[])
 	orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_sub, &gps);
 	lat_current = ((double)(gps.lat)) * 1e-7;
 	lon_current = ((double)(gps.lon)) * 1e-7;
+	alt_current = gps.alt * 1e-3;
+
+	/* initialize coordinates */
+	map_projection_init(lat_current, lon_current);
 
 	/* publish global position messages only after first GPS message */
-	struct vehicle_global_position_s global_pos = {
+	struct vehicle_local_position_s local_pos = {
-		.lat = lat_current * 1e7,
+		.x = 0,
-		.lon = lon_current * 1e7,
+		.y = 0,
-		.alt = gps.alt
+		.z = 0
 	};
-	orb_advert_t global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+	orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
 
 	printf("[multirotor position estimator] initialized projection with: lat: %.10f,  lon:%.10f\n", lat_current, lon_current);
 
@@ -320,91 +327,84 @@ int position_estimator_main(int argc, char *argv[])
 			u[1] = att.pitch;
 
 			/* initialize map projection with the last estimate (not at full rate) */
-			if (counter % PROJECTION_INITIALIZE_COUNTER_LIMIT == 0) {
+			if (gps.fix_type > 2) {
-				map_projection_init(lat_current, lon_current);
-				new_initialization = true;
-
-			} else {
-				new_initialization = false;
-			}
-
-			/*check if new gps values are available */
-			gps_valid = vstatus.gps_valid;
-
-
-			if (gps_valid) { //we are safe to use the gps signal (it has good quality)
-
-				predict_only = 0;
 				/* Project gps lat lon (Geographic coordinate system) to plane*/
-				map_projection_project((double)(gps.lat) * 1e-7, (double)(gps.lon) * 1e-7, &(z[0]), &(z[1]));
+				map_projection_project(((double)(gps.lat)) * 1e-7, ((double)(gps.lon)) * 1e-7, &(z[0]), &(z[1]));
 
-				/* copy altitude */
+				local_pos.x = z[0];
-				z[2] = (gps.alt) * 1e-3;
+				local_pos.y = z[1];
-
+				/* negative offset from initialization altitude */
-				gps_covariance[0] = gps.eph; //TODO: needs scaling
+				local_pos.z = alt_current - (gps.alt) * 1e-3;
-				gps_covariance[1] = gps.eph;
-				gps_covariance[2] = gps.epv;
-
-			} else {
-				/* we can not use the gps signal (it is of low quality) */
-				predict_only = 1;
-			}
-
-			//		predict_only = 0;																//TODO: only for testing, removeme, XXX
-			//		z[0] = sinf(((float)counter)/180.0f*3.14159265f);								//TODO: only for testing, removeme, XXX
-			//		usleep(100000);																	//TODO: only for testing, removeme, XXX
 
 
-			/*Get new estimation (this is calculated in the plane) */
+				orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
-			//TODO: if new_initialization == true: use 0,0,0, else use xapo
-			if (true == new_initialization) {																								//TODO,XXX: uncomment!
-				xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane
-				xapo[2] = 0;
-				xapo[4] = 0;
-				position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
-
-			} else {
-				position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
 			}
 
 
+			// 	gps_covariance[0] = gps.eph; //TODO: needs scaling
+			// 	gps_covariance[1] = gps.eph;
+			// 	gps_covariance[2] = gps.epv;
 
-			/* Copy values from xapo1 to xapo */
+			// } else {
-			int i;
+			// 	/* we can not use the gps signal (it is of low quality) */
+			// 	predict_only = 1;
+			// }
 
-			for (i = 0; i < N_STATES; i++) {
+			// //		predict_only = 0;																//TODO: only for testing, removeme, XXX
-				xapo[i] = xapo1[i];
+			// //		z[0] = sinf(((float)counter)/180.0f*3.14159265f);								//TODO: only for testing, removeme, XXX
-			}
+			// //		usleep(100000);																	//TODO: only for testing, removeme, XXX
 
-			if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) {
-				/* Reproject from plane to geographic coordinate system */
-				//		map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment!
-				map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme
-				//		//DEBUG
-				//			if(counter%500 == 0)
-				//			{
-				//				printf("phi_1: %.10f\n", phi_1);
-				//				printf("lambda_0: %.10f\n", lambda_0);
-				//				printf("lat_estimated: %.10f\n", lat_current);
-				//				printf("lon_estimated: %.10f\n", lon_current);
-				//				printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]);
-				//				fflush(stdout);
-				//
-				//			}
 
-				//			if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5]))
+			// /*Get new estimation (this is calculated in the plane) */
-				//			{
+			// //TODO: if new_initialization == true: use 0,0,0, else use xapo
-				/* send out */
+			// if (true == new_initialization) {																								//TODO,XXX: uncomment!
+			// 	xapo[0] = 0; //we have a new plane initialization. the current estimate is in the center of the plane
+			// 	xapo[2] = 0;
+			// 	xapo[4] = 0;
+			// 	position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
 
-				global_pos.lat = lat_current;
+			// } else {
-				global_pos.lon = lon_current;
+			// 	position_estimator(u, z, xapo, Papo, gps_covariance, predict_only, xapo1, Papo1);
-				global_pos.alt = xapo1[4];
+			// }
-				global_pos.vx = xapo1[1];
+
-				global_pos.vy = xapo1[3];
+
-				global_pos.vz = xapo1[5];
+
+			// /* Copy values from xapo1 to xapo */
+			// int i;
+
+			// for (i = 0; i < N_STATES; i++) {
+			// 	xapo[i] = xapo1[i];
+			// }
+
+			// if ((counter % REPROJECTION_COUNTER_LIMIT == 0) || (counter % (PROJECTION_INITIALIZE_COUNTER_LIMIT - 1) == 0)) {
+			// 	/* Reproject from plane to geographic coordinate system */
+			// 	//		map_projection_reproject(xapo1[0], xapo1[2], map_scale, phi_1, lambda_0, &lat_current, &lon_current) //TODO,XXX: uncomment!
+			// 	map_projection_reproject(z[0], z[1], &lat_current, &lon_current); //do not use estimator for projection testing, removeme
+			// 	//		//DEBUG
+			// 	//			if(counter%500 == 0)
+			// 	//			{
+			// 	//				printf("phi_1: %.10f\n", phi_1);
+			// 	//				printf("lambda_0: %.10f\n", lambda_0);
+			// 	//				printf("lat_estimated: %.10f\n", lat_current);
+			// 	//				printf("lon_estimated: %.10f\n", lon_current);
+			// 	//				printf("z[0]=%.10f, z[1]=%.10f, z[2]=%f\n", z[0], z[1], z[2]);
+			// 	//				fflush(stdout);
+			// 	//
+			// 	//			}
+
+			// 	//			if(!isnan(lat_current) && !isnan(lon_current))// && !isnan(xapo1[4]) && !isnan(xapo1[1]) && !isnan(xapo1[3]) && !isnan(xapo1[5]))
+			// 	//			{
+			// 	/* send out */
+
+			// 	global_pos.lat = lat_current;
+			// 	global_pos.lon = lon_current;
+			// 	global_pos.alt = xapo1[4];
+			// 	global_pos.vx = xapo1[1];
+			// 	global_pos.vy = xapo1[3];
+			// 	global_pos.vz = xapo1[5];
 
 				/* publish current estimate */
-				orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos);
+				// orb_publish(ORB_ID(vehicle_global_position), global_pos_pub, &global_pos);
 				//			}
 				//			else
 				//			{
@@ -412,7 +412,7 @@ int position_estimator_main(int argc, char *argv[])
 				//				fflush(stdout);
 				//			}
 
-			}
+			// }
 
 			counter++;
 		}

apps/px4/fmu/fmu.cpp

@@ -79,7 +79,7 @@ public:
 	FMUServo(Mode mode, int update_rate);
 	~FMUServo();
 
-	virtual int	ioctl(struct file *filp, int cmd, unsigned long arg);
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
 
 	virtual int	init();
 
@@ -93,6 +93,7 @@ private:
 	int		_t_armed;
 	orb_advert_t	_t_outputs;
 	unsigned	_num_outputs;
+	bool		_primary_pwm_device;
 
 	volatile bool	_task_should_exit;
 	bool		_armed;
@@ -118,7 +119,7 @@ FMUServo	*g_servo;
 } // namespace
 
 FMUServo::FMUServo(Mode mode, int update_rate) :
-	CDev("fmuservo", PWM_OUTPUT_DEVICE_PATH),
+	CDev("fmuservo", "/dev/px4fmu"),
 	_mode(mode),
 	_update_rate(update_rate),
 	_task(-1),
@@ -126,6 +127,7 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
 	_t_armed(-1),
 	_t_outputs(0),
 	_num_outputs(0),
+	_primary_pwm_device(false),
 	_task_should_exit(false),
 	_armed(false),
 	_mixers(nullptr)
@@ -135,18 +137,16 @@ FMUServo::FMUServo(Mode mode, int update_rate) :
 FMUServo::~FMUServo()
 {
 	if (_task != -1) {
-
+		/* tell the task we want it to go away */
-		/* task should wake up every 100ms or so at least */
 		_task_should_exit = true;
 
-		unsigned i = 0;
+		unsigned i = 10;
-
 		do {
-			/* wait 20ms */
+			/* wait 50ms - it should wake every 100ms or so worst-case */
-			usleep(20000);
+			usleep(50000);
 
 			/* if we have given up, kill it */
-			if (++i > 10) {
+			if (--i == 0) {
 				task_delete(_task);
 				break;
 			}
@@ -154,6 +154,10 @@ FMUServo::~FMUServo()
 		} while (_task != -1);
 	}
 
+	/* clean up the alternate device node */
+	if (_primary_pwm_device)
+		unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
 	g_servo = nullptr;
 }
 
@@ -170,9 +174,16 @@ FMUServo::init()
 	if (ret != OK)
 		return ret;
 
+	/* try to claim the generic PWM output device node as well - it's OK if we fail at this */
+	ret = register_driver(PWM_OUTPUT_DEVICE_PATH, &fops, 0666, (void *)this);
+	if (ret == OK) {
+		log("default PWM output device");
+		_primary_pwm_device = true;
+	}
+
 	/* start the IO interface task */
 	_task = task_spawn("fmuservo",
-			   SCHED_RR,
+			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
 			   1024,
 			   (main_t)&FMUServo::task_main_trampoline,
@@ -216,8 +227,12 @@ FMUServo::task_main()
 		break;
 	}
 
-	/* subscribe to objects that we are interested in watching */
+	/*
-	_t_actuators = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	 * Subscribe to the appropriate PWM output topic based on whether we are the
+	 * primary PWM output or not.
+	 */
+	_t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1));
 	/* convert the update rate in hz to milliseconds, rounding down if necessary */
 	int update_rate_in_ms = int(1000 / _update_rate);
 	orb_set_interval(_t_actuators, update_rate_in_ms);
@@ -226,11 +241,13 @@ FMUServo::task_main()
 	orb_set_interval(_t_armed, 200);		/* 5Hz update rate */
 
 	/* advertise the mixed control outputs */
-	struct actuator_outputs_s outputs;
+	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
-	_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+	/* advertise the mixed control outputs */
+	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
+				   &outputs);
 
-	struct pollfd fds[2];
+	pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
 	fds[1].fd = _t_armed;
@@ -282,7 +299,7 @@ FMUServo::task_main()
 
 		/* how about an arming update? */
 		if (fds[1].revents & POLLIN) {
-			struct actuator_armed_s aa;
+			actuator_armed_s aa;
 
 			/* get new value */
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
@@ -320,7 +337,7 @@ FMUServo::control_callback(uintptr_t handle,
 }
 
 int
-FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
+FMUServo::ioctl(file *filp, int cmd, unsigned long arg)
 {
 	int ret = OK;
 	int channel;
@@ -569,7 +586,7 @@ fake(int argc, char *argv[])
 		exit(1);
 	}
 
-	struct actuator_controls_s ac;
+	actuator_controls_s ac;
 
 	ac.control[0] = strtol(argv[1], 0, 0) / 100.0f;