Minor improvements to ardrone interface, ready for prime time

3 files changed, 33 insertions, 42 deletions

diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c
index e781d4613..fdbe3cb62 100644
--- a/apps/ardrone_interface/ardrone_interface.c
+++ b/apps/ardrone_interface/ardrone_interface.c
@@ -206,25 +206,44 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	memset(&state, 0, sizeof(state));
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
+	struct actuator_armed_s armed;
+	armed.armed = false;
 
 	/* subscribe to attitude, motor setpoints and system state */
 	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	while (!thread_should_exit) {
 
-		/* get a local copy of the vehicle state */
-		orb_copy(ORB_ID(vehicle_status), state_sub, &state);
-		/* get a local copy of the actuator controls */
-		orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
-		// if ..
-			ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode);
-		// } else {
-		// 	/* Silently lock down motor speeds to zero */
-		// 	ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
-		// }
-
-		if (counter % 30 == 0) {
+		if (motor_test_mode) {
+			/* set motors to idle speed */
+			ardrone_write_motor_commands(ardrone_write, 10, 0, 0, 0);
+			sleep(2);
+			ardrone_write_motor_commands(ardrone_write, 0, 10, 0, 0);
+			sleep(2);
+			ardrone_write_motor_commands(ardrone_write, 0, 0, 10, 0);
+			sleep(2);
+			ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 10);
+			sleep(2);
+		} else {
+			/* MAIN OPERATION MODE */
+
+			/* get a local copy of the vehicle state */
+			orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+			/* get a local copy of the actuator controls */
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
+			orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
+			
+			if (armed.armed) {
+				ardrone_mixing_and_output(ardrone_write, &actuator_controls);
+			} else {
+				/* Silently lock down motor speeds to zero */
+				ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
+			}
+		}
+
+		if (counter % 22 == 0) {
 			if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
 
 			if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
@@ -257,13 +276,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 		/* run at approximately 50 Hz */
 		usleep(20000);
 
-		// This is a hardcore debug code piece to validate
-		// the motor interface
-		// uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
-		// ar_get_motor_packet(motorSpeedBuf, 20, 20, 20, 20);
-		// write(ardrone_write, motorSpeedBuf, 5);
-		// usleep(15000);
-
 		counter++;
 	}
 
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c
index ad5893963..81ea809f9 100644
--- a/apps/ardrone_interface/ardrone_motor_control.c
+++ b/apps/ardrone_interface/ardrone_motor_control.c
@@ -303,15 +303,13 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
 	}
 }
 
-void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose) {
+void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) {
 
 	float roll_control = actuators->control[0];
 	float pitch_control = actuators->control[1];
 	float yaw_control = actuators->control[2];
 	float motor_thrust = actuators->control[3];
 
-	unsigned int motor_skip_counter = 0;
-
 	const float min_thrust = 0.02f;			/**< 2% minimum thrust */
 	const float max_thrust = 1.0f;			/**< 100% max thrust */
 	const float scaling = 512.0f;			/**< 100% thrust equals a value of 512 */
@@ -342,10 +340,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 		output_band = band_factor * (max_thrust - motor_thrust);
 	}
 
-	if (verbose && motor_skip_counter % 100 == 0) {
-		printf("1: mot1: %3.1f band: %3.1f r: %3.1f n: %3.1f y: %3.1f\n", (double)motor_thrust, (double)output_band, (double)roll_control, (double)pitch_control, (double)yaw_control);
-	}
-
 	//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
 
 	// FRONT (MOTOR 1)
@@ -380,10 +374,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control * yaw_factor);
 	}
 
-	if (verbose && motor_skip_counter % 100 == 0) {
-		printf("2: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]);
-	}
-
 	for (int i = 0; i < 4; i++) {
 		//check for limits
 		if (motor_calc[i] < motor_thrust - output_band) {
@@ -395,10 +385,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 		}
 	}
 
-	if (verbose && motor_skip_counter % 100 == 0) {
-		printf("3: band lim: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]);
-	}
-
 	/* set the motor values */
 
 	/* scale up from 0..1 to 10..512) */
@@ -407,10 +393,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas);
 	motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas);
 
-	if (verbose && motor_skip_counter % 100 == 0) {
-		printf("4: scaled: m1: %d m2: %d m3: %d m4: %d\n", motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
-	}
-
 	/* Keep motors spinning while armed and prevent overflows */
 
 	/* Failsafe logic - should never be necessary */
@@ -426,8 +408,5 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
 
 	/* send motors via UART */
-	if (verbose && motor_skip_counter % 100 == 0) printf("5: mot: %3.1f-%i-%i-%i-%i\n\n", (double)motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
 	ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
-
-	motor_skip_counter++;
 }
diff --git a/apps/ardrone_interface/ardrone_motor_control.h b/apps/ardrone_interface/ardrone_motor_control.h
index b8dd2abe4..8af143742 100644
--- a/apps/ardrone_interface/ardrone_motor_control.h
+++ b/apps/ardrone_interface/ardrone_motor_control.h
@@ -76,4 +76,4 @@ int ar_init_motors(int ardrone_uart, int *gpios_pin);
  */
 void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
 
-void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose);
+void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators);