Controller and estimator updates

1 files changed, 7 insertions, 12 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 63c7d0097..cb82036fe 100755..100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -172,7 +172,7 @@ 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;
-						//printf("thrust_rate=%8.4f\n",offboard_sp.p4);
+//						printf("thrust_rate=%8.4f\n",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) {
@@ -180,7 +180,7 @@ mc_thread_main(int argc, char *argv[])
 						att_sp.pitch_body = offboard_sp.p2;
 						att_sp.yaw_body = offboard_sp.p3;
 						att_sp.thrust = offboard_sp.p4;
-						//printf("thrust_att=%8.4f\n",offboard_sp.p4);
+//						printf("thrust_att=%8.4f\n",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);
@@ -193,6 +193,7 @@ mc_thread_main(int argc, char *argv[])
 
 			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;
@@ -203,7 +204,7 @@ mc_thread_main(int argc, char *argv[])
 			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
+				att_sp.yaw_body = manual.yaw; // XXX Hack, clean up
 				/* set yaw rate */
 				rates_sp.yaw = manual.yaw;
 				att_sp.thrust = manual.throttle;
@@ -226,21 +227,15 @@ 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);
-		 	orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
-		 } else if (state.flag_control_attitude_enabled && state.flag_control_rates_enabled) {
+		if (state.flag_control_attitude_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};
+			float gyro[3];
 
 			/* get current rate setpoint */
 			bool rates_sp_valid = false;