Merge remote-tracking branch 'refs/remotes/origin/master' into Failsafe-and-toggles

UAV-ControlSystem/inc/drivers/motormix.h

@@ -0,0 +1,62 @@
+ /**********************************************************************
+ * NAME: motormix.h                                                    *
+ * AUTHOR: Philip Johansson                                            *
+ * PURPOSE: Combine the control system outputs to motor values         *
+ * INFORMATION:                                                        *
+ * Each control loop has its output which is a combination of error    *
+ * in the input unit times some tuned constants. These outputs are     *
+ * read by the mixer, combined and scaled into a valid output for      *
+ * each motor.                                                         *
+ *                                                                     *
+ *                                                                     *
+ * GLOBAL VARIABLES:                                                   *
+ * Variable		Type		Description                                *
+ * --------		----		-----------                                *
+ * 																	   *
+ **********************************************************************/
+
+
+#ifndef DRIVERS_MOTORMIX_H_
+#define DRIVERS_MOTORMIX_H_
+
+#include "stm32f4xx_revo.h"
+
+/* Amount of motors */
+#define MOTOR_COUNT 10
+
+// TODO: These are only temporary before merge with PID part
+uint16_t PID_Out[3];
+
+// TODO: Temporary before PID
+typedef enum {
+	ROLL = 0,
+	PITCH,
+	YAW
+} RollPitchYaw;
+
+
+// TODO: Implement in EEPROM
+/* Important values to be used by the mixer */
+typedef struct  {
+	uint16_t minThrottle;			// Pulse when motors are stopped
+	uint16_t maxThrottle; 			// Pulse when motors are maxed out
+	uint16_t minCommand;			// Pulse when motors are running idle (Armed)
+	uint16_t maxCommand;			// Max throttle allowed. Mixer can go higher than this though.
+	uint16_t minCheck; 				// In Non Airmode: If throttle is below minCheck we set motors to minCommand
+	bool pid_at_min_throttle; 		// When enabled PIDs are used at minimum throttle
+	bool motorstop;					// If enabled motors will stop spinning at no throttle when Armed
+	bool yaw_reverse_direction; 	// Default should be 1. Can be either -1 or 1
+} mixerConfig_s;
+
+/* Global mixerConfig to bee available to EEPROM */
+extern mixerConfig_s mixerConfig;
+
+/***********************************************************************
+* BRIEF: The motormixer                                                *
+* INFORMATION: Sums the output from all control loops and adapts the   *
+* result to a suitable motor signal                                    *
+***********************************************************************/
+void mix();
+
+
+#endif /* DRIVERS_MOTORMIX_H_ */

UAV-ControlSystem/inc/drivers/motors.h

@@ -8,10 +8,11 @@
 #ifndef DRIVERS_MOTORS_H_
 #define DRIVERS_MOTORS_H_
 
+#include "stm32f4xx_revo.h"
 /* Struct of motor output protocol */
