/********************************************************************** * 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 }; /*********************************************************************** * 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]; if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += 1482; /* 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 motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, 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]); } }