void setupAllMotorPoewrLevel(unsigned short CW1, unsigned short CW2,
unsigned short CCW1, unsigned short CCW2) {
setupCcw1MotorPoewrLevel(CCW1);
setupCcw2MotorPoewrLevel(CCW2);
setupCw1MotorPoewrLevel(CW1);
setupCw2MotorPoewrLevel(CW2);
}
/**
* this function controls motors by PID output
*
* @param
* void
*
* @return
* void
*
*/
void motorControler() {
float rollRateOutput = 0.f;
float rollCcw1 = 0.f;
float rollCcw2 = 0.f;
float rollCw1 = 0.f;
float rollCw2 = 0.f;
float pitchRateOutput = 0.f;
float pitchCcw1 = 0.f;
float pitchCcw2 = 0.f;
float pitchCw1 = 0.f;
float pitchCw2 = 0.f;
float yawRateOutput = 0.f;
float yawCcw1 = 0.f;
float yawCcw2 = 0.f;
float yawCw1 = 0.f;
float yawCw2 = 0.f;
float outCcw1 = 0.f;
float outCcw2 = 0.f;
float outCw1 = 0.f;
float outCw2 = 0.f;
float maxLimit = 0.f;
float minLimit = 0.f;
float accelThrottleOffset = 0.f;
float altholdThrottleOffset = 0.f;
float throttleOffset = 0.f;
float centerThrottle = 0.f;
altholdThrottleOffset = (getEnableAltHold() && getAltHoldIsReady()) ? getThrottleOffsetByAltHold(updateAltHold()) : 0.f;
accelThrottleOffset = getThrottleOffsetByAcceleration();
throttleOffset = altholdThrottleOffset + accelThrottleOffset;
centerThrottle = (float) getThrottlePowerLevel() + throttleOffset;
maxLimit = (float) min(centerThrottle + getAdjustPowerLeveRange(),
getMaxPowerLeve());
minLimit = (float) max(centerThrottle - getAdjustPowerLeveRange(),
getMinPowerLevel());
getAttitudePidOutput();
getRatePidOutput(&rollRateOutput, &pitchRateOutput, &yawRateOutput);
/*
* rollCa>0
* - CCW2 CW2 +
* X
* - CW1 CCW1 +
* F
*
* rollCa<0
* + CCW2 CW2 -
* X
* + CW1 CCW1 -
* F
*/
rollCcw1 = rollRateOutput;
rollCcw2 = -rollRateOutput;
rollCw1 = -rollRateOutput;
rollCw2 = rollRateOutput;
/*
* pitchCa>0
* + CCW2 CW2 +
* X
* - CW1 CCW1 -
* F
*
* pitchCa<0
* - CCW2 CW2 -
* X
* + CW1 CCW1 +
* F
*/
pitchCcw1 = -pitchRateOutput;
pitchCcw2 = pitchRateOutput;
pitchCw1 = -pitchRateOutput;
pitchCw2 = pitchRateOutput;
/*
* yawCa>0
* + CCW2 CW2 -
* X
* - CW1 CCW1 +
* F
*
* yawCa<0
* - CCW2 CW2 +
* X
* + CW1 CCW1 -
* F
*/
yawCcw1 = yawRateOutput;
yawCcw2 = yawRateOutput;
yawCw1 = -yawRateOutput;
yawCw2 = -yawRateOutput;
outCcw1 = centerThrottle
+ LIMIT_MIN_MAX_VALUE(rollCcw1 + pitchCcw1 + yawCcw1,
-getPidOutputLimitation(), getPidOutputLimitation());
outCcw2 = centerThrottle
+ LIMIT_MIN_MAX_VALUE(rollCcw2 + pitchCcw2 + yawCcw2,
-getPidOutputLimitation(), getPidOutputLimitation());
outCw1 = centerThrottle
+ LIMIT_MIN_MAX_VALUE(rollCw1 + pitchCw1 + yawCw1,
-getPidOutputLimitation(), getPidOutputLimitation());
outCw2 = centerThrottle
+ LIMIT_MIN_MAX_VALUE(rollCw2 + pitchCw2 + yawCw2,
-getPidOutputLimitation(), getPidOutputLimitation());
outCcw1 = getMotorGain(
SOFT_PWM_CCW1) * LIMIT_MIN_MAX_VALUE(outCcw1, minLimit, maxLimit);
outCcw2 = getMotorGain(
SOFT_PWM_CCW2) * LIMIT_MIN_MAX_VALUE(outCcw2, minLimit, maxLimit);
outCw1 = getMotorGain(
SOFT_PWM_CW1) * LIMIT_MIN_MAX_VALUE(outCw1, minLimit, maxLimit);
outCw2 = getMotorGain(
SOFT_PWM_CW2) * LIMIT_MIN_MAX_VALUE(outCw2, minLimit, maxLimit);
setupCcw1MotorPoewrLevel((unsigned short) outCcw1);
setupCcw2MotorPoewrLevel((unsigned short) outCcw2);
setupCw1MotorPoewrLevel((unsigned short) outCw1);
setupCw2MotorPoewrLevel((unsigned short) outCw2);
#if 0
_DEBUG(DEBUG_NORMAL,"outCcw1=%d,outCcw2=%d,outCw1=%d,outCw2=%d\n" ,
(unsigned short)outCcw1,
(unsigned short)outCcw2,
(unsigned short)outCw1,
(unsigned short)outCw2);
#endif
}
