Reworked ardrone / multirotor control

5 files changed, 146 insertions, 69 deletions

diff --git a/apps/ardrone_control/ardrone_control.c b/apps/ardrone_control/ardrone_control.c
index d5c31dc70..d457fc2ec 100644
--- a/apps/ardrone_control/ardrone_control.c
+++ b/apps/ardrone_control/ardrone_control.c
@@ -114,6 +114,8 @@ int ardrone_control_main(int argc, char *argv[])
 
 	char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n";
 
+	bool motor_test_mode = false;
+
 	/* read commandline arguments */
 	for (int i = 1; i < argc; i++) {
 		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set
@@ -141,6 +143,9 @@ int ardrone_control_main(int argc, char *argv[])
 				printf(commandline_usage);
 				return ERROR;
 			}
+
+		} else if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
+			motor_test_mode = true;
 		}
 	}
 
@@ -195,7 +200,8 @@ int ardrone_control_main(int argc, char *argv[])
 			state.state_machine == SYSTEM_STATE_STABILIZED ||
 			state.state_machine == SYSTEM_STATE_AUTO ||
 			state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
-			state.state_machine == SYSTEM_STATE_EMCY_LANDING) {
+			state.state_machine == SYSTEM_STATE_EMCY_LANDING ||
+			motor_test_mode) {
 
 			if (control_mode == CONTROL_MODE_RATES) {
 				orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
@@ -218,13 +224,45 @@ int ardrone_control_main(int argc, char *argv[])
 				att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
 				att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
 				att_sp.yaw_body = -manual.yaw * M_PI_F;
-				att_sp.thrust = manual.throttle/2.0f;
+				if (motor_test_mode) {
+					att_sp.roll_body = 0.0f;
+					att_sp.pitch_body = 0.0f;
+					att_sp.yaw_body = 0.0f;
+					att_sp.thrust = 0.3f;
+				} else {
+					if (state.state_machine == SYSTEM_STATE_MANUAL ||
+						state.state_machine == SYSTEM_STATE_GROUND_READY ||
+						state.state_machine == SYSTEM_STATE_STABILIZED ||
+						state.state_machine == SYSTEM_STATE_AUTO ||
+						state.state_machine == SYSTEM_STATE_MISSION_ABORT ||
+						state.state_machine == SYSTEM_STATE_EMCY_LANDING) {
+						att_sp.thrust = manual.throttle/2.0f;
+
+					} else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
+						/* immediately cut off motors */
+						att_sp.thrust = 0.0f;
+
+					} else {
+						/* limit motor throttle to zero for an unknown mode */
+						att_sp.thrust = 0.0f;
+					}
+					
+				}
+
+				float roll_control, pitch_control, yaw_control, thrust_control;
+
+				multirotor_control_attitude(&att_sp, &att, &state, motor_test_mode,
+					&roll_control, &pitch_control, &yaw_control, &thrust_control);
+				ardrone_mixing_and_output(ardrone_write, roll_control, pitch_control, yaw_control, thrust_control, motor_test_mode);
 
-				control_attitude(ardrone_write, &att_sp, &att, &state);
 			} else {
 				/* invalid mode, complain */
 				if (counter % 200 == 0) printf("[multirotor control] INVALID CONTROL MODE, locking down propulsion\n");
+				ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
 			}
+		} else {
+			/* Silently lock down motor speeds to zero */
+			ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
 		}
 
 		if (counter % 30 == 0) {
diff --git a/apps/ardrone_control/ardrone_motor_control.c b/apps/ardrone_control/ardrone_motor_control.c
index 414ec671f..e9733a1a0 100644
--- a/apps/ardrone_control/ardrone_motor_control.c
+++ b/apps/ardrone_control/ardrone_motor_control.c
@@ -280,3 +280,20 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t
 	leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1);
 	write(ardrone_uart, leds, 2);
 }
+
+int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) {
+	const int min_motor_interval = 20000;
+	static uint64_t last_motor_time = 0;
+	if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
+		uint8_t buf[5] = {0};
+		ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
+		int ret;
+		if ((ret = write(ardrone_fd, buf, sizeof(buf))) > 0) {
+			return OK;
+		} else {
+			return ret;
+		}
+	} else {
+		return -ERROR;
+	}
+}
diff --git a/apps/ardrone_control/ardrone_motor_control.h b/apps/ardrone_control/ardrone_motor_control.h
index 8b8aa777b..2e6df0e69 100644
--- a/apps/ardrone_control/ardrone_motor_control.h
+++ b/apps/ardrone_control/ardrone_motor_control.h
@@ -48,6 +48,9 @@
  */
 void ar_get_motor_packet(uint8_t *motor_buf, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
 
+/**
+ * Select a motor in the multiplexing.
+ */
 int ar_select_motor(int fd, uint8_t motor);
 
 void ar_enable_broadcast(int fd);
@@ -56,6 +59,15 @@ int ar_multiplexing_init(void);
 int ar_multiplexing_deinit(int fd);
 
 /**
+ * Write four motor commands to an already initialized port.
+ *
+ * Writing 0 stops a motor, values from 1-512 encode the full thrust range.
+ * on some motor controller firmware revisions a minimum value of 10 is
+ * required to spin the motors.
+ */
+int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4);
+
+/**
  * Initialize the motors.
  */
 int ar_init_motors(int ardrone_uart, int *gpios_pin);
