Added actuator controls effective topic

1 files changed, 28 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) */