312 lines
11 KiB
C
312 lines
11 KiB
C
/**********************************************************************
|
|
* 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"
|
|
#include "drivers/failsafe_toggles.h"
|
|
#include "Flight/pid.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];
|
|
|
|
/* Bool to see if motors are maxed out. Stops windup in PID implementation */
|
|
bool motorLimitReached = false;
|
|
|
|
/* Default values for the mixerConfig */
|
|
// TODO: Implement in EEPROM
|
|
mixerConfig_s mixerConfig = {
|
|
.minThrottle = 1040,
|
|
.maxThrottle = MAX_PULSE - 100,
|
|
.minCommand = 990,
|
|
.maxCommand = MAX_PULSE,
|
|
.minCheck = 1010,
|
|
.pid_at_min_throttle = true,
|
|
.motorstop = false,
|
|
.yaw_reverse_direction = true
|
|
};
|
|
|
|
/* 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
|
|
|
|
|
|
|
|
{ 1.0f, 1.0f, 1.0f, -1.0f}, //M1
|
|
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M2
|
|
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M3
|
|
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M4
|
|
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M5
|
|
{ 1.0f, -1.0f, 1.0f, -1.0f}, //M6
|
|
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M7
|
|
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M8
|
|
{ 0.0f, 0.0f, 0.0f, 0.0f}, //M9
|
|
{ 0.0f, 0.0f, 0.0f, 0.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
|
|
int16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor
|
|
int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor
|
|
int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]*throttleRate;
|
|
|
|
if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += HoverForce;
|
|
|
|
/* Mixer Full Scale enabled */
|
|
if (flags_IsSet_ID(systemFlags_mixerfullscale_id))
|
|
{
|
|
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] = \
|
|
(int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
|
|
(int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
|
|
PidProfile[PID_ID_GYRO].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];
|
|
}
|
|
|
|
|
|
|
|
int16_t RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min; // Range of the desired mixer outputs
|
|
int16_t throttleMin = mixerConfig.minThrottle; // Import system variable
|
|
int16_t throttleMax = mixerConfig.maxCommand; // Import
|
|
int16_t throttleRange = throttleMax - throttleMin; // The throttle range we have with current defines
|
|
int16_t throttleMid = (throttleMax + throttleMin) / 2;
|
|
|
|
/* 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;
|
|
|
|
//Temp fix may not be right - Set throttle to exakt half (As this craft is symmetric)
|
|
throttle = mixerConfig.minThrottle + mixReduction * RPY_Mix_Max;
|
|
|
|
}
|
|
|
|
// 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); // Can be removed. Just made to be sure
|
|
throttleMax -= (RPY_MixRange / 2); // Can be removed. Just made to be sure
|
|
|
|
/* The scaling of the throttle in the room thats left from the mixer */
|
|
float throttleRescale = ((float)(throttleRange - RPY_MixRange)) / (float)throttleRange;
|
|
|
|
/* Make sure throttle mostly is inside range. Mostly above minthrottle before scaling */
|
|
throttle = constrain(throttle, mixerConfig.minThrottle, mixerConfig.maxCommand);
|
|
|
|
/* Converting throttle to value centered around 0 */
|
|
throttle = (throttle - (mixerConfig.maxCommand - throttleRange / 2));
|
|
|
|
/* Rescaling throttle */
|
|
throttle = throttle*throttleRescale;
|
|
|
|
/* Adding new scaled throttle to throttleMid */
|
|
throttle = throttleMid + throttle;
|
|
|
|
}
|
|
|
|
// Now we add desired throttle
|
|
for (int i = 0; i < MOTOR_COUNT; i++)
|
|
// Constrain in within the regulation of the mix - OBS. Constrain can be removed. Just to make sure
|
|
|
|
// TODO: This line is backup as we discovered that motors could stop at times in airmode on M-UAV. But we have not seen this before
|
|
//motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax);
|
|
motor_output[i] = constrain(RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax), throttleMin, throttleMax);
|
|
|
|
}
|
|
else // Mixer full scale NOT active
|
|
{
|
|
uint16_t throttleMid = (mixerConfig.minCommand + mixerConfig.maxThrottle) / 2;
|
|
/* This time we need to check against mid throttle */
|
|
RPY_Mix_Min = throttleMid;
|
|
RPY_Mix_Max = throttleMid;
|
|
|
|
for (int i = 0; i < MOTOR_COUNT; i++) // Without airmode this includes throttle
|
|
{
|
|
RPY_Mix[i] = \
|
|
throttle * mixerUAV[i].throttle + \
|
|
(int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
|
|
(int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
|
|
PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1);
|
|
}
|
|
|
|
|
|
|
|
|
|
/* Mixer Low Scale - Scaling output around low throttle */
|
|
if (flags_IsSet_ID(systemFlags_mixerlowscale_id))
|
|
{
|
|
for (int i = 0; i < MOTOR_COUNT; i++)
|
|
// Find the minimum and maximum motor output
|
|
if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i];
|
|
|
|
uint16_t RPY_Mix_Min_Overshoot = (RPY_Mix_Min < mixerConfig.minThrottle) ? mixerConfig.minThrottle - RPY_Mix_Min : 0;
|
|
|
|
if (RPY_Mix_Min_Overshoot > 0 ) //|| RPY_Mix_Max_Overshoot > 0)
|
|
{
|
|
/* How far away are we from throttle */
|
|
uint16_t RPY_Mix_Thr_Displacement = throttle - RPY_Mix_Min;
|
|
|
|
/* Scaling factor */
|
|
float mix_scale_reduction = ((float)RPY_Mix_Min_Overshoot / (float)RPY_Mix_Thr_Displacement);
|
|
|
|
/* Rescaling */
|
|
for (int i = 0; i < MOTOR_COUNT; i++)
|
|
RPY_Mix[i] = (((float)(RPY_Mix[i] - throttle)) * (1 - mix_scale_reduction)) + throttle;
|
|
}
|
|
/* Recalculating RPY_Mix_Min */
|
|
RPY_Mix_Min = throttleMid;
|
|
}
|
|
|
|
|
|
|
|
for (int i = 0; i < MOTOR_COUNT; i++)
|
|
{
|
|
// Find the minimum and maximum motor output
|
|
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];
|
|
}
|
|
|
|
/* Check how far over maxCommand we are (overflow) */
|
|
int16_t maxThrottleDifference = 0;
|
|
if (RPY_Mix_Max > mixerConfig.maxCommand)
|
|
{
|
|
maxThrottleDifference = RPY_Mix_Max - mixerConfig.maxCommand;
|
|
//RPY_Mix_Min = throttleMid; // We need to recalculate if MaxThrottleDifference is higher than 0
|
|
}
|
|
|
|
|
|
// Last flag checks and output reduction
|
|
for (int i = 0; i < MOTOR_COUNT; i++)
|
|
{
|
|
if (maxThrottleDifference > 0)
|
|
{
|
|
RPY_Mix[i] -= maxThrottleDifference; // We reduce the highest overflow on all motors
|
|
if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i];
|
|
}
|
|
|
|
|
|
RPY_Mix[i] = constrain(RPY_Mix[i], mixerConfig.minThrottle, mixerConfig.maxCommand);
|
|
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);
|
|
}
|
|
motorLimitReached = (RPY_Mix_Min <= mixerConfig.minThrottle && maxThrottleDifference > 0);
|
|
}
|
|
|
|
// 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 (flags_IsSet_ID(systemFlags_armed_id) && !flags_IsSet_ID(systemFlags_Failsafe_noRcReceived_id))
|
|
motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
|
|
/* If not then stop motors */
|
|
else
|
|
motor_output[i] = mixerConfig.minCommand;
|
|
|
|
/* Update actuators */
|
|
pwmAdjustSpeedOfMotor( i + 1 /* Motors start from Motor 1 */,motor_output[i]);
|
|
}
|
|
}
|