From ff9ebbd15ebcdd4ad90992eaa657f281f0010624 Mon Sep 17 00:00:00 2001 From: philsson Date: Fri, 7 Oct 2016 17:36:09 +0200 Subject: [PATCH 1/3] Created files for motormixer --- UAV-ControlSystem/inc/drivers/motormix.h | 13 +++++++++++++ UAV-ControlSystem/src/drivers/motormix.c | 8 ++++++++ 2 files changed, 21 insertions(+) create mode 100644 UAV-ControlSystem/inc/drivers/motormix.h create mode 100644 UAV-ControlSystem/src/drivers/motormix.c diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h new file mode 100644 index 0000000..0b6e9b3 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/motormix.h @@ -0,0 +1,13 @@ +/* + * motormix.h + * + * Created on: 7 okt. 2016 + * Author: Philip + */ + +#ifndef DRIVERS_MOTORMIX_H_ +#define DRIVERS_MOTORMIX_H_ + + + +#endif /* DRIVERS_MOTORMIX_H_ */ diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c new file mode 100644 index 0000000..4ed6d49 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -0,0 +1,8 @@ +/* + * motormix.c + * + * Created on: 7 okt. 2016 + * Author: Philip + */ + + From 33398e756cb4244c812b5a1d9e511bbee5098175 Mon Sep 17 00:00:00 2001 From: philsson Date: Tue, 11 Oct 2016 12:02:07 +0200 Subject: [PATCH 2/3] Implemented all but EEPROM and check for Airmode and Armed --- UAV-ControlSystem/inc/drivers/motormix.h | 61 ++++++- UAV-ControlSystem/inc/drivers/motors.h | 7 +- UAV-ControlSystem/inc/utilities.h | 2 + UAV-ControlSystem/src/drivers/motormix.c | 220 ++++++++++++++++++++++- UAV-ControlSystem/src/utilities.c | 10 ++ 5 files changed, 286 insertions(+), 14 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h index 0b6e9b3..e2c3966 100644 --- a/UAV-ControlSystem/inc/drivers/motormix.h +++ b/UAV-ControlSystem/inc/drivers/motormix.h @@ -1,13 +1,62 @@ -/* - * motormix.h - * - * Created on: 7 okt. 2016 - * Author: Philip - */ + /********************************************************************** + * 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 + uint8_t yaw_motor_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_ */ diff --git a/UAV-ControlSystem/inc/drivers/motors.h b/UAV-ControlSystem/inc/drivers/motors.h index 282fba0..ac89685 100644 --- a/UAV-ControlSystem/inc/drivers/motors.h +++ b/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 -{ -PWM, Oneshot125 +typedef enum { + PWM, + Oneshot125 }motorOutput; /************************************************************************** diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h index 457c142..266b568 100644 --- a/UAV-ControlSystem/inc/utilities.h +++ b/UAV-ControlSystem/inc/utilities.h @@ -38,4 +38,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_ */ diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index 4ed6d49..e11eebf 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -1,8 +1,218 @@ -/* - * motormix.c - * - * Created on: 7 okt. 2016 - * Author: Philip + /********************************************************************** + * 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); + + uint16_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_motor_direction; + + // 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_motor_direction; + + // Find the maximum motor output + if (RPY_Mix[i] > maxMotor) maxMotor = RPY_Mix[i]; + } + + uint16_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]); + } +} diff --git a/UAV-ControlSystem/src/utilities.c b/UAV-ControlSystem/src/utilities.c index 731b5a9..8173165 100644 --- a/UAV-ControlSystem/src/utilities.c +++ b/UAV-ControlSystem/src/utilities.c @@ -51,3 +51,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; +} From 20f006a0ca2b93de69a67eb00b849708d6597fb5 Mon Sep 17 00:00:00 2001 From: philsson Date: Tue, 11 Oct 2016 18:02:11 +0200 Subject: [PATCH 3/3] Some changes to yaw inversion and changed some possible negative ints to int --- UAV-ControlSystem/inc/drivers/motormix.h | 2 +- UAV-ControlSystem/src/drivers/motormix.c | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h index e2c3966..c01139a 100644 --- a/UAV-ControlSystem/inc/drivers/motormix.h +++ b/UAV-ControlSystem/inc/drivers/motormix.h @@ -45,7 +45,7 @@ typedef struct { 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 - uint8_t yaw_motor_direction; // Default should be 1. Can be either -1 or 1 + bool yaw_reverse_direction; // Default should be 1. Can be either -1 or 1 } mixerConfig_s; /* Global mixerConfig to bee available to EEPROM */ diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index e11eebf..ce4b9e0 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -93,7 +93,7 @@ void mix() /* Calculate what "hover" could be. Not used now but might be useful later */ //uint16_t throttleIdle = mixerConfig.minThrottle + (throttleRange / 2); - uint16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array + 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 @@ -111,7 +111,7 @@ void mix() RPY_Mix[i] = \ PID_Out[PITCH] * mixerUAV[i].pitch + \ PID_Out[ROLL] * mixerUAV[i].roll + \ - PID_Out[YAW] * mixerUAV[i].yaw * mixerConfig.yaw_motor_direction; + 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]; @@ -167,13 +167,13 @@ void mix() throttle * mixerUAV[i].throttle + \ PID_Out[PITCH] * mixerUAV[i].pitch + \ PID_Out[ROLL] * mixerUAV[i].roll + \ - PID_Out[YAW] * mixerUAV[i].yaw * mixerConfig.yaw_motor_direction; + 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]; } - uint16_t maxThrottleDifference = 0; + int16_t maxThrottleDifference = 0; if (maxMotor > mixerConfig.maxThrottle) maxThrottleDifference = maxMotor - mixerConfig.maxThrottle;