void RunMotors() {
int16_t maxMotor;
uint8_t i;
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL]*X + axisPID[PITCH]*Y + YAW_DIRECTION * axisPID[YAW]*Z
motor[0] = PIDMIX(-1,+1,-1); //REAR_R
motor[1] = PIDMIX(-1,-1,+1); //FRONT_R
motor[2] = PIDMIX(+1,+1,+1); //REAR_L
motor[3] = PIDMIX(+1,-1,-1); //FRONT_L
maxMotor=motor[0];
for(i=1; i< 4; i++)
if (motor[i]>maxMotor) maxMotor=motor[i];
for(i=0; i< 4; i++) {
if (maxMotor > MAXTHROTTLE) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - MAXTHROTTLE;
motor[i] = constrain(motor[i], conf.minthrottle, MAXTHROTTLE);
if (rcData[THROTTLE] < MINCHECK)
motor[i] = MINCOMMAND;
}
writeMotors();
}
void mixTable(void)
{
int16_t maxMotor;
uint8_t i;
///////////////////////////////////
switch ( eepromConfig.mixerConfiguration )
{
case MIXERTYPE_QUADX:
motor[0] = PIDMIX( 1.0f, -1.0f, -1.0f ); // Front Left CW
motor[1] = PIDMIX( -1.0f, -1.0f, 1.0f ); // Front Right CCW
motor[2] = PIDMIX( -1.0f, 1.0f, -1.0f ); // Rear Right CW
motor[3] = PIDMIX( 1.0f, 1.0f, 1.0f ); // Rear Left CCW
break;
///////////////////////////////
case MIXERTYPE_HEX6X:
motor[0] = PIDMIX( 0.866025f, -1.0f, -1.0f ); // Front Left CW
motor[1] = PIDMIX( -0.866025f, -1.0f, 1.0f ); // Front Right CCW
motor[2] = PIDMIX( -0.866025f, 0.0f, -1.0f ); // Right CW
motor[3] = PIDMIX( -0.866025f, 1.0f, 1.0f ); // Rear Right CCW
motor[4] = PIDMIX( 0.866025f, 1.0f, -1.0f ); // Rear Left CW
motor[5] = PIDMIX( 0.866025f, 0.0f, 1.0f ); // Left CCW
break;
}
///////////////////////////////////
// this is a way to still have good gyro corrections if any motor reaches its max.
maxMotor = motor[0];
for (i = 1; i < numberMotor; i++)
if (motor[i] > maxMotor)
maxMotor = motor[i];
for (i = 0; i < numberMotor; i++)
{
if (maxMotor > eepromConfig.maxThrottle)
motor[i] -= maxMotor - eepromConfig.maxThrottle;
motor[i] = constrain(motor[i], eepromConfig.minThrottle, eepromConfig.maxThrottle);
if ((rxCommand[THROTTLE] < eepromConfig.minCheck) && (verticalModeState == ALT_DISENGAGED_THROTTLE_ACTIVE))
motor[i] = eepromConfig.minThrottle;
if ( armed == false )
motor[i] = (float)MINCOMMAND;
}
}
void RunMotors() {
int16_t maxMotor;
int i;
#define PIDMIX(X,Y,Z) rcCommand[THROTTLE] + axisPID[ROLL]*X + axisPID[PITCH]*Y + YAW_DIRECTION * axisPID[YAW]*Z
motor[0] = PIDMIX(-1,+1,-1); //REAR_R
motor[1] = PIDMIX(-1,-1,+1); //FRONT_R
motor[2] = PIDMIX(+1,+1,+1); //REAR_L
motor[3] = PIDMIX(+1,-1,-1); //FRONT_L
#if defined(TRACE)
Serial.print(">>>RunMotors: rcCommand[THROTTLE]:");Serial.print(rcCommand[THROTTLE]);Serial.print(" *** ");
Serial.print("axisPID[ROLL]:");Serial.print(axisPID[ROLL]);Serial.print(" *** ");
Serial.print("axisPID[PITCH]:");Serial.print(axisPID[PITCH]);Serial.print(" *** ");
Serial.print("axisPID[YAW]:");Serial.println(axisPID[YAW]);
for (i = 0; i < 4; i++) {
Serial.print(">>>RunMotors: motor[");Serial.print(i);Serial.print("]:");Serial.println(motor[i]);
}
#endif
maxMotor=motor[0];
for(i=1; i< 4; i++)
if (motor[i]>maxMotor) maxMotor=motor[i];
for(i=0; i< 4; i++) {
if (maxMotor > MAXCOMMAND) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - MAXCOMMAND;
motor[i] = constrain(motor[i], MINCOMMAND, MAXCOMMAND);
if (rcData[THROTTLE] < MINCHECK) // stop requested
motor[i] = MINCOMMAND;
}
writeMotors();
}
void mixTable(void)
{
int16_t maxMotor;
uint8_t i;
static uint8_t camCycle = 0;
static uint8_t camState = 0;
static uint32_t camTime = 0;
if (numberMotor > 3) {
//prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
}
switch (cfg.mixerConfiguration) {
case MULTITYPE_BI:
motor[0] = PIDMIX(+1, 0, 0); //LEFT
motor[1] = PIDMIX(-1, 0, 0); //RIGHT
servo[4] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT
servo[5] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT
break;
case MULTITYPE_TRI:
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
servo[4] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], TRI_YAW_CONSTRAINT_MIN, TRI_YAW_CONSTRAINT_MAX); //REAR
break;
case MULTITYPE_QUADP:
motor[0] = PIDMIX(0, +1, -1); //REAR
motor[1] = PIDMIX(-1, 0, +1); //RIGHT
motor[2] = PIDMIX(+1, 0, +1); //LEFT
motor[3] = PIDMIX(0, -1, -1); //FRONT
break;
case MULTITYPE_QUADX:
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
break;
case MULTITYPE_Y4:
motor[0] = PIDMIX(+0, +1, -1); //REAR_1 CW
motor[1] = PIDMIX(-1, -1, 0); //FRONT_R CCW
motor[2] = PIDMIX(+0, +1, +1); //REAR_2 CCW
motor[3] = PIDMIX(+1, -1, 0); //FRONT_L CW
break;
case MULTITYPE_Y6:
motor[0] = PIDMIX(+0, +4 / 3, +1); //REAR
motor[1] = PIDMIX(-1, -2 / 3, -1); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, -1); //LEFT
motor[3] = PIDMIX(+0, +4 / 3, -1); //UNDER_REAR
motor[4] = PIDMIX(-1, -2 / 3, +1); //UNDER_RIGHT
motor[5] = PIDMIX(+1, -2 / 3, +1); //UNDER_LEFT
break;
case MULTITYPE_HEX6:
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
motor[1] = PIDMIX(-1 / 2, -1 / 2, -1); //FRONT_R
motor[2] = PIDMIX(+1 / 2, +1 / 2, +1); //REAR_L
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
motor[4] = PIDMIX(+0, -1, +1); //FRONT
motor[5] = PIDMIX(+0, +1, -1); //REAR
break;
case MULTITYPE_HEX6X:
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
motor[1] = PIDMIX(-1 / 2, -1 / 2, +1); //FRONT_R
motor[2] = PIDMIX(+1 / 2, +1 / 2, -1); //REAR_L
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
motor[4] = PIDMIX(-1, +0, -1); //RIGHT
motor[5] = PIDMIX(+1, +0, +1); //LEFT
break;
case MULTITYPE_OCTOX8:
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
motor[4] = PIDMIX(-1, +1, +1); //UNDER_REAR_R
motor[5] = PIDMIX(-1, -1, -1); //UNDER_FRONT_R
motor[6] = PIDMIX(+1, +1, -1); //UNDER_REAR_L
motor[7] = PIDMIX(+1, -1, +1); //UNDER_FRONT_L
break;
case MULTITYPE_OCTOFLATP:
motor[0] = PIDMIX(+7 / 10, -7 / 10, +1); //FRONT_L
motor[1] = PIDMIX(-7 / 10, -7 / 10, +1); //FRONT_R
motor[2] = PIDMIX(-7 / 10, +7 / 10, +1); //REAR_R
motor[3] = PIDMIX(+7 / 10, +7 / 10, +1); //REAR_L
motor[4] = PIDMIX(+0, -1, -1); //FRONT
motor[5] = PIDMIX(-1, +0, -1); //RIGHT
motor[6] = PIDMIX(+0, +1, -1); //REAR
motor[7] = PIDMIX(+1, +0, -1); //LEFT
break;
case MULTITYPE_OCTOFLATX:
motor[0] = PIDMIX(+1, -1 / 2, +1); //MIDFRONT_L
motor[1] = PIDMIX(-1 / 2, -1, +1); //FRONT_R
motor[2] = PIDMIX(-1, +1 / 2, +1); //MIDREAR_R
motor[3] = PIDMIX(+1 / 2, +1, +1); //REAR_L
motor[4] = PIDMIX(+1 / 2, -1, -1); //FRONT_L
motor[5] = PIDMIX(-1, -1 / 2, -1); //MIDFRONT_R
motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R
motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L
break;
case MULTITYPE_VTAIL4:
motor[0] = PIDMIX(+0, +1, -1 / 2); //REAR_R
motor[1] = PIDMIX(-1, -1, +2 / 10); //FRONT_R
motor[2] = PIDMIX(+0, +1, +1 / 2); //REAR_L
motor[3] = PIDMIX(+1, -1, -2 / 10); //FRONT_L
break;
case MULTITYPE_GIMBAL:
servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
break;
case MULTITYPE_FLYING_WING:
motor[0] = rcCommand[THROTTLE];
if (passThruMode) { // do not use sensors for correction, simple 2 channel mixing
servo[0] = PITCH_DIRECTION_L * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_L * (rcData[ROLL] - cfg.midrc);
servo[1] = PITCH_DIRECTION_R * (rcData[PITCH] - cfg.midrc) + ROLL_DIRECTION_R * (rcData[ROLL] - cfg.midrc);
} else { // use sensors to correct (gyro only or gyro+acc according to aux1/aux2 configuration
servo[0] = PITCH_DIRECTION_L * axisPID[PITCH] + ROLL_DIRECTION_L * axisPID[ROLL];
servo[1] = PITCH_DIRECTION_R * axisPID[PITCH] + ROLL_DIRECTION_R * axisPID[ROLL];
}
servo[0] = constrain(servo[0] + cfg.wing_left_mid , WING_LEFT_MIN, WING_LEFT_MAX);
servo[1] = constrain(servo[1] + cfg.wing_right_mid, WING_RIGHT_MIN, WING_RIGHT_MAX);
break;
}
// do camstab
if (feature(FEATURE_SERVO_TILT)) {
uint16_t aux[2] = { 0, 0 };
if ((cfg.gimbal_flags & GIMBAL_NORMAL) || (cfg.gimbal_flags & GIMBAL_TILTONLY))
aux[0] = rcData[AUX3];
if (!(cfg.gimbal_flags & GIMBAL_DISABLEAUX34))
aux[1] = rcData[AUX4];
servo[0] = cfg.gimbal_pitch_mid + aux[0] - cfg.midrc;
servo[1] = cfg.gimbal_roll_mid + aux[1] - cfg.midrc;
if (rcOptions[BOXCAMSTAB]) {
servo[0] += cfg.gimbal_pitch_gain * angle[PITCH] / 16;
servo[1] += cfg.gimbal_roll_gain * angle[ROLL] / 16;
}
servo[0] = constrain(servo[0], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(servo[1], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
}
// do camtrig (this doesn't actually work)
if (feature(FEATURE_CAMTRIG)) {
if (camCycle == 1) {
if (camState == 0) {
servo[2] = CAM_SERVO_HIGH;
camState = 1;
camTime = millis();
} else if (camState == 1) {
if ((millis() - camTime) > CAM_TIME_HIGH) {
servo[2] = CAM_SERVO_LOW;
camState = 2;
camTime = millis();
}
} else { //camState ==2
if ((millis() - camTime) > CAM_TIME_LOW) {
camState = 0;
camCycle = 0;
}
}
}
if (rcOptions[BOXCAMTRIG])
camCycle = 1;
}
maxMotor = motor[0];
for (i = 1; i < numberMotor; i++)
if (motor[i] > maxMotor)
maxMotor = motor[i];
for (i = 0; i < numberMotor; i++) {
if (maxMotor > cfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - cfg.maxthrottle;
motor[i] = constrain(motor[i], cfg.minthrottle, cfg.maxthrottle);
if ((rcData[THROTTLE]) < cfg.mincheck) {
if (!feature(FEATURE_MOTOR_STOP))
motor[i] = cfg.minthrottle;
else
motor[i] = cfg.mincommand;
}
if (armed == 0)
motor[i] = cfg.mincommand;
}
}
void mixTable(void)
{
uint8_t i;
///////////////////////////////////
switch ( eepromConfig.mixerConfiguration )
{
///////////////////////////////
case MIXERTYPE_TRI:
motor[0] = PIDMIX( 1.0f, -0.666667f, 0.0f, 1.0f ); // Left CW
motor[1] = PIDMIX( -1.0f, -0.666667f, 0.0f, 1.0f ); // Right CCW
motor[2] = PIDMIX( 0.0f, 1.333333f, 0.0f, 1.0f ); // Rear CW or CCW
motor[7] = eepromConfig.triYawServoMid + eepromConfig.yawDirection * ratePID[YAW];
motor[7] = firstOrderFilter(motor[7], &firstOrderFilters[TRICOPTER_YAW_LOWPASS]);
motor[7] = constrain(motor[7], eepromConfig.triYawServoMin, eepromConfig.triYawServoMax );
break;
///////////////////////////////
case MIXERTYPE_QUADX:
motor[0] = PIDMIX( 1.0f, -1.0f, -1.0f, 1.0f ); // Front Left CW
motor[1] = PIDMIX( -1.0f, -1.0f, 1.0f, 1.0f ); // Front Right CCW
motor[2] = PIDMIX( -1.0f, 1.0f, -1.0f, 1.0f ); // Rear Right CW
motor[3] = PIDMIX( 1.0f, 1.0f, 1.0f, 1.0f ); // Rear Left CCW
break;
///////////////////////////////
case MIXERTYPE_HEX6X:
motor[0] = PIDMIX( 0.866025f, -1.0f, -1.0f, 1.0f ); // Front Left CW
motor[1] = PIDMIX( -0.866025f, -1.0f, 1.0f, 1.0f ); // Front Right CCW
motor[2] = PIDMIX( -0.866025f, 0.0f, -1.0f, 1.0f ); // Right CW
motor[3] = PIDMIX( -0.866025f, 1.0f, 1.0f, 1.0f ); // Rear Right CCW
motor[4] = PIDMIX( 0.866025f, 1.0f, -1.0f, 1.0f ); // Rear Left CW
motor[5] = PIDMIX( 0.866025f, 0.0f, 1.0f, 1.0f ); // Left CCW
break;
///////////////////////////////
case MIXERTYPE_Y6:
motor[0] = PIDMIX( 1.0f, -0.666667, -1.0f, 1.0f ); // Top Left CW
motor[1] = PIDMIX( -1.0f, -0.666667, 1.0f, 1.0f ); // Top Right CCW
motor[2] = PIDMIX( 0.0f, 1.333333, 1.0f, 1.0f ); // Top Rear CCW
motor[3] = PIDMIX( 1.0f, -0.666667, 1.0f, 1.0f ); // Bottom Left CCW
motor[4] = PIDMIX( -1.0f, -0.666667, -1.0f, 1.0f ); // Bottom Right CW
motor[5] = PIDMIX( 0.0f, 1.333333, -1.0f, 1.0f ); // Bottom Rear CW
break;
///////////////////////////////
case MIXERTYPE_FREE:
for ( i = 0; i < numberMotor; i++ )
motor[i] = PIDMIX ( eepromConfig.freeMix[i][ROLL], eepromConfig.freeMix[i][PITCH], eepromConfig.freeMix[i][YAW], eepromConfig.freeMix[i][THROTTLE] );
break;
///////////////////////////////
}
///////////////////////////////////
#if 0
// this is a way to still have good gyro corrections if any motor reaches its max.
int16_t maxMotor;
maxMotor = motor[0];
for (i = 1; i < numberMotor; i++)
if (motor[i] > maxMotor)
maxMotor = motor[i];
for (i = 0; i < numberMotor; i++)
{
if (maxMotor > eepromConfig.maxThrottle)
motor[i] -= maxMotor - eepromConfig.maxThrottle;
motor[i] = constrain(motor[i], eepromConfig.minThrottle, eepromConfig.maxThrottle);
if ((rxCommand[THROTTLE]) < eepromConfig.minCheck)
{
motor[i] = eepromConfig.minThrottle;
}
if ( armed == false )
motor[i] = (float)MINCOMMAND;
}
#else
float maxDeltaThrottle;
float minDeltaThrottle;
float deltaThrottle;
maxDeltaThrottle = (float)MAXCOMMAND - rxCommand[THROTTLE];
minDeltaThrottle = rxCommand[THROTTLE] - eepromConfig.minThrottle;
deltaThrottle = (minDeltaThrottle<maxDeltaThrottle) ? minDeltaThrottle : maxDeltaThrottle;
for (i=0; i<numberMotor; i++)
{
motor[i] = constrain(motor[i], rxCommand[THROTTLE] - deltaThrottle, rxCommand[THROTTLE] + deltaThrottle);
if ((rxCommand[THROTTLE]) < eepromConfig.minCheck)
motor[i] = eepromConfig.minThrottle;
if (armed == false)
motor[i] = (float)MINCOMMAND;
}
#endif
}
void mixTable(void)
{
int16_t maxMotor;
uint32_t i;
if (numberMotor > 3) {
// prevent "yaw jump" during yaw correction
axisPID[YAW] = constrain(axisPID[YAW], -100 - abs(rcCommand[YAW]), +100 + abs(rcCommand[YAW]));
}
switch (cfg.mixerConfiguration) {
case MULTITYPE_BI:
motor[0] = PIDMIX(+1, 0, 0); //LEFT
motor[1] = PIDMIX(-1, 0, 0); //RIGHT
servo[4] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT
servo[5] = constrain(1500 + (cfg.yaw_direction * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT
break;
case MULTITYPE_TRI:
motor[0] = PIDMIX(0, +4 / 3, 0); //REAR
motor[1] = PIDMIX(-1, -2 / 3, 0); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, 0); //LEFT
servo[5] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
break;
case MULTITYPE_QUADP:
motor[0] = PIDMIX(0, +1, -1); //REAR
motor[1] = PIDMIX(-1, 0, +1); //RIGHT
motor[2] = PIDMIX(+1, 0, +1); //LEFT
motor[3] = PIDMIX(0, -1, -1); //FRONT
break;
case MULTITYPE_QUADX:
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
break;
case MULTITYPE_Y4:
motor[0] = PIDMIX(+0, +1, -1); //REAR_1 CW
motor[1] = PIDMIX(-1, -1, 0); //FRONT_R CCW
motor[2] = PIDMIX(+0, +1, +1); //REAR_2 CCW
motor[3] = PIDMIX(+1, -1, 0); //FRONT_L CW
break;
case MULTITYPE_Y6:
motor[0] = PIDMIX(+0, +4 / 3, +1); //REAR
motor[1] = PIDMIX(-1, -2 / 3, -1); //RIGHT
motor[2] = PIDMIX(+1, -2 / 3, -1); //LEFT
motor[3] = PIDMIX(+0, +4 / 3, -1); //UNDER_REAR
motor[4] = PIDMIX(-1, -2 / 3, +1); //UNDER_RIGHT
motor[5] = PIDMIX(+1, -2 / 3, +1); //UNDER_LEFT
break;
case MULTITYPE_HEX6:
motor[0] = PIDMIX(-1 / 2, +1 / 2, +1); //REAR_R
motor[1] = PIDMIX(-1 / 2, -1 / 2, -1); //FRONT_R
motor[2] = PIDMIX(+1 / 2, +1 / 2, +1); //REAR_L
motor[3] = PIDMIX(+1 / 2, -1 / 2, -1); //FRONT_L
motor[4] = PIDMIX(+0, -1, +1); //FRONT
motor[5] = PIDMIX(+0, +1, -1); //REAR
break;
case MULTITYPE_HEX6X:
motor[0] = PIDMIX(-4/5,+9/10,+1); //REAR_R
motor[1] = PIDMIX(-4/5,-9/10,+1); //FRONT_R
motor[2] = PIDMIX(+4/5,+9/10,-1); //REAR_L
motor[3] = PIDMIX(+4/5,-9/10,-1); //FRONT_L
motor[4] = PIDMIX(-4/5 ,+0 ,-1); //RIGHT
motor[5] = PIDMIX(+4/5 ,+0 ,+1); //LEFT
break;
case MULTITYPE_OCTOX8:
motor[0] = PIDMIX(-1, +1, -1); //REAR_R
motor[1] = PIDMIX(-1, -1, +1); //FRONT_R
motor[2] = PIDMIX(+1, +1, +1); //REAR_L
motor[3] = PIDMIX(+1, -1, -1); //FRONT_L
motor[4] = PIDMIX(-1, +1, +1); //UNDER_REAR_R
motor[5] = PIDMIX(-1, -1, -1); //UNDER_FRONT_R
motor[6] = PIDMIX(+1, +1, -1); //UNDER_REAR_L
motor[7] = PIDMIX(+1, -1, +1); //UNDER_FRONT_L
break;
case MULTITYPE_OCTOFLATP:
motor[0] = PIDMIX(+7 / 10, -7 / 10, +1); //FRONT_L
motor[1] = PIDMIX(-7 / 10, -7 / 10, +1); //FRONT_R
motor[2] = PIDMIX(-7 / 10, +7 / 10, +1); //REAR_R
motor[3] = PIDMIX(+7 / 10, +7 / 10, +1); //REAR_L
motor[4] = PIDMIX(+0, -1, -1); //FRONT
motor[5] = PIDMIX(-1, +0, -1); //RIGHT
motor[6] = PIDMIX(+0, +1, -1); //REAR
motor[7] = PIDMIX(+1, +0, -1); //LEFT
break;
case MULTITYPE_OCTOFLATX:
motor[0] = PIDMIX(+1, -1 / 2, +1); //MIDFRONT_L
motor[1] = PIDMIX(-1 / 2, -1, +1); //FRONT_R
motor[2] = PIDMIX(-1, +1 / 2, +1); //MIDREAR_R
motor[3] = PIDMIX(+1 / 2, +1, +1); //REAR_L
motor[4] = PIDMIX(+1 / 2, -1, -1); //FRONT_L
motor[5] = PIDMIX(-1, -1 / 2, -1); //MIDFRONT_R
motor[6] = PIDMIX(-1 / 2, +1, -1); //REAR_R
motor[7] = PIDMIX(+1, +1 / 2, -1); //MIDREAR_L
break;
case MULTITYPE_VTAIL4:
motor[0] = PIDMIX(+0, +1, +1); //REAR_R
motor[1] = PIDMIX(-1, -1, +0); //FRONT_R
motor[2] = PIDMIX(+0, +1, -1); //REAR_L
motor[3] = PIDMIX(+1, -1, -0); //FRONT_L
break;
case MULTITYPE_GIMBAL:
servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * angle[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * angle[ROLL] / 16 + rcCommand[ROLL], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
break;
case MULTITYPE_AIRPLANE:
airplaneMixer();
break;
case MULTITYPE_FLYING_WING:
motor[0] = rcCommand[THROTTLE];
if (f.PASSTHRU_MODE) { // do not use sensors for correction, simple 2 channel mixing
int p = 0, r = 0;
servo[0] = p * (rcData[PITCH] - cfg.midrc) + r * (rcData[ROLL] - cfg.midrc);
servo[1] = p * (rcData[PITCH] - cfg.midrc) + r * (rcData[ROLL] - cfg.midrc);
} else { // use sensors to correct (gyro only or gyro+acc)
int p = 0, r = 0;
servo[0] = p * axisPID[PITCH] + r * axisPID[ROLL];
servo[1] = p * axisPID[PITCH] + r * axisPID[ROLL];
}
break;
}
// do camstab
if (feature(FEATURE_SERVO_TILT)) {
uint16_t aux[2] = { 0, 0 };
if ((cfg.gimbal_flags & GIMBAL_NORMAL) || (cfg.gimbal_flags & GIMBAL_TILTONLY))
aux[0] = rcData[AUX3] - cfg.midrc;
if (!(cfg.gimbal_flags & GIMBAL_DISABLEAUX34))
aux[1] = rcData[AUX4] - cfg.midrc;
servo[0] = cfg.gimbal_pitch_mid + aux[0];
servo[1] = cfg.gimbal_roll_mid + aux[1];
if (rcOptions[BOXCAMSTAB]) {
servo[0] += cfg.gimbal_pitch_gain * angle[PITCH] / 16;
servo[1] += cfg.gimbal_roll_gain * angle[ROLL] / 16;
}
servo[0] = constrain(servo[0], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(servo[1], cfg.gimbal_roll_min, cfg.gimbal_roll_max);
}
if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) {
int offset = 0;
if (feature(FEATURE_SERVO_TILT))
offset = 2;
for (i = 0; i < 4; i++)
pwmWriteServo(i + offset, rcData[AUX1 + i]);
}
maxMotor = motor[0];
for (i = 1; i < numberMotor; i++)
if (motor[i] > maxMotor)
maxMotor = motor[i];
for (i = 0; i < numberMotor; i++) {
if (maxMotor > cfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] -= maxMotor - cfg.maxthrottle;
motor[i] = constrain(motor[i], cfg.minthrottle, cfg.maxthrottle);
if ((rcData[THROTTLE]) < cfg.mincheck) {
if (!feature(FEATURE_MOTOR_STOP))
motor[i] = cfg.minthrottle;
else
motor[i] = cfg.mincommand;
}
if (!f.ARMED)
motor[i] = cfg.mincommand;
}
}
void mixTable(void)
{
int16_t maxMotor;
uint8_t i;
///////////////////////////////////
switch ( eepromConfig.mixerConfiguration )
{
///////////////////////////////
case MIXERTYPE_TRI:
motor[0] = PIDMIX( 1.0f, -0.666667f, 0.0f ); // Left CW
motor[1] = PIDMIX( -1.0f, -0.666667f, 0.0f ); // Right CCW
motor[2] = PIDMIX( 0.0f, 1.333333f, 0.0f ); // Rear CW or CCW
motor[7] = eepromConfig.triYawServoMid + eepromConfig.yawDirection * ratePID[YAW];
motor[7] = firstOrderFilter(motor[7], &firstOrderFilters[TRICOPTER_YAW_LOWPASS]);
motor[7] = constrain(motor[7], eepromConfig.triYawServoMin, eepromConfig.triYawServoMax );
break;
///////////////////////////////
case MIXERTYPE_QUADX:
motor[0] = PIDMIX( 1.0f, -1.0f, -1.0f ); // Front Left CW
motor[1] = PIDMIX( -1.0f, -1.0f, 1.0f ); // Front Right CCW
motor[2] = PIDMIX( -1.0f, 1.0f, -1.0f ); // Rear Right CW
motor[3] = PIDMIX( 1.0f, 1.0f, 1.0f ); // Rear Left CCW
break;
///////////////////////////////
case MIXERTYPE_HEX6X:
motor[0] = PIDMIX( 0.866025f, -1.0f, -1.0f ); // Front Left CW
motor[1] = PIDMIX( -0.866025f, -1.0f, 1.0f ); // Front Right CCW
motor[2] = PIDMIX( -0.866025f, 0.0f, -1.0f ); // Right CW
motor[3] = PIDMIX( -0.866025f, 1.0f, 1.0f ); // Rear Right CCW
motor[4] = PIDMIX( 0.866025f, 1.0f, -1.0f ); // Rear Left CW
motor[5] = PIDMIX( 0.866025f, 0.0f, 1.0f ); // Left CCW
break;
///////////////////////////////
case MIXERTYPE_FREE:
for ( i = 0; i < numberMotor; i++ )
motor[i] = PIDMIX ( eepromConfig.freeMix[i][ROLL], eepromConfig.freeMix[i][PITCH], eepromConfig.freeMix[i][YAW] );
break;
///////////////////////////////
}
///////////////////////////////////
// this is a way to still have good gyro corrections if any motor reaches its max.
maxMotor = motor[0];
for (i = 1; i < numberMotor; i++)
if (motor[i] > maxMotor)
maxMotor = motor[i];
for (i = 0; i < numberMotor; i++)
{
if (maxMotor > eepromConfig.maxThrottle)
motor[i] -= maxMotor - eepromConfig.maxThrottle;
motor[i] = constrain(motor[i], eepromConfig.minThrottle, eepromConfig.maxThrottle);
if ((rxCommand[THROTTLE] < eepromConfig.minCheck) && (verticalModeState == ALT_DISENGAGED_THROTTLE_ACTIVE))
{
motor[i] = eepromConfig.minThrottle;
}
if ( armed == false )
motor[i] = (float)MINCOMMAND;
}
}
