aboutsummaryrefslogtreecommitdiff
path: root/apps/uORB
diff options
context:
space:
mode:
authorpx4dev <px4@purgatory.org>2012-08-04 15:12:36 -0700
committerpx4dev <px4@purgatory.org>2012-08-04 15:12:36 -0700
commit8a365179eafdf3aea98e60ab9f5882b200d4c759 (patch)
tree4f38d6d4cd80bd0b6e22e2bb534c3f117ce44e56 /apps/uORB
downloadpx4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.tar.gz
px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.tar.bz2
px4-firmware-8a365179eafdf3aea98e60ab9f5882b200d4c759.zip
Fresh import of the PX4 firmware sources.
Diffstat (limited to 'apps/uORB')
-rw-r--r--apps/uORB/.context0
-rw-r--r--apps/uORB/Makefile42
-rw-r--r--apps/uORB/objects_common.cpp101
-rw-r--r--apps/uORB/parameter_storage.c379
-rw-r--r--apps/uORB/parameter_storage.h148
-rw-r--r--apps/uORB/topics/UNPORTED_subsystem_info.h68
-rw-r--r--apps/uORB/topics/actuator_controls.h64
-rw-r--r--apps/uORB/topics/ardrone_control.h83
-rw-r--r--apps/uORB/topics/ardrone_motors_setpoint.h70
-rw-r--r--apps/uORB/topics/fixedwing_control.h77
-rw-r--r--apps/uORB/topics/rc_channels.h116
-rw-r--r--apps/uORB/topics/sensor_combined.h103
-rw-r--r--apps/uORB/topics/vehicle_attitude.h92
-rw-r--r--apps/uORB/topics/vehicle_attitude_setpoint.h77
-rw-r--r--apps/uORB/topics/vehicle_command.h79
-rw-r--r--apps/uORB/topics/vehicle_global_position.h86
-rw-r--r--apps/uORB/topics/vehicle_global_position_setpoint.h78
-rw-r--r--apps/uORB/topics/vehicle_gps_position.h89
-rw-r--r--apps/uORB/topics/vehicle_local_position.h87
-rw-r--r--apps/uORB/topics/vehicle_local_position_setpoint.h67
-rw-r--r--apps/uORB/topics/vehicle_status.h140
-rw-r--r--apps/uORB/uORB.cpp969
-rw-r--r--apps/uORB/uORB.h248
23 files changed, 3263 insertions, 0 deletions
diff --git a/apps/uORB/.context b/apps/uORB/.context
new file mode 100644
index 000000000..e69de29bb
--- /dev/null
+++ b/apps/uORB/.context
diff --git a/apps/uORB/Makefile b/apps/uORB/Makefile
new file mode 100644
index 000000000..0766a66eb
--- /dev/null
+++ b/apps/uORB/Makefile
@@ -0,0 +1,42 @@
+############################################################################
+#
+# Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Makefile to build uORB
+#
+
+APPNAME = uorb
+PRIORITY = SCHED_PRIORITY_DEFAULT
+STACKSIZE = 2048
+
+include $(APPDIR)/mk/app.mk
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
new file mode 100644
index 000000000..8d84ff3c2
--- /dev/null
+++ b/apps/uORB/objects_common.cpp
@@ -0,0 +1,101 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Common object definitions without a better home.
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/drv_orb_dev.h>
+
+#include <drivers/drv_mag.h>
+ORB_DEFINE(sensor_mag, struct mag_report);
+
+#include <drivers/drv_accel.h>
+ORB_DEFINE(sensor_accel, struct accel_report);
+
+#include <drivers/drv_gyro.h>
+ORB_DEFINE(sensor_gyro, struct gyro_report);
+
+#include <drivers/drv_baro.h>
+ORB_DEFINE(sensor_baro, struct baro_report);
+
+#include <drivers/drv_pwm_output.h>
+ORB_DEFINE(output_pwm, struct pwm_output_values);
+
+#include <drivers/drv_rc_input.h>
+ORB_DEFINE(input_rc, struct rc_input_values);
+
+// XXX need to check wether these should be here
+#include "topics/vehicle_attitude.h"
+ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s);
+
+#include "topics/sensor_combined.h"
+ORB_DEFINE(sensor_combined, struct sensor_combined_s);
+
+#include "topics/vehicle_gps_position.h"
+ORB_DEFINE(vehicle_gps_position, struct vehicle_gps_position_s);
+
+#include "topics/vehicle_status.h"
+ORB_DEFINE(vehicle_status, struct vehicle_status_s);
+
+#include "topics/vehicle_global_position.h"
+ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s);
+
+#include "topics/vehicle_local_position.h"
+ORB_DEFINE(vehicle_local_position, struct vehicle_local_position_s);
+
+#include "topics/ardrone_control.h"
+ORB_DEFINE(ardrone_control, struct ardrone_control_s);
+
+#include "topics/ardrone_motors_setpoint.h"
+ORB_DEFINE(ardrone_motors_setpoint, struct ardrone_motors_setpoint_s);
+
+#include "topics/rc_channels.h"
+ORB_DEFINE(rc_channels, struct rc_channels_s);
+
+#include "topics/fixedwing_control.h"
+ORB_DEFINE(fixedwing_control, struct fixedwing_control_s);
+
+#include "topics/vehicle_command.h"
+ORB_DEFINE(vehicle_command, struct vehicle_command_s);
+
+#include "topics/vehicle_local_position_setpoint.h"
+ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s);
+
+#include "topics/vehicle_global_position_setpoint.h"
+ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setpoint_s);
+
+#include "topics/vehicle_attitude_setpoint.h"
+ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_s);
diff --git a/apps/uORB/parameter_storage.c b/apps/uORB/parameter_storage.c
new file mode 100644
index 000000000..f4bd1c712
--- /dev/null
+++ b/apps/uORB/parameter_storage.c
@@ -0,0 +1,379 @@
+
+#include "parameter_storage.h"
+#include <stdbool.h>
+
+
+/* Global symbols / flags */
+
+struct global_data_parameter_storage_t global_data_parameter_storage_d = { /*.counter = 0, .timestamp = 0,*/ .pm = {.size = PARAM_MAX_COUNT,
+ .param_values[PARAM_SYSTEM_ID] = 12,
+ .param_names[PARAM_SYSTEM_ID] = "SYS_ID",
+ .param_needs_write[PARAM_SYSTEM_ID] = false,
+
+ .param_values[PARAM_COMP_ID] = 200,
+ .param_names[PARAM_COMP_ID] = "COMP_ID",
+ .param_needs_write[PARAM_COMP_ID] = false,
+
+ .param_values[PARAM_FLIGHT_ENV] = (float)PX4_FLIGHT_ENVIRONMENT_INDOOR,
+ .param_names[PARAM_FLIGHT_ENV] = "FLIGHT_ENV",
+ .param_needs_write[PARAM_FLIGHT_ENV] = false,
+
+ .param_values[PARAM_BATTERYVOLTAGE_CONVERSION] = -1.f,
+ .param_names[PARAM_BATTERYVOLTAGE_CONVERSION] = "BATVOLTAG_CONV",
+ .param_needs_write[PARAM_BATTERYVOLTAGE_CONVERSION] = false,
+
+ .param_values[PARAM_PID_YAWPOS_P] = 0.3f,
+ .param_names[PARAM_PID_YAWPOS_P] = "PID_YAWPOS_P",
+ .param_needs_write[PARAM_PID_YAWPOS_P] = false,
+
+ .param_values[PARAM_PID_YAWPOS_I] = 0.15f,
+ .param_names[PARAM_PID_YAWPOS_I] = "PID_YAWPOS_I",
+ .param_needs_write[PARAM_PID_YAWPOS_I] = false,
+
+ .param_values[PARAM_PID_YAWPOS_D] = 0.0f,
+ .param_names[PARAM_PID_YAWPOS_D] = "PID_YAWPOS_D",
+ .param_needs_write[PARAM_PID_YAWPOS_D] = false,
+
+ .param_values[PARAM_PID_YAWPOS_AWU] = 1.0f,
+ .param_names[PARAM_PID_YAWPOS_AWU] = "PID_YAWPOS_AWU",
+ .param_needs_write[PARAM_PID_YAWPOS_AWU] = false,
+
+ .param_values[PARAM_PID_YAWPOS_LIM] = 3.0f,
+ .param_names[PARAM_PID_YAWPOS_LIM] = "PID_YAWPOS_LIM",
+ .param_needs_write[PARAM_PID_YAWPOS_LIM] = false,
+
+ .param_values[PARAM_PID_YAWSPEED_P] = 0.1f,
+ .param_names[PARAM_PID_YAWSPEED_P] = "PID_YAWSPD_P",
+ .param_needs_write[PARAM_PID_YAWSPEED_P] = false,
+
+ .param_values[PARAM_PID_YAWSPEED_I] = 0.02f,
+ .param_names[PARAM_PID_YAWSPEED_I] = "PID_YAWSPD_I",
+ .param_needs_write[PARAM_PID_YAWSPEED_I] = false,
+
+ .param_values[PARAM_PID_YAWSPEED_D] = 0.0f,
+ .param_names[PARAM_PID_YAWSPEED_D] = "PID_YAWSPD_D",
+ .param_needs_write[PARAM_PID_YAWSPEED_D] = false,
+
+ .param_values[PARAM_PID_YAWSPEED_AWU] = 0.02f,
+ .param_names[PARAM_PID_YAWSPEED_AWU] = "PID_YAWSPD_AWU",
+ .param_needs_write[PARAM_PID_YAWSPEED_AWU] = false,
+
+ .param_values[PARAM_PID_YAWSPEED_LIM] = 0.1f,
+ .param_names[PARAM_PID_YAWSPEED_LIM] = "PID_YAWSPD_LIM",
+ .param_needs_write[PARAM_PID_YAWSPEED_LIM] = false,
+
+ .param_values[PARAM_PID_ATT_P] = 0.3f,
+ .param_names[PARAM_PID_ATT_P] = "PID_ATT_P",
+ .param_needs_write[PARAM_PID_ATT_P] = false,
+
+ .param_values[PARAM_PID_ATT_I] = 0.0f,
+ .param_names[PARAM_PID_ATT_I] = "PID_ATT_I",
+ .param_needs_write[PARAM_PID_ATT_I] = false,
+
+ .param_values[PARAM_PID_ATT_D] = 0.1f,
+ .param_names[PARAM_PID_ATT_D] = "PID_ATT_D",
+ .param_needs_write[PARAM_PID_ATT_D] = false,
+
+ .param_values[PARAM_PID_ATT_AWU] = 0.05f,
+ .param_names[PARAM_PID_ATT_AWU] = "PID_ATT_AWU",
+ .param_needs_write[PARAM_PID_ATT_AWU] = false,
+
+ .param_values[PARAM_PID_ATT_LIM] = 0.3f,
+ .param_names[PARAM_PID_ATT_LIM] = "PID_ATT_LIM",
+ .param_needs_write[PARAM_PID_ATT_LIM] = false,
+
+ .param_values[PARAM_PID_POS_P] = 40.0f,
+ .param_names[PARAM_PID_POS_P] = "PID_POS_P",
+ .param_needs_write[PARAM_PID_POS_P] = false,
+
+ .param_values[PARAM_PID_POS_I] = 0.0f,
+ .param_names[PARAM_PID_POS_I] = "PID_POS_I",
+ .param_needs_write[PARAM_PID_POS_I] = false,
+
+ .param_values[PARAM_PID_POS_D] = 0.0f,
+ .param_names[PARAM_PID_POS_D] = "PID_POS_D",
+ .param_needs_write[PARAM_PID_POS_D] = false,
+
+ .param_values[PARAM_PID_POS_AWU] = 5.0f,
+ .param_names[PARAM_PID_POS_AWU] = "PID_POS_AWU",
+ .param_needs_write[PARAM_PID_POS_AWU] = false,
+
+ .param_values[PARAM_PID_POS_LIM] = 0.3f,
+ .param_names[PARAM_PID_POS_LIM] = "PID_POS_LIM",
+ .param_needs_write[PARAM_PID_POS_LIM] = false,
+
+ .param_values[PARAM_PID_POS_Z_P] = 10.0f,
+ .param_names[PARAM_PID_POS_Z_P] = "PID_POS_Z_P",
+ .param_needs_write[PARAM_PID_POS_Z_P] = false,
+
+ .param_values[PARAM_PID_POS_Z_I] = 0.0f,
+ .param_names[PARAM_PID_POS_Z_I] = "PID_POS_Z_I",
+ .param_needs_write[PARAM_PID_POS_Z_I] = false,
+
+ .param_values[PARAM_PID_POS_Z_D] = 0.0f,
+ .param_names[PARAM_PID_POS_Z_D] = "PID_POS_Z_D",
+ .param_needs_write[PARAM_PID_POS_Z_D] = false,
+
+ .param_values[PARAM_PID_POS_Z_AWU] = 3.0f,
+ .param_names[PARAM_PID_POS_Z_AWU] = "PID_POS_Z_AWU",
+ .param_needs_write[PARAM_PID_POS_Z_AWU] = false,
+
+ .param_values[PARAM_PID_POS_Z_LIM] = 0.3f,
+ .param_names[PARAM_PID_POS_Z_LIM] = "PID_POS_Z_LIM",
+ .param_needs_write[PARAM_PID_POS_Z_LIM] = false,
+
+ .param_values[PARAM_AIRSPEED] = 30.0f,
+ .param_names[PARAM_AIRSPEED] = "AIRSPEED",
+ .param_needs_write[PARAM_AIRSPEED] = false,
+
+ .param_values[PARAM_WPLON] = -120.0f,
+ .param_names[PARAM_WPLON] = "WPLON",
+ .param_needs_write[PARAM_WPLON] = false,
+
+ .param_values[PARAM_WPLAT] = 38.0f,
+ .param_names[PARAM_WPLAT] = "WPLAT",
+ .param_needs_write[PARAM_WPLAT] = false,
+
+ .param_values[PARAM_WPALT] = 500.0f,
+ .param_names[PARAM_WPALT] = "WPALT",
+ .param_needs_write[PARAM_WPALT] = false,
+
+ .param_values[PARAM_FLIGHTMODE] = CRUISE,
+ .param_names[PARAM_FLIGHTMODE] = "FLIGHTMODE",
+ .param_needs_write[PARAM_FLIGHTMODE] = false,
+
+ .param_values[PARAM_SENSOR_GYRO_XOFFSET] = 700.f,
+ .param_names[PARAM_SENSOR_GYRO_XOFFSET] = "SENSOR_GYRO_XOF",
+ .param_needs_write[PARAM_SENSOR_GYRO_XOFFSET] = false,
+
+ .param_values[PARAM_SENSOR_GYRO_YOFFSET] = 1400.0f,
+ .param_names[PARAM_SENSOR_GYRO_YOFFSET] = "SENSOR_GYRO_YOF",
+ .param_needs_write[PARAM_SENSOR_GYRO_YOFFSET] = false,
+
+ .param_values[PARAM_SENSOR_GYRO_ZOFFSET] = 0.0f,
+ .param_names[PARAM_SENSOR_GYRO_ZOFFSET] = "SENSOR_GYRO_ZOF",
+ .param_needs_write[PARAM_SENSOR_GYRO_ZOFFSET] = false,
+
+ .param_values[PARAM_SENSOR_MAG_XOFFSET] = 422.0f,
+ .param_names[PARAM_SENSOR_MAG_XOFFSET] = "SENSOR_MAG_XOF",
+ .param_needs_write[PARAM_SENSOR_MAG_XOFFSET] = false,
+
+ .param_values[PARAM_SENSOR_MAG_YOFFSET] = -85.0f,
+ .param_names[PARAM_SENSOR_MAG_YOFFSET] = "SENSOR_MAG_YOF",
+ .param_needs_write[PARAM_SENSOR_MAG_YOFFSET] = false,
+
+ .param_values[PARAM_SENSOR_MAG_ZOFFSET] = -370.0f,
+ .param_names[PARAM_SENSOR_MAG_ZOFFSET] = "SENSOR_MAG_ZOF",
+ .param_needs_write[PARAM_SENSOR_MAG_ZOFFSET] = false,
+
+ .param_values[PARAM_ATT_XOFFSET] = 0.0f,
+ .param_names[PARAM_ATT_XOFFSET] = "ATT_XOFF",
+ .param_needs_write[PARAM_ATT_XOFFSET] = false,
+
+ .param_values[PARAM_ATT_YOFFSET] = 0.0f,
+ .param_names[PARAM_ATT_YOFFSET] = "ATT_YOFF",
+ .param_needs_write[PARAM_ATT_YOFFSET] = false,
+
+ .param_values[PARAM_RC1_MIN] = 1000.0f,
+ .param_names[PARAM_RC1_MIN] = "RC1_MIN",
+ .param_needs_write[PARAM_RC1_MIN] = false,
+
+ .param_values[PARAM_RC1_MAX] = 2000.0f,
+ .param_names[PARAM_RC1_MAX] = "RC1_MAX",
+ .param_needs_write[PARAM_RC1_MAX] = false,
+
+ .param_values[PARAM_RC1_TRIM] = 1500.0f,
+ .param_names[PARAM_RC1_TRIM] = "RC1_TRIM",
+ .param_needs_write[PARAM_RC1_TRIM] = false,
+
+ .param_values[PARAM_RC1_REV] = 1.0f,
+ .param_names[PARAM_RC1_REV] = "RC1_REV",
+ .param_needs_write[PARAM_RC1_REV] = false,
+
+ .param_values[PARAM_RC2_MIN] = 1000.0f,
+ .param_names[PARAM_RC2_MIN] = "RC2_MIN",
+ .param_needs_write[PARAM_RC2_MIN] = false,
+
+ .param_values[PARAM_RC2_MAX] = 2000.0f,
+ .param_names[PARAM_RC2_MAX] = "RC2_MAX",
+ .param_needs_write[PARAM_RC2_MAX] = false,
+
+ .param_values[PARAM_RC2_TRIM] = 1500.0f,
+ .param_names[PARAM_RC2_TRIM] = "RC2_TRIM",
+ .param_needs_write[PARAM_RC2_TRIM] = false,
+
+ .param_values[PARAM_RC2_REV] = 1.0f,
+ .param_names[PARAM_RC2_REV] = "RC2_REV",
+ .param_needs_write[PARAM_RC2_REV] = false,
+
+ .param_values[PARAM_RC3_MIN] = 1000.0f,
+ .param_names[PARAM_RC3_MIN] = "RC3_MIN",
+ .param_needs_write[PARAM_RC3_MIN] = false,
+
+ .param_values[PARAM_RC3_MAX] = 2000.0f,
+ .param_names[PARAM_RC3_MAX] = "RC3_MAX",
+ .param_needs_write[PARAM_RC3_MAX] = false,
+
+ .param_values[PARAM_RC3_TRIM] = 1500.0f,
+ .param_names[PARAM_RC3_TRIM] = "RC3_TRIM",
+ .param_needs_write[PARAM_RC3_TRIM] = false,
+
+ .param_values[PARAM_RC3_REV] = 1.0f,
+ .param_names[PARAM_RC3_REV] = "RC3_REV",
+ .param_needs_write[PARAM_RC3_REV] = false,
+
+ .param_values[PARAM_RC4_MIN] = 1000.0f,
+ .param_names[PARAM_RC4_MIN] = "RC4_MIN",
+ .param_needs_write[PARAM_RC4_MIN] = false,
+
+ .param_values[PARAM_RC4_MAX] = 2000.0f,
+ .param_names[PARAM_RC4_MAX] = "RC4_MAX",
+ .param_needs_write[PARAM_RC4_MAX] = false,
+
+ .param_values[PARAM_RC4_TRIM] = 1500.0f,
+ .param_names[PARAM_RC4_TRIM] = "RC4_TRIM",
+ .param_needs_write[PARAM_RC4_TRIM] = false,
+
+ .param_values[PARAM_RC4_REV] = 1.0f,
+ .param_names[PARAM_RC4_REV] = "RC4_REV",
+ .param_needs_write[PARAM_RC4_MIN] = false,
+
+ .param_values[PARAM_RC5_MIN] = 1000.0f,
+ .param_names[PARAM_RC5_MIN] = "RC5_MIN",
+ .param_needs_write[PARAM_RC5_MIN] = false,
+
+ .param_values[PARAM_RC5_MAX] = 2000.0f,
+ .param_names[PARAM_RC5_MAX] = "RC5_MAX",
+ .param_needs_write[PARAM_RC5_MAX] = false,
+
+ .param_values[PARAM_RC5_TRIM] = 1500.0f,
+ .param_names[PARAM_RC5_TRIM] = "RC5_TRIM",
+ .param_needs_write[PARAM_RC5_TRIM] = false,
+
+ .param_values[PARAM_RC5_REV] = 1.0f,
+ .param_names[PARAM_RC5_REV] = "RC5_REV",
+ .param_needs_write[PARAM_RC5_REV] = false,
+
+ .param_values[PARAM_RC6_MIN] = 1000.0f,
+ .param_names[PARAM_RC6_MIN] = "RC6_MIN",
+ .param_needs_write[PARAM_RC6_MIN] = false,
+
+ .param_values[PARAM_RC6_MAX] = 2000.0f,
+ .param_names[PARAM_RC6_MAX] = "RC6_MAX",
+ .param_needs_write[PARAM_RC6_MAX] = false,
+
+ .param_values[PARAM_RC6_TRIM] = 1500.0f,
+ .param_names[PARAM_RC6_TRIM] = "RC6_TRIM",
+ .param_needs_write[PARAM_RC6_TRIM] = false,
+
+ .param_values[PARAM_RC6_REV] = 1.0f,
+ .param_names[PARAM_RC6_REV] = "RC6_REV",
+ .param_needs_write[PARAM_RC6_REV] = false,
+
+ .param_values[PARAM_RC7_MIN] = 1000,
+ .param_names[PARAM_RC7_MIN] = "RC7_MIN",
+ .param_needs_write[PARAM_RC7_MIN] = false,
+
+ .param_values[PARAM_RC7_MAX] = 2000,
+ .param_names[PARAM_RC7_MAX] = "RC7_MAX",
+ .param_needs_write[PARAM_RC7_MAX] = false,
+
+ .param_values[PARAM_RC7_TRIM] = 1500,
+ .param_names[PARAM_RC7_TRIM] = "RC7_TRIM",
+ .param_needs_write[PARAM_RC7_TRIM] = false,
+
+ .param_values[PARAM_RC7_REV] = 1.0f,
+ .param_names[PARAM_RC7_REV] = "RC7_REV",
+ .param_needs_write[PARAM_RC7_REV] = false,
+
+ .param_values[PARAM_RC8_MIN] = 1000,
+ .param_names[PARAM_RC8_MIN] = "RC8_MIN",
+ .param_needs_write[PARAM_RC8_MIN] = false,
+
+ .param_values[PARAM_RC8_MAX] = 2000,
+ .param_names[PARAM_RC8_MAX] = "RC8_MAX",
+ .param_needs_write[PARAM_RC8_MAX] = false,
+
+ .param_values[PARAM_RC8_TRIM] = 1500,
+ .param_names[PARAM_RC8_TRIM] = "RC8_TRIM",
+ .param_needs_write[PARAM_RC8_TRIM] = false,
+
+ .param_values[PARAM_RC8_REV] = 1.0f,
+ .param_names[PARAM_RC8_REV] = "RC8_REV",
+ .param_needs_write[PARAM_RC8_REV] = false,
+
+ .param_values[PARAM_ROLL_CHAN] = 1,
+ .param_names[PARAM_ROLL_CHAN] = "ROLL_CHAN",
+ .param_needs_write[PARAM_ROLL_CHAN] = false,
+
+ .param_values[PARAM_PITCH_CHAN] = 2,
+ .param_names[PARAM_PITCH_CHAN] = "PITCH_CHAN",
+ .param_needs_write[PARAM_PITCH_CHAN] = false,
+
+ .param_values[PARAM_THROTTLE_CHAN] = 3,
+ .param_names[PARAM_THROTTLE_CHAN] = "THROTTLE_CHAN",
+ .param_needs_write[PARAM_THROTTLE_CHAN] = false,
+
+ .param_values[PARAM_YAW_CHAN] = 4,
+ .param_names[PARAM_YAW_CHAN] = "YAW_CHAN",
+ .param_needs_write[PARAM_YAW_CHAN] = false,
+
+ .param_values[PARAM_OVERRIDE_CHAN] = 5,
+ .param_names[PARAM_OVERRIDE_CHAN] = "OVERRIDE_CHAN",
+ .param_needs_write[PARAM_OVERRIDE_CHAN] = false,
+
+ .param_values[PARAM_SERVO1_MIN] = 1000.0f,
+ .param_names[PARAM_SERVO1_MIN] = "SERVO1_MIN",
+ .param_needs_write[PARAM_SERVO1_MIN] = false,
+
+ .param_values[PARAM_SERVO1_MAX] = 2000.0f,
+ .param_names[PARAM_SERVO1_MAX] = "SERVO1_MAX",
+ .param_needs_write[PARAM_SERVO1_MAX] = false,
+
+ .param_values[PARAM_SERVO1_TRIM] = 1500.0f,
+ .param_names[PARAM_SERVO1_TRIM] = "SERVO1_TRIM",
+ .param_needs_write[PARAM_SERVO1_TRIM] = false,
+
+ .param_values[PARAM_SERVO2_MIN] = 1000.0f,
+ .param_names[PARAM_SERVO2_MIN] = "SERVO2_MIN",
+ .param_needs_write[PARAM_SERVO2_MIN] = false,
+
+ .param_values[PARAM_SERVO2_MAX] = 2000.0f,
+ .param_names[PARAM_SERVO2_MAX] = "SERVO2_MAX",
+ .param_needs_write[PARAM_SERVO2_MAX] = false,
+
+ .param_values[PARAM_SERVO2_TRIM] = 1500.0f,
+ .param_names[PARAM_SERVO2_TRIM] = "SERVO2_TRIM",
+ .param_needs_write[PARAM_SERVO2_TRIM] = false,
+
+ .param_values[PARAM_SERVO3_MIN] = 1000.0f,
+ .param_names[PARAM_SERVO3_MIN] = "SERVO3_MIN",
+ .param_needs_write[PARAM_SERVO3_MIN] = false,
+
+ .param_values[PARAM_SERVO3_MAX] = 2000.0f,
+ .param_names[PARAM_SERVO3_MAX] = "SERVO3_MAX",
+ .param_needs_write[PARAM_SERVO3_MAX] = false,
+
+ .param_values[PARAM_SERVO3_TRIM] = 1500.0f,
+ .param_names[PARAM_SERVO3_TRIM] = "SERVO3_TRIM",
+ .param_needs_write[PARAM_SERVO3_TRIM] = false,
+
+ .param_values[PARAM_SERVO4_MIN] = 1000.0f,
+ .param_names[PARAM_SERVO4_MIN] = "SERVO4_MIN",
+ .param_needs_write[PARAM_SERVO4_MIN] = false,
+
+ .param_values[PARAM_SERVO4_MAX] = 2000.0f,
+ .param_names[PARAM_SERVO4_MAX] = "SERVO4_MAX",
+ .param_needs_write[PARAM_SERVO4_MAX] = false,
+
+ .param_values[PARAM_SERVO4_TRIM] = 1500.0f,
+ .param_names[PARAM_SERVO4_TRIM] = "SERVO4_TRIM",
+ .param_needs_write[PARAM_SERVO4_TRIM] = false,
+
+ .param_values[PARAM_SERVO_SCALE] = 20.0f,
+ .param_names[PARAM_SERVO_SCALE] = "SERVO_SCALE",
+ .param_needs_write[PARAM_SERVO_SCALE] = false
+ }
+};
+
+struct global_data_parameter_storage_t *global_data_parameter_storage = &global_data_parameter_storage_d;
diff --git a/apps/uORB/parameter_storage.h b/apps/uORB/parameter_storage.h
new file mode 100644
index 000000000..54ae63cb9
--- /dev/null
+++ b/apps/uORB/parameter_storage.h
@@ -0,0 +1,148 @@
+/* Structure for storage of parameters */
+
+#ifndef GLOBAL_DATA_PARAMETER_STORAGE_T_H_ //adjust this line!
+#define GLOBAL_DATA_PARAMETER_STORAGE_T_H_ //adjust this line!
+
+#define PX4_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16
+#define MAX_PARAM_NAME_LEN 16
+
+#include <stdbool.h>
+#include <stdint.h>
+
+enum MODE {
+ TAKEOFF = 1,
+ CRUISE,
+ LOITER,
+ LAND
+};
+
+enum PARAM {
+ PARAM_SYSTEM_ID = 0, ///< System ID used for communication
+ PARAM_COMP_ID, ///< Component ID
+ PARAM_FLIGHT_ENV, ///< Flight environment, indoor or outdoor
+ PARAM_BATTERYVOLTAGE_CONVERSION, ///< Conversion factor from adc measurement to millivolts. if not set the sensor app will use the default value for the ardrone board
+ PARAM_PID_YAWPOS_P,
+ PARAM_PID_YAWPOS_I,
+ PARAM_PID_YAWPOS_D,
+ PARAM_PID_YAWPOS_AWU,
+ PARAM_PID_YAWPOS_LIM,
+ PARAM_PID_YAWSPEED_P,
+ PARAM_PID_YAWSPEED_I,
+ PARAM_PID_YAWSPEED_D,
+ PARAM_PID_YAWSPEED_AWU,
+ PARAM_PID_YAWSPEED_LIM,
+ PARAM_PID_ATT_P,
+ PARAM_PID_ATT_I,
+ PARAM_PID_ATT_D,
+ PARAM_PID_ATT_AWU,
+ PARAM_PID_ATT_LIM,
+ PARAM_PID_POS_P,
+ PARAM_PID_POS_I,
+ PARAM_PID_POS_D,
+ PARAM_PID_POS_AWU,
+ PARAM_PID_POS_LIM,
+ PARAM_PID_POS_Z_P,
+ PARAM_PID_POS_Z_I,
+ PARAM_PID_POS_Z_D,
+ PARAM_PID_POS_Z_AWU,
+ PARAM_PID_POS_Z_LIM,
+ PARAM_AIRSPEED,
+ PARAM_WPLON,
+ PARAM_WPLAT,
+ PARAM_WPALT,
+ PARAM_FLIGHTMODE,
+ PARAM_SENSOR_GYRO_XOFFSET,
+ PARAM_SENSOR_GYRO_YOFFSET,
+ PARAM_SENSOR_GYRO_ZOFFSET,
+ PARAM_SENSOR_MAG_XOFFSET,
+ PARAM_SENSOR_MAG_YOFFSET,
+ PARAM_SENSOR_MAG_ZOFFSET,
+ PARAM_ATT_XOFFSET,
+ PARAM_ATT_YOFFSET,
+ PARAM_RC1_MIN,
+ PARAM_RC1_MAX,
+ PARAM_RC1_TRIM,
+ PARAM_RC1_REV,
+ PARAM_RC2_MIN,
+ PARAM_RC2_MAX,
+ PARAM_RC2_TRIM,
+ PARAM_RC2_REV,
+ PARAM_RC3_MIN,
+ PARAM_RC3_MAX,
+ PARAM_RC3_TRIM,
+ PARAM_RC3_REV,
+ PARAM_RC4_MIN,
+ PARAM_RC4_MAX,
+ PARAM_RC4_TRIM,
+ PARAM_RC4_REV,
+ PARAM_RC5_MIN,
+ PARAM_RC5_MAX,
+ PARAM_RC5_TRIM,
+ PARAM_RC5_REV,
+ PARAM_RC6_MIN,
+ PARAM_RC6_MAX,
+ PARAM_RC6_TRIM,
+ PARAM_RC6_REV,
+ PARAM_RC7_MIN,
+ PARAM_RC7_MAX,
+ PARAM_RC7_TRIM,
+ PARAM_RC7_REV,
+ PARAM_RC8_MIN,
+ PARAM_RC8_MAX,
+ PARAM_RC8_TRIM,
+ PARAM_RC8_REV,
+ PARAM_THROTTLE_CHAN,
+ PARAM_ROLL_CHAN,
+ PARAM_PITCH_CHAN,
+ PARAM_YAW_CHAN,
+ PARAM_OVERRIDE_CHAN,
+ PARAM_SERVO1_MIN,
+ PARAM_SERVO1_MAX,
+ PARAM_SERVO1_TRIM,
+ PARAM_SERVO2_MIN,
+ PARAM_SERVO2_MAX,
+ PARAM_SERVO2_TRIM,
+ PARAM_SERVO3_MIN,
+ PARAM_SERVO3_MAX,
+ PARAM_SERVO3_TRIM,
+ PARAM_SERVO4_MIN,
+ PARAM_SERVO4_MAX,
+ PARAM_SERVO4_TRIM,
+ PARAM_SERVO_SCALE,
+ PARAM_MAX_COUNT ///< LEAVE THIS IN PLACE AS LAST ELEMENT!
+};
+
+struct px4_parameter_storage_t {
+ float param_values[PARAM_MAX_COUNT]; ///< Parameter values
+ const char *param_names[PARAM_MAX_COUNT]; ///< Parameter names
+ bool param_needs_write[PARAM_MAX_COUNT];
+ uint16_t next_param;
+ uint16_t size;
+};
+
+
+#define PX4_FLIGHT_ENVIRONMENT_INDOOR 0
+#define PX4_FLIGHT_ENVIRONMENT_OUTDOOR 1
+#define PX4_FLIGHT_ENVIRONMENT_TESTING 2 //NO check for position fix in this environment
+
+struct global_data_parameter_storage_t {
+
+ /* use of a counter and timestamp recommended (but not necessary) */
+
+// uint16_t counter; //incremented by the writing thread everytime new data is stored
+// uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
+
+ /* Actual data, this is specific to the type of data which is stored in this struct */
+
+ //***** Start: Add your variables here *****
+
+ /* Parameters (set by a param_set mavlink message */
+ struct px4_parameter_storage_t pm;
+
+ //*****END: Add your variables here *****
+
+};
+
+__attribute__ ((visibility ("default"))) extern struct global_data_parameter_storage_t *global_data_parameter_storage; //adjust this line!
+
+#endif
diff --git a/apps/uORB/topics/UNPORTED_subsystem_info.h b/apps/uORB/topics/UNPORTED_subsystem_info.h
new file mode 100644
index 000000000..24f973a02
--- /dev/null
+++ b/apps/uORB/topics/UNPORTED_subsystem_info.h
@@ -0,0 +1,68 @@
+/* Structure for storage of shared variables (extended( */
+
+/* global_data_subsystem_info stores a buffer of the sensor info/status messages (sent by sensors or gps app), this is then handled by the commander. (The commander then writes to global_data_sys_status which is read by mavlink) */
+
+/* (any app) --> global_data_subsystem_info (buffered) --> (commander app) --> global_data_sys_status --> (mavlink app) */
+
+#ifndef GLOBAL_DATA_SUBSYSTEM_INFO_T_H_ //adjust this line!
+#define GLOBAL_DATA_SUBSYSTEM_INFO_T_H_ //adjust this line!
+
+#define SUBSYSTEM_INFO_BUFFER_SIZE 10
+
+#include "global_data_access.h"
+
+typedef enum //values correspond to bitmasks of mavlink message sys_status, this is crucial for the underlying bitmask to work
+{
+ SUBSYSTEM_TYPE_GYRO = 0,
+ SUBSYSTEM_TYPE_ACC = 1,
+ SUBSYSTEM_TYPE_MAG = 2,
+ SUBSYSTEM_TYPE_ABSPRESSURE = 3,
+ SUBSYSTEM_TYPE_DIFFPRESSURE = 4,
+ SUBSYSTEM_TYPE_GPS = 5,
+ SUBSYSTEM_TYPE_OPTICALFLOW = 6,
+ SUBSYSTEM_TYPE_CVPOSITION = 7,
+ SUBSYSTEM_TYPE_LASERPOSITION = 8,
+ SUBSYSTEM_TYPE_EXTERNALGROUNDTRUTH = 9,
+ SUBSYSTEM_TYPE_ANGULARRATECONTROL = 10,
+ SUBSYSTEM_TYPE_ATTITUDESTABILIZATION = 11,
+ SUBSYSTEM_TYPE_YAWPOSITION = 12,
+ SUBSYSTEM_TYPE_ALTITUDECONTROL = 13,
+ SUBSYSTEM_TYPE_POSITIONCONTROL = 14,
+ SUBSYSTEM_TYPE_MOTORCONTROL = 15
+
+} subsystem_type_t;
+
+typedef struct
+{
+ uint8_t present;
+ uint8_t enabled;
+ uint8_t health;
+
+ subsystem_type_t subsystem_type;
+
+} __attribute__((__packed__)) subsystem_info_t;
+
+typedef struct
+{
+ /* Struct which stores the access configuration, this is the same for all shared structs */
+
+ access_conf_t access_conf; //don't remove this line!
+
+ /* use of a counter and timestamp recommended (but not necessary) */
+
+ uint16_t counter; //incremented by the writing thread everytime new data is stored
+ uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
+
+ /* Actual data, this is specific to the type of data which is stored in this struct */
+
+ //***** Start: Add your variables here *****
+ subsystem_info_t info[SUBSYSTEM_INFO_BUFFER_SIZE];
+ uint8_t current_info;
+
+ //*****END: Add your variables here *****
+
+} global_data_subsystem_info_t; //adjust this line!
+
+extern global_data_subsystem_info_t* global_data_subsystem_info; //adjust this line!
+
+#endif
diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h
new file mode 100644
index 000000000..03c0c7b7d
--- /dev/null
+++ b/apps/uORB/topics/actuator_controls.h
@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Actuator control topic - mixer inputs.
+ *
+ * Values published to this topic are the outputs of the vehicle control
+ * system, and are expected to be mixed and used to drive the actuators
+ * (servos, speed controls, etc.) that operate the vehicle.
+ */
+
+#ifndef TOPIC_ACTUATOR_CONTROLS_H
+#define TOPIC_ACTUATOR_CONTROLS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_ACTUATOR_CONTROLS 16
+
+struct actuator_controls
+{
+ float control[NUM_ACTUATOR_CONTROLS];
+};
+
+ORB_DECLARE(actuator_controls);
+
+struct actuator_armed
+{
+ bool armed;
+};
+
+ORB_DECLARE(actuator_armed);
+
+#endif \ No newline at end of file
diff --git a/apps/uORB/topics/ardrone_control.h b/apps/uORB/topics/ardrone_control.h
new file mode 100644
index 000000000..1f582f52a
--- /dev/null
+++ b/apps/uORB/topics/ardrone_control.h
@@ -0,0 +1,83 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file ardrone_control.c
+ * Definition of the ardrone_control uORB topic.
+ */
+
+#ifndef TOPIC_ARDRONE_CONTROL_H_
+#define TOPIC_ARDRONE_CONTROL_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct ardrone_control_s
+{
+ uint16_t counter; /**< incremented by the publishing thread everytime new data is stored. */
+ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data. */
+
+ float setpoint_rate_cast[3];
+ float setpoint_thrust_cast; /**< LOGME */
+ float setpoint_attitude_rate[3];
+ float setpoint_attitude[3]; /**< LOGME */
+ float attitude_control_output[3]; /**< roll, pitch, yaw. */
+ float position_control_output[3]; /**< x, y, z. */
+ float attitude_setpoint_navigationframe_from_positioncontroller[3]; /**< LOGME */
+ float gyro_scaled[3];
+ float gyro_filtered[3];
+ float gyro_filtered_offset[3];
+ float zcompensation;
+ uint16_t motor_front_nw;
+ uint16_t motor_right_ne;
+ uint16_t motor_back_se;
+ uint16_t motor_left_sw;
+
+}; /**< ardrone control status */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(ardrone_control);
+
+#endif /* TOPIC_ARDRONE_CONTROL_H_ */
diff --git a/apps/uORB/topics/ardrone_motors_setpoint.h b/apps/uORB/topics/ardrone_motors_setpoint.h
new file mode 100644
index 000000000..54d81b706
--- /dev/null
+++ b/apps/uORB/topics/ardrone_motors_setpoint.h
@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ardrone_motors_setpoint.h
+ * Definition of the ardrone_motors_setpoint uORB topic.
+ */
+
+#ifndef TOPIC_ARDRONE_MOTORS_SETPOINT_H_
+#define TOPIC_ARDRONE_MOTORS_SETPOINT_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct ardrone_motors_setpoint_s
+{
+ uint16_t counter; //incremented by the writing thread everytime new data is stored
+ uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
+
+ uint16_t motor_front_nw; ///< Front motor in + configuration, front left motor in x configuration
+ uint16_t motor_right_ne; ///< Right motor in + configuration, front right motor in x configuration
+ uint16_t motor_back_se; ///< Back motor in + configuration, back right motor in x configuration
+ uint16_t motor_left_sw; ///< Left motor in + configuration, back left motor in x configuration
+
+}; /**< AR.Drone low level motors */
+
+ /**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(ardrone_motors_setpoint);
+
+#endif
diff --git a/apps/uORB/topics/fixedwing_control.h b/apps/uORB/topics/fixedwing_control.h
new file mode 100644
index 000000000..e904709b6
--- /dev/null
+++ b/apps/uORB/topics/fixedwing_control.h
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file fixedwing_control.h
+ * Definition of the fixedwing_control uORB topic.
+ */
+
+#ifndef TOPIC_FIXEDWING_CONTROL_H_
+#define TOPIC_FIXEDWING_CONTROL_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * fixed wing control status.
+ */
+struct fixedwing_control_s
+{
+ /* use of a counter and timestamp recommended (but not necessary) */
+
+ uint16_t counter; //incremented by the writing thread everytime new data is stored
+ uint64_t timestamp; //in microseconds since system start, is set whenever the writing thread stores new data
+
+ float setpoint_rate_cast[3];
+ float setpoint_attitude_rate[3];
+ float setpoint_attitude[3];
+ int16_t attitude_control_output[4]; /**< roll, pitch, yaw, throttle */
+ float position_control_output[4];
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(fixedwing_control);
+
+#endif //GLOBAL_DATA_FIXEDWING_CONTROL_T_H_
diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h
new file mode 100644
index 000000000..5bd2adeec
--- /dev/null
+++ b/apps/uORB/topics/rc_channels.h
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ * Author: @author Nils Wenzler <wenzlern@student.ethz.ch>
+ * @author Ivan Ovinnikov <oivan@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rc_channels.h
+ * Definition of the rc_channels uORB topic.
+ */
+
+#ifndef RC_CHANNELS_H_
+#define RC_CHANNELS_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+enum RC_CHANNELS_STATUS
+{
+ UNKNOWN = 0,
+ KNOWN = 1,
+ SIGNAL = 2,
+ TIMEOUT = 3
+};
+
+/**
+ * This defines the mapping of the RC functions.
+ * The value assigned to the specific function corresponds to the entry of
+ * the channel array chan[].
+ * Ex. To read out the scaled Pitch value:
+ * pitch = global_data_rc_channels->chan[PITCH].scale;
+ * The override is on channel 8, since we don't usually have a 12 channel RC
+ * and channel 5/6 (GRAUPNER/FUTABA) are mapped to the second ROLL servo, which
+ * can only be disabled on more advanced RC sets.
+ */
+enum RC_CHANNELS_FUNCTION
+{
+ THROTTLE = 0,
+ ROLL = 1,
+ PITCH = 2,
+ YAW = 3,
+ OVERRIDE = 4,
+ FUNC_0 = 5,
+ FUNC_1 = 6,
+ FUNC_2 = 7,
+ FUNC_3 = 8,
+ FUNC_4 = 9,
+ FUNC_5 = 10,
+ FUNC_6 = 11,
+ RC_CHANNELS_FUNCTION_MAX = 12
+};
+
+struct rc_channels_s {
+
+ uint64_t timestamp; /**< In microseconds since boot time. */
+ uint64_t timestamp_last_valid; /**< timestamp of last valid RC signal. */
+ struct {
+ uint16_t mid; /**< midpoint (0). */
+ float scaling_factor; /**< scaling factor from raw counts to 0..1 */
+ uint16_t raw; /**< current raw value */
+ int16_t scale;
+ float scaled; /**< Scaled */
+ uint16_t override;
+ enum RC_CHANNELS_STATUS status; /**< status of the channel */
+ } chan[RC_CHANNELS_FUNCTION_MAX];
+ uint8_t chan_count; /**< maximum number of valid channels */
+
+ /*String array to store the names of the functions*/
+ const char function_name[RC_CHANNELS_FUNCTION_MAX][20];
+ uint8_t function[RC_CHANNELS_FUNCTION_MAX];
+ uint8_t rssi; /**< Overall receive signal strength */
+}; /**< radio control channels. */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(rc_channels);
+
+#endif
diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h
new file mode 100644
index 000000000..ecf5ea81d
--- /dev/null
+++ b/apps/uORB/topics/sensor_combined.h
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sensor_combined.h
+ * Definition of the sensor_combined uORB topic.
+ */
+
+#ifndef SENSOR_COMBINED_H_
+#define SENSOR_COMBINED_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Sensor readings in raw and SI-unit form.
+ *
+ * These values are read from the sensors. Raw values are in sensor-specific units,
+ * the scaled values are in SI-units, as visible from the ending of the variable
+ * or the comments. The use of the SI fields is in general advised, as these fields
+ * are scaled and offset-compensated where possible and do not change with board
+ * revisions and sensor updates.
+ *
+ */
+struct sensor_combined_s {
+
+ /*
+ * Actual data, this is specific to the type of data which is stored in this struct
+ * A line containing L0GME will be added by the Python logging code generator to the
+ * logged dataset.
+ */
+
+ /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */
+
+ uint64_t timestamp; /**< Timestamp in microseconds since boot LOGME */
+
+ int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity LOGME */
+ uint16_t gyro_raw_counter; /**< Number of raw measurments taken LOGME */
+ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds LOGME */
+ int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame LOGME */
+ uint16_t accelerometer_raw_counter; /**< Number of raw acc measurements taken LOGME */
+ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 LOGME */
+ int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame LOGME */
+ float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss LOGME */
+ uint16_t magnetometer_raw_counter; /**< Number of raw mag measurements taken LOGME */
+ float baro_pres_mbar; /**< Barometric pressure, already temp. comp. LOGME */
+ float baro_alt_meter; /**< Altitude, already temp. comp. LOGME */
+ float baro_temp_celcius; /**< Temperature in degrees celsius LOGME */
+ float battery_voltage_v; /**< Battery voltage in volts, filtered LOGME */
+ float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 LOGME */
+ uint16_t baro_raw_counter; /**< Number of raw baro measurements taken LOGME */
+ uint16_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */
+ bool battery_voltage_valid; /**< True if battery voltage can be measured LOGME */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(sensor_combined);
+
+#endif
diff --git a/apps/uORB/topics/vehicle_attitude.h b/apps/uORB/topics/vehicle_attitude.h
new file mode 100644
index 000000000..e365a6557
--- /dev/null
+++ b/apps/uORB/topics/vehicle_attitude.h
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_attitude.h
+ * Definition of the attitude uORB topic.
+ */
+
+#ifndef VEHICLE_ATTITUDE_H_
+#define VEHICLE_ATTITUDE_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ */
+
+/**
+ * Attitude in NED body frame in SI units.
+ *
+ * @see http://en.wikipedia.org/wiki/International_System_of_Units
+ */
+struct vehicle_attitude_s {
+
+ /*
+ * Actual data, this is specific to the type of data which is stored in this struct
+ * A line containing L0GME will be added by the Python logging code generator to the
+ * logged dataset.
+ */
+ uint64_t timestamp; /**< in microseconds since system start */
+
+ /* This is similar to the mavlink message ATTITUDE, but for onboard use */
+
+ /* @warning Roll, pitch and yaw have always to be valid, the rotation matrix and quaternion are optional */
+
+ float roll; /**< Roll angle (rad, Tait-Bryan, NED) LOGME */
+ float pitch; /**< Pitch angle (rad, Tait-Bryan, NED) LOGME */
+ float yaw; /**< Yaw angle (rad, Tait-Bryan, NED) LOGME */
+ float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) LOGME */
+ float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) LOGME */
+ float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) LOGME */
+ float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */
+ float q[4]; /**< Quaternion (NED) */
+ bool R_valid; /**< Rotation matrix valid */
+ bool q_valid; /**< Quaternion valid */
+ uint16_t counter; /**< Counter of all attitude messages (wraps) */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_attitude);
+
+#endif
diff --git a/apps/uORB/topics/vehicle_attitude_setpoint.h b/apps/uORB/topics/vehicle_attitude_setpoint.h
new file mode 100644
index 000000000..6b3757402
--- /dev/null
+++ b/apps/uORB/topics/vehicle_attitude_setpoint.h
@@ -0,0 +1,77 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_attitude_setpoint.h
+ * Definition of the vehicle attitude setpoint uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
+#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <uORB.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * vehicle attitude setpoint.
+ */
+struct vehicle_attitude_setpoint_s
+{
+ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
+
+ float roll_tait_bryan; /**< Tait-Bryan angle in NED frame */
+ float pitch_tait_bryan; /**< Tait-Bryan angle in NED frame */
+ float yaw_tait_bryan; /**< Tait-Bryan angle in NED frame */
+ float tait_bryan_valid; /**< Set to true if Tait-Bryan angles are valid */
+
+ float R_body[9]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
+ bool R_valid; /**< Set to true if rotation matrix is valid */
+ float thrust; /**< Thrust in Newton the power system should generate */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_attitude_setpoint);
+
+#endif /* TOPIC_ARDRONE_CONTROL_H_ */
diff --git a/apps/uORB/topics/vehicle_command.h b/apps/uORB/topics/vehicle_command.h
new file mode 100644
index 000000000..dca928efd
--- /dev/null
+++ b/apps/uORB/topics/vehicle_command.h
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_command.h
+ * Definition of the vehicle command uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_COMMAND_H_
+#define TOPIC_VEHICLE_COMMAND_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_command_s
+{
+ float param1; /**< Parameter 1, as defined by MAVLink MAV_CMD enum. */
+ float param2; /**< Parameter 2, as defined by MAVLink MAV_CMD enum. */
+ float param3; /**< Parameter 3, as defined by MAVLink MAV_CMD enum. */
+ float param4; /**< Parameter 4, as defined by MAVLink MAV_CMD enum. */
+ float param5; /**< Parameter 5, as defined by MAVLink MAV_CMD enum. */
+ float param6; /**< Parameter 6, as defined by MAVLink MAV_CMD enum. */
+ float param7; /**< Parameter 7, as defined by MAVLink MAV_CMD enum. */
+ uint16_t command; /**< Command ID, as defined MAVLink by MAV_CMD enum. */
+ uint8_t target_system; /**< System which should execute the command */
+ uint8_t target_component; /**< Component which should execute the command, 0 for all components */
+ uint8_t source_system; /**< System sending the command */
+ uint8_t source_component; /**< Component sending the command */
+ uint8_t confirmation; /**< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) */
+}; /**< command sent to vehicle */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_command);
+
+
+
+#endif
diff --git a/apps/uORB/topics/vehicle_global_position.h b/apps/uORB/topics/vehicle_global_position.h
new file mode 100644
index 000000000..f036c7223
--- /dev/null
+++ b/apps/uORB/topics/vehicle_global_position.h
@@ -0,0 +1,86 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_global_position.h
+ * Definition of the global fused WGS84 position uORB topic.
+ */
+
+#ifndef VEHICLE_GLOBAL_POSITION_T_H_
+#define VEHICLE_GLOBAL_POSITION_T_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+ /**
+ * Fused global position in WGS84.
+ *
+ * This struct contains the system's believ about its position. It is not the raw GPS
+ * measurement (@see vehicle_gps_position). This topic is usually published by the position
+ * estimator, which will take more sources of information into account than just GPS,
+ * e.g. control inputs of the vehicle in a Kalman-filter implementation.
+ */
+struct vehicle_global_position_s
+{
+ uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
+ uint64_t time_gps_usec; /**< GPS timestamp in microseconds */
+ bool valid; /**< true if position satisfies validity criteria of estimator */
+
+ int32_t lat; /**< Latitude in 1E7 degrees LOGME */
+ int32_t lon; /**< Longitude in 1E7 degrees LOGME */
+ float alt; /**< Altitude in meters LOGME */
+ float relative_alt; /**< Altitude above home position in meters, LOGME */
+ float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */
+ float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */
+ float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */
+ float hdg; /**< Compass heading in radians -PI..+PI. */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_global_position);
+
+#endif
diff --git a/apps/uORB/topics/vehicle_global_position_setpoint.h b/apps/uORB/topics/vehicle_global_position_setpoint.h
new file mode 100644
index 000000000..b73e2a363
--- /dev/null
+++ b/apps/uORB/topics/vehicle_global_position_setpoint.h
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_global_position_setpoint.h
+ * Definition of the global WGS84 position setpoint uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
+#define TOPIC_VEHICLE_GLOBAL_POSITION_SETPOINT_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Global position setpoint in WGS84 coordinates.
+ *
+ * This is the position the MAV is heading towards. If it of type loiter,
+ * the MAV is circling around it with the given loiter radius in meters.
+ */
+struct vehicle_global_position_setpoint_s
+{
+ bool altitude_is_relative; /**< true if altitude is relative from start point */
+ int32_t lat; /**< latitude in degrees * 1E7 */
+ int32_t lon; /**< longitude in degrees * 1E7 */
+ float altitude; /**< altitude in meters */
+ float yaw; /**< in radians NED -PI..+PI */
+ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */
+ bool is_loiter; /**< true if loitering is enabled */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_global_position_setpoint);
+
+#endif
diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h
new file mode 100644
index 000000000..60d01a831
--- /dev/null
+++ b/apps/uORB/topics/vehicle_gps_position.h
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_gps_position.h
+ * Definition of the GPS WGS84 uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_GPS_H_
+#define TOPIC_VEHICLE_GPS_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * GPS position in WGS84 coordinates.
+ */
+struct vehicle_gps_position_s
+{
+ uint64_t timestamp; /**< Timestamp (microseconds since system boot) */
+ uint32_t counter; /**< Count of GPS messages */
+ uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */
+
+ int32_t lat; /**< Latitude in 1E7 degrees //LOGME */
+ int32_t lon; /**< Longitude in 1E7 degrees //LOGME */
+ int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL //LOGME */
+ uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */
+ uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */
+ uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */
+ uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */
+ uint16_t cog; /**< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */
+ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */
+ uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */
+
+ uint8_t satellite_prn[20]; /**< Global satellite ID */
+ uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */
+ uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+ uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+ uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */
+ uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_gps_position);
+
+#endif
diff --git a/apps/uORB/topics/vehicle_local_position.h b/apps/uORB/topics/vehicle_local_position.h
new file mode 100644
index 000000000..76eddeacd
--- /dev/null
+++ b/apps/uORB/topics/vehicle_local_position.h
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_local_position.h
+ * Definition of the local fused NED position uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_LOCAL_POSITION_H_
+#define TOPIC_VEHICLE_LOCAL_POSITION_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Fused local position in NED.
+ */
+struct vehicle_local_position_s
+{
+ uint64_t timestamp; /**< time of this estimate, in microseconds since system start */
+ bool valid; /**< true if position satisfies validity criteria of estimator */
+
+ float x; /**< X positin in meters in NED earth-fixed frame */
+ float y; /**< X positin in meters in NED earth-fixed frame */
+ float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */
+ float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */
+ float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */
+ float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */
+ float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */
+ float hdg; /**< Compass heading in radians -PI..+PI. */
+
+ // TODO Add covariances here
+
+ /* Reference position in GPS / WGS84 frame */
+ uint64_t home_timestamp;/**< Time when home position was set */
+ int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */
+ int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */
+ float home_alt; /**< Altitude in meters LOGME */
+ float home_hdg; /**< Compass heading in radians -PI..+PI. */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_local_position);
+
+#endif
diff --git a/apps/uORB/topics/vehicle_local_position_setpoint.h b/apps/uORB/topics/vehicle_local_position_setpoint.h
new file mode 100644
index 000000000..d24d81e3a
--- /dev/null
+++ b/apps/uORB/topics/vehicle_local_position_setpoint.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_local_position_setpoint.h
+ * Definition of the local NED position setpoint uORB topic.
+ */
+
+#ifndef TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_
+#define TOPIC_VEHICLE_LOCAL_POSITION_SETPOINT_H_
+
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+struct vehicle_local_position_setpoint_s
+{
+ float x; /**< in meters NED */
+ float y; /**< in meters NED */
+ float z; /**< in meters NED */
+ float yaw; /**< in radians NED -PI..+PI */
+}; /**< Local position in NED frame to go to */
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_local_position_setpoint);
+
+#endif
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
new file mode 100644
index 000000000..d92b80046
--- /dev/null
+++ b/apps/uORB/topics/vehicle_status.h
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vehicle_status.h
+ * Definition of the vehicle_status uORB topic.
+ *
+ * Published the state machine and the system status bitfields
+ * (see SYS_STATUS mavlink message), published only by commander app.
+ *
+ * All apps should write to subsystem_info:
+ *
+ * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app)
+ */
+
+#ifndef VEHICLE_STATUS_H_
+#define VEHICLE_STATUS_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics @{
+ */
+
+/* State Machine */
+typedef enum
+{
+ SYSTEM_STATE_PREFLIGHT = 0,
+ SYSTEM_STATE_STANDBY = 1,
+ SYSTEM_STATE_GROUND_READY = 2,
+ SYSTEM_STATE_MANUAL = 3,
+ SYSTEM_STATE_STABILIZED = 4,
+ SYSTEM_STATE_AUTO = 5,
+ SYSTEM_STATE_MISSION_ABORT = 6,
+ SYSTEM_STATE_EMCY_LANDING = 7,
+ SYSTEM_STATE_EMCY_CUTOFF = 8,
+ SYSTEM_STATE_GROUND_ERROR = 9,
+ SYSTEM_STATE_REBOOT= 10,
+
+} commander_state_machine_t;
+
+enum VEHICLE_MODE_FLAG {
+ VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
+ VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
+ VEHICLE_MODE_FLAG_HIL_ENABLED = 32,
+ VEHICLE_MODE_FLAG_STABILIZE_ENABLED = 16,
+ VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8,
+ VEHICLE_MODE_FLAG_AUTO_ENABLED = 4,
+ VEHICLE_MODE_FLAG_TEST_ENABLED = 2,
+ VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1
+}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */
+
+/**
+ * state machine / state of vehicle.
+ *
+ * Encodes the complete system state and is set by the commander app.
+ */
+struct vehicle_status_s
+{
+ /* use of a counter and timestamp recommended (but not necessary) */
+
+ uint16_t counter; /**< incremented by the writing thread everytime new data is stored */
+ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
+
+ commander_state_machine_t state_machine;
+ uint8_t mode;
+
+ bool control_manual_enabled; /**< true if manual input is mixed in */
+ bool control_rates_enabled; /**< true if rates are stabilized */
+ bool control_attitude_enabled; /**< true if attitude stabilization is mixed in */
+ bool control_speed_enabled; /**< true if speed (implies direction) is controlled */
+ bool control_position_enabled; /**< true if position is controlled */
+
+ bool preflight_gyro_calibration; /**< true if gyro calibration is requested */
+ bool preflight_mag_calibration; /**< true if mag calibration is requested */
+
+ /* see SYS_STATUS mavlink message for the following */
+ uint32_t onboard_control_sensors_present;
+ uint32_t onboard_control_sensors_enabled;
+ uint32_t onboard_control_sensors_health;
+ uint16_t load;
+ uint16_t voltage_battery;
+ int16_t current_battery;
+ int8_t battery_remaining;
+ uint16_t drop_rate_comm;
+ uint16_t errors_comm;
+ uint16_t errors_count1;
+ uint16_t errors_count2;
+ uint16_t errors_count3;
+ uint16_t errors_count4;
+
+// bool remote_manual; /**< set to true by the commander when the manual-switch on the remote is set to manual */
+ bool gps_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vehicle_status);
+
+#endif
diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp
new file mode 100644
index 000000000..270e3861c
--- /dev/null
+++ b/apps/uORB/uORB.cpp
@@ -0,0 +1,969 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file A lightweight object broker.
+ */
+
+#include <nuttx/config.h>
+
+#include <drivers/device/device.h>
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <poll.h>
+#include <errno.h>
+#include <stdio.h>
+#include <math.h>
+#include <unistd.h>
+
+#include <nuttx/arch.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/clock.h>
+
+#include <arch/board/up_hrt.h>
+
+#include <drivers/drv_orb_dev.h>
+
+#include "uORB.h"
+
+/**
+ * Utility functions.
+ */
+namespace
+{
+
+static const unsigned orb_maxpath = 64;
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+const int ERROR = -1;
+
+enum Flavor {
+ PUBSUB,
+ PARAM
+};
+
+int
+node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta)
+{
+ unsigned len;
+
+ len = snprintf(buf, orb_maxpath, "/%s/%s",
+ (f == PUBSUB) ? "obj" : "param",
+ meta->o_name);
+
+ if (len >= orb_maxpath)
+ return -ENAMETOOLONG;
+
+ return OK;
+}
+
+}
+
+/**
+ * Per-object device instance.
+ */
+class ORBDevNode : public device::CDev
+{
+public:
+ ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path);
+ ~ORBDevNode();
+
+ virtual int open(struct file *filp);
+ virtual int close(struct file *filp);
+ virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
+ virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen);
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+
+protected:
+ virtual pollevent_t poll_state(struct file *filp);
+ virtual void poll_notify_one(struct pollfd *fds, pollevent_t events);
+
+private:
+ struct SubscriberData {
+ unsigned generation; /**< last generation the subscriber has seen */
+ unsigned update_interval; /**< if nonzero minimum interval between updates */
+ struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */
+ void *poll_priv; /**< saved copy of fds->f_priv while poll is active */
+ bool update_reported; /**< true if we have reported the update via poll/check */
+ };
+
+ const struct orb_metadata *_meta; /**< object metadata information */
+ uint8_t *_data; /**< allocated object buffer */
+ hrt_abstime _last_update; /**< time the object was last updated */
+ volatile unsigned _generation; /**< object generation count */
+ pid_t _publisher; /**< if nonzero, current publisher */
+
+ SubscriberData *filp_to_sd(struct file *filp) {
+ SubscriberData *sd = (SubscriberData *)(filp->f_priv);
+ return sd;
+ }
+
+ /**
+ * Perform a deferred update for a rate-limited subscriber.
+ */
+ void update_deferred();
+
+ /**
+ * Bridge from hrt_call to update_deferred
+ *
+ * void *arg ORBDevNode pointer for which the deferred update is performed.
+ */
+ static void update_deferred_trampoline(void *arg);
+
+ /**
+ * Check whether a topic appears updated to a subscriber.
+ *
+ * @param sd The subscriber for whom to check.
+ * @return True if the topic should appear updated to the subscriber
+ */
+ bool appears_updated(SubscriberData *sd);
+};
+
+ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) :
+ CDev(name, path),
+ _meta(meta),
+ _data(nullptr),
+ _last_update(0),
+ _generation(0),
+ _publisher(0)
+{
+ // enable debug() calls
+ _debug_enabled = true;
+}
+
+ORBDevNode::~ORBDevNode()
+{
+ if (_data != nullptr)
+ delete[] _data;
+}
+
+int
+ORBDevNode::open(struct file *filp)
+{
+ int ret;
+
+ /* is this a publisher? */
+ if (filp->f_oflags == O_WRONLY) {
+
+ /* become the publisher if we can */
+ lock();
+
+ if (_publisher == 0) {
+ _publisher = getpid();
+ ret = OK;
+
+ } else {
+ ret = -EBUSY;
+ }
+
+ unlock();
+
+ /* now complete the open */
+ if (ret == OK) {
+ ret = CDev::open(filp);
+
+ /* open failed - not the publisher anymore */
+ if (ret != OK)
+ _publisher = 0;
+ }
+
+ return ret;
+ }
+
+ /* is this a new subscriber? */
+ if (filp->f_oflags == O_RDONLY) {
+
+ /* allocate subscriber data */
+ SubscriberData *sd = new SubscriberData;
+
+ if (nullptr == sd)
+ return -ENOMEM;
+
+ memset(sd, 0, sizeof(*sd));
+
+ /* default to no pending update */
+ sd->generation = _generation;
+
+ filp->f_priv = (void *)sd;
+
+ ret = CDev::open(filp);
+
+ if (ret != OK)
+ free(sd);
+
+ return ret;
+ }
+
+ /* can only be pub or sub, not both */
+ return -EINVAL;
+}
+
+int
+ORBDevNode::close(struct file *filp)
+{
+ /* is this the publisher closing? */
+ if (getpid() == _publisher) {
+ _publisher = 0;
+
+ } else {
+ SubscriberData *sd = filp_to_sd(filp);
+
+ if (sd != nullptr)
+ delete sd;
+ }
+
+ return CDev::close(filp);
+}
+
+ssize_t
+ORBDevNode::read(struct file *filp, char *buffer, size_t buflen)
+{
+ SubscriberData *sd = (SubscriberData *)filp_to_sd(filp);
+
+ /* if the object has not been written yet, return zero */
+ if (_data == nullptr)
+ return 0;
+
+ /* if the caller's buffer is the wrong size, that's an error */
+ if (buflen != _meta->o_size)
+ return -EIO;
+
+ /*
+ * Perform an atomic copy & state update
+ */
+ irqstate_t flags = irqsave();
+
+ /* if the caller doesn't want the data, don't give it to them */
+ if (nullptr != buffer)
+ memcpy(buffer, _data, _meta->o_size);
+
+ /* track the last generation that the file has seen */
+ sd->generation = _generation;
+
+ /*
+ * Clear the flag that indicates that an update has been reported, as
+ * we have just collected it.
+ */
+ sd->update_reported = false;
+
+ irqrestore(flags);
+
+ return _meta->o_size;
+}
+
+ssize_t
+ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen)
+{
+ /*
+ * Writes are legal from interrupt context as long as the
+ * object has already been initialised from thread context.
+ *
+ * Writes outside interrupt context will allocate the object
+ * if it has not yet been allocated.
+ */
+ if (nullptr == _data) {
+ if (!up_interrupt_context()) {
+
+ lock();
+
+ /* re-check size */
+ if (nullptr == _data)
+ _data = new uint8_t[_meta->o_size];
+
+ unlock();
+ }
+
+ /* failed or could not allocate */
+ if (nullptr == _data)
+ return -ENOMEM;
+ }
+
+ /* If write size does not match, that is an error */
+ if (_meta->o_size != buflen)
+ return -EIO;
+
+ /* Perform an atomic copy. */
+ irqstate_t flags = irqsave();
+ memcpy(_data, buffer, _meta->o_size);
+ irqrestore(flags);
+
+ /* update the timestamp and generation count */
+ _last_update = hrt_absolute_time();
+ _generation++;
+
+ /* notify any poll waiters */
+ poll_notify(POLLIN);
+
+ return _meta->o_size;
+}
+
+int
+ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ SubscriberData *sd = filp_to_sd(filp);
+
+ switch (cmd) {
+ case ORBIOCLASTUPDATE:
+ *(hrt_abstime *)arg = _last_update;
+ return OK;
+
+ case ORBIOCUPDATED:
+ *(bool *)arg = appears_updated(sd);
+ return OK;
+
+ case ORBIOCSETINTERVAL:
+ sd->update_interval = arg;
+ return OK;
+
+ default:
+ /* give it to the superclass */
+ return CDev::ioctl(filp, cmd, arg);
+ }
+}
+
+pollevent_t
+ORBDevNode::poll_state(struct file *filp)
+{
+ SubscriberData *sd = filp_to_sd(filp);
+
+ /*
+ * If the topic appears updated to the subscriber, say so.
+ */
+ if (appears_updated(sd))
+ return POLLIN;
+
+ return 0;
+}
+
+void
+ORBDevNode::poll_notify_one(struct pollfd *fds, pollevent_t events)
+{
+ SubscriberData *sd = filp_to_sd((struct file *)fds->priv);
+
+ /*
+ * If the topic looks updated to the subscriber, go ahead and notify them.
+ */
+ if (appears_updated(sd))
+ CDev::poll_notify_one(fds, events);
+}
+
+bool
+ORBDevNode::appears_updated(SubscriberData *sd)
+{
+ /* assume it doesn't look updated */
+ bool ret = false;
+
+ /* avoid racing between interrupt and non-interrupt context calls */
+ irqstate_t state = irqsave();
+
+ /*
+ * If the subscriber's generation count matches the update generation
+ * count, there has been no update from their perspective; if they
+ * don't match then we might have a visible update.
+ */
+ while (sd->generation != _generation) {
+
+ /*
+ * Handle non-rate-limited subscribers.
+ */
+ if (sd->update_interval == 0) {
+ ret = true;
+ break;
+ }
+
+ /*
+ * If we have previously told the subscriber that there is data,
+ * and they have not yet collected it, continue to tell them
+ * that there has been an update. This mimics the non-rate-limited
+ * behaviour where checking / polling continues to report an update
+ * until the topic is read.
+ */
+ if (sd->update_reported) {
+ ret = true;
+ break;
+ }
+
+ /*
+ * If the interval timer is still running, the topic should not
+ * appear updated, even though at this point we know that it has.
+ * We have previously been through here, so the subscriber
+ * must have collected the update we reported, otherwise
+ * update_reported would still be true.
+ */
+ if (!hrt_called(&sd->update_call))
+ break;
+
+ /*
+ * Make sure that we don't consider the topic to be updated again
+ * until the interval has passed once more by restarting the interval
+ * timer and thereby re-scheduling a poll notification at that time.
+ */
+ hrt_call_after(&sd->update_call,
+ sd->update_interval,
+ &ORBDevNode::update_deferred_trampoline,
+ (void *)this);
+
+ /*
+ * Remember that we have told the subscriber that there is data.
+ */
+ sd->update_reported = true;
+ ret = true;
+
+ break;
+ }
+
+ irqrestore(state);
+
+ /* consider it updated */
+ return ret;
+}
+
+void
+ORBDevNode::update_deferred()
+{
+ /*
+ * Instigate a poll notification; any subscribers whose intervals have
+ * expired will be woken.
+ */
+ poll_notify(POLLIN);
+}
+
+void
+ORBDevNode::update_deferred_trampoline(void *arg)
+{
+ ORBDevNode *node = (ORBDevNode *)arg;
+
+ node->update_deferred();
+}
+
+/**
+ * Master control device for ObjDev.
+ *
+ * Used primarily to create new objects via the ORBIOCCREATE
+ * ioctl.
+ */
+class ORBDevMaster : public device::CDev
+{
+public:
+ ORBDevMaster(Flavor f);
+ ~ORBDevMaster();
+
+ virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
+private:
+ Flavor _flavor;
+};
+
+ORBDevMaster::ORBDevMaster(Flavor f) :
+ CDev((f == PUBSUB) ? "obj_master" : "param_master",
+ (f == PUBSUB) ? TOPIC_MASTER_DEVICE_PATH : PARAM_MASTER_DEVICE_PATH),
+ _flavor(f)
+{
+ // enable debug() calls
+ _debug_enabled = true;
+
+}
+
+ORBDevMaster::~ORBDevMaster()
+{
+}
+
+int
+ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ int ret;
+
+ switch (cmd) {
+ case ORBIOCADVERTISE: {
+ const struct orb_metadata *meta = (const struct orb_metadata *)arg;
+ const char *objname;
+ char nodepath[orb_maxpath];
+ ORBDevNode *node;
+
+ /* construct a path to the node - this also checks the node name */
+ ret = node_mkpath(nodepath, _flavor, meta);
+
+ if (ret != OK)
+ return ret;
+
+ /* driver wants a permanent copy of the node name, so make one here */
+ objname = strdup(meta->o_name);
+
+ if (objname == nullptr)
+ return -ENOMEM;
+
+ /* construct the new node */
+ node = new ORBDevNode(meta, objname, nodepath);
+
+ /* initialise the node - this may fail if e.g. a node with this name already exists */
+ if (node != nullptr)
+ ret = node->init();
+
+ /* if we didn't get a device, that's bad */
+ if (node == nullptr)
+ return -ENOMEM;
+
+ /* if init failed, discard the node and its name */
+ if (ret != OK) {
+ delete node;
+ free((void *)objname);
+ }
+
+ return ret;
+ }
+
+ default:
+ /* give it to the superclass */
+ return CDev::ioctl(filp, cmd, arg);
+ }
+}
+
+
+/**
+ * Local functions in support of the shell command.
+ */
+
+namespace
+{
+
+ORBDevMaster *g_dev;
+
+struct orb_test {
+ int val;
+};
+
+ORB_DEFINE(orb_test, struct orb_test);
+
+int
+test_fail(const char *fmt, ...)
+{
+ va_list ap;
+
+ fprintf(stderr, "FAIL: ");
+ va_start(ap, fmt);
+ vfprintf(stderr, fmt, ap);
+ va_end(ap);
+ fprintf(stderr, "\n");
+ fflush(stderr);
+ return ERROR;
+}
+
+int
+test_note(const char *fmt, ...)
+{
+ va_list ap;
+
+ fprintf(stderr, "note: ");
+ va_start(ap, fmt);
+ vfprintf(stderr, fmt, ap);
+ va_end(ap);
+ fprintf(stderr, "\n");
+ fflush(stderr);
+ return OK;
+}
+
+ORB_DECLARE(sensor_combined);
+
+int
+test()
+{
+ struct orb_test t, u;
+ int pfd, sfd;
+ bool updated;
+
+ t.val = 0;
+ pfd = orb_advertise(ORB_ID(orb_test), &t);
+
+ if (pfd < 0)
+ return test_fail("advertise failed: %d", errno);
+
+ test_note("publish fd %d", pfd);
+ sfd = orb_subscribe(ORB_ID(orb_test));
+
+ if (sfd < 0)
+ return test_fail("subscribe failed: %d", errno);
+
+ test_note("subscribe fd %d", sfd);
+ u.val = 1;
+
+ if (OK != orb_copy(ORB_ID(orb_test), sfd, &u))
+ return test_fail("copy(1) failed: %d", errno);
+
+ if (u.val != t.val)
+ return test_fail("copy(1) mismatch: %d expected %d", u.val, t.val);
+
+ if (OK != orb_check(sfd, &updated))
+ return test_fail("check(1) failed");
+
+ if (updated)
+ return test_fail("spurious updated flag");
+
+ t.val = 2;
+ test_note("try publish");
+
+ if (OK != orb_publish(ORB_ID(orb_test), pfd, &t))
+ return test_fail("publish failed");
+
+ if (OK != orb_check(sfd, &updated))
+ return test_fail("check(2) failed");
+
+ if (!updated)
+ return test_fail("missing updated flag");
+
+ if (OK != orb_copy(ORB_ID(orb_test), sfd, &u))
+ return test_fail("copy(2) failed: %d", errno);
+
+ if (u.val != t.val)
+ return test_fail("copy(2) mismatch: %d expected %d", u.val, t.val);
+
+ orb_unsubscribe(sfd);
+ close(pfd);
+
+#if 0
+ /* this is a hacky test that exploits the sensors app to test rate-limiting */
+
+ sfd = orb_subscribe(ORB_ID(sensor_combined));
+
+ hrt_abstime start, end;
+ unsigned count;
+
+ start = hrt_absolute_time();
+ count = 0;
+
+ do {
+ orb_check(sfd, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(sensor_combined), sfd, nullptr);
+ count++;
+ }
+ } while (count < 100);
+
+ end = hrt_absolute_time();
+ test_note("full-speed, 100 updates in %llu", end - start);
+
+ orb_set_interval(sfd, 10);
+
+ start = hrt_absolute_time();
+ count = 0;
+
+ do {
+ orb_check(sfd, &updated);
+
+ if (updated) {
+ orb_copy(ORB_ID(sensor_combined), sfd, nullptr);
+ count++;
+ }
+ } while (count < 100);
+
+ end = hrt_absolute_time();
+ test_note("100Hz, 100 updates in %llu", end - start);
+
+ orb_unsubscribe(sfd);
+#endif
+
+ return test_note("PASS");
+}
+
+int
+info()
+{
+ return OK;
+}
+
+
+} // namespace
+
+/*
+ * uORB server 'main'.
+ */
+extern "C" { __EXPORT int uorb_main(int argc, char *argv[]); }
+
+int
+uorb_main(int argc, char *argv[])
+{
+ /*
+ * Start/load the driver.
+ *
+ * XXX it would be nice to have a wrapper for this...
+ */
+ if (!strcmp(argv[1], "start")) {
+
+ if (g_dev != nullptr) {
+ fprintf(stderr, "[uorb] already loaded\n");
+ return -EBUSY;
+ }
+
+ /* create the driver */
+ g_dev = new ORBDevMaster(PUBSUB);
+
+ if (g_dev == nullptr) {
+ fprintf(stderr, "[uorb] driver alloc failed\n");
+ return -ENOMEM;
+ }
+
+ if (OK != g_dev->init()) {
+ fprintf(stderr, "[uorb] driver init failed\n");
+ delete g_dev;
+ g_dev = nullptr;
+ return -EIO;
+ }
+
+ printf("[uorb] ready\n");
+ return OK;
+ }
+
+ /*
+ * Test the driver/device.
+ */
+ if (!strcmp(argv[1], "test"))
+ return test();
+
+ /*
+ * Print driver information.
+ */
+ if (!strcmp(argv[1], "info"))
+ return info();
+
+ fprintf(stderr, "unrecognised command, try 'start', 'test' or 'info'\n");
+ return -EINVAL;
+}
+
+/*
+ * Library functions.
+ */
+namespace
+{
+
+void debug(const char *fmt, ...)
+{
+ va_list ap;
+
+ va_start(ap, fmt);
+ vfprintf(stderr, fmt, ap);
+ va_end(ap);
+ fprintf(stderr, "\n");
+ fflush(stderr);
+ usleep(100000);
+}
+
+/**
+ * Advertise a node; don't consider it an error if the node has
+ * already been advertised.
+ *
+ * @todo verify that the existing node is the same as the one
+ * we tried to advertise.
+ */
+int
+node_advertise(const struct orb_metadata *meta)
+{
+ int fd = -1;
+ int ret = ERROR;
+
+ /* open the control device */
+ fd = open(TOPIC_MASTER_DEVICE_PATH, 0);
+
+ if (fd < 0)
+ goto out;
+
+ /* advertise the object */
+ ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta);
+
+ /* it's OK if it already exists */
+ if ((OK != ret) && (EEXIST == errno))
+ ret = OK;
+
+out:
+
+ if (fd >= 0)
+ close(fd);
+
+ return ret;
+}
+
+/**
+ * Common implementation for orb_advertise and orb_subscribe.
+ *
+ * Handles creation of the object and the initial publication for
+ * advertisers.
+ */
+int
+node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser)
+{
+ char path[orb_maxpath];
+ int fd, ret;
+
+ /*
+ * If meta is null, the object was not defined, i.e. it is not
+ * known to the system. We can't advertise/subscribe such a thing.
+ */
+ if (nullptr == meta) {
+ errno = ENOENT;
+ return ERROR;
+ }
+
+ /*
+ * Advertiser must publish an initial value.
+ */
+ if (advertiser && (data == nullptr)) {
+ errno = EINVAL;
+ return ERROR;
+ }
+
+ /*
+ * Generate the path to the node and try to open it.
+ */
+ ret = node_mkpath(path, f, meta);
+
+ if (ret != OK) {
+ errno = -ret;
+ return ERROR;
+ }
+
+ /* open the path as either the advertiser or the subscriber */
+ fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
+
+ /* we may need to advertise the node... */
+ if (fd < 0) {
+
+ /* try to create the node */
+ ret = node_advertise(meta);
+
+ /* on success, try the open again */
+ if (ret == OK)
+ fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY);
+ }
+
+ if (fd < 0) {
+ errno = EIO;
+ return ERROR;
+ }
+
+ /* the advertiser must perform an initial publish to initialise the object */
+ if (advertiser) {
+ ret = orb_publish(meta, fd, data);
+
+ if (ret != OK) {
+ /* save errno across the close */
+ ret = errno;
+ close(fd);
+ errno = ret;
+ return ERROR;
+ }
+ }
+
+ /* everything has been OK, we can return the handle now */
+ return fd;
+}
+
+} // namespace
+
+int
+orb_advertise(const struct orb_metadata *meta, const void *data)
+{
+ return node_open(PUBSUB, meta, data, true);
+}
+
+int
+orb_subscribe(const struct orb_metadata *meta)
+{
+ return node_open(PUBSUB, meta, nullptr, false);
+}
+
+int
+orb_unsubscribe(int handle)
+{
+ return close(handle);
+}
+
+int
+orb_publish(const struct orb_metadata *meta, int handle, const void *data)
+{
+ int ret;
+
+ ret = write(handle, data, meta->o_size);
+
+ if (ret < 0)
+ return ERROR;
+
+ if (ret != (int)meta->o_size) {
+ errno = EIO;
+ return ERROR;
+ }
+
+ return OK;
+}
+
+int
+orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
+{
+ int ret;
+
+ ret = read(handle, buffer, meta->o_size);
+
+ if (ret < 0)
+ return ERROR;
+
+ if (ret != (int)meta->o_size) {
+ errno = EIO;
+ return ERROR;
+ }
+
+ return OK;
+}
+
+int
+orb_check(int handle, bool *updated)
+{
+ return ioctl(handle, ORBIOCUPDATED, (unsigned long)(uintptr_t)updated);
+}
+
+int
+orb_stat(int handle, uint64_t *time)
+{
+ return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time);
+}
+
+int
+orb_set_interval(int handle, unsigned interval)
+{
+ return ioctl(handle, ORBIOCSETINTERVAL, interval * 1000);
+}
+
diff --git a/apps/uORB/uORB.h b/apps/uORB/uORB.h
new file mode 100644
index 000000000..c36d0044f
--- /dev/null
+++ b/apps/uORB/uORB.h
@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+#ifndef _UORB_UORB_H
+#define _UORB_UORB_H
+
+/**
+ * @file API for the uORB lightweight object broker.
+ */
+
+#include <sys/types.h>
+#include <stdint.h>
+#include <stdbool.h>
+ // Hack until parameter storage is cleaned up
+#include "parameter_storage.h"
+
+// Hack until everything is using this header
+#include <systemlib/visibility.h>
+
+/**
+ * Object metadata.
+ */
+struct orb_metadata {
+ const char *o_name; /**< unique object name */
+ const size_t o_size; /**< object size */
+};
+
+/**
+ * Generates a pointer to the uORB metadata structure for
+ * a given topic.
+ *
+ * The topic must have been declared previously in scope
+ * with ORB_DECLARE().
+ *
+ * @param _name The name of the topic.
+ */
+#define ORB_ID(_name) &__orb_##_name
+
+/**
+ * Declare (prototype) the uORB metadata for a topic.
+ *
+ * Note that optional topics are declared weak; this allows a potential
+ * subscriber to attempt to subscribe to a topic that is not known to the
+ * system at runtime. The ORB_ID() macro will return NULL/nullptr for
+ * such a topic, and attempts to advertise or subscribe to it will
+ * return -1/ENOENT (see below).
+ *
+ * @param _name The name of the topic.
+ */
+#if defined(__cplusplus)
+# define ORB_DECLARE(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT
+# define ORB_DECLARE_OPTIONAL(_name) extern "C" const struct orb_metadata __orb_##_name __EXPORT __attribute__((weak))
+#else
+# define ORB_DECLARE(_name) extern const struct orb_metadata __orb_##_name __EXPORT
+# define ORB_DECLARE_OPTIONAL(_name) extern const struct orb_metadata __orb_##_name __EXPORT __attribute__((weak))
+#endif
+
+/**
+ * Define (instantiate) the uORB metadata for a topic.
+ *
+ * The uORB metadata is used to help ensure that updates and
+ * copies are accessing the right data.
+ *
+ * Note that there must be no more than one instance of this macro
+ * for each topic.
+ *
+ * @param _name The name of the topic.
+ * @param _struct The structure the topic provides.
+ */
+#define ORB_DEFINE(_name, _struct) \
+ const struct orb_metadata __orb_##_name = { \
+ #_name, \
+ sizeof(_struct) \
+ }; struct hack
+
+__BEGIN_DECLS
+
+/**
+ * Advertise as the publisher of a topic.
+ *
+ * This performs the initial advertisement of a topic; it creates the topic
+ * node in /obj if required and publishes the initial data.
+ *
+ * @param meta The uORB metadata (usually from the ORB_ID() macro)
+ * for the topic.
+ * @param data A pointer to the initial data to be published.
+ * For topics updated by interrupt handlers, the advertisement
+ * must be performed from non-interrupt context.
+ * @return ERROR on error, otherwise returns a handle
+ * that can be used to publish to the topic.
+ * If the topic in question is not known (due to an
+ * ORB_DEFINE with no corresponding ORB_DECLARE)
+ * this function will return -1 and set errno to ENOENT.
+ */
+extern int orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT;
+
+/**
+ * Publish new data to a topic.
+ *
+ * The data is atomically published to the topic and any waiting subscribers
+ * will be notified. Subscribers that are not waiting can check the topic
+ * for updates using orb_check and/or orb_stat.
+ *
+ * @handle The handle returned from orb_advertise.
+ * @param meta The uORB metadata (usually from the ORB_ID() macro)
+ * for the topic.
+ * @param data A pointer to the data to be published.
+ * @return OK on success, ERROR otherwise with errno set accordingly.
+ */
+extern int orb_publish(const struct orb_metadata *meta, int handle, const void *data) __EXPORT;
+
+/**
+ * Subscribe to a topic.
+ *
+ * The returned value is a file descriptor that can be passed to poll()
+ * in order to wait for updates to a topic, as well as topic_read,
+ * orb_check and orb_stat.
+ *
+ * Subscription will succeed even if the topic has not been advertised;
+ * in this case the topic will have a timestamp of zero, it will never
+ * signal a poll() event, checking will always return false and it cannot
+ * be copied. When the topic is subsequently advertised, poll, check,
+ * stat and copy calls will react to the initial publication that is
+ * performed as part of the advertisement.
+ *
+ * Subscription will fail if the topic is not known to the system, i.e.
+ * there is nothing in the system that has declared the topic and thus it
+ * can never be published.
+ *
+ * @param meta The uORB metadata (usually from the ORB_ID() macro)
+ * for the topic.
+ * @return ERROR on error, otherwise returns a handle
+ * that can be used to read and update the topic.
+ * If the topic in question is not known (due to an
+ * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE)
+ * this function will return -1 and set errno to ENOENT.
+ */
+extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT;
+
+/**
+ * Unsubscribe from a topic.
+ *
+ * @param handle A handle returned from orb_subscribe.
+ * @return OK on success, ERROR otherwise with errno set accordingly.
+ */
+extern int orb_unsubscribe(int handle) __EXPORT;
+
+/**
+ * Fetch data from a topic.
+ *
+ * This is the only operation that will reset the internal marker that
+ * indicates that a topic has been updated for a subscriber. Once poll
+ * or check return indicating that an updaet is available, this call
+ * must be used to update the subscription.
+ *
+ * @param meta The uORB metadata (usually from the ORB_ID() macro)
+ * for the topic.
+ * @param handle A handle returned from orb_subscribe.
+ * @param buffer Pointer to the buffer receiving the data, or NULL
+ * if the caller wants to clear the updated flag without
+ * using the data.
+ * @return OK on success, ERROR otherwise with errno set accordingly.
+ */
+extern int orb_copy(const struct orb_metadata *meta, int handle, void *buffer) __EXPORT;
+
+/**
+ * Check whether a topic has been published to since the last orb_copy.
+ *
+ * This check can be used to determine whether to copy the topic when
+ * not using poll(), or to avoid the overhead of calling poll() when the
+ * topic is likely to have updated.
+ *
+ * Updates are tracked on a per-handle basis; this call will continue to
+ * return true until orb_copy is called using the same handle. This interface
+ * should be preferred over calling orb_stat due to the race window between
+ * stat and copy that can lead to missed updates.
+ *
+ * @param handle A handle returned from orb_subscribe.
+ * @param updated Set to true if the topic has been updated since the
+ * last time it was copied using this handle.
+ * @return OK if the check was successful, ERROR otherwise with
+ * errno set accordingly.
+ */
+extern int orb_check(int handle, bool *updated) __EXPORT;
+
+/**
+ * Return the last time that the topic was updated.
+ *
+ * @param handle A handle returned from orb_subscribe.
+ * @param time Returns the absolute time that the topic was updated, or zero if it has
+ * never been updated. Time is measured in microseconds.
+ * @return OK on success, ERROR otherwise with errno set accordingly.
+ */
+extern int orb_stat(int handle, uint64_t *time) __EXPORT;
+
+/**
+ * Set the minimum interval between which updates are seen for a subscription.
+ *
+ * If this interval is set, the subscriber will not see more than one update
+ * within the period.
+ *
+ * Specifically, the first time an update is reported to the subscriber a timer
+ * is started. The update will continue to be reported via poll and orb_check, but
+ * once fetched via orb_copy another update will not be reported until the timer
+ * expires.
+ *
+ * This feature can be used to pace a subscriber that is watching a topic that
+ * would otherwise update too quickly.
+ *
+ * @param handle A handle returned from orb_subscribe.
+ * @param interval An interval period in milliseconds.
+ * @return OK on success, ERROR otherwise with ERRNO set accordingly.
+ */
+extern int orb_set_interval(int handle, unsigned interval) __EXPORT;
+
+__END_DECLS
+
+#endif /* _UORB_UORB_H */