Added actuator controls effective topic

3 files changed, 105 insertions, 4 deletions

diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index 70b8ad6de..f15c74a22 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -45,6 +45,7 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_controls_effective.h>
 #include <systemlib/err.h>
 
 #include "ardrone_motor_control.h"
@@ -385,6 +386,11 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
 	float yaw_factor = 1.0f;
 
+	static bool initialized = false;
+	/* publish effective outputs */
+	static struct actuator_controls_effective_s actuator_controls_effective;
+	static orb_advert_t actuator_controls_effective_pub;
+
 	if (motor_thrust <= min_thrust) {
 		motor_thrust = min_thrust;
 		output_band = 0.0f;
@@ -417,17 +423,18 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	      && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
 
 		yaw_factor = 0.5f;
+		yaw_control *= yaw_factor;
 		// FRONT (MOTOR 1)
-		motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor);
+		motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
 
 		// RIGHT (MOTOR 2)
-		motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor);
+		motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
 
 		// BACK (MOTOR 3)
-		motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor);
+		motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
 
 		// LEFT (MOTOR 4)
-		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor);
+		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
 	}
 
 	for (int i = 0; i < 4; i++) {
@@ -441,6 +448,23 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 		}
 	}
 
+	/* publish effective outputs */
+	actuator_controls_effective.control_effective[0] = roll_control;
+	actuator_controls_effective.control_effective[1] = pitch_control;
+	/* yaw output after limiting */
+	actuator_controls_effective.control_effective[2] = yaw_control;
+	/* possible motor thrust limiting */
+	actuator_controls_effective.control_effective[3] = (motor_calc[0] + motor_calc[1] + motor_calc[2] + motor_calc[3]) / 4.0f;
+
+	if (!initialized) {
+		/* advertise and publish */
+		actuator_controls_effective_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, &actuator_controls_effective);
+		initialized = true;
+	} else {
+		/* already initialized, just publishing */
+		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_controls_effective_pub, &actuator_controls_effective);
+	}
+
 	/* set the motor values */
 
 	/* scale up from 0..1 to 10..512) */
diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index c2e82aad9..dbee15050 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -113,6 +113,7 @@ ORB_DEFINE(optical_flow, struct optical_flow_s);
 #include "topics/subsystem_info.h"
 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);
@@ -120,6 +121,13 @@ 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);
 
+/* actuator controls, as set by actuators / mixers after limiting */
+#include "topics/actuator_controls_effective.h"
+ORB_DEFINE(actuator_controls_effective_0, struct actuator_controls_effective_s);
+ORB_DEFINE(actuator_controls_effective_1, struct actuator_controls_effective_s);
+ORB_DEFINE(actuator_controls_effective_2, struct actuator_controls_effective_s);
+ORB_DEFINE(actuator_controls_effective_3, struct actuator_controls_effective_s);
+
 #include "topics/actuator_outputs.h"
 ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
diff --git a/apps/uORB/topics/actuator_controls_effective.h b/apps/uORB/topics/actuator_controls_effective.h
new file mode 100644
index 000000000..3c72e4cb7
--- /dev/null
+++ b/apps/uORB/topics/actuator_controls_effective.h
@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   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_controls.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
+ */
+
+#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
+#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+#define NUM_ACTUATOR_CONTROLS		8
+#define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
+
+struct actuator_controls_effective_s {
+	uint64_t timestamp;
+	float	control_effective[NUM_ACTUATOR_CONTROLS];
+};
+
+/* actuator control sets; this list can be expanded as more controllers emerge */
+ORB_DECLARE(actuator_controls_effective_0);
+ORB_DECLARE(actuator_controls_effective_1);
+ORB_DECLARE(actuator_controls_effective_2);
+ORB_DECLARE(actuator_controls_effective_3);
+
+/* control sets with pre-defined applications */
+#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE	ORB_ID(actuator_controls_effective_0)
+
+#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */
\ No newline at end of file