philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

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

Browse Source
This commit is contained in:
Jonas Holmberg 2016-10-12 09:49:51 +02:00
commit 09daa76d63
5 changed files with 296 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
UAV-ControlSystem/inc/drivers/motormix.h Normal file
View File

@ -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_ */

7
UAV-ControlSystem/inc/drivers/motors.h
View File

@ -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;
/**************************************************************************

2
UAV-ControlSystem/inc/utilities.h
View File

@ -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_ */

218
UAV-ControlSystem/src/drivers/motormix.c Normal file
View File

@ -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]);
}
}

10
UAV-ControlSystem/src/utilities.c
View File

@ -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;
}