Fixed inner yaw rate loop

2 files changed, 76 insertions, 127 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 5e492268e..4b386ffd4 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -82,64 +82,6 @@ static orb_advert_t actuator_pub;
 
 static struct vehicle_status_s state;
 
-/**
- * Perform rate control right after gyro reading
- */
-static void *rate_control_thread_main(void *arg)
-{
-	prctl(PR_SET_NAME, "mc rate control", getpid());
-
-	struct actuator_controls_s actuators;
-
-	int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-
-	struct pollfd fds = { .fd = att_sub, .events = POLLIN };
-
-	struct vehicle_attitude_s vehicle_attitude;
-	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 current angular rate */
-			orb_copy(ORB_ID(vehicle_attitude), att_sub, &vehicle_attitude);
-			/* get current rate setpoint */
-			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] = vehicle_attitude.rollspeed;
-				gyro_lp[1] = vehicle_attitude.pitchspeed;
-				gyro_lp[2] = vehicle_attitude.yawspeed;
-
-				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[])
 {
@@ -186,6 +128,7 @@ mc_thread_main(int argc, char *argv[])
 	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);
+	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
 	/* register the perf counter */
 	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control");
@@ -193,13 +136,6 @@ 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 */
@@ -232,13 +168,24 @@ mc_thread_main(int argc, char *argv[])
 
 		if (state.flag_control_manual_enabled) {
 			/* manual inputs, from RC control or joystick */
-			att_sp.roll_body = manual.roll;
-			att_sp.pitch_body = manual.pitch;
-			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();
+
+			if (state.flag_control_rates_enabled && !state.flag_control_attitude_enabled) {
+				rates_sp.roll = manual.roll;
+				rates_sp.pitch = manual.pitch;
+				rates_sp.yaw = manual.yaw;
+				rates_sp.thrust = manual.throttle;
+				rates_sp.timestamp = hrt_absolute_time();
+			}
+
+			if (state.flag_control_attitude_enabled) {
+				att_sp.roll_body = manual.roll;
+				att_sp.pitch_body = manual.pitch;
+				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);
 			
@@ -277,12 +224,33 @@ mc_thread_main(int argc, char *argv[])
 		/** STEP 3: Identify the controller setup to run and set up the inputs correctly */
 
 		/* run attitude controller */
-		if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
-			multirotor_control_attitude(&att_sp, &att, NULL, &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);
+		// }
+
+
+		if (state.flag_control_rates_enabled) {
+
+			float gyro[3] = {0.0f, 0.0f, 0.0f};
+
+			/* get current rate setpoint */
+			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);
+			}
+
+			/* apply controller */
+			gyro[0] = att.rollspeed;
+			gyro[1] = att.pitchspeed;
+			gyro[2] = att.yawspeed;
+
+			multirotor_control_rates(&rates_sp, gyro, &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);
 		}
 
 
@@ -306,8 +274,6 @@ 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);
 }
diff --git a/apps/multirotor_att_control/multirotor_rate_control.c b/apps/multirotor_att_control/multirotor_rate_control.c
index b4eb3469b..d332660f6 100644
--- a/apps/multirotor_att_control/multirotor_rate_control.c
+++ b/apps/multirotor_att_control/multirotor_rate_control.c
@@ -141,11 +141,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	static int motor_skip_counter = 0;
 
-	// static PID_t yaw_pos_controller;
-	static PID_t yaw_speed_controller;
-	static PID_t pitch_controller;
-	static PID_t roll_controller;
-
 	static struct mc_rate_control_params p;
 	static struct mc_rate_control_param_handles h;
 
@@ -155,37 +150,25 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	if (initialized == false) {
 		parameters_init(&h);
 		parameters_update(&h, &p);
-
-		pid_init(&yaw_speed_controller, p.yawrate_p, 0, p.yawrate_i, p.yawrate_awu,
-			PID_MODE_DERIVATIV_SET);
-		pid_init(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
-			PID_MODE_DERIVATIV_SET);
-		pid_init(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu,
-			PID_MODE_DERIVATIV_SET);
-
 		initialized = true;
 	}
 
 	/* load new parameters with lower rate */
-	if (motor_skip_counter % 250 == 0) {
+	if (motor_skip_counter % 2500 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		/* apply parameters */
-		pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu);
-		pid_set_parameters(&pitch_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu);
-		pid_set_parameters(&roll_controller, p.attrate_p, p.attrate_i, 0, p.attrate_awu);
+		printf("p.yawrate_p: %8.4f", (double)p.yawrate_p);
 	}
 
 	/* calculate current control outputs */
 	
 	/* control pitch (forward) output */
-	float pitch_control = pid_calculate(&pitch_controller, rate_sp->pitch,
-					rates[1], 0.0f, deltaT);
+	float pitch_control = 5 * deltaT * (rates[1] - rate_sp->pitch);
 	/* control roll (left/right) output */
-	float roll_control = pid_calculate(&roll_controller, rate_sp->roll,
-					rates[0], 0.0f, deltaT);
+	float roll_control = p.attrate_p * deltaT * (rates[0] - rate_sp->roll);
+
 	/* control yaw rate */
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
+	float yaw_rate_control = p.yawrate_p * deltaT * (rates[2] - rate_sp->yaw);
 
 	/*
 	 * compensate the vertical loss of thrust
@@ -215,37 +198,37 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	/* compensate thrust vector for roll / pitch contributions */
 	motor_thrust *= zcompensation;
 
-	/* limit yaw rate output */
-	if (yaw_rate_control > p.yawrate_lim) {
-		yaw_rate_control = p.yawrate_lim;
-		yaw_speed_controller.saturated = 1;
-	}
+	// /* limit yaw rate output */
+	// if (yaw_rate_control > p.yawrate_lim) {
+	// 	yaw_rate_control = p.yawrate_lim;
+	// 	yaw_speed_controller.saturated = 1;
+	// }
 
-	if (yaw_rate_control < -p.yawrate_lim) {
-		yaw_rate_control = -p.yawrate_lim;
-		yaw_speed_controller.saturated = 1;
-	}
+	// if (yaw_rate_control < -p.yawrate_lim) {
+	// 	yaw_rate_control = -p.yawrate_lim;
+	// 	yaw_speed_controller.saturated = 1;
+	// }
 
-	if (pitch_control > p.attrate_lim) {
-		pitch_control = p.attrate_lim;
-		pitch_controller.saturated = 1;
-	}
+	// if (pitch_control > p.attrate_lim) {
+	// 	pitch_control = p.attrate_lim;
+	// 	pitch_controller.saturated = 1;
+	// }
 
-	if (pitch_control < -p.attrate_lim) {
-		pitch_control = -p.attrate_lim;
-		pitch_controller.saturated = 1;
-	}
+	// if (pitch_control < -p.attrate_lim) {
+	// 	pitch_control = -p.attrate_lim;
+	// 	pitch_controller.saturated = 1;
+	// }
 
 
-	if (roll_control > p.attrate_lim) {
-		roll_control = p.attrate_lim;
-		roll_controller.saturated = 1;
-	}
+	// if (roll_control > p.attrate_lim) {
+	// 	roll_control = p.attrate_lim;
+	// 	roll_controller.saturated = 1;
+	// }
 
-	if (roll_control < -p.attrate_lim) {
-		roll_control = -p.attrate_lim;
-		roll_controller.saturated = 1;
-	}
+	// if (roll_control < -p.attrate_lim) {
+	// 	roll_control = -p.attrate_lim;
+	// 	roll_controller.saturated = 1;
+	// }
 
 	actuators->control[0] = roll_control;
 	actuators->control[1] = pitch_control;