Untested, but fully implemented attitude and/or inner rate control

3 files changed, 106 insertions, 49 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 5bc0d5fa3..21c720096 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -56,6 +56,7 @@
 #include <sys/prctl.h>
 #include <arch/board/up_hrt.h>
 #include <uORB/uORB.h>
+#include <drivers/drv_gyro.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -72,16 +73,66 @@
 
 __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 
-
-static enum {
-	CONTROL_MODE_RATES = 0,
-	CONTROL_MODE_ATTITUDE = 1,
-} control_mode;
-
-
 static bool thread_should_exit;
 static int mc_task;
 static bool motor_test_mode = false;
+static struct actuator_controls_s actuators;
+static orb_advert_t actuator_pub;
+
+/**
+ * Perform rate control right after gyro reading
+ */
+static void *rate_control_thread_main(void *arg)
+{
+	prctl(PR_SET_NAME, "mc rate control", getpid());
+
+	int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
+	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+
+	struct pollfd fds = { .fd = gyro_sub, .events = POLLIN };
+
+	struct gyro_report gyro_report;
+	struct vehicle_rates_setpoint_s rates_sp;
+	memset(&rates_sp, 0, sizeof(rates_sp));
+	float gyro_lp[3] = {0.0f, 0.0f, 0.0f};
+
+	while (!thread_should_exit) {
+		/* rate control at maximum rate */
+		/* wait for a sensor update, check for exit condition every 1000 ms */
+		int ret = poll(&fds, 1, 1000);
+
+		if (ret < 0) {
+			/* XXX this is seriously bad - should be an emergency */
+		} else if (ret == 0) {
+			/* XXX this means no sensor data - should be critical or emergency */
+			printf("[mc att control] WARNING: Not getting gyro data, no rate control\n");
+		} else {
+			/* get data */
+			orb_copy(ORB_ID(sensor_gyro), gyro_sub, &gyro_report);
+			bool rates_sp_valid = false;
+			orb_check(rates_sp_sub, &rates_sp_valid);
+			if (rates_sp_valid) {
+				orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp);
+			}
+
+			/* perform local lowpass */
+
+			/* apply controller */
+			// if (state.flag_control_rates_enabled) {
+				/* lowpass gyros */
+				// XXX
+				gyro_lp[0] = gyro_report.x;
+				gyro_lp[1] = gyro_report.y;
+				gyro_lp[2] = gyro_report.z;
+
+				multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
+				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+			// }
+		}
+	}
+
+	return NULL;
+}
 
 static int
 mc_thread_main(int argc, char *argv[])
@@ -102,8 +153,6 @@ mc_thread_main(int argc, char *argv[])
 	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));
@@ -127,8 +176,9 @@ mc_thread_main(int argc, char *argv[])
 		actuators.control[i] = 0.0f;
 	}
 
-	orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
+	actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators);
 	orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+	orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
 
 	/* register the perf counter */
 	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
@@ -136,6 +186,13 @@ mc_thread_main(int argc, char *argv[])
 	/* welcome user */
 	printf("[multirotor_att_control] starting\n");
 
+	/* ready, spawn pthread */
+	pthread_attr_t rate_control_attr;
+	pthread_attr_init(&rate_control_attr);
+	pthread_attr_setstacksize(&rate_control_attr, 2048);
+	pthread_t rate_control_thread;
+	pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL);
+
 	while (!thread_should_exit) {
 
 		/* wait for a sensor update, check for exit condition every 500 ms */
@@ -170,16 +227,21 @@ mc_thread_main(int argc, char *argv[])
 			att_sp.yaw_body = manual.yaw; // XXX Hack, remove, switch to yaw rate controller
 			/* set yaw rate */
 			rates_sp.yaw = manual.yaw;
+			att_sp.thrust = manual.throttle;
 			att_sp.timestamp = hrt_absolute_time();
+			/* STEP 2: publish the result to the vehicle actuators */
+			orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 			
 			if (motor_test_mode) {
 				att_sp.roll_body = 0.0f;
 				att_sp.pitch_body = 0.0f;
 				att_sp.yaw_body = 0.0f;
 				att_sp.thrust = 0.1f;
-			} else {
-				att_sp.thrust = manual.throttle;
+				att_sp.timestamp = hrt_absolute_time();
+				/* STEP 2: publish the result to the vehicle actuators */
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 			}
+
 		} else if (state.flag_control_offboard_enabled) {
 			/* offboard inputs */
 			if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) {
@@ -187,38 +249,32 @@ mc_thread_main(int argc, char *argv[])
 				rates_sp.pitch = offboard_sp.p2;
 				rates_sp.yaw = offboard_sp.p3;
 				rates_sp.thrust = offboard_sp.p4;
+				rates_sp.timestamp = hrt_absolute_time();
+				orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 			} else if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE) {
 				att_sp.roll_body = offboard_sp.p1;
 				att_sp.pitch_body = offboard_sp.p2;
 				att_sp.yaw_body = offboard_sp.p3;
 				att_sp.thrust = offboard_sp.p4;
+				att_sp.timestamp = hrt_absolute_time();
+				/* STEP 2: publish the result to the vehicle actuators */
+				orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 			}
 
 			/* decide wether we want rate or position input */
 		}
 
