aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/ardrone_interface/ardrone_interface.c8
-rw-r--r--src/drivers/hil/hil.cpp22
-rw-r--r--src/drivers/mkblctrl/mkblctrl.cpp1
-rw-r--r--src/drivers/px4fmu/fmu.cpp6
-rw-r--r--src/drivers/px4io/px4io.cpp8
-rw-r--r--src/drivers/roboclaw/RoboClaw.cpp17
-rw-r--r--src/examples/fixedwing_control/main.c20
-rw-r--r--src/examples/flow_position_estimator/flow_position_estimator_main.c3
-rw-r--r--src/examples/hwtest/hwtest.c4
-rw-r--r--src/examples/publisher/module.mk44
-rw-r--r--src/examples/publisher/publisher_example.cpp68
-rw-r--r--src/examples/publisher/publisher_example.h (renamed from src/modules/uORB/topics/parameter_update.h)38
-rw-r--r--src/examples/publisher/publisher_main.cpp56
-rw-r--r--src/examples/publisher/publisher_start_nuttx.cpp99
-rw-r--r--src/examples/subscriber/module.mk45
-rw-r--r--src/examples/subscriber/subscriber_example.cpp102
-rw-r--r--src/examples/subscriber/subscriber_example.h64
-rw-r--r--src/examples/subscriber/subscriber_main.cpp55
-rw-r--r--src/examples/subscriber/subscriber_params.c (renamed from src/modules/uORB/topics/actuator_armed.h)41
-rw-r--r--src/examples/subscriber/subscriber_params.h43
-rw-r--r--src/examples/subscriber/subscriber_start_nuttx.cpp99
-rw-r--r--src/include/containers/List.hpp1
-rw-r--r--src/include/px4.h48
-rw-r--r--src/lib/geo/geo.h5
-rw-r--r--src/lib/mathlib/math/Limits.cpp5
-rw-r--r--src/lib/mathlib/math/Limits.hpp4
-rw-r--r--src/lib/mathlib/math/Matrix.hpp58
-rw-r--r--src/lib/mathlib/math/Quaternion.hpp2
-rw-r--r--src/lib/mathlib/math/Vector.hpp12
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp2
-rw-r--r--src/modules/bottle_drop/bottle_drop.cpp5
-rw-r--r--src/modules/commander/commander.cpp4
-rw-r--r--src/modules/controllib/block/Block.cpp4
-rw-r--r--src/modules/controllib/block/Block.hpp12
-rw-r--r--src/modules/controllib/uorb/blocks.cpp18
-rw-r--r--src/modules/controllib/uorb/blocks.hpp4
-rw-r--r--src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp3
-rw-r--r--src/modules/fw_att_control/fw_att_control_main.cpp29
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp4
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs.cpp2
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp2
-rw-r--r--src/modules/mc_att_control/mc_att_control_main.cpp8
-rw-r--r--src/modules/mc_att_control_multiplatform/mc_att_control.cpp301
-rw-r--r--src/modules/mc_att_control_multiplatform/mc_att_control.h130
-rw-r--r--src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp309
-rw-r--r--src/modules/mc_att_control_multiplatform/mc_att_control_base.h136
-rw-r--r--src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp (renamed from src/modules/uORB/topics/rc_channels.h)88
-rw-r--r--src/modules/mc_att_control_multiplatform/mc_att_control_params.c248
-rw-r--r--src/modules/mc_att_control_multiplatform/mc_att_control_params.h65
-rw-r--r--src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp142
-rw-r--r--src/modules/mc_att_control_multiplatform/mc_att_control_sim.h97
-rw-r--r--src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp99
-rw-r--r--src/modules/mc_att_control_multiplatform/module.mk44
-rw-r--r--src/modules/mc_pos_control/mc_pos_control_main.cpp31
-rw-r--r--src/modules/position_estimator_inav/position_estimator_inav_main.c23
-rw-r--r--src/modules/sdlog2/sdlog2.c6
-rw-r--r--src/modules/sensors/sensors.cpp94
-rw-r--r--src/modules/systemlib/circuit_breaker.cpp55
-rw-r--r--src/modules/systemlib/circuit_breaker.h2
-rw-r--r--src/modules/systemlib/circuit_breaker_params.c (renamed from src/modules/systemlib/circuit_breaker.c)39
-rw-r--r--src/modules/systemlib/circuit_breaker_params.h7
-rw-r--r--src/modules/systemlib/module.mk3
-rw-r--r--src/modules/systemlib/perf_counter.h1
-rw-r--r--src/modules/uORB/Publication.cpp8
-rw-r--r--src/modules/uORB/Publication.hpp96
-rw-r--r--src/modules/uORB/Subscription.cpp20
-rw-r--r--src/modules/uORB/Subscription.hpp117
-rw-r--r--src/modules/uORB/objects_common.cpp24
-rw-r--r--src/modules/uORB/topics/airspeed.h2
-rw-r--r--src/modules/uORB/topics/fence.h2
-rw-r--r--src/modules/uORB/topics/manual_control_setpoint.h111
-rwxr-xr-xsrc/modules/uORB/topics/vehicle_attitude.h91
-rw-r--r--src/modules/uORB/topics/vehicle_attitude_setpoint.h89
-rw-r--r--src/modules/uORB/topics/vehicle_global_position.h2
-rw-r--r--src/modules/uORB/topics/vehicle_status.h263
-rw-r--r--src/modules/uORB/uORB.h11
-rw-r--r--src/modules/vtol_att_control/vtol_att_control_main.cpp4
-rw-r--r--src/platforms/empty.c3
-rw-r--r--src/platforms/nuttx/module.mk40
-rw-r--r--src/platforms/nuttx/px4_nuttx_impl.cpp57
-rw-r--r--src/platforms/px4_defines.h204
-rw-r--r--src/platforms/px4_includes.h93
-rw-r--r--src/platforms/px4_message.h (renamed from src/modules/uORB/topics/actuator_controls.h)73
-rw-r--r--src/platforms/px4_middleware.h (renamed from src/modules/uORB/topics/vehicle_rates_setpoint.h)64
-rw-r--r--src/platforms/px4_nodehandle.h305
-rw-r--r--src/platforms/px4_publisher.h161
-rw-r--r--src/platforms/px4_subscriber.h284
-rwxr-xr-xsrc/platforms/ros/eigen_math.h19
-rw-r--r--src/platforms/ros/geo.cpp (renamed from src/modules/uORB/topics/vehicle_control_mode.h)164
-rw-r--r--src/platforms/ros/nodes/README.md22
-rw-r--r--src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp149
-rw-r--r--src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h62
-rw-r--r--src/platforms/ros/nodes/commander/commander.cpp116
-rw-r--r--src/platforms/ros/nodes/commander/commander.h69
-rw-r--r--src/platforms/ros/nodes/manual_input/manual_input.cpp118
-rw-r--r--src/platforms/ros/nodes/manual_input/manual_input.h74
-rw-r--r--src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp276
-rwxr-xr-xsrc/platforms/ros/perf_counter.cpp176
-rw-r--r--src/platforms/ros/px4_nodehandle.cpp44
-rw-r--r--src/platforms/ros/px4_publisher.cpp41
-rw-r--r--src/platforms/ros/px4_ros_impl.cpp56
-rw-r--r--src/systemcmds/esc_calib/esc_calib.c5
102 files changed, 5498 insertions, 1012 deletions
diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index 7f1b21a95..7d4b7d880 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -55,6 +55,10 @@
#include <uORB/uORB.h>
#include <uORB/topics/safety.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/vehicle_control_mode.h>
@@ -98,7 +102,7 @@ usage(const char *reason)
* The deamon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
- *
+ *
* The actual stack size should be set in the call
* to task_create().
*/
@@ -319,7 +323,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* get a local copy of the actuator controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
-
+
/* for now only spin if armed and immediately shut down
* if in failsafe
*/
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index 9b5c8133b..6ffa8252e 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -75,6 +75,8 @@
#include <systemlib/mixer/mixer.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_outputs.h>
@@ -250,7 +252,7 @@ HIL::task_main_trampoline(int argc, char *argv[])
int
HIL::set_mode(Mode mode)
{
- /*
+ /*
* Configure for PWM output.
*
* Note that regardless of the configured mode, the task is always
@@ -269,19 +271,19 @@ HIL::set_mode(Mode mode)
/* multi-port as 4 PWM outs */
_update_rate = 50; /* default output rate */
break;
-
+
case MODE_8PWM:
debug("MODE_8PWM");
/* multi-port as 8 PWM outs */
_update_rate = 50; /* default output rate */
break;
-
+
case MODE_12PWM:
debug("MODE_12PWM");
/* multi-port as 12 PWM outs */
_update_rate = 50; /* default output rate */
break;
-
+
case MODE_16PWM:
debug("MODE_16PWM");
/* multi-port as 16 PWM outs */
@@ -513,12 +515,12 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_UPDATE_RATE:
- // HIL always outputs at the alternate (usually faster) rate
+ // HIL always outputs at the alternate (usually faster) rate
g_hil->set_pwm_rate(arg);
break;
case PWM_SERVO_SET_SELECT_UPDATE_RATE:
- // HIL always outputs at the alternate (usually faster) rate
+ // HIL always outputs at the alternate (usually faster) rate
break;
case PWM_SERVO_SET(2):
@@ -659,7 +661,7 @@ int
hil_new_mode(PortMode new_mode)
{
// uint32_t gpio_bits;
-
+
// /* reset to all-inputs */
// g_hil->ioctl(0, GPIO_RESET, 0);
@@ -691,17 +693,17 @@ hil_new_mode(PortMode new_mode)
/* select 2-pin PWM mode */
servo_mode = HIL::MODE_2PWM;
break;
-
+
case PORT2_8PWM:
/* select 8-pin PWM mode */
servo_mode = HIL::MODE_8PWM;
break;
-
+
case PORT2_12PWM:
/* select 12-pin PWM mode */
servo_mode = HIL::MODE_12PWM;
break;
-
+
case PORT2_16PWM:
/* select 16-pin PWM mode */
servo_mode = HIL::MODE_16PWM;
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index 1055487cb..a4505fe6f 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -74,6 +74,7 @@
#include <drivers/drv_mixer.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/esc_status.h>
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 112c01513..2552e7e6a 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -70,6 +70,10 @@
#include <drivers/drv_rc_input.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
@@ -1144,7 +1148,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
* and PWM under control of the flight config
* parameters. Note that this does not allow for
* changing a set of pins to be used for serial on
- * FMUv1
+ * FMUv1
*/
switch (arg) {
case 0:
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 5e3d28fe9..ffcb1c907 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -75,6 +75,10 @@
#include <systemlib/circuit_breaker.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/safety.h>
@@ -1206,7 +1210,7 @@ PX4IO::io_set_arming_state()
if (armed.ready_to_arm) {
set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
-
+
} else {
clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
}
@@ -2597,7 +2601,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
case PWM_SERVO_SET_RC_CONFIG: {
/* enable setting of RC configuration without relying
- on param_get()
+ on param_get()
*/
struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
if (config->channel >= RC_INPUT_MAX_CHANNELS) {
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index fdaa7f27b..5b945e452 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -54,6 +54,7 @@
#include <mavlink/mavlink_log.h>
#include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_controls_0.h>
#include <uORB/topics/debug_key_value.h>
#include <drivers/drv_hrt.h>
@@ -65,7 +66,7 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
_pulsesPerRev(pulsesPerRev),
_uart(0),
_controlPoll(),
- _actuators(NULL, ORB_ID(actuator_controls_0), 20),
+ _actuators(ORB_ID(actuator_controls_0), 20),
_motor1Position(0),
_motor1Speed(0),
_motor1Overflow(0),
@@ -114,7 +115,7 @@ int RoboClaw::readEncoder(e_motor motor)
uint8_t rbuf[50];
usleep(5000);
int nread = read(_uart, rbuf, 50);
- if (nread < 6) {
+ if (nread < 6) {
printf("failed to read\n");
return -1;
}
@@ -131,7 +132,7 @@ int RoboClaw::readEncoder(e_motor motor)
countBytes[0] = rbuf[3];
uint8_t status = rbuf[4];
uint8_t checksum = rbuf[5];
- uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
+ uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) &
checksum_mask;
// check if checksum is valid
if (checksum != checksum_computed) {
@@ -156,11 +157,11 @@ int RoboClaw::readEncoder(e_motor motor)
static int64_t overflowAmount = 0x100000000LL;
if (motor == MOTOR_1) {
_motor1Overflow += overFlow;
- _motor1Position = float(int64_t(count) +
+ _motor1Position = float(int64_t(count) +
_motor1Overflow*overflowAmount)/_pulsesPerRev;
} else if (motor == MOTOR_2) {
_motor2Overflow += overFlow;
- _motor2Position = float(int64_t(count) +
+ _motor2Position = float(int64_t(count) +
_motor2Overflow*overflowAmount)/_pulsesPerRev;
}
return 0;
@@ -242,7 +243,7 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
return -1;
}
-int RoboClaw::resetEncoders()
+int RoboClaw::resetEncoders()
{
uint16_t sum = 0;
return _sendCommand(CMD_RESET_ENCODERS,
@@ -278,7 +279,7 @@ uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
return sum;
}
-int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
+int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
size_t n_data, uint16_t & prev_sum)
{
tcflush(_uart, TCIOFLUSH); // flush buffers
@@ -299,7 +300,7 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
return write(_uart, buf, n_data + 3);
}
-int roboclawTest(const char *deviceName, uint8_t address,
+int roboclawTest(const char *deviceName, uint8_t address,
uint16_t pulsesPerRev)
{
printf("roboclaw test: starting\n");
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index fcbb54c8e..d8b777b91 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -61,6 +61,10 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/parameter_update.h>
@@ -108,7 +112,7 @@ static void usage(const char *reason);
* @param att The current attitude. The controller should make the attitude match the setpoint
* @param rates_sp The angular rate setpoint. This is the output of the controller.
*/
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators);
/**
@@ -133,18 +137,18 @@ static int deamon_task; /**< Handle of deamon task / thread */
static struct params p;
static struct param_handles ph;
-void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
+void control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp,
struct actuator_controls_s *actuators)
{
- /*
+ /*
* The PX4 architecture provides a mixer outside of the controller.
* The mixer is fed with a default vector of actuator controls, representing
* moments applied to the vehicle frame. This vector
* is structured as:
*
* Control Group 0 (attitude):
- *
+ *
* 0 - roll (-1..+1)
* 1 - pitch (-1..+1)
* 2 - yaw (-1..+1)
@@ -226,7 +230,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
*
* Wikipedia description:
* http://en.wikipedia.org/wiki/Publish–subscribe_pattern
- *
+ *
*/
@@ -234,7 +238,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
/*
* Declare and safely initialize all structs to zero.
- *
+ *
* These structs contain the system state and things
* like attitude, position, the current waypoint, etc.
*/
@@ -300,7 +304,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
if (ret < 0) {
/*
* Poll error, this will not really happen in practice,
- * but its good design practice to make output an error message.
+ * but its good design practice to make output an error message.
*/
warnx("poll error");
@@ -340,7 +344,7 @@ int fixedwing_control_thread_main(int argc, char *argv[])
}
if (manual_sp_updated)
- /* get the RC (or otherwise user based) input */
+ /* get the RC (or otherwise user based) input */
orb_copy(ORB_ID(manual_control_setpoint), manual_sp_sub, &manual_sp);
/* check if the throttle was ever more than 50% - go later only to failsafe if yes */
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index a89ccf933..1ac0c2d04 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -67,6 +67,7 @@
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <poll.h>
+#include <platforms/px4_defines.h>
#include "flow_position_estimator_params.h"
@@ -337,7 +338,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[])
{
float sum = 0.0f;
for(uint8_t j = 0; j < 3; j++)
- sum = sum + speed[j] * att.R[i][j];
+ sum = sum + speed[j] * PX4_R(att.R, i, j);
global_speed[i] = sum;
}
diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c
index 95ff346bb..8fd8870da 100644
--- a/src/examples/hwtest/hwtest.c
+++ b/src/examples/hwtest/hwtest.c
@@ -47,6 +47,10 @@
#include <systemlib/err.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
#include <uORB/uORB.h>
__EXPORT int ex_hwtest_main(int argc, char *argv[]);
diff --git a/src/examples/publisher/module.mk b/src/examples/publisher/module.mk
new file mode 100644
index 000000000..62a5f8dd1
--- /dev/null
+++ b/src/examples/publisher/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+# Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Publisher Example Application
+#
+
+MODULE_COMMAND = publisher
+
+SRCS = publisher_main.cpp \
+ publisher_start_nuttx.cpp \
+ publisher_example.cpp
+
+MODULE_STACKSIZE = 1200
diff --git a/src/examples/publisher/publisher_example.cpp b/src/examples/publisher/publisher_example.cpp
new file mode 100644
index 000000000..9952a16e4
--- /dev/null
+++ b/src/examples/publisher/publisher_example.cpp
@@ -0,0 +1,68 @@
+
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_example.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "publisher_example.h"
+
+using namespace px4;
+
+PublisherExample::PublisherExample() :
+ _n(),
+ _rc_channels_pub(_n.advertise<px4_rc_channels>())
+{
+}
+
+int PublisherExample::main()
+{
+ px4::Rate loop_rate(10);
+
+ while (px4::ok()) {
+ loop_rate.sleep();
+ _n.spinOnce();
+
+ px4_rc_channels msg;
+ msg.data().timestamp_last_valid = px4::get_time_micros();
+ PX4_INFO("%llu", msg.data().timestamp_last_valid);
+ _rc_channels_pub->publish(msg);
+
+ }
+
+ return 0;
+}
diff --git a/src/modules/uORB/topics/parameter_update.h b/src/examples/publisher/publisher_example.h
index 68964deb0..4ff59a226 100644
--- a/src/modules/uORB/topics/parameter_update.h
+++ b/src/examples/publisher/publisher_example.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,30 +32,22 @@
****************************************************************************/
/**
- * @file parameter_update.h
- * Notification about a parameter update.
+ * @file publisher.h
+ * Example publisher for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*/
+#pragma once
+#include <px4.h>
-#ifndef TOPIC_PARAMETER_UPDATE_H
-#define TOPIC_PARAMETER_UPDATE_H
+class PublisherExample {
+public:
+ PublisherExample();
-#include <stdint.h>
-#include "../uORB.h"
+ ~PublisherExample() {};
-/**
- * @addtogroup topics
- * @{
- */
-
-struct parameter_update_s {
- /** time at which the latest parameter was updated */
- uint64_t timestamp;
+ int main();
+protected:
+ px4::NodeHandle _n;
+ px4::Publisher<px4::px4_rc_channels> * _rc_channels_pub;
};
-
-/**
- * @}
- */
-
-ORB_DECLARE(parameter_update);
-
-#endif \ No newline at end of file
diff --git a/src/examples/publisher/publisher_main.cpp b/src/examples/publisher/publisher_main.cpp
new file mode 100644
index 000000000..11439acff
--- /dev/null
+++ b/src/examples/publisher/publisher_main.cpp
@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_main.cpp
+ * Example publisher for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include "publisher_example.h"
+
+bool thread_running = false; /**< Deamon status flag */
+
+int main(int argc, char **argv)
+{
+ px4::init(argc, argv, "publisher");
+
+ PX4_INFO("starting");
+ PublisherExample p;
+ thread_running = true;
+ p.main();
+
+ PX4_INFO("exiting.");
+ thread_running = false;
+ return 0;
+}
diff --git a/src/examples/publisher/publisher_start_nuttx.cpp b/src/examples/publisher/publisher_start_nuttx.cpp
new file mode 100644
index 000000000..db9d85269
--- /dev/null
+++ b/src/examples/publisher/publisher_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file publisher_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task; /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int publisher_main(int argc, char *argv[]);
+int publisher_main(int argc, char *argv[])
+{
+ if (argc < 1) {
+ errx(1, "usage: publisher {start|stop|status}");
+ }
+
+ if (!strcmp(argv[1], "start")) {
+
+ if (thread_running) {
+ warnx("already running");
+ /* this is not an error */
+ exit(0);
+ }
+
+ task_should_exit = false;
+
+ daemon_task = task_spawn_cmd("publisher",
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 5,
+ 2000,
+ main,
+ (argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "stop")) {
+ task_should_exit = true;
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "status")) {
+ if (thread_running) {
+ warnx("is running");
+
+ } else {
+ warnx("not started");
+ }
+
+ exit(0);
+ }
+
+ warnx("unrecognized command");
+ return 1;
+}
diff --git a/src/examples/subscriber/module.mk b/src/examples/subscriber/module.mk
new file mode 100644
index 000000000..0e02c65b4
--- /dev/null
+++ b/src/examples/subscriber/module.mk
@@ -0,0 +1,45 @@
+############################################################################
+#
+# Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Subscriber Example Application
+#
+
+MODULE_COMMAND = subscriber
+
+SRCS = subscriber_main.cpp \
+ subscriber_start_nuttx.cpp \
+ subscriber_example.cpp \
+ subscriber_params.c
+
+MODULE_STACKSIZE = 2400
diff --git a/src/examples/subscriber/subscriber_example.cpp b/src/examples/subscriber/subscriber_example.cpp
new file mode 100644
index 000000000..bc0975a2b
--- /dev/null
+++ b/src/examples/subscriber/subscriber_example.cpp
@@ -0,0 +1,102 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_example.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include "subscriber_params.h"
+#include "subscriber_example.h"
+
+using namespace px4;
+
+void rc_channels_callback_function(const px4_rc_channels &msg) {
+ PX4_INFO("I heard: [%llu]", msg.data().timestamp_last_valid);
+}
+
+SubscriberExample::SubscriberExample() :
+ _n(),
+ _p_sub_interv(PX4_PARAM_INIT(SUB_INTERV)),
+ _interval(0),
+ _p_test_float(PX4_PARAM_INIT(SUB_TESTF)),
+ _test_float(0.0f)
+{
+ /* Read the parameter back as example */
+ PX4_PARAM_GET(_p_sub_interv, &_interval);
+ PX4_INFO("Param SUB_INTERV = %d", _interval);
+ PX4_PARAM_GET(_p_test_float, &_test_float);
+ PX4_INFO("Param SUB_TESTF = %.3f", (double)_test_float);
+
+ /* Do some subscriptions */
+ /* Function */
+ _n.subscribe<px4_rc_channels>(rc_channels_callback_function, _interval);
+
+ /* No callback */
+ // _sub_rc_chan = _n.subscribe<px4_rc_channels>(500);
+ _n.subscribe<px4_rc_channels>(500);
+
+ /* Class Method */
+ _n.subscribe<px4_rc_channels>(&SubscriberExample::rc_channels_callback, this, 1000);
+
+ /* Class Method 2 */
+ _n.subscribe<px4_vehicle_attitude>(&SubscriberExample::vehicle_attitude_callback, this, 1000);
+
+ /* Class Method 3 */
+ // _n.subscribe<px4_vehicle_rates_setpoint>(&SubscriberExample::vehicle_rates_setpoint_callback, this, 1000);
+
+ PX4_INFO("subscribed");
+}
+
+/**
+ * This tutorial demonstrates simple receipt of messages over the PX4 middleware system.
+ * Also the current value of the _sub_rc_chan subscription is printed
+ */
+void SubscriberExample::rc_channels_callback(const px4_rc_channels &msg) {
+ PX4_INFO("Callback (method): [%llu]",
+ msg.data().timestamp_last_valid);
+ PX4_INFO("Callback (method): value of _sub_rc_chan: [%llu]",
+ _sub_rc_chan->data().timestamp_last_valid);
+}
+
+void SubscriberExample::vehicle_attitude_callback(const px4_vehicle_attitude &msg) {
+ PX4_INFO("Callback (method): [%llu]",
+ msg.data().timestamp);
+}
+
+void SubscriberExample::vehicle_rates_setpoint_callback(const px4_vehicle_rates_setpoint &msg) {
+ PX4_INFO("Callback (method): [%llu]",
+ msg.data().timestamp);
+}
diff --git a/src/examples/subscriber/subscriber_example.h b/src/examples/subscriber/subscriber_example.h
new file mode 100644
index 000000000..8af94a458
--- /dev/null
+++ b/src/examples/subscriber/subscriber_example.h
@@ -0,0 +1,64 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_example.h
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <px4.h>
+
+using namespace px4;
+
+void rc_channels_callback_function(const px4_rc_channels &msg);
+
+class SubscriberExample {
+public:
+ SubscriberExample();
+
+ ~SubscriberExample() {};
+
+ void spin() {_n.spin();}
+protected:
+ px4::NodeHandle _n;
+ px4_param_t _p_sub_interv;
+ int32_t _interval;
+ px4_param_t _p_test_float;
+ float _test_float;
+ px4::Subscriber<px4_rc_channels> * _sub_rc_chan;
+
+ __EXPORT void rc_channels_callback(const px4_rc_channels &msg);
+ __EXPORT void vehicle_attitude_callback(const px4_vehicle_attitude &msg);
+ __EXPORT void vehicle_rates_setpoint_callback(const px4_vehicle_rates_setpoint &msg);
+};
diff --git a/src/examples/subscriber/subscriber_main.cpp b/src/examples/subscriber/subscriber_main.cpp
new file mode 100644
index 000000000..798c74163
--- /dev/null
+++ b/src/examples/subscriber/subscriber_main.cpp
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_main.cpp
+ * Example subscriber for ros and px4
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include "subscriber_example.h"
+bool thread_running = false; /**< Deamon status flag */
+
+int main(int argc, char **argv)
+{
+ px4::init(argc, argv, "subscriber");
+
+ PX4_INFO("starting");
+ SubscriberExample s;
+ thread_running = true;
+ s.spin();
+
+ PX4_INFO("exiting.");
+ thread_running = false;
+ return 0;
+}
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/examples/subscriber/subscriber_params.c
index 0f6c9aca1..a43817b5b 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/examples/subscriber/subscriber_params.c
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,38 +32,25 @@
****************************************************************************/
/**
- * @file actuator_armed.h
- *
- * Actuator armed topic
+ * @file subscriber_params.c
+ * Parameters for the subscriber example
*
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*/
-#ifndef TOPIC_ACTUATOR_ARMED_H
-#define TOPIC_ACTUATOR_ARMED_H
-
-#include <stdint.h>
-#include "../uORB.h"
+#include <px4_defines.h>
+#include "subscriber_params.h"
/**
- * @addtogroup topics
- * @{
+ * Interval of one subscriber in the example in ms
+ *
+ * @group Subscriber Example
*/
-
-/** global 'actuator output is live' control. */
-struct actuator_armed_s {
-
- uint64_t timestamp; /**< Microseconds since system boot */
- bool armed; /**< Set to true if system is armed */
- bool ready_to_arm; /**< Set to true if system is ready to be armed */
- bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
- bool force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */
-};
+PX4_PARAM_DEFINE_INT32(SUB_INTERV);
/**
- * @}
+ * Float Demonstration Parameter in the Example
+ *
+ * @group Subscriber Example
*/
-
-/* register this as object request broker structure */
-ORB_DECLARE(actuator_armed);
-
-#endif
+PX4_PARAM_DEFINE_FLOAT(SUB_TESTF);
diff --git a/src/examples/subscriber/subscriber_params.h b/src/examples/subscriber/subscriber_params.h
new file mode 100644
index 000000000..f6f1d6ba0
--- /dev/null
+++ b/src/examples/subscriber/subscriber_params.h
@@ -0,0 +1,43 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_params.h
+ * Default parameters for the subscriber example
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#pragma once
+
+#define PARAM_SUB_INTERV_DEFAULT 100
+#define PARAM_SUB_TESTF_DEFAULT 3.14f
diff --git a/src/examples/subscriber/subscriber_start_nuttx.cpp b/src/examples/subscriber/subscriber_start_nuttx.cpp
new file mode 100644
index 000000000..6129b19ac
--- /dev/null
+++ b/src/examples/subscriber/subscriber_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file subscriber_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task; /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int subscriber_main(int argc, char *argv[]);
+int subscriber_main(int argc, char *argv[])
+{
+ if (argc < 1) {
+ errx(1, "usage: subscriber {start|stop|status}");
+ }
+
+ if (!strcmp(argv[1], "start")) {
+
+ if (thread_running) {
+ warnx("already running");
+ /* this is not an error */
+ exit(0);
+ }
+
+ task_should_exit = false;
+
+ daemon_task = task_spawn_cmd("subscriber",
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 5,
+ 2000,
+ main,
+ (argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "stop")) {
+ task_should_exit = true;
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "status")) {
+ if (thread_running) {
+ warnx("is running");
+
+ } else {
+ warnx("not started");
+ }
+
+ exit(0);
+ }
+
+ warnx("unrecognized command");
+ return 1;
+}
diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp
index 13cbda938..5c0ba59fd 100644
--- a/src/include/containers/List.hpp
+++ b/src/include/containers/List.hpp
@@ -38,6 +38,7 @@
*/
#pragma once
+#include <cstddef>
template<class T>
class __EXPORT ListNode
diff --git a/src/include/px4.h b/src/include/px4.h
new file mode 100644
index 000000000..34137ee6f
--- /dev/null
+++ b/src/include/px4.h
@@ -0,0 +1,48 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4.h
+ *
+ * Main system header with common convenience functions
+ */
+
+#pragma once
+
+#include "../platforms/px4_includes.h"
+#include "../platforms/px4_defines.h"
+#ifdef __cplusplus
+#include "../platforms/px4_middleware.h"
+#include "../platforms/px4_nodehandle.h"
+#include "../platforms/px4_subscriber.h"
+#endif
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 2311e0a7c..ca3587b93 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -44,10 +44,7 @@
*/
#pragma once
-
-#include "uORB/topics/fence.h"
-#include "uORB/topics/vehicle_global_position.h"
-
+#include <platforms/px4_defines.h>
__BEGIN_DECLS
#include "geo_lookup/geo_mag_declination.h"
diff --git a/src/lib/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp
index d4c892d8a..e16f33bd6 100644
--- a/src/lib/mathlib/math/Limits.cpp
+++ b/src/lib/mathlib/math/Limits.cpp
@@ -46,6 +46,9 @@
namespace math {
+#ifndef CONFIG_ARCH_ARM
+#define M_PI_F 3.14159265358979323846f
+#endif
float __EXPORT min(float val1, float val2)
{
@@ -143,4 +146,4 @@ double __EXPORT degrees(double radians)
return (radians / M_PI) * 180.0;
}
-} \ No newline at end of file
+}
diff --git a/src/lib/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index fb778dd66..fca4197b8 100644
--- a/src/lib/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
@@ -39,7 +39,7 @@
#pragma once
-#include <nuttx/config.h>
+#include <platforms/px4_defines.h>
#include <stdint.h>
namespace math {
@@ -84,4 +84,4 @@ float __EXPORT degrees(float radians);
double __EXPORT degrees(double radians);
-} \ No newline at end of file
+}
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index ac1f1538f..f6f4fc5ea 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -44,12 +44,20 @@
#define MATRIX_HPP
#include <stdio.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
+#else
+#include <platforms/ros/eigen_math.h>
+#include <Eigen/Eigen>
+#endif
+#include <platforms/px4_defines.h>
namespace math
{
-template <unsigned int M, unsigned int N>
+template<unsigned int M, unsigned int N>
class __EXPORT Matrix;
// MxN matrix with float elements
@@ -65,7 +73,11 @@ public:
/**
* struct for using arm_math functions
*/
+#ifdef CONFIG_ARCH_ARM
arm_matrix_instance_f32 arm_mat;
+#else
+ eigen_matrix_instance arm_mat;
+#endif
/**
* trivial ctor
@@ -114,6 +126,15 @@ public:
memcpy(data, d, sizeof(data));
}
+#if defined(__PX4_ROS)
+ /**
+ * set data from boost::array
+ */
+ void set(const boost::array<float, 9ul> d) {
+ set(static_cast<const float*>(d.data()));
+ }
+#endif
+
/**
* access by index
*/
@@ -273,27 +294,53 @@ public:
*/
template <unsigned int P>
Matrix<M, P> operator *(const Matrix<N, P> &m) const {
+#ifdef CONFIG_ARCH_ARM
Matrix<M, P> res;
arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
return res;
+#else
+ Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+ (this->arm_mat.pData);
+ Eigen::Matrix<float, N, P, Eigen::RowMajor> Him = Eigen::Map<Eigen::Matrix<float, N, P, Eigen::RowMajor> >
+ (m.arm_mat.pData);
+ Eigen::Matrix<float, M, P, Eigen::RowMajor> Product = Me * Him;
+ Matrix<M, P> res(Product.data());
+ return res;
+#endif
}
/**
* transpose the matrix
*/
Matrix<N, M> transposed(void) const {
+#ifdef CONFIG_ARCH_ARM
Matrix<N, M> res;
arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
return res;
+#else
+ Eigen::Matrix<float, N, M, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, N, M, Eigen::RowMajor> >
+ (this->arm_mat.pData);
+ Me.transposeInPlace();
+ Matrix<N, M> res(Me.data());
+ return res;
+#endif
}
/**
* invert the matrix
*/
Matrix<M, N> inversed(void) const {
+#ifdef CONFIG_ARCH_ARM
Matrix<M, N> res;
arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
return res;
+#else
+ Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+ (this->arm_mat.pData);
+ Eigen::Matrix<float, M, N, Eigen::RowMajor> MyInverse = Me.inverse(); //not sure if A = A.inverse() is a good idea
+ Matrix<M, N> res(MyInverse.data());
+ return res;
+#endif
}
/**
@@ -352,8 +399,17 @@ public:
* multiplication by a vector
*/
Vector<M> operator *(const Vector<N> &v) const {
+#ifdef CONFIG_ARCH_ARM
Vector<M> res;
arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
+#else
+ //probably nicer if this could go into a function like "eigen_mat_mult" or so
+ Eigen::Matrix<float, M, N, Eigen::RowMajor> Me = Eigen::Map<Eigen::Matrix<float, M, N, Eigen::RowMajor> >
+ (this->arm_mat.pData);
+ Eigen::VectorXf Vec = Eigen::Map<Eigen::VectorXf>(v.arm_col.pData, N);
+ Eigen::VectorXf Product = Me * Vec;
+ Vector<M> res(Product.data());
+#endif
return res;
}
};
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 21d05c7ef..b3cca30c6 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -44,7 +44,7 @@
#define QUATERNION_HPP
#include <math.h>
-#include "../CMSIS/Include/arm_math.h"
+
#include "Vector.hpp"
#include "Matrix.hpp"
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 0ddf77615..781c14d53 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -45,7 +45,14 @@
#include <stdio.h>
#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
#include "../CMSIS/Include/arm_math.h"
+#else
+#include <platforms/ros/eigen_math.h>
+#endif
+
+#include <platforms/px4_defines.h>
namespace math
{
@@ -65,7 +72,12 @@ public:
/**
* struct for using arm_math functions, represents column vector
*/
+ #ifdef CONFIG_ARCH_ARM
arm_matrix_instance_f32 arm_col;
+ #else
+ eigen_matrix_instance arm_col;
+ #endif
+
/**
* trivial ctor
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index b0086676a..86f59f0e6 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -592,7 +592,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
R = R_decl * R_body;
/* copy rotation matrix */
- memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
+ memcpy(&att.R[0], &R.data[0][0], sizeof(att.R));
att.R_valid = true;
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index 4580b338d..b267209fe 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -57,8 +57,13 @@
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/wind_estimate.h>
#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_global_position.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 247a2c5b8..a1c4e205d 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -74,6 +74,10 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp
index 6768bfa7e..f3cd87728 100644
--- a/src/modules/controllib/block/Block.cpp
+++ b/src/modules/controllib/block/Block.cpp
@@ -101,7 +101,7 @@ void Block::updateParams()
void Block::updateSubscriptions()
{
- uORB::SubscriptionBase *sub = getSubscriptions().getHead();
+ uORB::SubscriptionNode *sub = getSubscriptions().getHead();
int count = 0;
while (sub != NULL) {
@@ -119,7 +119,7 @@ void Block::updateSubscriptions()
void Block::updatePublications()
{
- uORB::PublicationBase *pub = getPublications().getHead();
+ uORB::PublicationNode *pub = getPublications().getHead();
int count = 0;
while (pub != NULL) {
diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp
index 9bd80b15b..d2f9cdf07 100644
--- a/src/modules/controllib/block/Block.hpp
+++ b/src/modules/controllib/block/Block.hpp
@@ -46,8 +46,8 @@
// forward declaration
namespace uORB {
- class SubscriptionBase;
- class PublicationBase;
+ class SubscriptionNode;
+ class PublicationNode;
}
namespace control
@@ -83,15 +83,15 @@ public:
protected:
// accessors
SuperBlock *getParent() { return _parent; }
- List<uORB::SubscriptionBase *> & getSubscriptions() { return _subscriptions; }
- List<uORB::PublicationBase *> & getPublications() { return _publications; }
+ List<uORB::SubscriptionNode *> & getSubscriptions() { return _subscriptions; }
+ List<uORB::PublicationNode *> & getPublications() { return _publications; }
List<BlockParamBase *> & getParams() { return _params; }
// attributes
const char *_name;
SuperBlock *_parent;
float _dt;
- List<uORB::SubscriptionBase *> _subscriptions;
- List<uORB::PublicationBase *> _publications;
+ List<uORB::SubscriptionNode *> _subscriptions;
+ List<uORB::PublicationNode *> _publications;
List<BlockParamBase *> _params;
private:
diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp
index e8fecef0d..454d0db19 100644
--- a/src/modules/controllib/uorb/blocks.cpp
+++ b/src/modules/controllib/uorb/blocks.cpp
@@ -82,16 +82,16 @@ void BlockWaypointGuidance::update(vehicle_global_position_s &pos,
BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// subscriptions
- _att(&getSubscriptions(), ORB_ID(vehicle_attitude), 20),
- _attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
- _ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
- _pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
- _missionCmd(&getSubscriptions(), ORB_ID(position_setpoint_triplet), 20),
- _manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
- _status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
- _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
+ _att(ORB_ID(vehicle_attitude), 20, &getSubscriptions()),
+ _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, &getSubscriptions()),
+ _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, &getSubscriptions()),
+ _pos(ORB_ID(vehicle_global_position), 20, &getSubscriptions()),
+ _missionCmd(ORB_ID(position_setpoint_triplet), 20, &getSubscriptions()),
+ _manual(ORB_ID(manual_control_setpoint), 20, &getSubscriptions()),
+ _status(ORB_ID(vehicle_status), 20, &getSubscriptions()),
+ _param_update(ORB_ID(parameter_update), 1000, &getSubscriptions()), // limit to 1 Hz
// publications
- _actuators(&getPublications(), ORB_ID(actuator_controls_0))
+ _actuators(ORB_ID(actuator_controls_0), &getPublications())
{
}
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index a8a70507e..491d4681d 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -47,6 +47,10 @@
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/parameter_update.h>
#include <stdio.h>
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 923aa2861..d51075b8c 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -84,6 +84,7 @@
#include <mathlib/mathlib.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <mavlink/mavlink_log.h>
+#include <platforms/px4_defines.h>
#include "estimator_22states.h"
@@ -1416,7 +1417,7 @@ FixedwingEstimator::task_main()
math::Vector<3> euler = R.to_euler();
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
- _att.R[i][j] = R(i, j);
+ PX4_R(_att.R, i, j) = R(i, j);
_att.timestamp = _last_sensor_timestamp;
_att.q[0] = _ekf->states[0];
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 6f225bb48..88918333d 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -59,7 +59,11 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_virtual_fw.h>
+#include <uORB/topics/actuator_controls_virtual_mc.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/fw_virtual_rates_setpoint.h>
+#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
@@ -76,6 +80,7 @@
#include <ecl/attitude_fw/ecl_pitch_controller.h>
#include <ecl/attitude_fw/ecl_roll_controller.h>
#include <ecl/attitude_fw/ecl_yaw_controller.h>
+#include <platforms/px4_defines.h>
/**
* Fixedwing attitude control app start / stop handling function
@@ -742,15 +747,15 @@ FixedwingAttitudeControl::task_main()
_att.roll = euler_angles(0);
_att.pitch = euler_angles(1);
_att.yaw = euler_angles(2);
- _att.R[0][0] = R_adapted(0, 0);
- _att.R[0][1] = R_adapted(0, 1);
- _att.R[0][2] = R_adapted(0, 2);
- _att.R[1][0] = R_adapted(1, 0);
- _att.R[1][1] = R_adapted(1, 1);
- _att.R[1][2] = R_adapted(1, 2);
- _att.R[2][0] = R_adapted(2, 0);
- _att.R[2][1] = R_adapted(2, 1);
- _att.R[2][2] = R_adapted(2, 2);
+ PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
+ PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
+ PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
+ PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
+ PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
+ PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
+ PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
+ PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
+ PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
/* lastly, roll- and yawspeed have to be swaped */
float helper = _att.rollspeed;
@@ -924,9 +929,9 @@ FixedwingAttitudeControl::task_main()
float speed_body_v = 0.0f;
float speed_body_w = 0.0f;
if(_att.R_valid) {
- speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d;
- speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
- speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
+ speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
+ speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
+ speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
} else {
if (_debug && loop_counter % 10 == 0) {
warnx("Did not get a valid R\n");
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index dbf15d98a..74249c9c5 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -77,6 +77,7 @@
#include <uORB/topics/navigation_capabilities.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
@@ -90,6 +91,7 @@
#include <external_lgpl/tecs/tecs.h>
#include "landingslope.h"
#include "mtecs/mTecs.h"
+#include <platforms/px4_defines.h>
static int _control_task = -1; /**< task handle for sensor task */
@@ -735,7 +737,7 @@ FixedwingPositionControl::vehicle_attitude_poll()
/* set rotation matrix */
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
- _R_nb(i, j) = _att.R[i][j];
+ _R_nb(i, j) = PX4_R(_att.R, i, j);
}
}
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index bffeefc1f..ecdab2936 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -52,7 +52,7 @@ mTecs::mTecs() :
_mTecsEnabled(this, "ENABLED"),
_airspeedMin(this, "FW_AIRSPD_MIN", false),
/* Publications */
- _status(&getPublications(), ORB_ID(tecs_status)),
+ _status(ORB_ID(tecs_status), &getPublications()),
/* control blocks */
_controlTotalEnergy(this, "THR"),
_controlEnergyDistribution(this, "PIT", true),
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index dfbf00b66..f4e3dc441 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -799,7 +799,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
att_sp.timestamp = hrt_absolute_time();
mavlink_quaternion_to_euler(set_attitude_target.q,
&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
- mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body);
+ mavlink_quaternion_to_dcm(set_attitude_target.q, (float(*)[3])att_sp.R_body);
att_sp.R_valid = true;
att_sp.thrust = set_attitude_target.thrust;
memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index a094ed2c6..562033db1 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -63,7 +63,11 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_virtual_fw.h>
+#include <uORB/topics/actuator_controls_virtual_mc.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/fw_virtual_rates_setpoint.h>
+#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_status.h>
@@ -615,14 +619,14 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (_v_att_sp.R_valid) {
/* rotation matrix in _att_sp is valid, use it */
- R_sp.set(&_v_att_sp.R_body[0][0]);
+ R_sp.set(&_v_att_sp.R_body[0]);
} else {
/* rotation matrix in _att_sp is not valid, use euler angles instead */
R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
/* copy rotation matrix back to setpoint struct */
- memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
+ memcpy(&_v_att_sp.R_body[0], &R_sp.data[0], sizeof(_v_att_sp.R_body));
_v_att_sp.R_valid = true;
}
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
new file mode 100644
index 000000000..1e40c2b05
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control.cpp
@@ -0,0 +1,301 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control.cpp
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ */
+
+#include "mc_att_control.h"
+#include "mc_att_control_params.h"
+#include "math.h"
+
+#define YAW_DEADZONE 0.05f
+#define MIN_TAKEOFF_THRUST 0.2f
+#define RATES_I_LIMIT 0.3f
+
+namespace mc_att_control
+{
+
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+}
+
+MulticopterAttitudeControl::MulticopterAttitudeControl() :
+ MulticopterAttitudeControlBase(),
+ _task_should_exit(false),
+ _actuators_0_circuit_breaker_enabled(false),
+
+ /* publications */
+ _att_sp_pub(nullptr),
+ _v_rates_sp_pub(nullptr),
+ _actuators_0_pub(nullptr),
+ _n(),
+
+ /* performance counters */
+ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+
+{
+ _params_handles.roll_p = PX4_PARAM_INIT(MC_ROLL_P);
+ _params_handles.roll_rate_p = PX4_PARAM_INIT(MC_ROLLRATE_P);
+ _params_handles.roll_rate_i = PX4_PARAM_INIT(MC_ROLLRATE_I);
+ _params_handles.roll_rate_d = PX4_PARAM_INIT(MC_ROLLRATE_D);
+ _params_handles.pitch_p = PX4_PARAM_INIT(MC_PITCH_P);
+ _params_handles.pitch_rate_p = PX4_PARAM_INIT(MC_PITCHRATE_P);
+ _params_handles.pitch_rate_i = PX4_PARAM_INIT(MC_PITCHRATE_I);
+ _params_handles.pitch_rate_d = PX4_PARAM_INIT(MC_PITCHRATE_D);
+ _params_handles.yaw_p = PX4_PARAM_INIT(MC_YAW_P);
+ _params_handles.yaw_rate_p = PX4_PARAM_INIT(MC_YAWRATE_P);
+ _params_handles.yaw_rate_i = PX4_PARAM_INIT(MC_YAWRATE_I);
+ _params_handles.yaw_rate_d = PX4_PARAM_INIT(MC_YAWRATE_D);
+ _params_handles.yaw_ff = PX4_PARAM_INIT(MC_YAW_FF);
+ _params_handles.yaw_rate_max = PX4_PARAM_INIT(MC_YAWRATE_MAX);
+ _params_handles.man_roll_max = PX4_PARAM_INIT(MC_MAN_R_MAX);
+ _params_handles.man_pitch_max = PX4_PARAM_INIT(MC_MAN_P_MAX);
+ _params_handles.man_yaw_max = PX4_PARAM_INIT(MC_MAN_Y_MAX);
+ _params_handles.acro_roll_max = PX4_PARAM_INIT(MC_ACRO_R_MAX);
+ _params_handles.acro_pitch_max = PX4_PARAM_INIT(MC_ACRO_P_MAX);
+ _params_handles.acro_yaw_max = PX4_PARAM_INIT(MC_ACRO_Y_MAX);
+
+ /* fetch initial parameter values */
+ parameters_update();
+
+ /*
+ * do subscriptions
+ */
+ _v_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterAttitudeControl::handle_vehicle_attitude, this, 0);
+ _v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
+ _v_rates_sp = _n.subscribe<px4_vehicle_rates_setpoint>(0);
+ _v_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
+ _parameter_update = _n.subscribe<px4_parameter_update>(
+ &MulticopterAttitudeControl::handle_parameter_update, this, 1000);
+ _manual_control_sp = _n.subscribe<px4_manual_control_setpoint>(0);
+ _armed = _n.subscribe<px4_actuator_armed>(0);
+ _v_status = _n.subscribe<px4_vehicle_status>(0);
+
+}
+
+MulticopterAttitudeControl::~MulticopterAttitudeControl()
+{
+}
+
+int
+MulticopterAttitudeControl::parameters_update()
+{
+ float v;
+
+ /* roll gains */
+ PX4_PARAM_GET(_params_handles.roll_p, &v);
+ _params.att_p(0) = v;
+ PX4_PARAM_GET(_params_handles.roll_rate_p, &v);
+ _params.rate_p(0) = v;
+ PX4_PARAM_GET(_params_handles.roll_rate_i, &v);
+ _params.rate_i(0) = v;
+ PX4_PARAM_GET(_params_handles.roll_rate_d, &v);
+ _params.rate_d(0) = v;
+
+ /* pitch gains */
+ PX4_PARAM_GET(_params_handles.pitch_p, &v);
+ _params.att_p(1) = v;
+ PX4_PARAM_GET(_params_handles.pitch_rate_p, &v);
+ _params.rate_p(1) = v;
+ PX4_PARAM_GET(_params_handles.pitch_rate_i, &v);
+ _params.rate_i(1) = v;
+ PX4_PARAM_GET(_params_handles.pitch_rate_d, &v);
+ _params.rate_d(1) = v;
+
+ /* yaw gains */
+ PX4_PARAM_GET(_params_handles.yaw_p, &v);
+ _params.att_p(2) = v;
+ PX4_PARAM_GET(_params_handles.yaw_rate_p, &v);
+ _params.rate_p(2) = v;
+ PX4_PARAM_GET(_params_handles.yaw_rate_i, &v);
+ _params.rate_i(2) = v;
+ PX4_PARAM_GET(_params_handles.yaw_rate_d, &v);
+ _params.rate_d(2) = v;
+
+ PX4_PARAM_GET(_params_handles.yaw_ff, &_params.yaw_ff);
+ PX4_PARAM_GET(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
+ _params.yaw_rate_max = math::radians(_params.yaw_rate_max);
+
+ /* manual control scale */
+ PX4_PARAM_GET(_params_handles.man_roll_max, &_params.man_roll_max);
+ PX4_PARAM_GET(_params_handles.man_pitch_max, &_params.man_pitch_max);
+ PX4_PARAM_GET(_params_handles.man_yaw_max, &_params.man_yaw_max);
+ _params.man_roll_max = math::radians(_params.man_roll_max);
+ _params.man_pitch_max = math::radians(_params.man_pitch_max);
+ _params.man_yaw_max = math::radians(_params.man_yaw_max);
+
+ /* acro control scale */
+ PX4_PARAM_GET(_params_handles.acro_roll_max, &v);
+ _params.acro_rate_max(0) = math::radians(v);
+ PX4_PARAM_GET(_params_handles.acro_pitch_max, &v);
+ _params.acro_rate_max(1) = math::radians(v);
+ PX4_PARAM_GET(_params_handles.acro_yaw_max, &v);
+ _params.acro_rate_max(2) = math::radians(v);
+
+ _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY);
+
+ return OK;
+}
+
+void MulticopterAttitudeControl::handle_parameter_update(const px4_parameter_update &msg)
+{
+ parameters_update();
+}
+
+void MulticopterAttitudeControl::handle_vehicle_attitude(const px4_vehicle_attitude &msg) {
+
+ perf_begin(_loop_perf);
+
+ /* run controller on attitude changes */
+ static uint64_t last_run = 0;
+ float dt = (px4::get_time_micros() - last_run) / 1000000.0f;
+ last_run = px4::get_time_micros();
+
+ /* guard against too small (< 2ms) and too large (> 20ms) dt's */
+ if (dt < 0.002f) {
+ dt = 0.002f;
+
+ } else if (dt > 0.02f) {
+ dt = 0.02f;
+ }
+
+ if (_v_control_mode->data().flag_control_attitude_enabled) {
+ control_attitude(dt);
+
+ /* publish the attitude setpoint if needed */
+ if (_publish_att_sp && _v_status->data().is_rotary_wing) {
+ _v_att_sp_mod.data().timestamp = px4::get_time_micros();
+
+ if (_att_sp_pub != nullptr) {
+ _att_sp_pub->publish(_v_att_sp_mod);
+
+ } else {
+ _att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+ }
+ }
+
+ /* publish attitude rates setpoint */
+ _v_rates_sp_mod.data().roll = _rates_sp(0);
+ _v_rates_sp_mod.data().pitch = _rates_sp(1);
+ _v_rates_sp_mod.data().yaw = _rates_sp(2);
+ _v_rates_sp_mod.data().thrust = _thrust_sp;
+ _v_rates_sp_mod.data().timestamp = px4::get_time_micros();
+
+ if (_v_rates_sp_pub != nullptr) {
+ _v_rates_sp_pub->publish(_v_rates_sp_mod);
+ } else {
+ if (_v_status->data().is_vtol) {
+ //XXX add a second publisher?
+ // _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>();
+ } else {
+ _v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>();
+ }
+ }
+
+ } else {
+ /* attitude controller disabled, poll rates setpoint topic */
+ if (_v_control_mode->data().flag_control_manual_enabled) {
+ /* manual rates control - ACRO mode */
+ _rates_sp = math::Vector<3>(_manual_control_sp->data().y, -_manual_control_sp->data().x,
+ _manual_control_sp->data().r).emult(_params.acro_rate_max);
+ _thrust_sp = _manual_control_sp->data().z;
+
+ /* reset yaw setpoint after ACRO */
+ _reset_yaw_sp = true;
+
+ /* publish attitude rates setpoint */
+ _v_rates_sp_mod.data().roll = _rates_sp(0);
+ _v_rates_sp_mod.data().pitch = _rates_sp(1);
+ _v_rates_sp_mod.data().yaw = _rates_sp(2);
+ _v_rates_sp_mod.data().thrust = _thrust_sp;
+ _v_rates_sp_mod.data().timestamp = px4::get_time_micros();
+
+ if (_v_rates_sp_pub != nullptr) {
+ _v_rates_sp_pub->publish(_v_rates_sp_mod);
+
+ } else {
+ if (_v_status->data().is_vtol) {
+ //XXX add a second publisher?
+ // _v_rates_sp_pub = _n.advertise<px4_mc_virtual_rates_setpoint>();
+ } else {
+ _v_rates_sp_pub = _n.advertise<px4_vehicle_rates_setpoint>();
+ }
+ }
+
+ } else {
+ /* attitude controller disabled, poll rates setpoint topic */
+ _rates_sp(0) = _v_rates_sp->data().roll;
+ _rates_sp(1) = _v_rates_sp->data().pitch;
+ _rates_sp(2) = _v_rates_sp->data().yaw;
+ _thrust_sp = _v_rates_sp->data().thrust;
+ }
+ }
+
+ if (_v_control_mode->data().flag_control_rates_enabled) {
+ control_attitude_rates(dt);
+
+ /* publish actuator controls */
+ _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+ _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+ _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+ _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+ _actuators.data().timestamp = px4::get_time_micros();
+
+ if (!_actuators_0_circuit_breaker_enabled) {
+ if (_actuators_0_pub != nullptr) {
+ _actuators_0_pub->publish(_actuators);
+
+ } else {
+ if (_v_status->data().is_vtol) {
+ //XXX add a second publisher?
+ // _actuators_0_pub = _n.advertise<px4_actuator_controls_virtual_mc>();
+ } else {
+ _actuators_0_pub = _n.advertise<px4_actuator_controls_0>();
+ }
+ }
+ }
+ }
+}
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control.h b/src/modules/mc_att_control_multiplatform/mc_att_control.h
new file mode 100644
index 000000000..95d608684
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control.h
@@ -0,0 +1,130 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control.h
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ * The controller has two loops: P loop for angular error and PD loop for angular rate error.
+ * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
+ * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
+ * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
+ * These two approaches fused seamlessly with weight depending on angular error.
+ * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/circuit_breaker.h>
+#include <lib/mathlib/mathlib.h>
+
+#include "mc_att_control_base.h"
+
+class MulticopterAttitudeControl :
+ public MulticopterAttitudeControlBase
+{
+public:
+ /**
+ * Constructor
+ */
+ MulticopterAttitudeControl();
+
+ /**
+ * Destructor, also kills the sensors task.
+ */
+ ~MulticopterAttitudeControl();
+
+ /* Callbacks for topics */
+ void handle_vehicle_attitude(const px4_vehicle_attitude &msg);
+ void handle_parameter_update(const px4_parameter_update &msg);
+
+ void spin() { _n.spin(); }
+
+private:
+ bool _task_should_exit; /**< if true, sensor task should exit */
+ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
+
+ px4::Publisher<px4_vehicle_attitude_setpoint> * _att_sp_pub; /**< attitude setpoint publication */
+ px4::Publisher<px4_vehicle_rates_setpoint> * _v_rates_sp_pub; /**< rate setpoint publication */
+ px4::Publisher<px4_actuator_controls_0> * _actuators_0_pub; /**< attitude actuator controls publication */
+
+ px4::NodeHandle _n;
+
+ struct {
+ px4_param_t roll_p;
+ px4_param_t roll_rate_p;
+ px4_param_t roll_rate_i;
+ px4_param_t roll_rate_d;
+ px4_param_t pitch_p;
+ px4_param_t pitch_rate_p;
+ px4_param_t pitch_rate_i;
+ px4_param_t pitch_rate_d;
+ px4_param_t yaw_p;
+ px4_param_t yaw_rate_p;
+ px4_param_t yaw_rate_i;
+ px4_param_t yaw_rate_d;
+ px4_param_t yaw_ff;
+ px4_param_t yaw_rate_max;
+
+ px4_param_t man_roll_max;
+ px4_param_t man_pitch_max;
+ px4_param_t man_yaw_max;
+ px4_param_t acro_roll_max;
+ px4_param_t acro_pitch_max;
+ px4_param_t acro_yaw_max;
+
+ px4_param_t autostart_id;
+ } _params_handles; /**< handles for interesting parameters */
+
+ perf_counter_t _loop_perf; /**< loop performance counter */
+
+ /**
+ * Update our local parameter cache.
+ */
+ int parameters_update();
+};
+
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
new file mode 100644
index 000000000..0de832df9
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
@@ -0,0 +1,309 @@
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.cpp
+ *
+ * MC Attitude Controller : Control and math code
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "mc_att_control_base.h"
+#include <geo/geo.h>
+#include <math.h>
+#include <lib/mathlib/mathlib.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
+ _publish_att_sp(false)
+
+{
+ memset(&_v_att_sp_mod, 0, sizeof(_v_att_sp_mod));
+ memset(&_v_rates_sp_mod, 0, sizeof(_v_rates_sp_mod));
+ memset(&_actuators, 0, sizeof(_actuators));
+
+ _params.att_p.zero();
+ _params.rate_p.zero();
+ _params.rate_i.zero();
+ _params.rate_d.zero();
+ _params.yaw_ff = 0.0f;
+ _params.yaw_rate_max = 0.0f;
+ _params.man_roll_max = 0.0f;
+ _params.man_pitch_max = 0.0f;
+ _params.man_yaw_max = 0.0f;
+ _params.acro_rate_max.zero();
+
+ _rates_prev.zero();
+ _rates_sp.zero();
+ _rates_int.zero();
+ _thrust_sp = 0.0f;
+ _att_control.zero();
+
+ _I.identity();
+}
+
+MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase()
+{
+}
+
+void MulticopterAttitudeControlBase::control_attitude(float dt)
+{
+ float yaw_sp_move_rate = 0.0f;
+ _publish_att_sp = false;
+
+
+ if (_v_control_mode->data().flag_control_manual_enabled) {
+ /* manual input, set or modify attitude setpoint */
+
+ if (_v_control_mode->data().flag_control_velocity_enabled
+ || _v_control_mode->data().flag_control_climb_rate_enabled) {
+ /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
+ memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));
+ }
+
+ if (!_v_control_mode->data().flag_control_climb_rate_enabled) {
+ /* pass throttle directly if not in altitude stabilized mode */
+ _v_att_sp_mod.data().thrust = _manual_control_sp->data().z;
+ _publish_att_sp = true;
+ }
+
+ if (!_armed->data().armed) {
+ /* reset yaw setpoint when disarmed */
+ _reset_yaw_sp = true;
+ }
+
+ /* move yaw setpoint in all modes */
+ if (_v_att_sp_mod.data().thrust < 0.1f) {
+ // TODO
+ //if (_status.condition_landed) {
+ /* reset yaw setpoint if on ground */
+ // reset_yaw_sp = true;
+ //}
+ } else {
+ /* move yaw setpoint */
+ yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
+ _v_att_sp_mod.data().yaw_body = _wrap_pi(
+ _v_att_sp_mod.data().yaw_body + yaw_sp_move_rate * dt);
+ float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
+ float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);
+
+ if (yaw_offs < -yaw_offs_max) {
+ _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);
+
+ } else if (yaw_offs > yaw_offs_max) {
+ _v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
+ }
+
+ _v_att_sp_mod.data().R_valid = false;
+ // _publish_att_sp = true;
+ }
+
+ /* reset yaw setpint to current position if needed */
+ if (_reset_yaw_sp) {
+ _reset_yaw_sp = false;
+ _v_att_sp_mod.data().yaw_body = _v_att->data().yaw;
+ _v_att_sp_mod.data().R_valid = false;
+ // _publish_att_sp = true;
+ }
+
+ if (!_v_control_mode->data().flag_control_velocity_enabled) {
+ /* update attitude setpoint if not in position control mode */
+ _v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
+ _v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x
+ * _params.man_pitch_max;
+ _v_att_sp_mod.data().R_valid = false;
+ // _publish_att_sp = true;
+ }
+
+ } else {
+ /* in non-manual mode use 'vehicle_attitude_setpoint' topic */
+ memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));
+
+ /* reset yaw setpoint after non-manual control mode */
+ _reset_yaw_sp = true;
+ }
+
+ _thrust_sp = _v_att_sp_mod.data().thrust;
+
+ /* construct attitude setpoint rotation matrix */
+ math::Matrix<3, 3> R_sp;
+
+ if (_v_att_sp_mod.data().R_valid) {
+ /* rotation matrix in _att_sp is valid, use it */
+ R_sp.set(&_v_att_sp_mod.data().R_body[0]);
+
+ } else {
+ /* rotation matrix in _att_sp is not valid, use euler angles instead */
+ R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body,
+ _v_att_sp_mod.data().yaw_body);
+
+ /* copy rotation matrix back to setpoint struct */
+ memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0],
+ sizeof(_v_att_sp_mod.data().R_body));
+ _v_att_sp_mod.data().R_valid = true;
+ }
+
+ /* rotation matrix for current state */
+ math::Matrix<3, 3> R;
+ R.set(_v_att->data().R);
+
+ /* all input data is ready, run controller itself */
+
+ /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
+ math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
+ math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+
+ /* axis and sin(angle) of desired rotation */
+ math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);
+
+ /* calculate angle error */
+ float e_R_z_sin = e_R.length();
+ float e_R_z_cos = R_z * R_sp_z;
+
+ /* calculate weight for yaw control */
+ float yaw_w = R_sp(2, 2) * R_sp(2, 2);
+
+ /* calculate rotation matrix after roll/pitch only rotation */
+ math::Matrix<3, 3> R_rp;
+
+ if (e_R_z_sin > 0.0f) {
+ /* get axis-angle representation */
+ float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
+ math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;
+
+ e_R = e_R_z_axis * e_R_z_angle;
+
+ /* cross product matrix for e_R_axis */
+ math::Matrix<3, 3> e_R_cp;
+ e_R_cp.zero();
+ e_R_cp(0, 1) = -e_R_z_axis(2);
+ e_R_cp(0, 2) = e_R_z_axis(1);
+ e_R_cp(1, 0) = e_R_z_axis(2);
+ e_R_cp(1, 2) = -e_R_z_axis(0);
+ e_R_cp(2, 0) = -e_R_z_axis(1);
+ e_R_cp(2, 1) = e_R_z_axis(0);
+
+ /* rotation matrix for roll/pitch only rotation */
+ R_rp = R
+ * (_I + e_R_cp * e_R_z_sin
+ + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+
+ } else {
+ /* zero roll/pitch rotation */
+ R_rp = R;
+ }
+
+ /* R_rp and R_sp has the same Z axis, calculate yaw error */
+ math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
+ math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+ e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
+
+ if (e_R_z_cos < 0.0f) {
+ /* for large thrust vector rotations use another rotation method:
+ * calculate angle and axis for R -> R_sp rotation directly */
+ math::Quaternion q;
+ q.from_dcm(R.transposed() * R_sp);
+ math::Vector < 3 > e_R_d = q.imag();
+ e_R_d.normalize();
+ e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
+
+ /* use fusion of Z axis based rotation and direct rotation */
+ float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
+ e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
+ }
+
+ /* calculate angular rates setpoint */
+ _rates_sp = _params.att_p.emult(e_R);
+
+ /* limit yaw rate */
+ _rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
+ _params.yaw_rate_max);
+
+ /* feed forward yaw setpoint rate */
+ _rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+}
+
+void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
+{
+ /* reset integral if disarmed */
+ if (!_armed->data().armed || !_v_status->data().is_rotary_wing) {
+ _rates_int.zero();
+ }
+
+ /* current body angular rates */
+ math::Vector < 3 > rates;
+ rates(0) = _v_att->data().rollspeed;
+ rates(1) = _v_att->data().pitchspeed;
+ rates(2) = _v_att->data().yawspeed;
+
+ /* angular rates error */
+ math::Vector < 3 > rates_err = _rates_sp - rates;
+ _att_control = _params.rate_p.emult(rates_err)
+ + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+ _rates_prev = rates;
+
+ /* update integral only if not saturated on low limit */
+ if (_thrust_sp > MIN_TAKEOFF_THRUST) {
+ for (int i = 0; i < 3; i++) {
+ if (fabsf(_att_control(i)) < _thrust_sp) {
+ float rate_i = _rates_int(i)
+ + _params.rate_i(i) * rates_err(i) * dt;
+
+ if (isfinite(
+ rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+ _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+ _rates_int(i) = rate_i;
+ }
+ }
+ }
+ }
+
+}
+
+void MulticopterAttitudeControlBase::set_actuator_controls()
+{
+ _actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+ _actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+ _actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+ _actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+}
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.h b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
new file mode 100644
index 000000000..124147079
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.h
@@ -0,0 +1,136 @@
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_base.h
+ *
+ * MC Attitude Controller : Control and math code
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+#include <px4.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+
+#include <systemlib/perf_counter.h>
+#include <lib/mathlib/mathlib.h>
+
+
+
+#define YAW_DEADZONE 0.05f
+#define MIN_TAKEOFF_THRUST 0.2f
+#define RATES_I_LIMIT 0.3f
+
+using namespace px4;
+
+class MulticopterAttitudeControlBase
+{
+public:
+ /**
+ * Constructor
+ */
+ MulticopterAttitudeControlBase();
+
+ /**
+ * Destructor
+ */
+ ~MulticopterAttitudeControlBase();
+
+ /**
+ * Start the sensors task.
+ *
+ * @return OK on success.
+ */
+ void control_attitude(float dt);
+ void control_attitude_rates(float dt);
+
+ void set_actuator_controls();
+
+protected:
+ px4::Subscriber<px4_vehicle_attitude> *_v_att; /**< vehicle attitude */
+ px4::Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp; /**< vehicle attitude setpoint */
+ px4::Subscriber<px4_vehicle_rates_setpoint> *_v_rates_sp; /**< vehicle rates setpoint */
+ px4::Subscriber<px4_vehicle_control_mode> *_v_control_mode; /**< vehicle control mode */
+ px4::Subscriber<px4_parameter_update> *_parameter_update; /**< parameter update */
+ px4::Subscriber<px4_manual_control_setpoint> *_manual_control_sp; /**< manual control setpoint */
+ px4::Subscriber<px4_actuator_armed> *_armed; /**< actuator arming status */
+ px4::Subscriber<px4_vehicle_status> *_v_status; /**< vehicle status */
+
+ px4_vehicle_attitude_setpoint _v_att_sp_mod; /**< modified vehicle attitude setpoint
+ that gets published eventually */
+ px4_vehicle_rates_setpoint _v_rates_sp_mod; /**< vehicle rates setpoint
+ that gets published eventually*/
+ px4_actuator_controls_0 _actuators; /**< actuator controls */
+
+ math::Vector<3> _rates_prev; /**< angular rates on previous step */
+ math::Vector<3> _rates_sp; /**< angular rates setpoint */
+ math::Vector<3> _rates_int; /**< angular rates integral error */
+ float _thrust_sp; /**< thrust setpoint */
+ math::Vector<3> _att_control; /**< attitude control vector */
+
+ math::Matrix<3, 3> _I; /**< identity matrix */
+
+ bool _reset_yaw_sp; /**< reset yaw setpoint flag */
+
+ struct {
+ math::Vector<3> att_p; /**< P gain for angular error */
+ math::Vector<3> rate_p; /**< P gain for angular rate error */
+ math::Vector<3> rate_i; /**< I gain for angular rate error */
+ math::Vector<3> rate_d; /**< D gain for angular rate error */
+ float yaw_ff; /**< yaw control feed-forward */
+ float yaw_rate_max; /**< max yaw rate */
+
+ float man_roll_max;
+ float man_pitch_max;
+ float man_yaw_max;
+ math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
+
+ int32_t autostart_id;
+ } _params;
+
+ bool _publish_att_sp;
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
index 2fde5d424..5a79a8c6b 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_main.cpp
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,68 +32,42 @@
****************************************************************************/
/**
- * @file rc_channels.h
- * Definition of the rc_channels uORB topic.
+ * @file mc_att_control_main.cpp
+ * Multicopter attitude controller.
*
- * @deprecated DO NOT USE FOR NEW CODE
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Julian Oes <julian@oes.ch>
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ * The controller has two loops: P loop for angular error and PD loop for angular rate error.
+ * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
+ * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
+ * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
+ * These two approaches fused seamlessly with weight depending on angular error.
+ * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
*/
-#ifndef RC_CHANNELS_H_
-#define RC_CHANNELS_H_
+#include "mc_att_control.h"
-#include <stdint.h>
-#include "../uORB.h"
+bool thread_running = false; /**< Deamon status flag */
-/**
- * This defines the mapping of the RC functions.
- * The value assigned to the specific function corresponds to the entry of
- * the channel array channels[].
- */
-enum RC_CHANNELS_FUNCTION {
- THROTTLE = 0,
- ROLL,
- PITCH,
- YAW,
- MODE,
- RETURN,
- POSCTL,
- LOITER,
- OFFBOARD,
- ACRO,
- FLAPS,
- AUX_1,
- AUX_2,
- AUX_3,
- AUX_4,
- AUX_5,
- PARAM_1,
- PARAM_2,
- PARAM_3
-};
+int main(int argc, char **argv)
+{
+ px4::init(argc, argv, "mc_att_control_m");
-// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
+ PX4_INFO("starting");
+ MulticopterAttitudeControl attctl;
+ thread_running = true;
+ attctl.spin();
-#define RC_CHANNELS_FUNCTION_MAX 19
-
-/**
- * @addtogroup topics
- * @{
- */
-struct rc_channels_s {
- uint64_t timestamp; /**< Timestamp in microseconds since boot time */
- uint64_t timestamp_last_valid; /**< Timestamp of last valid RC signal */
- float channels[RC_CHANNELS_FUNCTION_MAX]; /**< Scaled to -1..1 (throttle: 0..1) */
- uint8_t channel_count; /**< Number of valid channels */
- int8_t function[RC_CHANNELS_FUNCTION_MAX]; /**< Functions mapping */
- uint8_t rssi; /**< Receive signal strength index */
- bool signal_lost; /**< Control signal lost, should be checked together with topic timeout */
-}; /**< radio control channels. */
-
-/**
- * @}
- */
+ PX4_INFO("exiting.");
+ thread_running = false;
+ return 0;
+}
-/* register this as object request broker structure */
-ORB_DECLARE(rc_channels);
-#endif
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.c b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
new file mode 100644
index 000000000..a0b1706f2
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.c
@@ -0,0 +1,248 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_params.c
+ * Parameters for multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <px4_defines.h>
+#include "mc_att_control_params.h"
+#include <systemlib/param/param.h>
+
+/**
+ * Roll P gain
+ *
+ * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLL_P);
+
+/**
+ * Roll rate P gain
+ *
+ * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_P);
+
+/**
+ * Roll rate I gain
+ *
+ * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_I);
+
+/**
+ * Roll rate D gain
+ *
+ * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ROLLRATE_D);
+
+/**
+ * Pitch P gain
+ *
+ * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCH_P);
+
+/**
+ * Pitch rate P gain
+ *
+ * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_P);
+
+/**
+ * Pitch rate I gain
+ *
+ * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_I);
+
+/**
+ * Pitch rate D gain
+ *
+ * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_PITCHRATE_D);
+
+/**
+ * Yaw P gain
+ *
+ * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad.
+ *
+ * @unit 1/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAW_P);
+
+/**
+ * Yaw rate P gain
+ *
+ * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_P);
+
+/**
+ * Yaw rate I gain
+ *
+ * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_I);
+
+/**
+ * Yaw rate D gain
+ *
+ * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again.
+ *
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_D);
+
+/**
+ * Yaw feed forward
+ *
+ * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAW_FF);
+
+/**
+ * Max yaw rate
+ *
+ * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX);
+
+/**
+ * Max manual roll
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_MAN_R_MAX);
+
+/**
+ * Max manual pitch
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_MAN_P_MAX);
+
+/**
+ * Max manual yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX);
+
+/**
+ * Max acro roll rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX);
+
+/**
+ * Max acro pitch rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX);
+
+/**
+ * Max acro yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PX4_PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX);
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_params.h b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
new file mode 100644
index 000000000..59dd5240f
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_params.h
@@ -0,0 +1,65 @@
+
+/****************************************************************************
+ *
+ * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_params.h
+ * Parameters for multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#pragma once
+
+#define PARAM_MC_ROLL_P_DEFAULT 6.0f
+#define PARAM_MC_ROLLRATE_P_DEFAULT 0.1f
+#define PARAM_MC_ROLLRATE_I_DEFAULT 0.0f
+#define PARAM_MC_ROLLRATE_D_DEFAULT 0.002f
+#define PARAM_MC_PITCH_P_DEFAULT 6.0f
+#define PARAM_MC_PITCHRATE_P_DEFAULT 0.1f
+#define PARAM_MC_PITCHRATE_I_DEFAULT 0.0f
+#define PARAM_MC_PITCHRATE_D_DEFAULT 0.002f
+#define PARAM_MC_YAW_P_DEFAULT 2.0f
+#define PARAM_MC_YAWRATE_P_DEFAULT 0.3f
+#define PARAM_MC_YAWRATE_I_DEFAULT 0.0f
+#define PARAM_MC_YAWRATE_D_DEFAULT 0.0f
+#define PARAM_MC_YAW_FF_DEFAULT 0.5f
+#define PARAM_MC_YAWRATE_MAX_DEFAULT 120.0f
+#define PARAM_MC_MAN_R_MAX_DEFAULT 35.0f
+#define PARAM_MC_MAN_P_MAX_DEFAULT 35.0f
+#define PARAM_MC_MAN_Y_MAX_DEFAULT 120.0f
+#define PARAM_MC_ACRO_R_MAX_DEFAULT 35.0f
+#define PARAM_MC_ACRO_P_MAX_DEFAULT 35.0f
+#define PARAM_MC_ACRO_Y_MAX_DEFAULT 120.0f
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp
new file mode 100644
index 000000000..d516d7e52
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.cpp
@@ -0,0 +1,142 @@
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_sim.cpp
+ *
+ * MC Attitude Controller Interface for usage in a simulator
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include "mc_att_control_sim.h"
+#include <geo/geo.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+MulticopterAttitudeControlSim::MulticopterAttitudeControlSim()
+{
+ /* setup standard gains */
+ //XXX: make these configurable
+ _params.att_p(0) = 5.0;
+ _params.rate_p(0) = 0.05;
+ _params.rate_i(0) = 0.0;
+ _params.rate_d(0) = 0.003;
+ /* pitch gains */
+ _params.att_p(1) = 5.0;
+ _params.rate_p(1) = 0.05;
+ _params.rate_i(1) = 0.0;
+ _params.rate_d(1) = 0.003;
+ /* yaw gains */
+ _params.att_p(2) = 2.8;
+ _params.rate_p(2) = 0.2;
+ _params.rate_i(2) = 0.1;
+ _params.rate_d(2) = 0.0;
+ _params.yaw_rate_max = 0.5;
+ _params.yaw_ff = 0.5;
+ _params.man_roll_max = 0.6;
+ _params.man_pitch_max = 0.6;
+ _params.man_yaw_max = 0.6;
+}
+
+MulticopterAttitudeControlSim::~MulticopterAttitudeControlSim()
+{
+}
+
+void MulticopterAttitudeControlSim::set_attitude(const Eigen::Quaternion<double> attitude)
+{
+ math::Quaternion quat;
+ quat(0) = (float)attitude.w();
+ quat(1) = (float)attitude.x();
+ quat(2) = (float)attitude.y();
+ quat(3) = (float)attitude.z();
+
+ _v_att.q[0] = quat(0);
+ _v_att.q[1] = quat(1);
+ _v_att.q[2] = quat(2);
+ _v_att.q[3] = quat(3);
+
+ math::Matrix<3, 3> Rot = quat.to_dcm();
+ _v_att.R[0][0] = Rot(0, 0);
+ _v_att.R[1][0] = Rot(1, 0);
+ _v_att.R[2][0] = Rot(2, 0);
+ _v_att.R[0][1] = Rot(0, 1);
+ _v_att.R[1][1] = Rot(1, 1);
+ _v_att.R[2][1] = Rot(2, 1);
+ _v_att.R[0][2] = Rot(0, 2);
+ _v_att.R[1][2] = Rot(1, 2);
+ _v_att.R[2][2] = Rot(2, 2);
+
+ _v_att.R_valid = true;
+}
+
+void MulticopterAttitudeControlSim::set_attitude_rates(const Eigen::Vector3d &angular_rate)
+{
+ // check if this is consistent !!!
+ _v_att.rollspeed = angular_rate(0);
+ _v_att.pitchspeed = angular_rate(1);
+ _v_att.yawspeed = angular_rate(2);
+}
+
+void MulticopterAttitudeControlSim::set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference)
+{
+ _v_att_sp.roll_body = control_attitude_thrust_reference(0);
+ _v_att_sp.pitch_body = control_attitude_thrust_reference(1);
+ _v_att_sp.yaw_body = control_attitude_thrust_reference(2);
+ _v_att_sp.thrust = (control_attitude_thrust_reference(3) - 30) * (-1) / 30;
+
+ // setup rotation matrix
+ math::Matrix<3, 3> Rot_sp;
+ Rot_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+ _v_att_sp.R_body[0][0] = Rot_sp(0, 0);
+ _v_att_sp.R_body[1][0] = Rot_sp(1, 0);
+ _v_att_sp.R_body[2][0] = Rot_sp(2, 0);
+ _v_att_sp.R_body[0][1] = Rot_sp(0, 1);
+ _v_att_sp.R_body[1][1] = Rot_sp(1, 1);
+ _v_att_sp.R_body[2][1] = Rot_sp(2, 1);
+ _v_att_sp.R_body[0][2] = Rot_sp(0, 2);
+ _v_att_sp.R_body[1][2] = Rot_sp(1, 2);
+ _v_att_sp.R_body[2][2] = Rot_sp(2, 2);
+}
+
+void MulticopterAttitudeControlSim::get_mixer_input(Eigen::Vector4d &motor_inputs)
+{
+ motor_inputs(0) = _actuators.control[0];
+ motor_inputs(1) = _actuators.control[1];
+ motor_inputs(2) = _actuators.control[2];
+ motor_inputs(3) = _actuators.control[3];
+}
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h
new file mode 100644
index 000000000..a1bf44fc9
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_sim.h
@@ -0,0 +1,97 @@
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* Copyright (c) 2014 PX4 Development Team. All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* 1. Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright
+* notice, this list of conditions and the following disclaimer in
+* the documentation and/or other materials provided with the
+* distribution.
+* 3. Neither the name PX4 nor the names of its contributors may be
+* used to endorse or promote products derived from this software
+* without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+****************************************************************************/
+
+/**
+ * @file mc_att_control_sim.h
+ *
+ * MC Attitude Controller Interface for usage in a simulator
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <lib/mathlib/mathlib.h>
+#inlcude "mc_att_control_base.h"
+
+
+
+#define YAW_DEADZONE 0.05f
+#define MIN_TAKEOFF_THRUST 0.2f
+#define RATES_I_LIMIT 0.3f
+
+class MulticopterAttitudeControlSim :
+ public MulticopterAttitudeControlBase
+
+{
+public:
+ /**
+ * Constructor
+ */
+ MulticopterAttitudeControlSim();
+
+ /**
+ * Destructor
+ */
+ ~MulticopterAttitudeControlSim();
+
+ /* setters and getters for interface with euroc-gazebo simulator */
+ void set_attitude(const Eigen::Quaternion<double> attitude);
+ void set_attitude_rates(const Eigen::Vector3d &angular_rate);
+ void set_attitude_reference(const Eigen::Vector4d &control_attitude_thrust_reference);
+ void get_mixer_input(Eigen::Vector4d &motor_inputs);
+
+protected:
+ void vehicle_attitude_setpoint_poll() {};
+
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */
diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
new file mode 100644
index 000000000..e982ae354
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_start_nuttx.cpp
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_att_control_m_start_nuttx.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <cstdlib>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+extern bool thread_running;
+int daemon_task; /**< Handle of deamon task / thread */
+namespace px4
+{
+bool task_should_exit = false;
+}
+using namespace px4;
+
+extern int main(int argc, char **argv);
+
+extern "C" __EXPORT int mc_att_control_m_main(int argc, char *argv[]);
+int mc_att_control_m_main(int argc, char *argv[])
+{
+ if (argc < 1) {
+ errx(1, "usage: mc_att_control_m {start|stop|status}");
+ }
+
+ if (!strcmp(argv[1], "start")) {
+
+ if (thread_running) {
+ warnx("already running");
+ /* this is not an error */
+ exit(0);
+ }
+
+ task_should_exit = false;
+
+ daemon_task = task_spawn_cmd("mc_att_control_m",
+ SCHED_DEFAULT,
+ SCHED_PRIORITY_MAX - 5,
+ 2000,
+ main,
+ (argv) ? (char* const*)&argv[2] : (char* const*)NULL);
+
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "stop")) {
+ task_should_exit = true;
+ exit(0);
+ }
+
+ if (!strcmp(argv[1], "status")) {
+ if (thread_running) {
+ warnx("is running");
+
+ } else {
+ warnx("not started");
+ }
+
+ exit(0);
+ }
+
+ warnx("unrecognized command");
+ return 1;
+}
diff --git a/src/modules/mc_att_control_multiplatform/module.mk b/src/modules/mc_att_control_multiplatform/module.mk
new file mode 100644
index 000000000..959f6492b
--- /dev/null
+++ b/src/modules/mc_att_control_multiplatform/module.mk
@@ -0,0 +1,44 @@
+############################################################################
+#
+# Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Multirotor attitude controller (vector based, no Euler singularities)
+#
+
+MODULE_COMMAND = mc_att_control_m
+
+SRCS = mc_att_control_main.cpp \
+ mc_att_control_start_nuttx.cpp \
+ mc_att_control.cpp \
+ mc_att_control_base.cpp \
+ mc_att_control_params.c
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 3b631e2ce..bf65d2805 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -43,8 +43,9 @@
* @author Anton Babushkin <anton.babushkin@me.com>
*/
-#include <nuttx/config.h>
-#include <stdio.h>
+#include <px4.h>
+#include <functional>
+#include <cstdio>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
@@ -54,8 +55,7 @@
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
-#include <uORB/uORB.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
+
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@@ -67,12 +67,13 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/vehicle_global_velocity_setpoint.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
-#include <systemlib/param/param.h>
-#include <systemlib/err.h>
+// #include <systemlib/param/param.h>
+// #include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <mavlink/mavlink_log.h>
+#include <platforms/px4_defines.h>
#define TILT_COS_MAX 0.7f
#define SIGMA 0.000001f
@@ -531,9 +532,9 @@ MulticopterPositionControl::reset_pos_sp()
if (_reset_pos_sp) {
_reset_pos_sp = false;
/* shift position setpoint to make attitude setpoint continuous */
- _pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
+ _pos_sp(0) = _pos(0) + (_vel(0) - PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust / _params.vel_p(0)
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
- _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
+ _pos_sp(1) = _pos(1) + (_vel(1) - PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
}
@@ -987,7 +988,7 @@ MulticopterPositionControl::task_main()
if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == SETPOINT_TYPE_IDLE) {
/* idle state, don't run controller and set zero thrust */
R.identity();
- memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+ memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
@@ -1148,11 +1149,11 @@ MulticopterPositionControl::task_main()
/* thrust compensation for altitude only control mode */
float att_comp;
- if (_att.R[2][2] > TILT_COS_MAX) {
- att_comp = 1.0f / _att.R[2][2];
+ if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
+ att_comp = 1.0f / PX4_R(_att.R, 2, 2);
- } else if (_att.R[2][2] > 0.0f) {
- att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f;
+ } else if (PX4_R(_att.R, 2, 2) > 0.0f) {
+ att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
saturation_z = true;
} else {
@@ -1260,7 +1261,7 @@ MulticopterPositionControl::task_main()
}
/* copy rotation matrix to attitude setpoint topic */
- memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+ memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
/* calculate euler angles, for logging only, must not be used for control */
@@ -1275,7 +1276,7 @@ MulticopterPositionControl::task_main()
R.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
/* copy rotation matrix to attitude setpoint topic */
- memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body));
+ memcpy(&_att_sp.R_body[0], R.data, sizeof(_att_sp.R_body));
_att_sp.R_valid = true;
_att_sp.roll_body = 0.0f;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 39294e3c0..6de283396 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -53,6 +53,10 @@
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -68,6 +72,7 @@
#include <geo/geo.h>
#include <systemlib/systemlib.h>
#include <drivers/drv_hrt.h>
+#include <platforms/px4_defines.h>
#include "position_estimator_inav_params.h"
#include "inertial_filter.h"
@@ -458,7 +463,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
acc[i] = 0.0f;
for (int j = 0; j < 3; j++) {
- acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
+ acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
}
}
@@ -491,7 +496,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if ((flow.ground_distance_m > 0.31f) &&
(flow.ground_distance_m < 4.0f) &&
- (att.R[2][2] > 0.7f) &&
+ (PX4_R(att.R, 2, 2) > 0.7f) &&
(fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) {
sonar_time = t;
@@ -528,15 +533,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q = flow.quality / 255.0f;
float dist_bottom = - z_est[0] - surface_offset;
- if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) {
+ if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) {
/* distance to surface */
- float flow_dist = dist_bottom / att.R[2][2];
+ float flow_dist = dist_bottom / PX4_R(att.R, 2, 2);
/* check if flow if too large for accurate measurements */
/* calculate estimated velocity in body frame */
float body_v_est[2] = { 0.0f, 0.0f };
for (int i = 0; i < 2; i++) {
- body_v_est[i] = att.R[0][i] * x_est[1] + att.R[1][i] * y_est[1] + att.R[2][i] * z_est[1];
+ body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
}
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
@@ -559,7 +564,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
- flow_v[i] += att.R[i][j] * flow_m[j];
+ flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
}
}
@@ -568,7 +573,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_flow[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
- w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
+ w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
@@ -938,7 +943,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float c = 0.0f;
for (int j = 0; j < 3; j++) {
- c += att.R[j][i] * accel_bias_corr[j];
+ c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (isfinite(c)) {
@@ -963,7 +968,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float c = 0.0f;
for (int j = 0; j < 3; j++) {
- c += att.R[j][i] * accel_bias_corr[j];
+ c += PX4_R(att.R, j, i) * accel_bias_corr[j];
}
if (isfinite(c)) {
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 7fd7e6b47..b0127d02c 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -71,6 +71,10 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_local_position.h>
@@ -1123,7 +1127,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.system_power_sub = orb_subscribe(ORB_ID(system_power));
subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status));
subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate));
-
+
/* we need to rate-limit wind, as we do not need the full update rate */
orb_set_interval(subs.wind_sub, 90);
subs.encoders_sub = orb_subscribe(ORB_ID(encoders));
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 793b7c2b6..6aa6b6bbd 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -184,13 +184,13 @@ private:
/**
* Get and limit value for specified RC function. Returns NAN if not mapped.
*/
- float get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value);
+ float get_rc_value(uint8_t func, float min_value, float max_value);
/**
* Get switch position for specified function.
*/
- switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv);
- switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv);
+ switch_pos_t get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv);
+ switch_pos_t get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv);
/**
* Update paramters from RC channels if the functionality is activated and the
@@ -813,28 +813,28 @@ Sensors::parameters_update()
_parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
/* update RC function mappings */
- _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
- _rc.function[ROLL] = _parameters.rc_map_roll - 1;
- _rc.function[PITCH] = _parameters.rc_map_pitch - 1;
- _rc.function[YAW] = _parameters.rc_map_yaw - 1;
-
- _rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
- _rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
- _rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
- _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
- _rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
- _rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
-
- _rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
-
- _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
- _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
- _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
- _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1;
- _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_ROLL] = _parameters.rc_map_roll - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_PITCH] = _parameters.rc_map_pitch - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_YAW] = _parameters.rc_map_yaw - 1;
+
+ _rc.function[RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
+
+ _rc.function[RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
+
+ _rc.function[RC_CHANNELS_FUNCTION_AUX_1] = _parameters.rc_map_aux1 - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_AUX_2] = _parameters.rc_map_aux2 - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_AUX_3] = _parameters.rc_map_aux3 - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_AUX_4] = _parameters.rc_map_aux4 - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_AUX_5] = _parameters.rc_map_aux5 - 1;
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
- _rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
+ _rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] = _parameters.rc_map_param[i] - 1;
}
/* gyro offsets */
@@ -891,8 +891,8 @@ Sensors::parameters_update()
return ERROR;
}
close(flowfd);
- }
-
+ }
+
get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
@@ -1498,7 +1498,7 @@ Sensors::rc_parameter_map_poll(bool forced)
orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
/* update paramter handles to which the RC channels are mapped */
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
- if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+ if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
*/
@@ -1646,7 +1646,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
float
-Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max_value)
+Sensors::get_rc_value(uint8_t func, float min_value, float max_value)
{
if (_rc.function[func] >= 0) {
float value = _rc.channels[_rc.function[func]];
@@ -1667,7 +1667,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
}
switch_pos_t
-Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
+Sensors::get_rc_sw3pos_position(uint8_t func, float on_th, bool on_inv, float mid_th, bool mid_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
@@ -1688,7 +1688,7 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo
}
switch_pos_t
-Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
+Sensors::get_rc_sw2pos_position(uint8_t func, float on_th, bool on_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.channels[_rc.function[func]] + 0.5f;
@@ -1710,14 +1710,14 @@ Sensors::set_params_from_rc()
{
static float param_rc_values[RC_PARAM_MAP_NCHAN] = {};
for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) {
- if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
+ if (_rc.function[RC_CHANNELS_FUNCTION_PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) {
/* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0)
* or no request to map this channel to a param has been sent via mavlink
*/
continue;
}
- float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0);
+ float rc_val = get_rc_value((RC_CHANNELS_FUNCTION_PARAM_1 + i), -1.0, 1.0);
/* Check if the value has changed,
* maybe we need to introduce a more aggressive limit here */
if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) {
@@ -1847,24 +1847,24 @@ Sensors::rc_poll()
manual.timestamp = rc_input.timestamp_last_signal;
/* limit controls */
- manual.y = get_rc_value(ROLL, -1.0, 1.0);
- manual.x = get_rc_value(PITCH, -1.0, 1.0);
- manual.r = get_rc_value(YAW, -1.0, 1.0);
- manual.z = get_rc_value(THROTTLE, 0.0, 1.0);
- manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
- manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
- manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
- manual.aux3 = get_rc_value(AUX_3, -1.0, 1.0);
- manual.aux4 = get_rc_value(AUX_4, -1.0, 1.0);
- manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
+ manual.y = get_rc_value(RC_CHANNELS_FUNCTION_ROLL, -1.0, 1.0);
+ manual.x = get_rc_value(RC_CHANNELS_FUNCTION_PITCH, -1.0, 1.0);
+ manual.r = get_rc_value(RC_CHANNELS_FUNCTION_YAW, -1.0, 1.0);
+ manual.z = get_rc_value(RC_CHANNELS_FUNCTION_THROTTLE, 0.0, 1.0);
+ manual.flaps = get_rc_value(RC_CHANNELS_FUNCTION_FLAPS, -1.0, 1.0);
+ manual.aux1 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_1, -1.0, 1.0);
+ manual.aux2 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_2, -1.0, 1.0);
+ manual.aux3 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_3, -1.0, 1.0);
+ manual.aux4 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_4, -1.0, 1.0);
+ manual.aux5 = get_rc_value(RC_CHANNELS_FUNCTION_AUX_5, -1.0, 1.0);
/* mode switches */
- manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
- manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
- manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
- manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
- manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
- manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
+ manual.mode_switch = get_rc_sw3pos_position(RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
+ manual.posctl_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
+ manual.return_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
+ manual.loiter_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
+ manual.acro_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
+ manual.offboard_switch = get_rc_sw2pos_position(RC_CHANNELS_FUNCTION_OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
/* publish manual_control_setpoint topic */
if (_manual_control_pub > 0) {
diff --git a/src/modules/systemlib/circuit_breaker.cpp b/src/modules/systemlib/circuit_breaker.cpp
new file mode 100644
index 000000000..ea478a60f
--- /dev/null
+++ b/src/modules/systemlib/circuit_breaker.cpp
@@ -0,0 +1,55 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file circuit_breaker.c
+ *
+ * Circuit breaker parameters.
+ * Analog to real aviation circuit breakers these parameters
+ * allow to disable subsystems. They are not supported as standard
+ * operation procedure and are only provided for development purposes.
+ * To ensure they are not activated accidentally, the associated
+ * parameter needs to set to the key (magic).
+ */
+
+#include <px4.h>
+#include <systemlib/circuit_breaker.h>
+
+bool circuit_breaker_enabled(const char* breaker, int32_t magic)
+{
+ int32_t val;
+ (void)PX4_PARAM_GET_BYNAME(breaker, &val);
+
+ return (val == magic);
+}
+
diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h
index b3431722f..c97dbc26f 100644
--- a/src/modules/systemlib/circuit_breaker.h
+++ b/src/modules/systemlib/circuit_breaker.h
@@ -61,7 +61,7 @@
__BEGIN_DECLS
-__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
+extern "C" __EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic);
__END_DECLS
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker_params.c
index 12187d70e..e499ae27a 100644
--- a/src/modules/systemlib/circuit_breaker.c
+++ b/src/modules/systemlib/circuit_breaker_params.c
@@ -42,8 +42,8 @@
* parameter needs to set to the key (magic).
*/
-#include <systemlib/param/param.h>
-#include <systemlib/circuit_breaker.h>
+#include <px4.h>
+#include <systemlib/circuit_breaker_params.h>
/**
* Circuit breaker for power supply check
@@ -56,7 +56,7 @@
* @max 894281
* @group Circuit Breaker
*/
-PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK);
/**
* Circuit breaker for rate controller output
@@ -69,7 +69,7 @@ PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0);
* @max 140253
* @group Circuit Breaker
*/
-PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_RATE_CTRL);
/**
* Circuit breaker for IO safety
@@ -81,7 +81,7 @@ PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0);
* @max 22027
* @group Circuit Breaker
*/
-PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_IO_SAFETY);
/**
* Circuit breaker for airspeed sensor
@@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0);
* @max 162128
* @group Circuit Breaker
*/
-PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
+PX4_PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK);
/**
* Circuit breaker for flight termination
@@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
* @max 121212
* @group Circuit Breaker
*/
-PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
+PX4_PARAM_DEFINE_INT32(CBRK_FLIGHTTERM);
/**
* Circuit breaker for engine failure detection
@@ -120,27 +120,4 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212);
* @max 284953
* @group Circuit Breaker
*/
-PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953);
-
-/**
- * Circuit breaker for gps failure detection
- *
- * Setting this parameter to 240024 will disable the gps failure detection.
- * If the aircraft is in gps failure mode the gps failure flag will be
- * set to healthy
- * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
- *
- * @min 0
- * @max 240024
- * @group Circuit Breaker
- */
-PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024);
-
-bool circuit_breaker_enabled(const char* breaker, int32_t magic)
-{
- int32_t val;
- (void)param_get(param_find(breaker), &val);
-
- return (val == magic);
-}
-
+PX4_PARAM_DEFINE_INT32(CBRK_ENGINEFAIL);
diff --git a/src/modules/systemlib/circuit_breaker_params.h b/src/modules/systemlib/circuit_breaker_params.h
new file mode 100644
index 000000000..768bf7f53
--- /dev/null
+++ b/src/modules/systemlib/circuit_breaker_params.h
@@ -0,0 +1,7 @@
+#define PARAM_CBRK_SUPPLY_CHK_DEFAULT 0
+#define PARAM_CBRK_RATE_CTRL_DEFAULT 0
+#define PARAM_CBRK_IO_SAFETY_DEFAULT 0
+#define PARAM_CBRK_AIRSPD_CHK_DEFAULT 0
+#define PARAM_CBRK_FLIGHTTERM_DEFAULT 121212
+#define PARAM_CBRK_ENGINEFAIL_DEFAULT 284953
+#define PARAM_CBRK_GPSFAIL_DEFAULT 240024
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index f4dff2838..f2499bbb1 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -53,7 +53,8 @@ SRCS = err.c \
otp.c \
board_serial.c \
pwm_limit/pwm_limit.c \
- circuit_breaker.c \
+ circuit_breaker.cpp \
+ circuit_breaker_params.c \
mcu_version.c
MAXOPTIMIZATION = -Os
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index 0c1243de3..8543ba7bb 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -40,6 +40,7 @@
#define _SYSTEMLIB_PERF_COUNTER_H value
#include <stdint.h>
+#include <platforms/px4_defines.h>
/**
* Counter types.
diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp
index 71757e1f4..41a866968 100644
--- a/src/modules/uORB/Publication.cpp
+++ b/src/modules/uORB/Publication.cpp
@@ -49,15 +49,16 @@
#include "topics/actuator_direct.h"
#include "topics/encoders.h"
#include "topics/tecs_status.h"
+#include "topics/rc_channels.h"
namespace uORB {
template<class T>
Publication<T>::Publication(
- List<PublicationBase *> * list,
- const struct orb_metadata *meta) :
+ const struct orb_metadata *meta,
+ List<PublicationNode *> * list) :
T(), // initialize data structure to zero
- PublicationBase(list, meta) {
+ PublicationNode(meta, list) {
}
template<class T>
@@ -80,5 +81,6 @@ template class __EXPORT Publication<actuator_outputs_s>;
template class __EXPORT Publication<actuator_direct_s>;
template class __EXPORT Publication<encoders_s>;
template class __EXPORT Publication<tecs_status_s>;
+template class __EXPORT Publication<rc_channels_s>;
}
diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp
index 8650b3df8..fd1ee4dec 100644
--- a/src/modules/uORB/Publication.hpp
+++ b/src/modules/uORB/Publication.hpp
@@ -38,6 +38,8 @@
#pragma once
+#include <assert.h>
+
#include <uORB/uORB.h>
#include <containers/List.hpp>
@@ -49,55 +51,112 @@ namespace uORB
* Base publication warapper class, used in list traversal
* of various publications.
*/
-class __EXPORT PublicationBase : public ListNode<uORB::PublicationBase *>
+class __EXPORT PublicationBase
{
public:
- PublicationBase(
- List<PublicationBase *> * list,
- const struct orb_metadata *meta) :
+ /**
+ * Constructor
+ *
+ *
+ * @param meta The uORB metadata (usually from the ORB_ID()
+ * macro) for the topic.
+ */
+ PublicationBase(const struct orb_metadata *meta) :
_meta(meta),
_handle(-1) {
- if (list != NULL) list->add(this);
}
- void update() {
+
+ /**
+ * Update the struct
+ * @param data The uORB message struct we are updating.
+ */
+ void update(void * data) {
if (_handle > 0) {
- orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+ orb_publish(getMeta(), getHandle(), data);
} else {
- setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
+ setHandle(orb_advertise(getMeta(), data));
}
}
- virtual void *getDataVoidPtr() = 0;
+
+ /**
+ * Deconstructor
+ */
virtual ~PublicationBase() {
orb_unsubscribe(getHandle());
}
+// accessors
const struct orb_metadata *getMeta() { return _meta; }
int getHandle() { return _handle; }
protected:
+// accessors
void setHandle(orb_advert_t handle) { _handle = handle; }
+// attributes
const struct orb_metadata *_meta;
orb_advert_t _handle;
};
/**
+ * alias class name so it is clear that the base class
+ * can be used by itself if desired
+ */
+typedef PublicationBase PublicationTiny;
+
+/**
+ * The publication base class as a list node.
+ */
+class __EXPORT PublicationNode :
+ public PublicationBase,
+ public ListNode<PublicationNode *>
+{
+public:
+ /**
+ * Constructor
+ *
+ *
+ * @param meta The uORB metadata (usually from the ORB_ID()
+ * macro) for the topic.
+ * @param list A pointer to a list of subscriptions
+ * that this should be appended to.
+ */
+ PublicationNode(const struct orb_metadata *meta,
+ List<PublicationNode *> * list=nullptr) :
+ PublicationBase(meta) {
+ if (list != nullptr) list->add(this);
+ }
+
+ /**
+ * This function is the callback for list traversal
+ * updates, a child class must implement it.
+ */
+ virtual void update() = 0;
+};
+
+/**
* Publication wrapper class
*/
template<class T>
class Publication :
public T, // this must be first!
- public PublicationBase
+ public PublicationNode
{
public:
/**
* Constructor
*
- * @param list A list interface for adding to list during construction
- * @param meta The uORB metadata (usually from the ORB_ID() macro)
- * for the topic.
+ * @param meta The uORB metadata (usually from
+ * the ORB_ID() macro) for the topic.
+ * @param list A list interface for adding to
+ * list during construction
*/
- Publication(List<PublicationBase *> * list,
- const struct orb_metadata *meta);
+ Publication(const struct orb_metadata *meta,
+ List<PublicationNode *> * list=nullptr);
+
+ /**
+ * Deconstructor
+ **/
virtual ~Publication();
+
/*
* XXX
* This function gets the T struct, assuming
@@ -106,6 +165,13 @@ public:
* seem to be available
*/
void *getDataVoidPtr();
+
+ /**
+ * Create an update function that uses the embedded struct.
+ */
+ void update() {
+ PublicationBase::update(getDataVoidPtr());
+ }
};
} // namespace uORB
diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp
index 44b6debc7..fa0594c2e 100644
--- a/src/modules/uORB/Subscription.cpp
+++ b/src/modules/uORB/Subscription.cpp
@@ -52,25 +52,18 @@
#include "topics/vehicle_local_position.h"
#include "topics/vehicle_attitude_setpoint.h"
#include "topics/vehicle_rates_setpoint.h"
+#include "topics/rc_channels.h"
namespace uORB
{
-bool __EXPORT SubscriptionBase::updated()
-{
- bool isUpdated = false;
- orb_check(_handle, &isUpdated);
- return isUpdated;
-}
-
template<class T>
Subscription<T>::Subscription(
- List<SubscriptionBase *> * list,
- const struct orb_metadata *meta, unsigned interval) :
+ const struct orb_metadata *meta,
+ unsigned interval,
+ List<SubscriptionNode *> * list) :
T(), // initialize data structure to zero
- SubscriptionBase(list, meta) {
- setHandle(orb_subscribe(getMeta()));
- orb_set_interval(getHandle(), interval);
+ SubscriptionNode(meta, interval, list) {
}
template<class T>
@@ -101,5 +94,8 @@ template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
template class __EXPORT Subscription<vehicle_local_position_s>;
template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
+template class __EXPORT Subscription<rc_channels_s>;
+template class __EXPORT Subscription<vehicle_control_mode_s>;
+template class __EXPORT Subscription<actuator_armed_s>;
} // namespace uORB
diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp
index 34e9a83e0..f82586285 100644
--- a/src/modules/uORB/Subscription.hpp
+++ b/src/modules/uORB/Subscription.hpp
@@ -38,10 +38,11 @@
#pragma once
+#include <assert.h>
+
#include <uORB/uORB.h>
#include <containers/List.hpp>
-
namespace uORB
{
@@ -49,8 +50,7 @@ namespace uORB
* Base subscription warapper class, used in list traversal
* of various subscriptions.
*/
-class __EXPORT SubscriptionBase :
- public ListNode<SubscriptionBase *>
+class __EXPORT SubscriptionBase
{
public:
// methods
@@ -58,23 +58,42 @@ public:
/**
* Constructor
*
- * @param meta The uORB metadata (usually from the ORB_ID() macro)
- * for the topic.
+ * @param meta The uORB metadata (usually from the ORB_ID()
+ * macro) for the topic.
+ *
+ * @param interval The minimum interval in milliseconds
+ * between updates
*/
- SubscriptionBase(
- List<SubscriptionBase *> * list,
- const struct orb_metadata *meta) :
+ SubscriptionBase(const struct orb_metadata *meta,
+ unsigned interval=0) :
_meta(meta),
_handle() {
- if (list != NULL) list->add(this);
+ setHandle(orb_subscribe(getMeta()));
+ orb_set_interval(getHandle(), interval);
}
- bool updated();
- void update() {
+
+ /**
+ * Check if there is a new update.
+ * */
+ bool updated() {
+ bool isUpdated = false;
+ orb_check(_handle, &isUpdated);
+ return isUpdated;
+ }
+
+ /**
+ * Update the struct
+ * @param data The uORB message struct we are updating.
+ */
+ void update(void * data) {
if (updated()) {
- orb_copy(_meta, _handle, getDataVoidPtr());
+ orb_copy(_meta, _handle, data);
}
}
- virtual void *getDataVoidPtr() = 0;
+
+ /**
+ * Deconstructor
+ */
virtual ~SubscriptionBase() {
orb_unsubscribe(_handle);
}
@@ -90,30 +109,86 @@ protected:
};
/**
+ * alias class name so it is clear that the base class
+ */
+typedef SubscriptionBase SubscriptionTiny;
+
+/**
+ * The publication base class as a list node.
+ */
+class __EXPORT SubscriptionNode :
+
+ public SubscriptionBase,
+ public ListNode<SubscriptionNode *>
+{
+public:
+ /**
+ * Constructor
+ *
+ *
+ * @param meta The uORB metadata (usually from the ORB_ID()
+ * macro) for the topic.
+ * @param interval The minimum interval in milliseconds
+ * between updates
+ * @param list A pointer to a list of subscriptions
+ * that this should be appended to.
+ */
+ SubscriptionNode(const struct orb_metadata *meta,
+ unsigned interval=0,
+ List<SubscriptionNode *> * list=nullptr) :
+ SubscriptionBase(meta, interval),
+ _interval(interval) {
+ if (list != nullptr) list->add(this);
+ }
+
+ /**
+ * This function is the callback for list traversal
+ * updates, a child class must implement it.
+ */
+ virtual void update() = 0;
+// accessors
+ unsigned getInterval() { return _interval; }
+protected:
+// attributes
+ unsigned _interval;
+
+};
+
+/**
* Subscription wrapper class
*/
template<class T>
class __EXPORT Subscription :
public T, // this must be first!
- public SubscriptionBase
+ public SubscriptionNode
{
public:
/**
* Constructor
*
- * @param list A list interface for adding to list during construction
- * @param meta The uORB metadata (usually from the ORB_ID() macro)
- * for the topic.
- * @param interval The minimum interval in milliseconds between updates
+ * @param meta The uORB metadata (usually from
+ * the ORB_ID() macro) for the topic.
+ * @param interval The minimum interval in milliseconds
+ * between updates
+ * @param list A list interface for adding to
+ * list during construction
*/
- Subscription(
- List<SubscriptionBase *> * list,
- const struct orb_metadata *meta, unsigned interval);
+ Subscription(const struct orb_metadata *meta,
+ unsigned interval=0,
+ List<SubscriptionNode *> * list=nullptr);
/**
* Deconstructor
*/
virtual ~Subscription();
+
+ /**
+ * Create an update function that uses the embedded struct.
+ */
+ void update() {
+ SubscriptionBase::update(getDataVoidPtr());
+ }
+
/*
* XXX
* This function gets the T struct, assuming
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 78fdf4de7..204a114e1 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -121,8 +121,10 @@ ORB_DEFINE(vehicle_vicon_position, struct vehicle_vicon_position_s);
#include "topics/vehicle_rates_setpoint.h"
ORB_DEFINE(vehicle_rates_setpoint, struct vehicle_rates_setpoint_s);
-ORB_DEFINE(mc_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
-ORB_DEFINE(fw_virtual_rates_setpoint, struct vehicle_rates_setpoint_s);
+#include "topics/mc_virtual_rates_setpoint.h"
+ORB_DEFINE(mc_virtual_rates_setpoint, struct mc_virtual_rates_setpoint_s);
+#include "topics/fw_virtual_rates_setpoint.h"
+ORB_DEFINE(fw_virtual_rates_setpoint, struct fw_virtual_rates_setpoint_s);
#include "topics/rc_channels.h"
ORB_DEFINE(rc_channels, struct rc_channels_s);
@@ -192,13 +194,19 @@ ORB_DEFINE(subsystem_info, struct subsystem_info_s);
/* actuator controls, as requested by controller */
#include "topics/actuator_controls.h"
-ORB_DEFINE(actuator_controls_0, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_1, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_2, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_3, struct actuator_controls_s);
+#include "topics/actuator_controls_0.h"
+ORB_DEFINE(actuator_controls_0, struct actuator_controls_0_s);
+#include "topics/actuator_controls_1.h"
+ORB_DEFINE(actuator_controls_1, struct actuator_controls_1_s);
+#include "topics/actuator_controls_2.h"
+ORB_DEFINE(actuator_controls_2, struct actuator_controls_2_s);
+#include "topics/actuator_controls_3.h"
+ORB_DEFINE(actuator_controls_3, struct actuator_controls_3_s);
//Virtual control groups, used for VTOL operation
-ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_s);
-ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s);
+#include "topics/actuator_controls_virtual_mc.h"
+ORB_DEFINE(actuator_controls_virtual_mc, struct actuator_controls_virtual_mc_s);
+#include "topics/actuator_controls_virtual_fw.h"
+ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_virtual_fw_s);
#include "topics/actuator_armed.h"
ORB_DEFINE(actuator_armed, struct actuator_armed_s);
diff --git a/src/modules/uORB/topics/airspeed.h b/src/modules/uORB/topics/airspeed.h
index d2ee754cd..676c37c77 100644
--- a/src/modules/uORB/topics/airspeed.h
+++ b/src/modules/uORB/topics/airspeed.h
@@ -40,7 +40,7 @@
#ifndef TOPIC_AIRSPEED_H_
#define TOPIC_AIRSPEED_H_
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
#include <stdint.h>
/**
diff --git a/src/modules/uORB/topics/fence.h b/src/modules/uORB/topics/fence.h
index 6f16c51cf..a61f078ba 100644
--- a/src/modules/uORB/topics/fence.h
+++ b/src/modules/uORB/topics/fence.h
@@ -42,7 +42,7 @@
#include <stdint.h>
#include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
/**
* @addtogroup topics
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
deleted file mode 100644
index 50b7bd9e5..000000000
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ /dev/null
@@ -1,111 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- * Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file manual_control_setpoint.h
- * Definition of the manual_control_setpoint uORB topic.
- */
-
-#ifndef TOPIC_MANUAL_CONTROL_SETPOINT_H_
-#define TOPIC_MANUAL_CONTROL_SETPOINT_H_
-
-#include <stdint.h>
-#include "../uORB.h"
-
-/**
- * Switch position
- */
-typedef enum {
- SWITCH_POS_NONE = 0, /**< switch is not mapped */
- SWITCH_POS_ON, /**< switch activated (value = 1) */
- SWITCH_POS_MIDDLE, /**< middle position (value = 0) */
- SWITCH_POS_OFF /**< switch not activated (value = -1) */
-} switch_pos_t;
-
-/**
- * @addtogroup topics
- * @{
- */
-
-struct manual_control_setpoint_s {
- uint64_t timestamp;
-
- /**
- * Any of the channels may not be available and be set to NaN
- * to indicate that it does not contain valid data.
- * The variable names follow the definition of the
- * MANUAL_CONTROL mavlink message.
- * The default range is from -1 to 1 (mavlink message -1000 to 1000)
- * The range for the z variable is defined from 0 to 1. (The z field of
- * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
- */
- float x; /**< stick position in x direction -1..1
- in general corresponds to forward/back motion or pitch of vehicle,
- in general a positive value means forward or negative pitch and
- a negative value means backward or positive pitch */
- float y; /**< stick position in y direction -1..1
- in general corresponds to right/left motion or roll of vehicle,
- in general a positive value means right or positive roll and
- a negative value means left or negative roll */
- float z; /**< throttle stick position 0..1
- in general corresponds to up/down motion or thrust of vehicle,
- in general the value corresponds to the demanded throttle by the user,
- if the input is used for setting the setpoint of a vertical position
- controller any value > 0.5 means up and any value < 0.5 means down */
- float r; /**< yaw stick/twist positon, -1..1
- in general corresponds to the righthand rotation around the vertical
- (downwards) axis of the vehicle */
- float flaps; /**< flap position */
- float aux1; /**< default function: camera yaw / azimuth */
- float aux2; /**< default function: camera pitch / tilt */
- float aux3; /**< default function: camera trigger */
- float aux4; /**< default function: camera roll */
- float aux5; /**< default function: payload drop */
-
- switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
- switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
- switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
- switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */
- switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */
- switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(manual_control_setpoint);
-
-#endif
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
deleted file mode 100755
index 019944dc0..000000000
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ /dev/null
@@ -1,91 +0,0 @@
-/****************************************************************************
- *
- * 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 {
-
- 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) */
- float pitch; /**< Pitch angle (rad, Tait-Bryan, NED) */
- float yaw; /**< Yaw angle (rad, Tait-Bryan, NED) */
- float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) */
- float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) */
- float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) */
- float rollacc; /**< Roll angular accelration (rad/s, Tait-Bryan, NED) */
- float pitchacc; /**< Pitch angular acceleration (rad/s, Tait-Bryan, NED) */
- float yawacc; /**< Yaw angular acceleration (rad/s, Tait-Bryan, NED) */
- float rate_offsets[3]; /**< Offsets of the body angular rates from zero */
- float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */
- float q[4]; /**< Quaternion (NED) */
- float g_comp[3]; /**< Compensated gravity vector */
- bool R_valid; /**< Rotation matrix valid */
- bool q_valid; /**< Quaternion valid */
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_attitude);
-
-#endif
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
deleted file mode 100644
index 1cfc37cc6..000000000
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ /dev/null
@@ -1,89 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file vehicle_attitude_setpoint.h
- * Definition of the vehicle attitude setpoint uORB topic.
- */
-
-#ifndef TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
-#define TOPIC_VEHICLE_ATTITUDE_SETPOINT_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * vehicle attitude setpoint.
- */
-struct vehicle_attitude_setpoint_s {
- uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
-
- float roll_body; /**< body angle in NED frame */
- float pitch_body; /**< body angle in NED frame */
- float yaw_body; /**< body angle in NED frame */
- //float body_valid; /**< Set to true if body angles are valid */
-
- float R_body[3][3]; /**< Rotation matrix describing the setpoint as rotation from the current body frame */
- bool R_valid; /**< Set to true if rotation matrix is valid */
-
- //! For quaternion-based attitude control
- float q_d[4]; /** Desired quaternion for quaternion control */
- bool q_d_valid; /**< Set to true if quaternion vector is valid */
- float q_e[4]; /** Attitude error in quaternion */
- bool q_e_valid; /**< Set to true if quaternion error vector is valid */
-
- float thrust; /**< Thrust in Newton the power system should generate */
-
- bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */
- bool pitch_reset_integral; /**< Reset pitch integral part (navigation logic change) */
- bool yaw_reset_integral; /**< Reset yaw integral part (navigation logic change) */
-
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_attitude_setpoint);
-ORB_DECLARE(mc_virtual_attitude_setpoint);
-ORB_DECLARE(fw_virtual_attitude_setpoint);
-
-#endif /* TOPIC_ARDRONE_CONTROL_H_ */
diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h
index bc7046690..137c86dd5 100644
--- a/src/modules/uORB/topics/vehicle_global_position.h
+++ b/src/modules/uORB/topics/vehicle_global_position.h
@@ -45,7 +45,7 @@
#include <stdint.h>
#include <stdbool.h>
-#include "../uORB.h"
+#include <platforms/px4_defines.h>
/**
* @addtogroup topics
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
deleted file mode 100644
index b56e81e04..000000000
--- a/src/modules/uORB/topics/vehicle_status.h
+++ /dev/null
@@ -1,263 +0,0 @@
-/****************************************************************************
- *
- * Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in
- * the documentation and/or other materials provided with the
- * distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- * used to endorse or promote products derived from this software
- * without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file vehicle_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)
- *
- * @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 <julian@oes.ch>
- */
-
-#ifndef VEHICLE_STATUS_H_
-#define VEHICLE_STATUS_H_
-
-#include <stdint.h>
-#include <stdbool.h>
-#include "../uORB.h"
-
-/**
- * @addtogroup topics @{
- */
-
-/**
- * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link.
- */
-typedef enum {
- MAIN_STATE_MANUAL = 0,
- MAIN_STATE_ALTCTL,
- MAIN_STATE_POSCTL,
- MAIN_STATE_AUTO_MISSION,
- MAIN_STATE_AUTO_LOITER,
- MAIN_STATE_AUTO_RTL,
- MAIN_STATE_ACRO,
- MAIN_STATE_OFFBOARD,
- MAIN_STATE_MAX
-} main_state_t;
-
-// If you change the order, add or remove arming_state_t states make sure to update the arrays
-// in state_machine_helper.cpp as well.
-typedef enum {
- ARMING_STATE_INIT = 0,
- ARMING_STATE_STANDBY,
- ARMING_STATE_ARMED,
- ARMING_STATE_ARMED_ERROR,
- ARMING_STATE_STANDBY_ERROR,
- ARMING_STATE_REBOOT,
- ARMING_STATE_IN_AIR_RESTORE,
- ARMING_STATE_MAX,
-} arming_state_t;
-
-typedef enum {
- HIL_STATE_OFF = 0,
- HIL_STATE_ON
-} hil_state_t;
-
-/**
- * Navigation state, i.e. "what should vehicle do".
- */
-typedef enum {
- NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */
- NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */
- NAVIGATION_STATE_POSCTL, /**< Position control mode */
- NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */
- NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */
- NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */
- NAVIGATION_STATE_AUTO_RCRECOVER, /**< RC recover mode */
- NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */
- NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */
- NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */
- NAVIGATION_STATE_ACRO, /**< Acro mode */
- NAVIGATION_STATE_LAND, /**< Land mode */
- NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
- NAVIGATION_STATE_TERMINATION, /**< Termination mode */
- NAVIGATION_STATE_OFFBOARD,
- NAVIGATION_STATE_MAX,
-} navigation_state_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_STABILIZED_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 */
-
-/**
- * Should match 1:1 MAVLink's MAV_TYPE ENUM
- */
-enum VEHICLE_TYPE {
- VEHICLE_TYPE_GENERIC = 0, /* Generic micro air vehicle. | */
- VEHICLE_TYPE_FIXED_WING = 1, /* Fixed wing aircraft. | */
- VEHICLE_TYPE_QUADROTOR = 2, /* Quadrotor | */
- VEHICLE_TYPE_COAXIAL = 3, /* Coaxial helicopter | */
- VEHICLE_TYPE_HELICOPTER = 4, /* Normal helicopter with tail rotor. | */
- VEHICLE_TYPE_ANTENNA_TRACKER = 5, /* Ground installation | */
- VEHICLE_TYPE_GCS = 6, /* Operator control unit / ground control station | */
- VEHICLE_TYPE_AIRSHIP = 7, /* Airship, controlled | */
- VEHICLE_TYPE_FREE_BALLOON = 8, /* Free balloon, uncontrolled | */
- VEHICLE_TYPE_ROCKET = 9, /* Rocket | */
- VEHICLE_TYPE_GROUND_ROVER = 10, /* Ground rover | */
- VEHICLE_TYPE_SURFACE_BOAT = 11, /* Surface vessel, boat, ship | */
- VEHICLE_TYPE_SUBMARINE = 12, /* Submarine | */
- VEHICLE_TYPE_HEXAROTOR = 13, /* Hexarotor | */
- VEHICLE_TYPE_OCTOROTOR = 14, /* Octorotor | */
- VEHICLE_TYPE_TRICOPTER = 15, /* Octorotor | */
- VEHICLE_TYPE_FLAPPING_WING = 16, /* Flapping wing | */
- VEHICLE_TYPE_KITE = 17, /* Kite | */
- VEHICLE_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */
- VEHICLE_TYPE_VTOL_DUOROTOR = 19, /* Vtol with two engines */
- VEHICLE_TYPE_VTOL_QUADROTOR = 20, /* Vtol with four engines*/
- VEHICLE_TYPE_ENUM_END = 21 /* | */
-};
-
-enum VEHICLE_BATTERY_WARNING {
- VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */
- VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */
- VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */
-};
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * 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 */
-
- main_state_t main_state; /**< main state machine */
- navigation_state_t nav_state; /**< set navigation state machine to specified value */
- arming_state_t arming_state; /**< current arming state */
- hil_state_t hil_state; /**< current hil state */
- bool failsafe; /**< true if system is in failsafe state */
-
- int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
- int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
- int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
-
- bool is_rotary_wing; /**< True if system is in rotary wing configuration, so for a VTOL
- this is only true while flying as a multicopter */
- bool is_vtol; /**< True if the system is VTOL capable */
-
- bool vtol_fw_permanent_stab; /**< True if vtol should stabilize attitude for fw in manual mode */
-
- bool condition_battery_voltage_valid;
- bool condition_system_in_air_restore; /**< true if we can restore in mid air */
- bool condition_system_sensors_initialized;
- bool condition_system_returned_to_home;
- bool condition_auto_mission_available;
- bool condition_global_position_valid; /**< set to true by the commander app if the quality of the position estimate is good enough to use it for navigation */
- bool condition_launch_position_valid; /**< indicates a valid launch position */
- bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
- bool condition_local_position_valid;
- bool condition_local_altitude_valid;
- bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
- bool condition_landed; /**< true if vehicle is landed, always true if disarmed */
- bool condition_power_input_valid; /**< set if input power is valid */
- float avionics_power_rail_voltage; /**< voltage of the avionics power rail */
-
- bool rc_signal_found_once;
- bool rc_signal_lost; /**< true if RC reception lost */
- uint64_t rc_signal_lost_timestamp; /**< Time at which the RC reception was lost */
- bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */
- bool rc_input_blocked; /**< set if RC input should be ignored */
-
- bool data_link_lost; /**< datalink to GCS lost */
- bool data_link_lost_cmd; /**< datalink to GCS lost mode commanded */
- uint8_t data_link_lost_counter; /**< counts unique data link lost events */
- bool engine_failure; /** Set to true if an engine failure is detected */
- bool engine_failure_cmd; /** Set to true if an engine failure mode is commanded */
- bool gps_failure; /** Set to true if a gps failure is detected */
- bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */
-
- bool barometer_failure; /** Set to true if a barometer failure is detected */
-
- bool offboard_control_signal_found_once;
- bool offboard_control_signal_lost;
- bool offboard_control_signal_weak;
- uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */
- bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command
- and should not be overridden by RC */
-
- /* 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;
-
- float load; /**< processor load from 0 to 1 */
- float battery_voltage;
- float battery_current;
- float battery_remaining;
-
- enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */
- 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 circuit_breaker_engaged_power_check;
- bool circuit_breaker_engaged_airspd_check;
- bool circuit_breaker_engaged_enginefailure_check;
- bool circuit_breaker_engaged_gpsfailure_check;
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_status);
-
-#endif
diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h
index beb23f61d..672f8d8d1 100644
--- a/src/modules/uORB/uORB.h
+++ b/src/modules/uORB/uORB.h
@@ -152,7 +152,7 @@ typedef intptr_t orb_advert_t;
* node in /obj if required and publishes the initial data.
*
* Any number of advertisers may publish to a topic; publications are atomic
- * but co-ordination between publishers is not provided by the ORB.
+ * but co-ordination between publishers is not provided by the ORB.
*
* @param meta The uORB metadata (usually from the ORB_ID() macro)
* for the topic.
@@ -288,4 +288,13 @@ extern int orb_set_interval(int handle, unsigned interval) __EXPORT;
__END_DECLS
+/* Diverse uORB header defines */ //XXX: move to better location
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
+typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s;
+typedef uint8_t arming_state_t;
+typedef uint8_t main_state_t;
+typedef uint8_t hil_state_t;
+typedef uint8_t navigation_state_t;
+typedef uint8_t switch_pos_t;
+
#endif /* _UORB_UORB_H */
diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp
index 8e68730b8..0a333eade 100644
--- a/src/modules/vtol_att_control/vtol_att_control_main.cpp
+++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp
@@ -58,7 +58,11 @@
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_virtual_mc.h>
+#include <uORB/topics/actuator_controls_virtual_fw.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/mc_virtual_rates_setpoint.h>
+#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vtol_vehicle_status.h>
diff --git a/src/platforms/empty.c b/src/platforms/empty.c
new file mode 100644
index 000000000..139531354
--- /dev/null
+++ b/src/platforms/empty.c
@@ -0,0 +1,3 @@
+/*
+ * This is an empty C source file, used when building default firmware configurations.
+ */
diff --git a/src/platforms/nuttx/module.mk b/src/platforms/nuttx/module.mk
new file mode 100644
index 000000000..4a2aff824
--- /dev/null
+++ b/src/platforms/nuttx/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+# Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in
+# the documentation and/or other materials provided with the
+# distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+# used to endorse or promote products derived from this software
+# without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# NuttX / uORB adapter library
+#
+
+SRCS = px4_nuttx_impl.cpp
+
+MAXOPTIMIZATION = -Os
diff --git a/src/platforms/nuttx/px4_nuttx_impl.cpp b/src/platforms/nuttx/px4_nuttx_impl.cpp
new file mode 100644
index 000000000..70e292320
--- /dev/null
+++ b/src/platforms/nuttx/px4_nuttx_impl.cpp
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nuttx_impl.cpp
+ *
+ * PX4 Middleware Wrapper NuttX Implementation
+ */
+
+#include <px4.h>
+#include <drivers/drv_hrt.h>
+
+
+namespace px4
+{
+
+void init(int argc, char *argv[], const char *process_name)
+{
+ PX4_WARN("process: %s", process_name);
+}
+
+uint64_t get_time_micros()
+{
+ return hrt_absolute_time();
+}
+
+}
diff --git a/src/platforms/px4_defines.h b/src/platforms/px4_defines.h
new file mode 100644
index 000000000..c8e2cf290
--- /dev/null
+++ b/src/platforms/px4_defines.h
@@ -0,0 +1,204 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_defines.h
+ *
+ * Generally used magic defines
+ */
+
+#pragma once
+/* Get the name of the default value fiven the param name */
+#define PX4_PARAM_DEFAULT_VALUE_NAME(_name) PARAM_##_name##_DEFAULT
+
+/* Shortcuts to define parameters when the default value is defined according to PX4_PARAM_DEFAULT_VALUE_NAME */
+#define PX4_PARAM_DEFINE_INT32(_name) PARAM_DEFINE_INT32(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
+#define PX4_PARAM_DEFINE_FLOAT(_name) PARAM_DEFINE_FLOAT(_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
+
+
+#if defined(__PX4_ROS)
+/*
+ * Building for running within the ROS environment
+ */
+#define noreturn_function
+#ifdef __cplusplus
+#include "ros/ros.h"
+#endif
+/* Main entry point */
+#define PX4_MAIN_FUNCTION(_prefix) int main(int argc, char **argv)
+
+/* Print/output wrappers */
+#define PX4_WARN ROS_WARN
+#define PX4_INFO ROS_INFO
+
+/* Topic Handle */
+#define PX4_TOPIC(_name) #_name
+
+/* Topic type */
+#define PX4_TOPIC_T(_name) px4::_name
+
+/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), &_cbf, _objptr);
+/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe(PX4_TOPIC(_name), _cbf);
+/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name));
+
+/* Parameter handle datatype */
+typedef const char *px4_param_t;
+
+/* Helper functions to set ROS params, only int and float supported */
+static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, int value)
+{
+ if (!ros::param::has(name)) {
+ ros::param::set(name, value);
+ }
+
+ return (px4_param_t)name;
+};
+static inline px4_param_t PX4_ROS_PARAM_SET(const char *name, float value)
+{
+ if (!ros::param::has(name)) {
+ ros::param::set(name, value);
+ }
+
+ return (px4_param_t)name;
+};
+
+/* Initialize a param, in case of ROS the parameter is sent to the parameter server here */
+#define PX4_PARAM_INIT(_name) PX4_ROS_PARAM_SET(#_name, PX4_PARAM_DEFAULT_VALUE_NAME(_name))
+
+/* Get value of parameter by handle */
+#define PX4_PARAM_GET(_handle, _destpt) ros::param::get(_handle, *_destpt)
+
+/* Get value of parameter by name, which is equal to the handle for ros */
+#define PX4_PARAM_GET_BYNAME(_name, _destpt) PX4_PARAM_GET(_name, _destpt)
+
+#define OK 0
+#define ERROR -1
+
+//XXX hack to be able to use isfinte from math.h, -D_GLIBCXX_USE_C99_MATH seems not to work
+#define isfinite(_value) std::isfinite(_value)
+
+/* Useful constants. */
+#define M_E_F 2.7182818284590452354f
+#define M_LOG2E_F 1.4426950408889634074f
+#define M_LOG10E_F 0.43429448190325182765f
+#define M_LN2_F _M_LN2_F
+#define M_LN10_F 2.30258509299404568402f
+#define M_PI_F 3.14159265358979323846f
+#define M_TWOPI_F (M_PI_F * 2.0f)
+#define M_PI_2_F 1.57079632679489661923f
+#define M_PI_4_F 0.78539816339744830962f
+#define M_3PI_4_F 2.3561944901923448370E0f
+#define M_SQRTPI_F 1.77245385090551602792981f
+#define M_1_PI_F 0.31830988618379067154f
+#define M_2_PI_F 0.63661977236758134308f
+#define M_2_SQRTPI_F 1.12837916709551257390f
+#define M_DEG_TO_RAD_F 0.01745329251994f
+#define M_RAD_TO_DEG_F 57.2957795130823f
+#define M_SQRT2_F 1.41421356237309504880f
+#define M_SQRT1_2_F 0.70710678118654752440f
+#define M_LN2LO_F 1.9082149292705877000E-10f
+#define M_LN2HI_F 6.9314718036912381649E-1f
+#define M_SQRT3_F 1.73205080756887719000f
+#define M_IVLN10_F 0.43429448190325182765f /* 1 / log(10) */
+#define M_LOG2_E_F _M_LN2_F
+#define M_INVLN2_F 1.4426950408889633870E0f /* 1 / log(2) */
+
+#else
+/*
+ * Building for NuttX
+ */
+#include <platforms/px4_includes.h>
+/* Main entry point */
+#define PX4_MAIN_FUNCTION(_prefix) int _prefix##_task_main(int argc, char *argv[])
+
+/* Print/output wrappers */
+#define PX4_WARN warnx
+#define PX4_INFO warnx
+
+/* Topic Handle */
+#define PX4_TOPIC(_name) ORB_ID(_name)
+
+/* Topic type */
+#define PX4_TOPIC_T(_name) _name##_s
+
+/* Subscribe and providing a class method as callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_CBMETH(_nodehandle, _name, _cbf, _objptr, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, _objptr, std::placeholders::_1), _interval)
+/* Subscribe and providing a function as callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_CBFUNC(_nodehandle, _name, _cbf, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), std::bind(&_cbf, std::placeholders::_1), _interval)
+/* Subscribe without a callback (do not use directly, use PX4_SUBSCRIBE instead) */
+#define PX4_SUBSCRIBE_NOCB(_nodehandle, _name, _interval) _nodehandle.subscribe<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name), _interval)
+
+/* Parameter handle datatype */
+#include <systemlib/param/param.h>
+typedef param_t px4_param_t;
+
+/* Initialize a param, get param handle */
+#define PX4_PARAM_INIT(_name) param_find(#_name)
+
+/* Get value of parameter by handle */
+#define PX4_PARAM_GET(_handle, _destpt) param_get(_handle, _destpt)
+
+/* Get value of parameter by name */
+#define PX4_PARAM_GET_BYNAME(_name, _destpt) param_get(param_find(_name), _destpt)
+
+/* XXX this is a hack to resolve conflicts with NuttX headers */
+#if !defined(__PX4_TESTS)
+#define isspace(c) \
+ ((c) == ' ' || (c) == '\t' || (c) == '\n' || \
+ (c) == '\r' || (c) == '\f' || c== '\v')
+#endif
+
+#endif
+
+/* Defines for all platforms */
+
+/* Shortcut for subscribing to topics
+ * Overload the PX4_SUBSCRIBE macro to suppport methods, pure functions as callback and no callback at all
+ */
+#define PX4_GET_SUBSCRIBE(_1, _2, _3, _4, _5, NAME, ...) NAME
+#define PX4_SUBSCRIBE(...) PX4_GET_SUBSCRIBE(__VA_ARGS__, PX4_SUBSCRIBE_CBMETH, PX4_SUBSCRIBE_CBFUNC, PX4_SUBSCRIBE_NOCB)(__VA_ARGS__)
+
+/* Get a subscriber class type based on the topic name */
+#define PX4_SUBSCRIBER(_name) Subscriber<PX4_TOPIC_T(_name)>
+
+/* shortcut for advertising topics */
+#define PX4_ADVERTISE(_nodehandle, _name) _nodehandle.advertise<PX4_TOPIC_T(_name)>(PX4_TOPIC(_name))
+
+/* wrapper for 2d matrices */
+#define PX4_ARRAY2D(_array, _ncols, _x, _y) (_array[_x * _ncols + _y])
+
+/* wrapper for rotation matrices stored in arrays */
+#define PX4_R(_array, _x, _y) PX4_ARRAY2D(_array, 3, _x, _y)
diff --git a/src/platforms/px4_includes.h b/src/platforms/px4_includes.h
new file mode 100644
index 000000000..1bd4509ca
--- /dev/null
+++ b/src/platforms/px4_includes.h
@@ -0,0 +1,93 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_includes.h
+ *
+ * Includes headers depending on the build target
+ */
+
+#pragma once
+
+#include <stdbool.h>
+
+#if defined(__PX4_ROS)
+/*
+ * Building for running within the ROS environment
+ */
+
+#ifdef __cplusplus
+#include "ros/ros.h"
+#include <px4_rc_channels.h>
+#include <px4_vehicle_attitude.h>
+#include <px4_vehicle_attitude_setpoint.h>
+#include <px4_manual_control_setpoint.h>
+#include <px4_actuator_controls.h>
+#include <px4_actuator_controls_0.h>
+#include <px4_actuator_controls_virtual_mc.h>
+#include <px4_vehicle_rates_setpoint.h>
+#include <px4_mc_virtual_rates_setpoint.h>
+#include <px4_vehicle_attitude.h>
+#include <px4_vehicle_control_mode.h>
+#include <px4_actuator_armed.h>
+#include <px4_parameter_update.h>
+#include <px4_vehicle_status.h>
+#endif
+
+#else
+/*
+ * Building for NuttX
+ */
+#include <nuttx/config.h>
+#include <uORB/uORB.h>
+#ifdef __cplusplus
+#include <platforms/nuttx/px4_messages/px4_rc_channels.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_attitude_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_manual_control_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_0.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_1.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_2.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_controls_3.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_rates_setpoint.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_attitude.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_control_mode.h>
+#include <platforms/nuttx/px4_messages/px4_actuator_armed.h>
+#include <platforms/nuttx/px4_messages/px4_parameter_update.h>
+#include <platforms/nuttx/px4_messages/px4_vehicle_status.h>
+#endif
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
+
+#endif
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/platforms/px4_message.h
index 668f8f164..bff7aa313 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/platforms/px4_message.h
@@ -1,6 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -32,51 +32,46 @@
****************************************************************************/
/**
- * @file actuator_controls.h
+ * @file px4_message.h
*
- * Actuator control topics - mixer inputs.
- *
- * Values published to these topics 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.
- *
- * Each topic can be published by a single controller
+ * Defines the message base types
*/
+#pragma once
-#ifndef TOPIC_ACTUATOR_CONTROLS_H
-#define TOPIC_ACTUATOR_CONTROLS_H
-
-#include <stdint.h>
-#include "../uORB.h"
-
-#define NUM_ACTUATOR_CONTROLS 8
-#define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */
+#if defined(__PX4_ROS)
+typedef const char* PX4TopicHandle;
+#else
+#include <uORB/uORB.h>
+typedef orb_id_t PX4TopicHandle;
+#endif
-/* control sets with pre-defined applications */
-#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0)
+namespace px4
+{
-/**
- * @addtogroup topics
- * @{
- */
+template <typename M>
+class __EXPORT PX4Message
+{
+ // friend class NodeHandle;
+// #if defined(__PX4_ROS)
+ // template<typename T>
+ // friend class SubscriberROS;
+// #endif
-struct actuator_controls_s {
- uint64_t timestamp;
- uint64_t timestamp_sample; /**< the timestamp the data this control response is based on was sampled */
- float control[NUM_ACTUATOR_CONTROLS];
-};
+public:
+ PX4Message() :
+ _data()
+ {}
-/**
- * @}
- */
+ PX4Message(M msg) :
+ _data(msg)
+ {}
-/* actuator control sets; this list can be expanded as more controllers emerge */
-ORB_DECLARE(actuator_controls_0);
-ORB_DECLARE(actuator_controls_1);
-ORB_DECLARE(actuator_controls_2);
-ORB_DECLARE(actuator_controls_3);
-ORB_DECLARE(actuator_controls_virtual_mc);
-ORB_DECLARE(actuator_controls_virtual_fw);
+ virtual ~PX4Message() {};
+ virtual M& data() {return _data;}
+ virtual const M& data() const {return _data;}
+private:
+ M _data;
+};
-#endif
+}
diff --git a/src/modules/uORB/topics/vehicle_rates_setpoint.h b/src/platforms/px4_middleware.h
index 47d51f199..735d34234 100644
--- a/src/modules/uORB/topics/vehicle_rates_setpoint.h
+++ b/src/platforms/px4_middleware.h
@@ -1,7 +1,6 @@
/****************************************************************************
*
- * Copyright (C) 2012 PX4 Development Team. All rights reserved.
- * Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -33,36 +32,53 @@
****************************************************************************/
/**
- * @file vehicle_rates_setpoint.h
- * Definition of the vehicle rates setpoint topic
+ * @file px4_middleware.h
+ *
+ * PX4 generic middleware wrapper
*/
-#ifndef TOPIC_VEHICLE_RATES_SETPOINT_H_
-#define TOPIC_VEHICLE_RATES_SETPOINT_H_
+#pragma once
#include <stdint.h>
-#include "../uORB.h"
+#include <unistd.h>
+
+namespace px4
+{
+
+__EXPORT void init(int argc, char *argv[], const char *process_name);
+__EXPORT uint64_t get_time_micros();
+
+#if defined(__PX4_ROS)
+/**
+ * Returns true if the app/task should continue to run
+ */
+inline bool ok() { return ros::ok(); }
+#else
+extern bool task_should_exit;
/**
- * @addtogroup topics
- * @{
+ * Returns true if the app/task should continue to run
*/
-struct vehicle_rates_setpoint_s {
- uint64_t timestamp; /**< in microseconds since system start */
+__EXPORT inline bool ok() { return !task_should_exit; }
+#endif
- float roll; /**< body angular rates in NED frame */
- float pitch; /**< body angular rates in NED frame */
- float yaw; /**< body angular rates in NED frame */
- float thrust; /**< thrust normalized to 0..1 */
+class Rate
+{
+public:
+ /**
+ * Construct the Rate object and set rate
+ * @param rate_hz rate from which sleep time is calculated in Hz
+ */
+ explicit Rate(unsigned rate_hz) { sleep_interval = 1e6 / rate_hz; }
-}; /**< vehicle_rates_setpoint */
+ /**
+ * Sleep for 1/rate_hz s
+ */
+ void sleep() { usleep(sleep_interval); }
-/**
-* @}
-*/
+private:
+ uint64_t sleep_interval;
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_rates_setpoint);
-ORB_DECLARE(mc_virtual_rates_setpoint);
-ORB_DECLARE(fw_virtual_rates_setpoint);
-#endif
+};
+
+} // namespace px4
diff --git a/src/platforms/px4_nodehandle.h b/src/platforms/px4_nodehandle.h
new file mode 100644
index 000000000..70ec1cfe0
--- /dev/null
+++ b/src/platforms/px4_nodehandle.h
@@ -0,0 +1,305 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.h
+ *
+ * PX4 Middleware Wrapper Node Handle
+ */
+#pragma once
+
+/* includes for all platforms */
+#include "px4_subscriber.h"
+#include "px4_publisher.h"
+#include "px4_middleware.h"
+
+#if defined(__PX4_ROS)
+/* includes when building for ros */
+#include "ros/ros.h"
+#include <list>
+#include <inttypes.h>
+#include <type_traits>
+#else
+/* includes when building for NuttX */
+#include <poll.h>
+#endif
+#include <functional>
+
+namespace px4
+{
+#if defined(__PX4_ROS)
+class NodeHandle :
+ private ros::NodeHandle
+{
+public:
+ NodeHandle() :
+ ros::NodeHandle(),
+ _subs(),
+ _pubs()
+ {}
+
+ ~NodeHandle()
+ {
+ _subs.clear();
+ _pubs.clear();
+ };
+
+ /**
+ * Subscribe with callback to function
+ * @param topic Name of the topic
+ * @param fb Callback, executed on receiving a new message
+ */
+ template<typename T>
+ Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval)
+ {
+ SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, std::placeholders::_1));
+ _subs.push_back(sub);
+ return (Subscriber<T> *)sub;
+ }
+
+ /**
+ * Subscribe with callback to class method
+ * @param fb Callback, executed on receiving a new message
+ * @param obj pointer class instance
+ */
+ template<typename T, typename C>
+ Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval)
+ {
+ SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this, std::bind(fp, obj, std::placeholders::_1));
+ _subs.push_back(sub);
+ return (Subscriber<T> *)sub;
+ }
+
+ /**
+ * Subscribe with no callback, just the latest value is stored on updates
+ */
+ template<typename T>
+ Subscriber<T> *subscribe(unsigned interval)
+ {
+ SubscriberBase *sub = new SubscriberROS<T>((ros::NodeHandle*)this);
+ _subs.push_back(sub);
+ return (Subscriber<T> *)sub;
+ }
+
+ /**
+ * Advertise topic
+ */
+ template<typename T>
+ Publisher<T>* advertise()
+ {
+ PublisherROS<T> *pub = new PublisherROS<T>((ros::NodeHandle*)this);
+ _pubs.push_back((PublisherBase*)pub);
+ return (Publisher<T>*)pub;
+ }
+
+ /**
+ * Calls all callback waiting to be called
+ */
+ void spinOnce() { ros::spinOnce(); }
+
+ /**
+ * Keeps calling callbacks for incomming messages, returns when module is terminated
+ */
+ void spin() { ros::spin(); }
+
+
+protected:
+ std::list<SubscriberBase *> _subs; /**< Subcriptions of node */
+ std::list<PublisherBase *> _pubs; /**< Publications of node */
+};
+#else //Building for NuttX
+class __EXPORT NodeHandle
+{
+public:
+ NodeHandle() :
+ _subs(),
+ _pubs(),
+ _sub_min_interval(nullptr)
+ {}
+
+ ~NodeHandle()
+ {
+ /* Empty subscriptions list */
+ SubscriberNode *sub = _subs.getHead();
+ int count = 0;
+
+ while (sub != nullptr) {
+ if (count++ > kMaxSubscriptions) {
+ PX4_WARN("exceeded max subscriptions");
+ break;
+ }
+
+ SubscriberNode *sib = sub->getSibling();
+ delete sub;
+ sub = sib;
+ }
+
+ /* Empty publications list */
+ PublisherNode *pub = _pubs.getHead();
+ count = 0;
+
+ while (pub != nullptr) {
+ if (count++ > kMaxPublications) {
+ PX4_WARN("exceeded max publications");
+ break;
+ }
+
+ PublisherNode *sib = pub->getSibling();
+ delete pub;
+ pub = sib;
+ }
+ };
+
+ /**
+ * Subscribe with callback to function
+ * @param fp Callback, executed on receiving a new message
+ * @param interval Minimal interval between calls to callback
+ */
+
+ template<typename T>
+ // Subscriber<T> *subscribe(void(*fp)(const T &), unsigned interval)
+ void subscribe(void(*fp)(const T &), unsigned interval)
+ {
+ (void)interval;
+ SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, std::placeholders::_1));
+ // update_sub_min_interval(interval, sub_px4);
+ // _subs.add((SubscriberNode *)sub_px4);
+ // return (Subscriber<T> *)sub_px4;
+ }
+
+ /**
+ * Subscribe with callback to class method
+ * @param fb Callback, executed on receiving a new message
+ * @param obj pointer class instance
+ */
+ template<typename T, typename C>
+ // Subscriber<T> *subscribe(void(C::*fp)(const T &), C *obj, unsigned interval)
+ void subscribe(void(C::*fp)(const T &), C *obj, unsigned interval)
+ {
+ (void)interval;
+ SubscriberUORBCallback<T> *sub_px4 = new SubscriberUORBCallback<T>(interval, std::bind(fp, obj, std::placeholders::_1));
+ // update_sub_min_interval(interval, sub_px4);
+ // _subs.add((SubscriberNode *)sub_px4);
+ // return (Subscriber<T> *)sub_px4;
+ }
+
+ /**
+ * Subscribe without callback to function
+ * @param interval Minimal interval between data fetches from orb
+ */
+
+ template<typename T>
+ // Subscriber<T> *subscribe(unsigned interval)
+ void subscribe(unsigned interval)
+ {
+ (void)interval;
+ SubscriberUORB<T> *sub_px4 = new SubscriberUORB<T>(interval);
+ // update_sub_min_interval(interval, sub_px4);
+ // _subs.add((SubscriberNode *)sub_px4);
+ // return (Subscriber<T> *)sub_px4;
+ }
+
+ /**
+ * Advertise topic
+ */
+ template<typename T>
+ Publisher<T> *advertise()
+ {
+ PublisherUORB<T> *pub = new PublisherUORB<T>();
+ _pubs.add(pub);
+ return (Publisher<T>*)pub;
+ }
+
+ /**
+ * Calls all callback waiting to be called
+ */
+ void spinOnce()
+ {
+ /* Loop through subscriptions, call callback for updated subscriptions */
+ SubscriberNode *sub = _subs.getHead();
+ int count = 0;
+
+ while (sub != nullptr) {
+ if (count++ > kMaxSubscriptions) {
+ PX4_WARN("exceeded max subscriptions");
+ break;
+ }
+
+ sub->update();
+ sub = sub->getSibling();
+ }
+ }
+
+ /**
+ * Keeps calling callbacks for incomming messages, returns when module is terminated
+ */
+ void spin()
+ {
+ while (ok()) {
+ const int timeout_ms = 100;
+
+ /* Only continue in the loop if the nodehandle has subscriptions */
+ if (_sub_min_interval == nullptr) {
+ usleep(timeout_ms * 1000);
+ continue;
+ }
+
+ /* Poll fd with smallest interval */
+ struct pollfd pfd;
+ pfd.fd = _sub_min_interval->getUORBHandle();
+ pfd.events = POLLIN;
+ poll(&pfd, 1, timeout_ms);
+ spinOnce();
+ }
+ }
+protected:
+ static const uint16_t kMaxSubscriptions = 100;
+ static const uint16_t kMaxPublications = 100;
+ List<SubscriberNode *> _subs; /**< Subcriptions of node */
+ List<PublisherNode *> _pubs; /**< Publications of node */
+ SubscriberNode *_sub_min_interval; /**< Points to the sub wtih the smallest interval
+ of all Subscriptions in _subs*/
+
+ /**
+ * Check if this is the smallest interval so far and update _sub_min_interval
+ */
+ template<typename T>
+ void update_sub_min_interval(unsigned interval, SubscriberUORB<T> *sub)
+ {
+ if (_sub_min_interval == nullptr || _sub_min_interval->get_interval() > interval) {
+ _sub_min_interval = sub;
+ }
+ }
+};
+#endif
+}
diff --git a/src/platforms/px4_publisher.h b/src/platforms/px4_publisher.h
new file mode 100644
index 000000000..d9cd7a3c1
--- /dev/null
+++ b/src/platforms/px4_publisher.h
@@ -0,0 +1,161 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.h
+ *
+ * PX4 Middleware Wrapper Node Handle
+ */
+#pragma once
+#if defined(__PX4_ROS)
+/* includes when building for ros */
+#include "ros/ros.h"
+#else
+/* includes when building for NuttX */
+#include <uORB/Publication.hpp>
+#include <containers/List.hpp>
+#endif
+
+#include <platforms/px4_message.h>
+
+namespace px4
+{
+
+/**
+ * Untemplated publisher base class
+ * */
+class __EXPORT PublisherBase
+{
+public:
+ PublisherBase() {};
+ ~PublisherBase() {};
+};
+
+/**
+ * Publisher base class, templated with the message type
+ * */
+template <typename T>
+class __EXPORT Publisher
+{
+public:
+ Publisher() {};
+ ~Publisher() {};
+
+ virtual int publish(const T &msg) = 0;
+};
+
+#if defined(__PX4_ROS)
+template <typename T>
+class PublisherROS :
+ public Publisher<T>
+{
+public:
+ /**
+ * Construct Publisher by providing a ros::Publisher
+ * @param ros_pub the ros publisher which will be used to perform the publications
+ */
+ PublisherROS(ros::NodeHandle *rnh) :
+ Publisher<T>(),
+ _ros_pub(rnh->advertise<typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &>(T::handle(), kQueueSizeDefault))
+ {}
+
+ ~PublisherROS() {};
+
+ /** Publishes msg
+ * @param msg the message which is published to the topic
+ */
+ int publish(const T &msg)
+ {
+ _ros_pub.publish(msg.data());
+ return 0;
+ }
+protected:
+ static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */
+ ros::Publisher _ros_pub; /**< Handle to the ros publisher */
+};
+#else
+/**
+ * Because we maintain a list of publishers we need a node class
+ */
+class __EXPORT PublisherNode :
+ public ListNode<PublisherNode *>
+{
+public:
+ PublisherNode() :
+ ListNode()
+ {}
+
+ virtual ~PublisherNode() {}
+
+ virtual void update() = 0;
+};
+
+template <typename T>
+class __EXPORT PublisherUORB :
+ public Publisher<T>,
+ public PublisherNode
+
+{
+public:
+ /**
+ * Construct Publisher by providing orb meta data
+ */
+ PublisherUORB() :
+ Publisher<T>(),
+ PublisherNode(),
+ _uorb_pub(new uORB::PublicationBase(T::handle()))
+ {}
+
+ ~PublisherUORB() {
+ delete _uorb_pub;
+ };
+
+ /** Publishes msg
+ * @param msg the message which is published to the topic
+ */
+ int publish(const T &msg)
+ {
+ _uorb_pub->update((void *)&(msg.data()));
+ return 0;
+ }
+
+ /**
+ * Empty callback for list traversal
+ */
+ void update() {} ;
+private:
+ uORB::PublicationBase * _uorb_pub; /**< Handle to the publisher */
+
+};
+#endif
+}
diff --git a/src/platforms/px4_subscriber.h b/src/platforms/px4_subscriber.h
new file mode 100644
index 000000000..9f32966dc
--- /dev/null
+++ b/src/platforms/px4_subscriber.h
@@ -0,0 +1,284 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_subscriber.h
+ *
+ * PX4 Middleware Wrapper Subscriber
+ */
+#pragma once
+
+#include <functional>
+#include <type_traits>
+
+#if defined(__PX4_ROS)
+/* includes when building for ros */
+#include "ros/ros.h"
+#else
+/* includes when building for NuttX */
+#include <uORB/Subscription.hpp>
+#include <containers/List.hpp>
+#include "px4_nodehandle.h"
+#endif
+
+namespace px4
+{
+
+/**
+ * Untemplated subscriber base class
+ * */
+class __EXPORT SubscriberBase
+{
+public:
+ SubscriberBase() {};
+ virtual ~SubscriberBase() {};
+
+};
+
+/**
+ * Subscriber class which is used by nodehandle
+ */
+template<typename T>
+class __EXPORT Subscriber :
+ public SubscriberBase
+{
+public:
+ Subscriber() :
+ SubscriberBase(),
+ _msg_current()
+ {};
+
+ virtual ~Subscriber() {}
+
+ /* Accessors*/
+ /**
+ * Get the last message value
+ */
+ virtual T& get() {return _msg_current;}
+
+ /**
+ * Get the last native message value
+ */
+ virtual decltype(((T*)nullptr)->data()) data()
+ {
+ return _msg_current.data();
+ }
+
+protected:
+ T _msg_current; /**< Current Message value */
+};
+
+#if defined(__PX4_ROS)
+/**
+ * Subscriber class that is templated with the ros n message type
+ */
+template<typename T>
+class SubscriberROS :
+ public Subscriber<T>
+{
+public:
+ /**
+ * Construct Subscriber without a callback function
+ */
+ SubscriberROS(ros::NodeHandle *rnh) :
+ px4::Subscriber<T>(),
+ _cbf(NULL),
+ _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this))
+ {}
+
+ /**
+ * Construct Subscriber by providing a callback function
+ */
+ SubscriberROS(ros::NodeHandle *rnh, std::function<void(const T &)> cbf) :
+ _cbf(cbf),
+ _ros_sub(rnh->subscribe(T::handle(), kQueueSizeDefault, &SubscriberROS<T>::callback, this))
+ {}
+
+ virtual ~SubscriberROS() {};
+
+protected:
+ static const uint32_t kQueueSizeDefault = 1; /**< Size of queue for ROS */
+
+ /**
+ * Called on topic update, saves the current message and then calls the provided callback function
+ * needs to use the native type as it is called by ROS
+ */
+ void callback(const typename std::remove_reference<decltype(((T*)nullptr)->data())>::type &msg)
+ {
+ /* Store data */
+ this->_msg_current.data() = msg;
+
+ /* Call callback */
+ if (_cbf != NULL) {
+ _cbf(this->get());
+ }
+
+ }
+
+ ros::Subscriber _ros_sub; /**< Handle to ros subscriber */
+ std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */
+
+};
+
+#else // Building for NuttX
+/**
+ * Because we maintain a list of subscribers we need a node class
+ */
+class __EXPORT SubscriberNode :
+ public ListNode<SubscriberNode *>
+{
+public:
+ SubscriberNode(unsigned interval) :
+ ListNode(),
+ _interval(interval)
+ {}
+
+ virtual ~SubscriberNode() {}
+
+ virtual void update() = 0;
+
+ virtual int getUORBHandle() = 0;
+
+ unsigned get_interval() { return _interval; }
+
+protected:
+ unsigned _interval;
+
+};
+
+/**
+ * Subscriber class that is templated with the uorb subscription message type
+ */
+template<typename T>
+class __EXPORT SubscriberUORB :
+ public Subscriber<T>,
+ public SubscriberNode
+{
+public:
+
+ /**
+ * Construct SubscriberUORB by providing orb meta data without callback
+ * @param interval Minimal interval between calls to callback
+ */
+ SubscriberUORB(unsigned interval) :
+ SubscriberNode(interval),
+ _uorb_sub(new uORB::SubscriptionBase(T::handle(), interval))
+ {}
+
+ virtual ~SubscriberUORB() {
+ delete _uorb_sub;
+ };
+
+ /**
+ * Update Subscription
+ * Invoked by the list traversal in NodeHandle::spinOnce
+ */
+ virtual void update()
+ {
+ if (!_uorb_sub->updated()) {
+ /* Topic not updated, do not call callback */
+ return;
+ }
+
+ _uorb_sub->update(get_void_ptr());
+ };
+
+ /* Accessors*/
+ int getUORBHandle() { return _uorb_sub->getHandle(); }
+
+protected:
+ uORB::SubscriptionBase * _uorb_sub; /**< Handle to the subscription */
+
+ typename std::remove_reference<decltype(((T*)nullptr)->data())>::type getUORBData()
+ {
+ return (typename std::remove_reference<decltype(((T*)nullptr)->data())>::type)*_uorb_sub;
+ }
+
+ /**
+ * Get void pointer to last message value
+ */
+ void *get_void_ptr() { return (void *)&(this->_msg_current.data()); }
+
+};
+
+//XXX reduce to one class with overloaded constructor?
+template<typename T>
+class __EXPORT SubscriberUORBCallback :
+ public SubscriberUORB<T>
+{
+public:
+ /**
+ * Construct SubscriberUORBCallback by providing orb meta data
+ * @param cbf Callback, executed on receiving a new message
+ * @param interval Minimal interval between calls to callback
+ */
+ SubscriberUORBCallback(unsigned interval,
+ std::function<void(const T &)> cbf) :
+ SubscriberUORB<T>(interval)//,
+ // _cbf(cbf)
+ {}
+
+ virtual ~SubscriberUORBCallback() {};
+
+ /**
+ * Update Subscription
+ * Invoked by the list traversal in NodeHandle::spinOnce
+ * If new data is available the callback is called
+ */
+ virtual void update()
+ {
+ if (!this->_uorb_sub->updated()) {
+ /* Topic not updated, do not call callback */
+ return;
+ }
+
+ /* get latest data */
+ this->_uorb_sub->update(this->get_void_ptr());
+
+
+ /* Check if there is a callback */
+ if (_cbf == nullptr) {
+ return;
+ }
+
+ /* Call callback which performs actions based on this data */
+ _cbf(Subscriber<T>::get());
+
+ };
+
+protected:
+ std::function<void(const T &)> _cbf; /**< Callback that the user provided on the subscription */
+};
+#endif
+
+}
diff --git a/src/platforms/ros/eigen_math.h b/src/platforms/ros/eigen_math.h
new file mode 100755
index 000000000..c7271c157
--- /dev/null
+++ b/src/platforms/ros/eigen_math.h
@@ -0,0 +1,19 @@
+/*
+ * eigen_math.h
+ *
+ * Created on: Aug 25, 2014
+ * Author: roman
+ */
+
+#ifndef EIGEN_MATH_H_
+#define EIGEN_MATH_H_
+
+
+struct eigen_matrix_instance {
+ int numRows;
+ int numCols;
+ float *pData;
+};
+
+
+#endif /* EIGEN_MATH_H_ */
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/platforms/ros/geo.cpp
index ca7705456..6fad681c9 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/platforms/ros/geo.cpp
@@ -32,64 +32,142 @@
****************************************************************************/
/**
- * @file vehicle_control_mode.h
- * Definition of the vehicle_control_mode uORB topic.
+ * @file geo.c
*
- * All control apps should depend their actions based on the flags set here.
+ * Geo / math functions to perform geodesic calculations
*
- * @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>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
*/
-#ifndef VEHICLE_CONTROL_MODE
-#define VEHICLE_CONTROL_MODE
-
-#include <stdint.h>
+#include <geo/geo.h>
+#include <px4.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <math.h>
#include <stdbool.h>
-#include "../uORB.h"
-#include "vehicle_status.h"
+#include <string.h>
+#include <float.h>
-/**
- * @addtogroup topics @{
- */
+__EXPORT float _wrap_pi(float bearing)
+{
+ /* value is inf or NaN */
+ if (!isfinite(bearing)) {
+ return bearing;
+ }
+ int c = 0;
-/**
- * state machine / state of vehicle.
- *
- * Encodes the complete system state and is set by the commander app.
- */
+ while (bearing >= M_PI_F) {
+ bearing -= M_TWOPI_F;
-struct vehicle_control_mode_s {
- uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
+ if (c++ > 3) {
+ return NAN;
+ }
+ }
- bool flag_armed;
+ c = 0;
- bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
+ while (bearing < -M_PI_F) {
+ bearing += M_TWOPI_F;
- // XXX needs yet to be set by state machine helper
- bool flag_system_hil_enabled;
+ if (c++ > 3) {
+ return NAN;
+ }
+ }
- bool flag_control_manual_enabled; /**< true if manual input is mixed in */
- bool flag_control_auto_enabled; /**< true if onboard autopilot should act */
- bool flag_control_offboard_enabled; /**< true if offboard control should be used */
- bool flag_control_rates_enabled; /**< true if rates are stabilized */
- bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
- bool flag_control_force_enabled; /**< true if force control is mixed in */
- bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */
- bool flag_control_position_enabled; /**< true if position is controlled */
- bool flag_control_altitude_enabled; /**< true if altitude is controlled */
- bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */
- bool flag_control_termination_enabled; /**< true if flighttermination is enabled */
-};
+ return bearing;
+}
-/**
- * @}
- */
+__EXPORT float _wrap_2pi(float bearing)
+{
+ /* value is inf or NaN */
+ if (!isfinite(bearing)) {
+ return bearing;
+ }
+
+ int c = 0;
+
+ while (bearing >= M_TWOPI_F) {
+ bearing -= M_TWOPI_F;
+
+ if (c++ > 3) {
+ return NAN;
+ }
+ }
+
+ c = 0;
+
+ while (bearing < 0.0f) {
+ bearing += M_TWOPI_F;
+
+ if (c++ > 3) {
+ return NAN;
+ }
+ }
+
+ return bearing;
+}
+
+__EXPORT float _wrap_180(float bearing)
+{
+ /* value is inf or NaN */
+ if (!isfinite(bearing)) {
+ return bearing;
+ }
+
+ int c = 0;
+
+ while (bearing >= 180.0f) {
+ bearing -= 360.0f;
+
+ if (c++ > 3) {
+ return NAN;
+ }
+ }
+
+ c = 0;
+
+ while (bearing < -180.0f) {
+ bearing += 360.0f;
+
+ if (c++ > 3) {
+ return NAN;
+ }
+ }
+
+ return bearing;
+}
+
+__EXPORT float _wrap_360(float bearing)
+{
+ /* value is inf or NaN */
+ if (!isfinite(bearing)) {
+ return bearing;
+ }
+
+ int c = 0;
+
+ while (bearing >= 360.0f) {
+ bearing -= 360.0f;
+
+ if (c++ > 3) {
+ return NAN;
+ }
+ }
+
+ c = 0;
+
+ while (bearing < 0.0f) {
+ bearing += 360.0f;
-/* register this as object request broker structure */
-ORB_DECLARE(vehicle_control_mode);
+ if (c++ > 3) {
+ return NAN;
+ }
+ }
-#endif
+ return bearing;
+}
diff --git a/src/platforms/ros/nodes/README.md b/src/platforms/ros/nodes/README.md
new file mode 100644
index 000000000..aafc647ff
--- /dev/null
+++ b/src/platforms/ros/nodes/README.md
@@ -0,0 +1,22 @@
+# PX4 Nodes
+
+This directory contains several small ROS nodes which are intended to run alongside the PX4 multi-platform nodes in
+ROS. They act as a bridge between the PX4 specific topics and the ROS topics.
+
+## Joystick Input
+
+You will need to install the ros joystick packages
+See http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick
+
+### Arch Linux
+```sh
+yaourt -Sy ros-indigo-joystick-drivers --noconfirm
+```
+check joystick
+```sh
+ls /dev/input/
+ls -l /dev/input/js0
+```
+(replace 0 by the number you find with the first command)
+
+make sure the joystick is accessible by the `input` group and that your user is in the `input` group
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
new file mode 100644
index 000000000..bcee0b479
--- /dev/null
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.cpp
@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file att_estimator.cpp
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ * @author Roman Bapst <romanbapst@yahoo.de>
+*/
+
+#include "attitude_estimator.h"
+
+#include <px4/vehicle_attitude.h>
+#include <mathlib/mathlib.h>
+#include <platforms/px4_defines.h>
+
+AttitudeEstimator::AttitudeEstimator() :
+ _n(),
+ // _sub_modelstates(_n.subscribe("/gazebo/model_states", 1, &AttitudeEstimator::ModelStatesCallback, this)),
+ _sub_imu(_n.subscribe("/px4_multicopter/imu", 1, &AttitudeEstimator::ImuCallback, this)),
+ _vehicle_attitude_pub(_n.advertise<px4::vehicle_attitude>("vehicle_attitude", 1))
+{
+}
+
+void AttitudeEstimator::ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
+{
+ px4::vehicle_attitude msg_v_att;
+
+ /* Fill px4 attitude topic with contents from modelstates topic */
+
+ /* Convert quaternion to rotation matrix */
+ math::Quaternion quat;
+ //XXX: search for ardrone or other (other than 'plane') vehicle here
+ int index = 1;
+ quat(0) = (float)msg->pose[index].orientation.w;
+ quat(1) = (float)msg->pose[index].orientation.x;
+ quat(2) = (float) - msg->pose[index].orientation.y;
+ quat(3) = (float) - msg->pose[index].orientation.z;
+
+ msg_v_att.q[0] = quat(0);
+ msg_v_att.q[1] = quat(1);
+ msg_v_att.q[2] = quat(2);
+ msg_v_att.q[3] = quat(3);
+
+ math::Matrix<3, 3> rot = quat.to_dcm();
+
+ for (int i = 0; i < 3; i++) {
+ for (int j = 0; j < 3; j++) {
+ PX4_R(msg_v_att.R, i, j) = rot(i, j);
+ }
+ }
+
+ msg_v_att.R_valid = true;
+
+ math::Vector<3> euler = rot.to_euler();
+ msg_v_att.roll = euler(0);
+ msg_v_att.pitch = euler(1);
+ msg_v_att.yaw = euler(2);
+
+ //XXX this is in inertial frame
+ // msg_v_att.rollspeed = (float)msg->twist[index].angular.x;
+ // msg_v_att.pitchspeed = -(float)msg->twist[index].angular.y;
+ // msg_v_att.yawspeed = -(float)msg->twist[index].angular.z;
+
+ _vehicle_attitude_pub.publish(msg_v_att);
+}
+
+void AttitudeEstimator::ImuCallback(const sensor_msgs::ImuConstPtr &msg)
+{
+ px4::vehicle_attitude msg_v_att;
+
+ /* Fill px4 attitude topic with contents from modelstates topic */
+
+ /* Convert quaternion to rotation matrix */
+ math::Quaternion quat;
+ //XXX: search for ardrone or other (other than 'plane') vehicle here
+ int index = 1;
+ quat(0) = (float)msg->orientation.w;
+ quat(1) = (float)msg->orientation.x;
+ quat(2) = (float) - msg->orientation.y;
+ quat(3) = (float) - msg->orientation.z;
+
+ msg_v_att.q[0] = quat(0);
+ msg_v_att.q[1] = quat(1);
+ msg_v_att.q[2] = quat(2);
+ msg_v_att.q[3] = quat(3);
+
+ math::Matrix<3, 3> rot = quat.to_dcm();
+
+ for (int i = 0; i < 3; i++) {
+ for (int j = 0; j < 3; j++) {
+ PX4_R(msg_v_att.R, i, j) = rot(i, j);
+ }
+ }
+
+ msg_v_att.R_valid = true;
+
+ math::Vector<3> euler = rot.to_euler();
+ msg_v_att.roll = euler(0);
+ msg_v_att.pitch = euler(1);
+ msg_v_att.yaw = euler(2);
+
+ msg_v_att.rollspeed = (float)msg->angular_velocity.x;
+ msg_v_att.pitchspeed = -(float)msg->angular_velocity.y;
+ msg_v_att.yawspeed = -(float)msg->angular_velocity.z;
+
+ _vehicle_attitude_pub.publish(msg_v_att);
+}
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "attitude_estimator");
+ AttitudeEstimator m;
+
+ ros::spin();
+
+ return 0;
+}
diff --git a/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
new file mode 100644
index 000000000..f760a39d8
--- /dev/null
+++ b/src/platforms/ros/nodes/attitude_estimator/attitude_estimator.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file att_estimator.h
+ * Dummy attitude estimator that forwards attitude from gazebo to px4 topic
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <gazebo_msgs/ModelStates.h>
+#include <sensor_msgs/Imu.h>
+
+class AttitudeEstimator
+{
+public:
+ AttitudeEstimator();
+
+ ~AttitudeEstimator() {}
+
+protected:
+ void ModelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg);
+ void ImuCallback(const sensor_msgs::ImuConstPtr &msg);
+
+ ros::NodeHandle _n;
+ ros::Subscriber _sub_modelstates;
+ ros::Subscriber _sub_imu;
+ ros::Publisher _vehicle_attitude_pub;
+
+
+};
diff --git a/src/platforms/ros/nodes/commander/commander.cpp b/src/platforms/ros/nodes/commander/commander.cpp
new file mode 100644
index 000000000..b9fc296f9
--- /dev/null
+++ b/src/platforms/ros/nodes/commander/commander.cpp
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander.cpp
+ * Dummy commander node that publishes the various status topics
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "commander.h"
+
+#include <px4/manual_control_setpoint.h>
+#include <px4/vehicle_control_mode.h>
+#include <px4/vehicle_status.h>
+#include <platforms/px4_middleware.h>
+
+Commander::Commander() :
+ _n(),
+ _man_ctrl_sp_sub(_n.subscribe("manual_control_setpoint", 10, &Commander::ManualControlInputCallback, this)),
+ _vehicle_control_mode_pub(_n.advertise<px4::vehicle_control_mode>("vehicle_control_mode", 10)),
+ _actuator_armed_pub(_n.advertise<px4::actuator_armed>("actuator_armed", 10)),
+ _vehicle_status_pub(_n.advertise<px4::vehicle_status>("vehicle_status", 10)),
+ _parameter_update_pub(_n.advertise<px4::parameter_update>("parameter_update", 10)),
+ _msg_parameter_update(),
+ _msg_actuator_armed()
+{
+}
+
+void Commander::ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg)
+{
+ px4::vehicle_control_mode msg_vehicle_control_mode;
+ px4::vehicle_status msg_vehicle_status;
+
+ /* fill vehicle control mode */
+ //XXX hardcoded
+ msg_vehicle_control_mode.timestamp = px4::get_time_micros();
+ msg_vehicle_control_mode.flag_control_manual_enabled = true;
+ msg_vehicle_control_mode.flag_control_rates_enabled = true;
+ msg_vehicle_control_mode.flag_control_attitude_enabled = true;
+
+ /* fill actuator armed */
+ float arm_th = 0.95;
+ _msg_actuator_armed.timestamp = px4::get_time_micros();
+
+ if (_msg_actuator_armed.armed) {
+ /* Check for disarm */
+ if (msg->r < -arm_th && msg->z < (1 - arm_th)) {
+ _msg_actuator_armed.armed = false;
+ }
+
+ } else {
+ /* Check for arm */
+ if (msg->r > arm_th && msg->z < (1 - arm_th)) {
+ _msg_actuator_armed.armed = true;
+ }
+ }
+
+ /* fill vehicle status */
+ //XXX hardcoded
+ msg_vehicle_status.timestamp = px4::get_time_micros();
+ msg_vehicle_status.main_state = msg_vehicle_status.MAIN_STATE_MANUAL;
+ msg_vehicle_status.nav_state = msg_vehicle_status.NAVIGATION_STATE_MANUAL;
+ msg_vehicle_status.arming_state = msg_vehicle_status.ARMING_STATE_ARMED;
+ msg_vehicle_status.hil_state = msg_vehicle_status.HIL_STATE_OFF;
+ msg_vehicle_status.hil_state = msg_vehicle_status.VEHICLE_TYPE_QUADROTOR;
+ msg_vehicle_status.is_rotary_wing = true;
+
+ _vehicle_control_mode_pub.publish(msg_vehicle_control_mode);
+ _actuator_armed_pub.publish(_msg_actuator_armed);
+ _vehicle_status_pub.publish(msg_vehicle_status);
+
+ /* Fill parameter update */
+ if (px4::get_time_micros() - _msg_parameter_update.timestamp > 1e6) {
+ _msg_parameter_update.timestamp = px4::get_time_micros();
+ _parameter_update_pub.publish(_msg_parameter_update);
+ }
+}
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "commander");
+ Commander m;
+ ros::spin();
+ return 0;
+}
diff --git a/src/platforms/ros/nodes/commander/commander.h b/src/platforms/ros/nodes/commander/commander.h
new file mode 100644
index 000000000..bd4092b3a
--- /dev/null
+++ b/src/platforms/ros/nodes/commander/commander.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file commander.h
+ * Dummy commander node that publishes the various status topics
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <px4/manual_control_setpoint.h>
+#include <px4/parameter_update.h>
+#include <px4/actuator_armed.h>
+
+class Commander
+{
+public:
+ Commander();
+
+ ~Commander() {}
+
+protected:
+ /**
+ * Based on manual control input the status will be set
+ */
+ void ManualControlInputCallback(const px4::manual_control_setpointConstPtr &msg);
+
+ ros::NodeHandle _n;
+ ros::Subscriber _man_ctrl_sp_sub;
+ ros::Publisher _vehicle_control_mode_pub;
+ ros::Publisher _actuator_armed_pub;
+ ros::Publisher _vehicle_status_pub;
+ ros::Publisher _parameter_update_pub;
+
+ px4::parameter_update _msg_parameter_update;
+ px4::actuator_armed _msg_actuator_armed;
+
+};
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.cpp b/src/platforms/ros/nodes/manual_input/manual_input.cpp
new file mode 100644
index 000000000..688df50e0
--- /dev/null
+++ b/src/platforms/ros/nodes/manual_input/manual_input.cpp
@@ -0,0 +1,118 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manual_input.cpp
+ * Reads from the ros joystick topic and publishes to the px4 manual control input topic.
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "manual_input.h"
+
+#include <px4/manual_control_setpoint.h>
+#include <platforms/px4_middleware.h>
+
+ManualInput::ManualInput() :
+ _n(),
+ _joy_sub(_n.subscribe("joy", 1, &ManualInput::JoyCallback, this)),
+ _man_ctrl_sp_pub(_n.advertise<px4::manual_control_setpoint>("manual_control_setpoint", 1))
+{
+ double dz_default = 0.2;
+ /* Load parameters, default values work for Microsoft XBox Controller */
+ _n.param("map_x", _param_axes_map[0], 4);
+ _n.param("scale_x", _param_axes_scale[0], 1.0);
+ _n.param("off_x", _param_axes_offset[0], 0.0);
+ _n.param("dz_x", _param_axes_dz[0], dz_default);
+
+ _n.param("map_y", _param_axes_map[1], 3);
+ _n.param("scale_y", _param_axes_scale[1], -1.0);
+ _n.param("off_y", _param_axes_offset[1], 0.0);
+ _n.param("dz_y", _param_axes_dz[1], dz_default);
+
+ _n.param("map_z", _param_axes_map[2], 2);
+ _n.param("scale_z", _param_axes_scale[2], -0.5);
+ _n.param("off_z", _param_axes_offset[2], -1.0);
+ _n.param("dz_z", _param_axes_dz[2], dz_default);
+
+ _n.param("map_r", _param_axes_map[3], 0);
+ _n.param("scale_r", _param_axes_scale[3], -1.0);
+ _n.param("off_r", _param_axes_offset[3], 0.0);
+ _n.param("dz_r", _param_axes_dz[3], dz_default);
+
+}
+
+void ManualInput::JoyCallback(const sensor_msgs::JoyConstPtr &msg)
+{
+ px4::manual_control_setpoint msg_out;
+
+ /* Fill px4 manual control setpoint topic with contents from ros joystick */
+ /* Map sticks to x, y, z, r */
+ MapAxis(msg, _param_axes_map[0], _param_axes_scale[0], _param_axes_offset[0], _param_axes_dz[0], msg_out.x);
+ MapAxis(msg, _param_axes_map[1], _param_axes_scale[1], _param_axes_offset[1], _param_axes_dz[1], msg_out.y);
+ MapAxis(msg, _param_axes_map[2], _param_axes_scale[2], _param_axes_offset[2], _param_axes_dz[2], msg_out.z);
+ MapAxis(msg, _param_axes_map[3], _param_axes_scale[3], _param_axes_offset[3], _param_axes_dz[3], msg_out.r);
+ //ROS_INFO("x: %1.4f y: %1.4f z: %1.4f r: %1.4f", msg_out.x, msg_out.y, msg_out.z, msg_out.r);
+
+ /* Map buttons to switches */
+ //XXX todo
+ /* for now just publish switches in position for manual flight */
+ msg_out.mode_switch = 0;
+ msg_out.return_switch = 0;
+ msg_out.posctl_switch = 0;
+ msg_out.loiter_switch = 0;
+ msg_out.acro_switch = 0;
+ msg_out.offboard_switch = 0;
+
+ msg_out.timestamp = px4::get_time_micros();
+
+ _man_ctrl_sp_pub.publish(msg_out);
+}
+
+void ManualInput::MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset,
+ double deadzone, float &out)
+{
+ double val = msg->axes[map_index];
+
+ if (val + offset > deadzone || val + offset < -deadzone) {
+ out = (float)((val + offset)) * scale;
+ }
+}
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "manual_input");
+ ManualInput m;
+ ros::spin();
+ return 0;
+}
diff --git a/src/platforms/ros/nodes/manual_input/manual_input.h b/src/platforms/ros/nodes/manual_input/manual_input.h
new file mode 100644
index 000000000..93e0abe64
--- /dev/null
+++ b/src/platforms/ros/nodes/manual_input/manual_input.h
@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file manual_input.h
+ * Reads from the ros joystick topic and publishes to the px4 manual control setpoint topic.
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+*/
+
+#include "ros/ros.h"
+#include <sensor_msgs/Joy.h>
+
+class ManualInput
+{
+public:
+ ManualInput();
+
+ ~ManualInput() {}
+
+protected:
+ /**
+ * Takes ROS joystick message and converts/publishes to px4 manual control setpoint topic
+ */
+ void JoyCallback(const sensor_msgs::JoyConstPtr &msg);
+
+ /**
+ * Helper function to map and scale joystick input
+ */
+ void MapAxis(const sensor_msgs::JoyConstPtr &msg, int map_index, double scale, double offset, double deadzone,
+ float &out);
+
+ ros::NodeHandle _n;
+ ros::Subscriber _joy_sub;
+ ros::Publisher _man_ctrl_sp_pub;
+
+ /* Parameters */
+ int _param_axes_map[4];
+ double _param_axes_scale[4];
+ double _param_axes_offset[4];
+ double _param_axes_dz[4];
+
+
+};
diff --git a/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
new file mode 100644
index 000000000..54f5fa78b
--- /dev/null
+++ b/src/platforms/ros/nodes/mc_mixer/mc_mixer.cpp
@@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mc_mixer.cpp
+ * Dummy multicopter mixer for euroc simulator (gazebo)
+ *
+ * @author Roman Bapst <romanbapst@yahoo.de>
+*/
+#include <ros/ros.h>
+#include <px4.h>
+#include <lib/mathlib/mathlib.h>
+#include <mav_msgs/MotorSpeed.h>
+#include <string>
+
+class MultirotorMixer
+{
+public:
+
+ MultirotorMixer();
+
+ struct Rotor {
+ float roll_scale;
+ float pitch_scale;
+ float yaw_scale;
+ };
+
+ void actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg);
+ void actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg);
+
+private:
+
+ ros::NodeHandle _n;
+ ros::Subscriber _sub;
+ ros::Publisher _pub;
+
+ const Rotor *_rotors;
+
+ unsigned _rotor_count;
+
+ struct {
+ float control[8];
+ } inputs;
+
+ struct {
+ float control[8];
+ } outputs;
+
+ bool _armed;
+ ros::Subscriber _sub_actuator_armed;
+
+ void mix();
+};
+
+const MultirotorMixer::Rotor _config_x[] = {
+ { -0.707107, 0.707107, 1.00 },
+ { 0.707107, -0.707107, 1.00 },
+ { 0.707107, 0.707107, -1.00 },
+ { -0.707107, -0.707107, -1.00 },
+};
+
+const MultirotorMixer::Rotor _config_quad_plus[] = {
+ { -1.000000, 0.000000, 1.00 },
+ { 1.000000, 0.000000, 1.00 },
+ { 0.000000, 1.000000, -1.00 },
+ { -0.000000, -1.000000, -1.00 },
+};
+
+const MultirotorMixer::Rotor _config_quad_plus_euroc[] = {
+ { 0.000000, 1.000000, 1.00 },
+ { -0.000000, -1.000000, 1.00 },
+ { 1.000000, 0.000000, -1.00 },
+ { -1.000000, 0.000000, -1.00 },
+};
+const MultirotorMixer::Rotor _config_quad_wide[] = {
+ { -0.927184, 0.374607, 1.000000 },
+ { 0.777146, -0.629320, 1.000000 },
+ { 0.927184, 0.374607, -1.000000 },
+ { -0.777146, -0.629320, -1.000000 },
+};
+const MultirotorMixer::Rotor _config_quad_iris[] = {
+ { -0.876559, 0.481295, 1.000000 },
+ { 0.826590, -0.562805, 1.000000 },
+ { 0.876559, 0.481295, -1.000000 },
+ { -0.826590, -0.562805, -1.000000 },
+};
+
+const MultirotorMixer::Rotor *_config_index[5] = {
+ &_config_x[0],
+ &_config_quad_plus[0],
+ &_config_quad_plus_euroc[0],
+ &_config_quad_wide[0],
+ &_config_quad_iris[0]
+};
+
+MultirotorMixer::MultirotorMixer():
+ _n(),
+ _rotor_count(4),
+ _rotors(_config_index[0])
+{
+ _sub = _n.subscribe("actuator_controls_0", 1, &MultirotorMixer::actuatorControlsCallback, this);
+ _pub = _n.advertise<mav_msgs::MotorSpeed>("/mixed_motor_commands", 10);
+
+ if (!_n.hasParam("motor_scaling_radps")) {
+ _n.setParam("motor_scaling_radps", 150.0);
+ }
+
+ if (!_n.hasParam("motor_offset_radps")) {
+ _n.setParam("motor_offset_radps", 600.0);
+ }
+
+ if (!_n.hasParam("mixer")) {
+ _n.setParam("mixer", "x");
+ }
+
+ _sub_actuator_armed = _n.subscribe("actuator_armed", 1, &MultirotorMixer::actuatorArmedCallback, this);
+}
+
+void MultirotorMixer::mix()
+{
+ float roll = math::constrain(inputs.control[0], -1.0f, 1.0f);
+ float pitch = math::constrain(inputs.control[1], -1.0f, 1.0f);
+ float yaw = math::constrain(inputs.control[2], -1.0f, 1.0f);
+ float thrust = math::constrain(inputs.control[3], 0.0f, 1.0f);
+ float min_out = 0.0f;
+ float max_out = 0.0f;
+
+ /* perform initial mix pass yielding unbounded outputs, ignore yaw */
+ for (unsigned i = 0; i < _rotor_count; i++) {
+ float out = roll * _rotors[i].roll_scale
+ + pitch * _rotors[i].pitch_scale + thrust;
+
+ /* limit yaw if it causes outputs clipping */
+ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
+ yaw = -out / _rotors[i].yaw_scale;
+ }
+
+ /* calculate min and max output values */
+ if (out < min_out) {
+ min_out = out;
+ }
+
+ if (out > max_out) {
+ max_out = out;
+ }
+
+ outputs.control[i] = out;
+ }
+
+ /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
+ if (min_out < 0.0f) {
+ float scale_in = thrust / (thrust - min_out);
+
+ /* mix again with adjusted controls */
+ for (unsigned i = 0; i < _rotor_count; i++) {
+ outputs.control[i] = scale_in
+ * (roll * _rotors[i].roll_scale
+ + pitch * _rotors[i].pitch_scale) + thrust;
+ }
+
+ } else {
+ /* roll/pitch mixed without limiting, add yaw control */
+ for (unsigned i = 0; i < _rotor_count; i++) {
+ outputs.control[i] += yaw * _rotors[i].yaw_scale;
+ }
+ }
+
+ /* scale down all outputs if some outputs are too large, reduce total thrust */
+ float scale_out;
+
+ if (max_out > 1.0f) {
+ scale_out = 1.0f / max_out;
+
+ } else {
+ scale_out = 1.0f;
+ }
+
+ /* scale outputs to range _idle_speed..1, and do final limiting */
+ for (unsigned i = 0; i < _rotor_count; i++) {
+ outputs.control[i] = math::constrain(outputs.control[i], 0.0f, 1.0f);
+ }
+}
+
+void MultirotorMixer::actuatorControlsCallback(const PX4_TOPIC_T(actuator_controls_0) &msg)
+{
+ // read message
+ for (int i = 0; i < msg.NUM_ACTUATOR_CONTROLS; i++) {
+ inputs.control[i] = msg.control[i];
+ }
+
+ // switch mixer if necessary
+ std::string mixer_name;
+ _n.getParamCached("mixer", mixer_name);
+ if (mixer_name == "x") {
+ _rotors = _config_index[0];
+ ROS_WARN("using x");
+ } else if (mixer_name == "+") {
+ _rotors = _config_index[1];
+ } else if (mixer_name == "e") {
+ _rotors = _config_index[2];
+ } else if (mixer_name == "w") {
+ _rotors = _config_index[3];
+ ROS_WARN("using w");
+ } else if (mixer_name == "i") {
+ _rotors = _config_index[4];
+ ROS_WARN("using i");
+ }
+ ROS_WARN("mixer_name %s", mixer_name.c_str());
+
+ // mix
+ mix();
+
+ // publish message
+ mav_msgs::MotorSpeed rotor_vel_msg;
+ double scaling;
+ double offset;
+ _n.getParamCached("motor_scaling_radps", scaling);
+ _n.getParamCached("motor_offset_radps", offset);
+
+ if (_armed) {
+ for (int i = 0; i < _rotor_count; i++) {
+ rotor_vel_msg.motor_speed.push_back(outputs.control[i] * scaling + offset);
+ }
+
+ } else {
+ for (int i = 0; i < _rotor_count; i++) {
+ rotor_vel_msg.motor_speed.push_back(0.0);
+ }
+ }
+
+ _pub.publish(rotor_vel_msg);
+}
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "mc_mixer");
+ MultirotorMixer mixer;
+ ros::spin();
+
+ return 0;
+}
+
+void MultirotorMixer::actuatorArmedCallback(const PX4_TOPIC_T(actuator_armed) &msg)
+{
+ _armed = msg.armed;
+}
diff --git a/src/platforms/ros/perf_counter.cpp b/src/platforms/ros/perf_counter.cpp
new file mode 100755
index 000000000..a71801397
--- /dev/null
+++ b/src/platforms/ros/perf_counter.cpp
@@ -0,0 +1,176 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file perf_counter.cpp
+ *
+ * Empty function calls for ros compatibility
+ *
+ * @author Roman Bapst <bapstr@ethz.ch>
+ *
+ */
+#include <stdlib.h>
+#include <stdio.h>
+#include <systemlib/perf_counter.h>
+
+
+
+perf_counter_t perf_alloc(enum perf_counter_type type, const char *name)
+{
+ return NULL;
+}
+
+/**
+ * Free a counter.
+ *
+ * @param handle The performance counter's handle.
+ */
+void perf_free(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Count a performance event.
+ *
+ * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc.
+ *
+ * @param handle The handle returned from perf_alloc.
+ */
+void perf_count(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Begin a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ *
+ * @param handle The handle returned from perf_alloc.
+ */
+void perf_begin(perf_counter_t handle)
+{
+
+}
+
+/**
+ * End a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresopnding perf_begin call, or if perf_cancel
+ * has been called subsequently, no change is made to the counter.
+ *
+ * @param handle The handle returned from perf_alloc.
+ */
+void perf_end(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Cancel a performance event.
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * It reverts the effect of a previous perf_begin.
+ *
+ * @param handle The handle returned from perf_alloc.
+ */
+void perf_cancel(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Reset a performance counter.
+ *
+ * This call resets performance counter to initial state
+ *
+ * @param handle The handle returned from perf_alloc.
+ */
+void perf_reset(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print one performance counter to stdout
+ *
+ * @param handle The counter to print.
+ */
+void perf_print_counter(perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print one performance counter to a fd.
+ *
+ * @param fd File descriptor to print to - e.g. 0 for stdout
+ * @param handle The counter to print.
+ */
+void perf_print_counter_fd(int fd, perf_counter_t handle)
+{
+
+}
+
+/**
+ * Print all of the performance counters.
+ *
+ * @param fd File descriptor to print to - e.g. 0 for stdout
+ */
+void perf_print_all(int fd)
+{
+
+}
+
+/**
+ * Reset all of the performance counters.
+ */
+void perf_reset_all(void)
+{
+
+}
+
+/**
+ * Return current event_count
+ *
+ * @param handle The counter returned from perf_alloc.
+ * @return event_count
+ */
+uint64_t perf_event_count(perf_counter_t handle)
+{
+
+}
+
+
diff --git a/src/platforms/ros/px4_nodehandle.cpp b/src/platforms/ros/px4_nodehandle.cpp
new file mode 100644
index 000000000..6ac3c76d3
--- /dev/null
+++ b/src/platforms/ros/px4_nodehandle.cpp
@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_nodehandle.cpp
+ *
+ * PX4 Middleware Wrapper Nodehandle
+ */
+#include <px4_nodehandle.h>
+
+namespace px4
+{
+}
+
diff --git a/src/platforms/ros/px4_publisher.cpp b/src/platforms/ros/px4_publisher.cpp
new file mode 100644
index 000000000..f02dbe4c9
--- /dev/null
+++ b/src/platforms/ros/px4_publisher.cpp
@@ -0,0 +1,41 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_publisher.cpp
+ *
+ * PX4 Middleware Wrapper for Publisher
+ */
+#include <px4_publisher.h>
+
+
diff --git a/src/platforms/ros/px4_ros_impl.cpp b/src/platforms/ros/px4_ros_impl.cpp
new file mode 100644
index 000000000..854986a7f
--- /dev/null
+++ b/src/platforms/ros/px4_ros_impl.cpp
@@ -0,0 +1,56 @@
+/****************************************************************************
+ *
+ * Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in
+ * the documentation and/or other materials provided with the
+ * distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ * used to endorse or promote products derived from this software
+ * without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_ros_impl.cpp
+ *
+ * PX4 Middleware Wrapper ROS Implementation
+ */
+
+#include <px4.h>
+
+namespace px4
+{
+
+void init(int argc, char *argv[], const char *process_name)
+{
+ ros::init(argc, argv, process_name);
+}
+
+uint64_t get_time_micros()
+{
+ ros::Time time = ros::Time::now();
+ return time.sec * 1e6 + time.nsec / 1000;
+}
+
+}
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index c464ab84d..0935bf44a 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -39,6 +39,7 @@
*/
#include <nuttx/config.h>
+#include <platforms/px4_defines.h>
#include <stdio.h>
#include <stdlib.h>
@@ -60,6 +61,10 @@
#include "drivers/drv_pwm_output.h"
#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_0.h>
+#include <uORB/topics/actuator_controls_1.h>
+#include <uORB/topics/actuator_controls_2.h>
+#include <uORB/topics/actuator_controls_3.h>
static void usage(const char *reason);
__EXPORT int esc_calib_main(int argc, char *argv[]);