-typedef enum
+typedef enum {
-{
+	PWM,
-PWM, Oneshot125
+	Oneshot125
 }motorOutput;
 
 /**************************************************************************

UAV-ControlSystem/inc/utilities.h

@@ -83,4 +83,6 @@ void Error_Handler(void);
 
 uint8_t reverse(uint8_t byte);
 
+uint16_t constrain(uint16_t value, uint16_t min, uint16_t max);
+
 #endif /* UTILITIES_H_ */

UAV-ControlSystem/src/drivers/motormix.c

@@ -0,0 +1,218 @@
+ /**********************************************************************
+ * NAME: motormix.c                                                    *
+ * AUTHOR: Philip Johansson                                            *
+ * PURPOSE: Combine the control system outputs to motor values         *
+ * INFORMATION:                                                        *
+ * Each control loop has its output which is a combination of error    *
+ * in the input unit times some tuned constants. These outputs are     *
+ * read by the mixer, combined and scaled into a valid output for      *
+ * each motor.                                                         *
+ *                                                                     *
+ *                                                                     *
+ * GLOBAL VARIABLES:                                                   *
+ * Variable		Type		Description                                *
+ * --------		----		-----------                                *
+ * 																	   *
+ **********************************************************************/
+
+#include "drivers/motormix.h"
+#include "drivers/motors.h"
+#include "utilities.h"
+#include "drivers/sbus.h"
+
+/* An illustration of the motor configuration
+
+  	________________|_______________
+ 	1CW  2CCW 3CCW 	| 4CW  5CCW  6CW
+					|
+					|
+					|
+		 ___________|___________
+ 		 7CCW  8CW	| 9CW  10CCW
+
+
+ */
+
+/* Set by EEPROM - This variable decides whether the control
+ * system should be active or not when throttle is below min_throttle */
+bool pid_at_min_throttle = true;
+
+/* An array containing the calculated motor outputs */
+uint16_t motor_output[MOTOR_COUNT];
+
+/* Default values for the mixerConfig */
+// TODO: Implement in EEPROM
+mixerConfig_s mixerConfig = {
+	.yaw_motor_direction = 1,
+	.minThrottle = 1000,
+	.maxThrottle = 2000,
+	.minCommand = 1050,
+	.maxCommand = 1950,
+	.minCheck = 1010
+};
+
+/* Used in "mixerUAV" to create the dynamic model of the UAV */
+typedef struct {
+	float throttle;
+	float roll;
+	float pitch;
+	float yaw;
+} motorMixer_s;
+
+
+/* Table of each motors authority in every degree of freedom
+ * Roll is defined positive clockwise seen from the back
+ * Pitch is defined positive clockwise seen from left (getting altitude / lifting the nose)
+ * Yaw is defined clockwise seen from above
+ */
+static const motorMixer_s mixerUAV[] = {
+	/* Throttle, Roll,   Pitch,  Yaw */
+
+	{	1.0f, 	 1.0f,	 1.0f, 	-1.0f}, //M1
+	{	1.0f, 	 1.0f,	 1.0f,	 1.0f}, //M2
+	{ 	1.0f, 	 0.0f, 	 1.0f,	 0.0f}, //M3
+	{	1.0f, 	 0.0f,	 1.0f,	 0.0f}, //M4
+	{ 	1.0f, 	-1.0f,	 1.0f,	 1.0f}, //M5
+	{ 	1.0f, 	-1.0f,	 1.0f,	-1.0f}, //M6
+	{ 	1.0f, 	 1.0f,	-1.0f,	 1.0f}, //M7
+	{ 	1.0f, 	 1.0f,	-1.0f,	-1.0f}, //M8
+	{ 	1.0f, 	-1.0f,	-1.0f,	-1.0f}, //M9
+	{ 	1.0f, 	-1.0f,	-1.0f,	 1.0f}, //M10
+};
+
+
+/***********************************************************************
+* BRIEF: The motormixer                                                *
+* INFORMATION: Sums the output from all control loops and adapts the   *
+* result to a suitable motor signal                                    *
+***********************************************************************/
+void mix()
+{
+
+
+	/* Calculate what "hover" could be. Not used now but might be useful later */
+	//uint16_t throttleIdle = mixerConfig.minThrottle + (throttleRange / 2);
+
+	int16_t RPY_Mix[MOTOR_COUNT];		// Roll Pitch and Yaw variables array
+	uint16_t RPY_Mix_Min = 0; 			// Stores the minimum desired command for any motor
+	uint16_t RPY_Mix_Max = 0;			// Maximum desired command for any motor
+	uint16_t throttle = sbusChannelData.chan3;	 // Import throttle value from remote
+
+	// Might be used for some debug if necessary
+	static bool motorLimitReached;
+
+	if (false) // TODO: replace with check for Airmode
+	{
+		for (int i = 0; i < MOTOR_COUNT; i++)
+		{
+			/* Calculate desired output on each motor from the motor mix table
+			 * Calculation is: Output from control system * weight from model for each motor
+			 */
+			RPY_Mix[i] = \
+					PID_Out[PITCH] 	* 	mixerUAV[i].pitch	+ 	\
+					PID_Out[ROLL] 	*	mixerUAV[i].roll 	+ 	\
+					PID_Out[YAW]	*	mixerUAV[i].yaw		*	((mixerConfig.yaw_reverse_direction) ? -1 : 1);
+
+			// Update min and max values
+			if (RPY_Mix[i] > RPY_Mix_Max) RPY_Mix_Max = RPY_Mix[i];
+			if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i];
+		}
+
+
+		uint16_t throttleRange = mixerConfig.maxThrottle - mixerConfig.minThrottle; // The throttle range we have with current defines
+		uint16_t RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min; // Range of the desired mixer outputs
+		uint16_t throttleMin = mixerConfig.minThrottle;	// Import system variable
+		uint16_t throttleMax = mixerConfig.maxThrottle; // Import
+
+
+		/* Check if we have enough interval for the adjustments */
+		// Check if we maxed out
+		if (RPY_MixRange > throttleRange)
+		{
+			motorLimitReached = true; // Yes, we maxed out
+
+			// Create a scale to the current mix for it to fit
+			float mixReduction = (float) throttleRange / RPY_MixRange;
+			// Apply the scaling to all outputs
+			for (int i = 0; i < MOTOR_COUNT; i++)
+				RPY_Mix[i] = RPY_Mix[i] * mixReduction;
+		}
+		// If we have the needed range no scaling is needed
+		else
+		{
+			motorLimitReached = false; // Not used but could be helpfull for debug
+
+			/* Update min and max throttle so we can add the
+			 * calculated adjustments and still just max out */
+			throttleMin += (RPY_MixRange / 2);
+			throttleMax -= (RPY_MixRange / 2);
+		}
+
+
+		// Now we add desired throttle
+		for (int i = 0; i < MOTOR_COUNT; i++)
+		{
+			// Constrain in within the regulation of the mix
+			motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax);
+			motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
+		}
+	}
+	else // Airmode not active
+	{
+		int16_t maxMotor = 0;
+
+		for (int i = 0; i < MOTOR_COUNT; i++) // Without airmode this includes throttle
+		{
+			RPY_Mix[i] = \
+					throttle 		*	mixerUAV[i].throttle	+	\
+					PID_Out[PITCH] 	* 	mixerUAV[i].pitch		+ 	\
+					PID_Out[ROLL] 	*	mixerUAV[i].roll 		+ 	\
+					PID_Out[YAW]	*	mixerUAV[i].yaw			*	((mixerConfig.yaw_reverse_direction) ? -1 : 1);
+
+			// Find the maximum motor output
+			if (RPY_Mix[i] > maxMotor) maxMotor = RPY_Mix[i];
+		}
+
+		int16_t maxThrottleDifference = 0;
+		if (maxMotor > mixerConfig.maxThrottle)
+			maxThrottleDifference = maxMotor - mixerConfig.maxThrottle;
+
+		// Approach at mixing
+		for (int i = 0; i < MOTOR_COUNT; i++)
+		{
+			RPY_Mix[i] -= maxThrottleDifference; // We reduce the highest overflow on all motors
+
+			RPY_Mix[i] = constrain(RPY_Mix[i], mixerConfig.minThrottle, mixerConfig.maxThrottle);
+			if (throttle < mixerConfig.minCheck)
+			{
+				if (mixerConfig.motorstop)
+					RPY_Mix[i] = mixerConfig.minCommand;
+
+				/* Motors set to idle with PIDs not active */
+				else if (!mixerConfig.pid_at_min_throttle)
+					RPY_Mix[i] = mixerConfig.minThrottle;
+
+				/* Else */
+				// Is per default to have the PIDS active.
+				// Though authority is very low with low throttle
+			}
+
+			// Constrain in within the valid motor outputs
+			motor_output[i] = constrain(RPY_Mix[i], mixerConfig.minCommand, mixerConfig.maxCommand);
+		}
+	}
+
+	// Updating the command to the actuators
+	for (int i = 0; i < MOTOR_COUNT; i++)
+	{
+		/* If engines are armed then give the output to the motors */
+		if (true) // TODO: replace with check for armed (IF ARMED)
+			motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
+		/* If not then stop motors */
+		else
+			motor_output[i] = mixerConfig.minThrottle;
+
+		/* Update actuators */
+		pwmAdjustSpeedOfMotor( i + 1 /* Motors start from Motor 1 */,motor_output[i]);
+	}
+}

UAV-ControlSystem/src/utilities.c

@@ -218,3 +218,13 @@ uint8_t reverse(uint8_t byte)
 
 	return byte;
 }
+
+uint16_t constrain(uint16_t value, uint16_t min, uint16_t max)
+{
+	if (value < min)
+		return min;
+	else if (value > max)
+		return max;
+	else
+		return value;
+}