diff --git a/apps/ardrone_control/attitude_control.c b/apps/ardrone_control/attitude_control.c
index 37edbf0e5..d31e36e33 100644
--- a/apps/ardrone_control/attitude_control.c
+++ b/apps/ardrone_control/attitude_control.c
@@ -54,31 +54,22 @@
 #include "pid.h"
 #include <arch/board/up_hrt.h>
 
-#define CONTROL_PID_ATTITUDE_INTERVAL	5e-3f
 #define MAX_MOTOR_COUNT 16
 
-void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status)
+void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att, const struct vehicle_status_s *status,
+	bool verbose, float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output)
 {
-	const unsigned int motor_count = 4;
+	static uint64_t last_run = 0;
+	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
+	last_run = hrt_absolute_time();
 
 	static int motor_skip_counter = 0;
 
 	static PID_t yaw_pos_controller;
 	static PID_t yaw_speed_controller;
-	static PID_t nick_controller;
+	static PID_t pitch_controller;
 	static PID_t roll_controller;
 
-	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 */
-
-	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 */
-
-	/* initialize all fields to zero */
-	static uint16_t motor_pwm[MAX_MOTOR_COUNT];
-	static float motor_calc[MAX_MOTOR_COUNT];
-
 	static float pid_yawpos_lim;
 	static float pid_yawspeed_lim;
 	static float pid_att_lim;
@@ -102,7 +93,7 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 			 global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU],
 			 PID_MODE_DERIVATIV_CALC, 155);
 
-		pid_init(&nick_controller,
+		pid_init(&pitch_controller,
 			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
 			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
 			 global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
@@ -137,7 +128,7 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_D],
 				   global_data_parameter_storage->pm.param_values[PARAM_PID_YAWSPEED_AWU]);
 
-		pid_set_parameters(&nick_controller,
+		pid_set_parameters(&pitch_controller,
 				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_P],
 				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_I],
 				   global_data_parameter_storage->pm.param_values[PARAM_PID_ATT_D],
@@ -156,13 +147,13 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 
 	/*Calculate Controllers*/
 	//control Nick
-	float nick_control = pid_calculate(&nick_controller, att_sp->pitch_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET],
-					att->pitch, att->pitchspeed, CONTROL_PID_ATTITUDE_INTERVAL);
+	float pitch_control = pid_calculate(&pitch_controller, att_sp->pitch_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_YOFFSET],
+					att->pitch, att->pitchspeed, deltaT);
 	//control Roll
 	float roll_control = pid_calculate(&roll_controller, att_sp->roll_body + global_data_parameter_storage->pm.param_values[PARAM_ATT_XOFFSET],
-					att->roll, att->rollspeed, CONTROL_PID_ATTITUDE_INTERVAL);
+					att->roll, att->rollspeed, deltaT);
 	//control Yaw Speed
-	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, CONTROL_PID_ATTITUDE_INTERVAL); 	//attitude_setpoint_bodyframe.z is yaw speed!
+	float yaw_rate_control = pid_calculate(&yaw_speed_controller, att_sp->yaw_body, att->yawspeed, 0.0f, deltaT); 	//attitude_setpoint_bodyframe.z is yaw speed!
 
 	/*
 	 * compensate the vertical loss of thrust
@@ -185,27 +176,12 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 		zcompensation *= 1.0f / cosf(att->pitch);
 	}
 
-	float motor_thrust;
+	float motor_thrust = 0.0f;
 
 	// FLYING MODES
-	if (status->state_machine == SYSTEM_STATE_MANUAL ||
-		status->state_machine == SYSTEM_STATE_GROUND_READY ||
-		status->state_machine == SYSTEM_STATE_STABILIZED ||
-		status->state_machine == SYSTEM_STATE_AUTO ||
-		status->state_machine == SYSTEM_STATE_MISSION_ABORT ||
-		status->state_machine == SYSTEM_STATE_EMCY_LANDING) {
-		motor_thrust = att_sp->thrust;
-
-	} else if (status->state_machine == SYSTEM_STATE_EMCY_CUTOFF) {
-		/* immediately cut off motors */
-		motor_thrust = 0.0f;
+	motor_thrust = att_sp->thrust;
 
