ardrone in the air again (workaround: rate controller disabled)

1 files changed, 13 insertions, 3 deletions

diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c
index 21c720096..70b9d8e28 100644
--- a/apps/multirotor_att_control/multirotor_att_control_main.c
+++ b/apps/multirotor_att_control/multirotor_att_control_main.c
@@ -76,7 +76,7 @@ __EXPORT int multirotor_att_control_main(int argc, char *argv[]);
 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;
 
 /**
@@ -86,6 +86,8 @@ static void *rate_control_thread_main(void *arg)
 {
 	prctl(PR_SET_NAME, "mc rate control", getpid());
 
+	struct actuator_controls_s actuators;
+
 	int gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
 	int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
 
@@ -125,8 +127,8 @@ static void *rate_control_thread_main(void *arg)
 				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);
+//				multirotor_control_rates(&rates_sp, gyro_lp, &actuators);
+//				orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 			// }
 		}
 	}
@@ -153,6 +155,8 @@ 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));
@@ -228,6 +232,7 @@ mc_thread_main(int argc, char *argv[])
 			/* set yaw rate */
 			rates_sp.yaw = manual.yaw;
 			att_sp.thrust = manual.throttle;
+			//printf("Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, att_sp.thrust);
 			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);
@@ -269,10 +274,15 @@ mc_thread_main(int argc, char *argv[])
 		/* run attitude controller */
 		if (state.flag_control_attitude_enabled && !state.flag_control_rates_enabled) {
 			multirotor_control_attitude(&att_sp, &att, NULL, &actuators);
+//			printf("publish actuator\n");
+
+//			printf("MAC_PUB: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuators.control[0], actuators.control[1], actuators.control[2], actuators.control[3]);
+
 			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);
+//			printf("publish attitude\n");
 		}