Add a set of ORB topics for advertising actuator outputs.

This is part of \#7

3 files changed, 101 insertions, 25 deletions

diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp
index 9d8847246..83bc62509 100644
--- a/apps/px4/fmu/fmu.cpp
+++ b/apps/px4/fmu/fmu.cpp
@@ -64,6 +64,7 @@
 #include <arch/board/up_pwm_servo.h>
 
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
 
 
 class FMUServo : public device::CDev
@@ -88,6 +89,7 @@ private:
 	int		_task;
 	int		_t_actuators;
 	int		_t_armed;
+	int		_t_outputs;
 	unsigned	_num_outputs;
 
 	volatile bool	_task_should_exit;
@@ -98,15 +100,12 @@ private:
 	actuator_controls_s _controls;
 
 	static void	task_main_trampoline(int argc, char *argv[]);
-	void		task_main();
+	void		task_main() __attribute__((noreturn));
 
-	static int	control_callback_trampoline(uintptr_t handle,
+	static int	control_callback(uintptr_t handle,
 			uint8_t control_group,
 			uint8_t control_index,
 			float &input);
-	int		control_callback(uint8_t control_group,
-					 uint8_t control_index,
-					 float &input);
 };
 
 namespace
@@ -212,6 +211,11 @@ FMUServo::task_main()
 	_t_armed = orb_subscribe(ORB_ID(actuator_armed));
 	orb_set_interval(_t_armed, 100);		/* 10Hz update rate */
 
+	/* advertise the mixed control outputs */
+	struct actuator_output_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+	_t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
+
 	struct pollfd fds[2];
 	fds[0].fd = _t_actuators;
 	fds[0].events = POLLIN;
@@ -237,7 +241,6 @@ FMUServo::task_main()
 
 		/* do we have a control update? */
 		if (fds[0].revents & POLLIN) {
-			float outputs[num_outputs];
 
 			/* get controls - must always do this to avoid spinning */
 			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
@@ -246,14 +249,20 @@ FMUServo::task_main()
 			if (_mixers != nullptr) {
 
 				/* do mixing */
-				_mixers->mix(&outputs[0], num_outputs);
+				_mixers->mix(&outputs.output[0], num_outputs);
 
 				/* iterate actuators */
 				for (unsigned i = 0; i < num_outputs; i++) {
 
 					/* scale for PWM output 900 - 2100us */
-					up_pwm_servo_set(i, 1500 + (600 * outputs[i]));
+					outputs.output[i] = 1500 + (600 * outputs.output[i]);
+
+					/* output to the servo */
+					up_pwm_servo_set(i, outputs.output[i]);
 				}
+
+				/* and publish for anyone that cares to see */
+				orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs);
 			}
 		}
 
@@ -264,13 +273,14 @@ FMUServo::task_main()
 			/* get new value */
 			orb_copy(ORB_ID(actuator_armed), _t_armed, &aa);
 
-			/* update PMW servo armed status */
+			/* update PWM servo armed status */
 			up_pwm_servo_arm(aa.armed);
 		}
 	}
 
 	::close(_t_actuators);
 	::close(_t_armed);
+	::close(_t_outputs);
 
 	/* make sure servos are off */
 	up_pwm_servo_deinit();
@@ -285,24 +295,14 @@ FMUServo::task_main()
 }
 
 int
-FMUServo::control_callback_trampoline(uintptr_t handle,
+FMUServo::control_callback(uintptr_t handle,
 				      uint8_t control_group,
 				      uint8_t control_index,
 				      float &input)
 {
-	return ((FMUServo *)handle)->control_callback(control_group, control_index, input);
-}
-
-int
-FMUServo::control_callback(uint8_t control_group,
-			   uint8_t control_index,
-			   float &input)
-{
-	/* XXX currently only supporting group zero */
-	if ((control_group != 0) || (control_index > NUM_ACTUATOR_CONTROLS))
-		return -1;
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
-	input = _controls.control[control_index];
+	input = controls->control[control_index];
 	return 0;
 }
 
@@ -377,7 +377,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 	case MIXERIOCADDSIMPLE: {
 			mixer_simple_s *mixinfo = (mixer_simple_s *)arg;
 
-			SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo);
+			SimpleMixer *mixer = new SimpleMixer(control_callback,
+							     (uintptr_t)&_controls, mixinfo);
 
 			if (mixer->check()) {
 				delete mixer;
@@ -385,7 +386,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 			} else {
 				if (_mixers == nullptr)
-					_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
+					_mixers = new MixerGroup(control_callback,
+								 (uintptr_t)&_controls);
 
 				_mixers->add_mixer(mixer);
 			}
@@ -406,7 +408,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg)
 				_mixers = nullptr;
 			}
 
-			_mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this);
+			_mixers = new MixerGroup(control_callback, (uintptr_t)&_controls);
 
 			if (_mixers->load_from_file(path) != 0) {
 				delete _mixers;
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index d6b566b6a..6ff66fceb 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -114,3 +114,9 @@ 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);
 ORB_DEFINE(actuator_armed, struct actuator_armed_s);
+
+#include "topics/actuator_outputs.h"
+ORB_DEFINE(actuator_outputs_0, struct actuator_output_s);
+ORB_DEFINE(actuator_outputs_1, struct actuator_output_s);
+ORB_DEFINE(actuator_outputs_2, struct actuator_output_s);
+ORB_DEFINE(actuator_outputs_3, struct actuator_output_s);
diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h
new file mode 100644
index 000000000..63441a059
--- /dev/null
+++ b/apps/uORB/topics/actuator_outputs.h
@@ -0,0 +1,68 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file actuator_outputs.h
+ *
+ * Actuator output values.
+ *
+ * Values published to these topics are the outputs of the control mixing
+ * system as sent to the actuators (servos, motors, etc.) that operate
+ * the vehicle.
+ *
+ * Each topic can be published by a single output driver.
+ */
+
+#ifndef TOPIC_ACTUATOR_OUTPUTS_H
+#define TOPIC_ACTUATOR_OUTPUTS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_ACTUATOR_OUTPUTS		16
+#define NUM_ACTUATOR_OUTPUT_GROUPS	4	/**< for sanity checking */
+
+struct actuator_output_s {
+	float	output[NUM_ACTUATOR_OUTPUTS];
+};
+
+/* actuator output sets; this list can be expanded as more drivers emerge */
+ORB_DECLARE(actuator_outputs_0);
+ORB_DECLARE(actuator_outputs_1);
+ORB_DECLARE(actuator_outputs_2);
+ORB_DECLARE(actuator_outputs_3);
+
+/* output sets with pre-defined applications */
+#define ORB_ID_VEHICLE_CONTROLS		ORB_ID(actuator_outputs_0)
+
+#endif
\ No newline at end of file