-	} else {
-		/* limit motor throttle to zero for an unknown mode */
-		motor_thrust = 0.0f;
-	}
-
-	printf("mot0: %3.1f\n", motor_thrust);
+	//printf("mot0: %3.1f\n", motor_thrust);
 
 	/* compensate thrust vector for roll / pitch contributions */
 	motor_thrust *= zcompensation;
@@ -221,14 +197,14 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 		yaw_speed_controller.saturated = 1;
 	}
 
-	if (nick_control > pid_att_lim) {
-		nick_control = pid_att_lim;
-		nick_controller.saturated = 1;
+	if (pitch_control > pid_att_lim) {
+		pitch_control = pid_att_lim;
+		pitch_controller.saturated = 1;
 	}
 
-	if (nick_control < -pid_att_lim) {
-		nick_control = -pid_att_lim;
-		nick_controller.saturated = 1;
+	if (pitch_control < -pid_att_lim) {
+		pitch_control = -pid_att_lim;
+		pitch_controller.saturated = 1;
 	}
 
 
@@ -242,7 +218,26 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 		roll_controller.saturated = 1;
 	}
 
-	printf("mot1: %3.1f\n", motor_thrust);
+	*roll_output = roll_control;
+	*pitch_output = pitch_control;
+	*yaw_output = yaw_rate_control;
+	*thrust_output = motor_thrust;
+}
+
+void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose) {
+
+	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 */
+
+	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 */
+
+	/* initialize all fields to zero */
+	uint16_t motor_pwm[MAX_MOTOR_COUNT] = {0};
+	float motor_calc[MAX_MOTOR_COUNT] = {0};
 
 	float output_band = 0.0f;
 	float band_factor = 0.75f;
@@ -263,19 +258,23 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 		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)
-	motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control);
+	motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control);
 
 	// RIGHT (MOTOR 2)
-	motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control);
+	motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control);
 
 	// BACK (MOTOR 3)
-	motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control);
+	motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control);
 
 	// LEFT (MOTOR 4)
-	motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control);
+	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
@@ -285,16 +284,20 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 
 		yaw_factor = 0.5f;
 		// FRONT (MOTOR 1)
-		motor_calc[0] = motor_thrust + (roll_control / 2 + nick_control / 2 - yaw_rate_control * yaw_factor);
+		motor_calc[0] = motor_thrust + (roll_control / 2 + pitch_control / 2 - yaw_control * yaw_factor);
 
 		// RIGHT (MOTOR 2)
-		motor_calc[1] = motor_thrust + (-roll_control / 2 + nick_control / 2 + yaw_rate_control * yaw_factor);
+		motor_calc[1] = motor_thrust + (-roll_control / 2 + pitch_control / 2 + yaw_control * yaw_factor);
 
 		// BACK (MOTOR 3)
-		motor_calc[2] = motor_thrust + (-roll_control / 2 - nick_control / 2 - yaw_rate_control * yaw_factor);
+		motor_calc[2] = motor_thrust + (-roll_control / 2 - pitch_control / 2 - yaw_control * yaw_factor);
 
 		// LEFT (MOTOR 4)
-		motor_calc[3] = motor_thrust + (roll_control / 2 - nick_control / 2 + yaw_rate_control * yaw_factor);
+		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++) {
@@ -308,13 +311,21 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 		}
 	}
 
+	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) */
-	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;
+	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);
+
+	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 */
 
@@ -331,12 +342,8 @@ void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_
 	motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
 
 	/* send motors via UART */
-	if (motor_skip_counter % 5 == 0) {
-		if (motor_skip_counter % 50 == 0) printf("mot: %3.1f-%i-%i-%i-%i\n", motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
-		uint8_t buf[5] = {1, 2, 3, 4, 5};
-		ar_get_motor_packet(buf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
-		write(ardrone_write, buf, sizeof(buf));
-	}
+	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_control/attitude_control.h b/apps/ardrone_control/attitude_control.h
index 1067d362c..56f52dabe 100644
--- a/apps/ardrone_control/attitude_control.h
+++ b/apps/ardrone_control/attitude_control.h
@@ -52,7 +52,10 @@
 #include <uORB/topics/ardrone_control.h>
 #include <uORB/topics/vehicle_status.h>
 
-void control_attitude(int ardrone_write, const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
-		const struct vehicle_status_s *status);
+void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, const struct vehicle_attitude_s *att,
+		const struct vehicle_status_s *status, bool verbose,
+		float* roll_output, float* pitch_output, float* yaw_output, float* thrust_output);
+
+void ardrone_mixing_and_output(int ardrone_write, float roll_control, float pitch_control, float yaw_control, float motor_thrust, bool verbose);
 
 #endif /* ATTITUDE_CONTROL_H_ */