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

Airmode is fixed. Still need to think about the "non Airmode" implementation

Browse Source
This commit is contained in:
philsson 2016-10-14 18:07:38 +02:00
parent 51ec5b4170
commit 82c8040520
1 changed files with 29 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
UAV-ControlSystem/src/drivers/motormix.c
View File

@ -127,6 +127,7 @@ void mix()
int16_t throttleMin = mixerConfig.minThrottle; // Import system variable int16_t throttleMin = mixerConfig.minThrottle; // Import system variable
int16_t throttleMax = mixerConfig.maxCommand; // Import int16_t throttleMax = mixerConfig.maxCommand; // Import
int16_t throttleRange = throttleMax - throttleMin; // The throttle range we have with current defines int16_t throttleRange = throttleMax - throttleMin; // The throttle range we have with current defines
int16_t throttleMid = (throttleMax + throttleMin) / 2;
//TODO: Seems to be causing an error when maxing any of the stick with low throttle //TODO: Seems to be causing an error when maxing any of the stick with low throttle
/* Check if we have enough interval for the adjustments */ /* Check if we have enough interval for the adjustments */
@ -143,14 +144,16 @@ void mix()
RPY_Mix[i] = RPY_Mix[i] * mixReduction; RPY_Mix[i] = RPY_Mix[i] * mixReduction;
//TEMPFIX recalculate //TEMPFIX recalculate
if (RPY_Mix[i] > RPY_Mix_Max) RPY_Mix_Max = RPY_Mix[i]; // 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]; // if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i];
} }
//Temp fix may not be right //Temp fix may not be right - Set throttle to exakt half (As this craft is symmetric)
RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min; throttle = mixerConfig.minThrottle + mixReduction * RPY_Mix_Max;
throttleMin += (RPY_MixRange / 2); //throttle += mixerConfig.minThrottle + throttleRange / 2;
throttleMax -= (RPY_MixRange / 2); // RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min;
// throttleMin += (RPY_MixRange / 2);
// throttleMax -= (RPY_MixRange / 2);
} }
// If we have the needed range no scaling is needed // If we have the needed range no scaling is needed
else else
@ -159,15 +162,30 @@ void mix()
/* Update min and max throttle so we can add the /* Update min and max throttle so we can add the
* calculated adjustments and still just max out */ * calculated adjustments and still just max out */
throttleMin += (RPY_MixRange / 2); throttleMin += (RPY_MixRange / 2); // Can be removed. Just made to be sure
throttleMax -= (RPY_MixRange / 2); 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 // Now we add desired throttle
for (int i = 0; i < MOTOR_COUNT; i++) for (int i = 0; i < MOTOR_COUNT; i++)
{ {
// Constrain in within the regulation of the mix // 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); motor_output[i] = RPY_Mix[i] + constrain(throttle * mixerUAV[i].throttle, throttleMin, throttleMax);
//motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand); //motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
} }
@ -221,7 +239,7 @@ void mix()
for (int i = 0; i < MOTOR_COUNT; i++) for (int i = 0; i < MOTOR_COUNT; i++)
{ {
/* If engines are armed then give the output to the motors */ /* 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) if (flags_IsSet_ID(systemFlags_armed_id))
motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand); motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
/* If not then stop motors */ /* If not then stop motors */
else else