Reworked control interface, needs testing / validation

4 files changed, 29 insertions, 16 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 530033701..3f0d56245 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -58,13 +58,14 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/ardrone_motors_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/actuator_controls.h>
 
 #include <systemlib/perf_counter.h>
 
 #include "multirotor_attitude_control.h"
+#include "multirotor_rate_control.h"
 
 __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 
@@ -93,18 +94,18 @@ mc_thread_main(int argc, char *argv[])
 	memset(&manual, 0, sizeof(manual));
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
-	struct ardrone_motors_setpoint_s setpoint;
-	memset(&setpoint, 0, sizeof(setpoint));
+	struct vehicle_rates_setpoint_s rates_sp;
+	memset(&rates_sp, 0, sizeof(rates_sp));
 
 	struct actuator_controls_s actuators;
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
 	int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	int setpoint_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
 	int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
 
 	/* 
 	 * Do not rate-limit the loop to prevent aliasing
@@ -145,6 +146,12 @@ mc_thread_main(int argc, char *argv[])
 		orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
 		/* get a local copy of attitude setpoint */
 		orb_copy(ORB_ID(vehicle_attitude_setpoint), att_setpoint_sub, &att_sp);
+		/* get a local copy of rates setpoint */
+		bool updated;
+		orb_check(setpoint_sub, &updated);
+		if (updated) {
+			orb_copy(ORB_ID(vehicle_rates_setpoint), setpoint_sub, &rates_sp);
+		}
 		/* get a local copy of the current sensor values */
 		orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
 
@@ -155,16 +162,15 @@ mc_thread_main(int argc, char *argv[])
 			/* manual inputs, from RC control or joystick */
 			att_sp.roll_body = manual.roll;
 			att_sp.pitch_body = manual.pitch;
-			att_sp.yaw_rate_body = manual.yaw;
+			att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
+			/* set yaw rate */
+			rates_sp.yaw = manual.yaw;
 			att_sp.timestamp = hrt_absolute_time();
 			
 			if (motor_test_mode) {
 				att_sp.roll_body = 0.0f;
 				att_sp.pitch_body = 0.0f;
 				att_sp.yaw_body = 0.0f;
-				att_sp.roll_rate_body = 0.0f;
-				att_sp.pitch_rate_body = 0.0f;
-				att_sp.yaw_rate_body = 0.0f;
 				att_sp.thrust = 0.1f;
 			} else {
 				att_sp.thrust = manual.throttle;
@@ -184,7 +190,14 @@ mc_thread_main(int argc, char *argv[])
 
 		/* XXX could be also run in an independent loop */
 		if (state.flag_control_rates_enabled) {
-			multirotor_control_rates(&att_sp, &raw.gyro_rad_s, &actuators);
+			/* lowpass gyros */
+			// XXX
+			static float gyro_lp[3] = {0, 0, 0};
+			gyro_lp[0] = raw.gyro_rad_s[0];
+			gyro_lp[1] = raw.gyro_rad_s[1];
+			gyro_lp[2] = raw.gyro_rad_s[2];
+
+			multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
 		}
 
 		/* STEP 3: publish the result to the vehicle actuators */
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 997aa0c10..d7413913b 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -243,7 +243,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + p.att_yoff,
 					att->roll, att->rollspeed, deltaT);
 	/* control yaw rate */
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_rate_body, att->yawspeed, 0.0f, deltaT);
+	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT);
 
 	/*
 	 * compensate the vertical loss of thrust
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index 8a3ac78cd..fefd1f767 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -132,7 +132,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 	return OK;
 }
 
-void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
+void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	const float rates[], struct actuator_controls_s *actuators)
 {
 	static uint64_t last_run = 0;
@@ -179,13 +179,13 @@ void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
 	/* calculate current control outputs */
 	
 	/* control pitch (forward) output */
-	float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch_rate_body,
+	float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch,
 					rates[1], 0.0f, deltaT);
 	/* control roll (left/right) output */
-	float roll_control = pid_calculate(&roll_controller, rate_sp->roll_rate_body,
+	float roll_control = pid_calculate(&roll_controller, rate_sp->roll,
 					rates[0], 0.0f, deltaT);
 	/* control yaw rate */
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw_rate_body, rates[2], 0.0f, deltaT);
+	float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
 
 	/*
 	 * compensate the vertical loss of thrust
diff --git a/apps/multirotor_att_control/multirotor_rate_control.h b/apps/multirotor_att_control/multirotor_rate_control.h
index 6f34d99e7..3c3c50801 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.h
+++ b/apps/multirotor_att_control/multirotor_rate_control.h
@@ -47,10 +47,10 @@
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 
-void multirotor_control_rates(const struct vehicle_attitude_setpoint_s *rate_sp,
+void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	const float rates[], struct actuator_controls_s *actuators);
 
 #endif /* MULTIROTOR_RATE_CONTROL_H_ */