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 throttleMax = mixerConfig.maxCommand; // Import
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
/* Check if we have enough interval for the adjustments */
@ -143,14 +144,16 @@ void mix()
RPY_Mix[i] = RPY_Mix[i] * mixReduction;
//TEMPFIX recalculate
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_Max) RPY_Mix_Max = RPY_Mix[i];
// if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i];
}
//Temp fix may not be right
RPY_MixRange = RPY_Mix_Max - RPY_Mix_Min;
throttleMin += (RPY_MixRange / 2);
throttleMax -= (RPY_MixRange / 2);
//Temp fix may not be right - Set throttle to exakt half (As this craft is symmetric)
throttle = mixerConfig.minThrottle + mixReduction * RPY_Mix_Max;
//throttle += mixerConfig.minThrottle + throttleRange / 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
else
@ -159,15 +162,30 @@ void mix()
/* 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);
}
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
// 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] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
}
@ -221,7 +239,7 @@ void mix()
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)
if (flags_IsSet_ID(systemFlags_armed_id))
motor_output[i] = constrain(motor_output[i], mixerConfig.minCommand, mixerConfig.maxCommand);
/* If not then stop motors */
else