-		/** STEP 2: Identify the controller setup to run and set up the inputs correctly */
+		/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
 
 		/* run attitude controller */
-		if (state.flag_control_attitude_enabled) {
-			multirotor_control_attitude(&att_sp, &att, &actuators);
-		}
-
-		/* XXX could be also run in an independent loop */
-		if (state.flag_control_rates_enabled) {
-			/* 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);
+		if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
+			multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
+			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
+		} else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
+			multirotor_control_attitude(&att_sp, &att, &rates_sp, NULL);
+			orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp);
 		}
 
-		/* STEP 3: publish the result to the vehicle actuators */
-		orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-		orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp);
 
 		perf_end(mc_loop_perf);
 	}
@@ -240,6 +296,8 @@ mc_thread_main(int argc, char *argv[])
 	perf_print_counter(mc_loop_perf);
 	perf_free(mc_loop_perf);
 
+	pthread_join(rate_control_thread, NULL);
+
 	fflush(stdout);
 	exit(0);
 }
@@ -258,22 +316,10 @@ usage(const char *reason)
 int multirotor_att_control_main(int argc, char *argv[])
 {
 	int	ch;
-
-	control_mode = CONTROL_MODE_RATES;
 	unsigned int optioncount = 0;
 
 	while ((ch = getopt(argc, argv, "tm:")) != EOF) {
 		switch (ch) {
-		case 'm':
-			if (!strcmp(optarg, "rates")) {
-				control_mode = CONTROL_MODE_RATES;
-			} else if (!strcmp(optarg, "attitude")) {
-				control_mode = CONTROL_MODE_RATES;
-			} else {
-				usage("unrecognized -m value");
-			}
-			optioncount += 2;
-			break;
 		case 't':
 			motor_test_mode = true;
 			optioncount += 1;
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c
index 6e2a54438..2129915d1 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.c
+++ b/apps/multirotor_att_control/multirotor_attitude_control.c
@@ -188,7 +188,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc
 }
 
 void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
-	const struct vehicle_attitude_s *att, struct actuator_controls_s *actuators)
+	const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators)
 {
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
@@ -305,10 +305,20 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		roll_controller.saturated = 1;
 	}
 
-	actuators->control[0] = roll_control;
-	actuators->control[1] = pitch_control;
-	actuators->control[2] = yaw_rate_control;
-	actuators->control[3] = motor_thrust;
+	if (actuators) {
+		actuators->control[0] = roll_control;
+		actuators->control[1] = pitch_control;
+		actuators->control[2] = yaw_rate_control;
+		actuators->control[3] = motor_thrust;
+	}
+
+	// XXX change yaw rate to yaw pos controller
+	if (rates_sp) {
+		rates_sp->roll = roll_control;
+		rates_sp->pitch = pitch_control;
+		rates_sp->yaw = yaw_rate_control;
+		rates_sp->thrust = motor_thrust;
+	}
 
 	motor_skip_counter++;
 }
diff --git a/apps/multirotor_att_control/multirotor_attitude_control.h b/apps/multirotor_att_control/multirotor_attitude_control.h
index aa8d27369..d9d3c0444 100644
--- a/apps/multirotor_att_control/multirotor_attitude_control.h
+++ b/apps/multirotor_att_control/multirotor_attitude_control.h
@@ -48,9 +48,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_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
-	struct actuator_controls_s *actuators);
+void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp,
+	const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, struct actuator_controls_s *actuators);
 
 #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */