2b05c843a1
Also added some statistical values that can be veiwed in CLI to see how they sytem performs.
222 lines
7.6 KiB
C
222 lines
7.6 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"
|
|
|
|
/* 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 = {
|
|
.minThrottle = 1000,
|
|
.maxThrottle = 2000,
|
|
.minCommand = 1050,
|
|
.maxCommand = 1950,
|
|
.minCheck = 1010,
|
|
.pid_at_min_throttle = true,
|
|
.motorstop = false,
|
|
.yaw_reverse_direction = false
|
|
};
|
|
|
|
/* 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 (flags_IsSet_ID(systemFlags_airmode_id)) // 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 (flags_IsSet_ID(systemFlags_armed_id)) // 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]);
|
|
}
|
|
}
|