Added ctrl debugging values

1 files changed, 14 insertions, 2 deletions

diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index 322c5485a..01bf383e2 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -54,6 +54,9 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_control_debug.h>
+
 
 PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
@@ -175,6 +178,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	static bool initialized = false;
 
+	static struct vehicle_control_debug_s control_debug;
+	static orb_advert_t control_debug_pub;
+	
+
+
 	/* initialize the pid controllers when the function is called for the first time */
 	if (initialized == false) {
 		parameters_init(&h);
@@ -185,6 +193,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 					 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
 		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f,
 					 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC);
+
+		control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug);
 	}
 
 	/* load new parameters with lower rate */
@@ -197,11 +207,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	/* control pitch (forward) output */
 	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
-			rates[1], 0.0f, deltaT, NULL, NULL, NULL);
+			rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d);
 
 	/* control roll (left/right) output */
 	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
-			rates[0], 0.0f, deltaT, NULL, NULL, NULL);
+			rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d);
 
 	/* increase resilience to faulty control inputs */
 	if (isfinite(pitch_control)) {
@@ -236,4 +246,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	actuators->control[3] = rate_sp->thrust;
 
 	motor_skip_counter++;
+
+	orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug);
 }