diff options
author | px4dev <px4@purgatory.org> | 2012-08-04 15:12:36 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-04 15:12:36 -0700 |
commit | 8a365179eafdf3aea98e60ab9f5882b200d4c759 (patch) | |
tree | 4f38d6d4cd80bd0b6e22e2bb534c3f117ce44e56 /apps/uORB | |
download | px4-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')
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 */ |