Prevent flips at high throttle

Conflicts:
	src/drivers/ardrone_interface/ardrone_motor_control.c

1 files changed, 38 insertions, 50 deletions

diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index ecd31a073..d3b414150 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -41,6 +41,7 @@
 #include <stdio.h>
 #include <fcntl.h>
 #include <unistd.h>
+#include <math.h>
 #include <drivers/drv_gpio.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/uORB.h>
@@ -369,11 +370,9 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	float yaw_control = actuators->control[2];
 	float motor_thrust = actuators->control[3];
 
-	//printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust);
-
 	const float min_thrust = 0.02f;			/**< 2% minimum thrust */
 	const float max_thrust = 1.0f;			/**< 100% max thrust */
-	const float scaling = 500.0f;			/**< 100% thrust equals a value of 500 which works, 512 leads to cutoff */
+	const float scaling = 510.0f;			/**< 100% thrust equals a value of 510 which works, 512 leads to cutoff */
 	const float min_gas = min_thrust * scaling;	/**< value range sent to motors, minimum */
 	const float max_gas = max_thrust * scaling;	/**< value range sent to motors, maximum */
 
@@ -382,71 +381,56 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 	float motor_calc[4] = {0};
 
 	float output_band = 0.0f;
-	float band_factor = 0.75f;
 	const float startpoint_full_control = 0.25f;	/**< start full control at 25% thrust */
-	float yaw_factor = 1.0f;
 
 	static bool initialized = false;
 	/* publish effective outputs */
 	static struct actuator_controls_effective_s actuator_controls_effective;
 	static orb_advert_t actuator_controls_effective_pub;
 
-	if (motor_thrust <= min_thrust) {
-		motor_thrust = min_thrust;
-		output_band = 0.0f;
-	} else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) {
-		output_band = band_factor * (motor_thrust - min_thrust);
-	} else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) {
-		output_band = band_factor * startpoint_full_control;
-	} else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) {
-		output_band = band_factor * (max_thrust - motor_thrust);
+	/* linearly scale the control inputs from 0 to startpoint_full_control */
+	if (motor_thrust < startpoint_full_control) {
+		output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1
+	} else {
+		output_band = 1.0f;
 	}
 
+	roll_control *= output_band;
+	pitch_control *= output_band;
+	yaw_control *= output_band;
+
+
 	//add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer
 
 	// FRONT (MOTOR 1)
 	motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
-
 	// RIGHT (MOTOR 2)
 	motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
-
 	// BACK (MOTOR 3)
 	motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
-
 	// LEFT (MOTOR 4)
 	motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
 
-	// if we are not in the output band
-	if (!(motor_calc[0] < motor_thrust + output_band && motor_calc[0] > motor_thrust - output_band
-	      && motor_calc[1] < motor_thrust + output_band && motor_calc[1] > motor_thrust - output_band
-	      && motor_calc[2] < motor_thrust + output_band && motor_calc[2] > motor_thrust - output_band
-	      && motor_calc[3] < motor_thrust + output_band && motor_calc[3] > motor_thrust - output_band)) {
+	/* if one motor is saturated, reduce throttle */
+	float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust;
+
+
+	if (saturation > 0.0f) {
+
+		/* reduce the motor thrust according to the saturation */
+		motor_thrust = motor_thrust - saturation;
 
-		yaw_factor = 0.5f;
-		yaw_control *= yaw_factor;
 		// FRONT (MOTOR 1)
 		motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
-
 		// RIGHT (MOTOR 2)
 		motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
-
 		// BACK (MOTOR 3)
 		motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
-
 		// LEFT (MOTOR 4)
 		motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control);
 	}
 
-	for (int i = 0; i < 4; i++) {
-		//check for limits
-		if (motor_calc[i] < motor_thrust - output_band) {
-			motor_calc[i] = motor_thrust - output_band;
-		}
 
-		if (motor_calc[i] > motor_thrust + output_band) {
-			motor_calc[i] = motor_thrust + output_band;
-		}
-	}
 
 	/* publish effective outputs */
 	actuator_controls_effective.control_effective[0] = roll_control;
@@ -467,25 +451,29 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls
 
 	/* set the motor values */
 
-	/* scale up from 0..1 to 10..512) */
+	/* scale up from 0..1 to 10..500) */
 	motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas);
 	motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas);
 	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);
 
-	/* Keep motors spinning while armed and prevent overflows */
-
-	/* Failsafe logic - should never be necessary */
-	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
-	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
-	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
-	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
-
-	/* Failsafe logic - should never be necessary */
-	motor_pwm[0] = (motor_pwm[0] <= 511) ? motor_pwm[0] : 511;
-	motor_pwm[1] = (motor_pwm[1] <= 511) ? motor_pwm[1] : 511;
-	motor_pwm[2] = (motor_pwm[2] <= 511) ? motor_pwm[2] : 511;
-	motor_pwm[3] = (motor_pwm[3] <= 511) ? motor_pwm[3] : 511;
+	/* scale up from 0..1 to 10..500) */
+	motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas));
+	motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas));
+	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));
+
+	/* Failsafe logic for min values - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas;
+	motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : min_gas;
+	motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : min_gas;
+	motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : min_gas;
+
+	/* Failsafe logic for max values - should never be necessary */
+	motor_pwm[0] = (motor_pwm[0] <= max_gas) ? motor_pwm[0] : max_gas;
+	motor_pwm[1] = (motor_pwm[1] <= max_gas) ? motor_pwm[1] : max_gas;
+	motor_pwm[2] = (motor_pwm[2] <= max_gas) ? motor_pwm[2] : max_gas;
+	motor_pwm[3] = (motor_pwm[3] <= max_gas) ? motor_pwm[3] : max_gas;
 
 	/* send motors via UART */
 	ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);