void mixTable(void)
{
int16_t maxMotor;
uint32_t i;
// prevent "yaw jump" during yaw correction
if (numberMotor > 3) axisPID[YAW] = constrain(axisPID[YAW], -100.0f - (float)abs(rcCommand[YAW]), +100.0f + (float)abs(rcCommand[YAW]));
// motors for non-servo mixes
if (numberMotor > 1)
for (i = 0; i < numberMotor; i++)
motor[i] = (float)rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + cfg.tri_ydir * axisPID[YAW] * currentMixer[i].yaw;
// airplane / servo mixes
switch (cfg.mixerConfiguration)
{
case MULTITYPE_BI:
servo[4] = constrain(1500 + (cfg.tri_ydir * axisPID[YAW]) + axisPID[PITCH], 1020, 2000); //LEFT
servo[5] = constrain(1500 + (cfg.tri_ydir * axisPID[YAW]) - axisPID[PITCH], 1020, 2000); //RIGHT
break;
case MULTITYPE_TRI:
servo[5] = constrain(cfg.tri_ymid + cfg.tri_ydir * axisPID[YAW], cfg.tri_ymin, cfg.tri_ymax); //REAR
break;
case MULTITYPE_GIMBAL:
servo[0] = constrain(cfg.gbl_pmd + (float)cfg.gbl_pgn * angle[PITCH] * 0.0625f + rcCommand[PITCH], cfg.gbl_pmn, cfg.gbl_pmx);
servo[1] = constrain(cfg.gbl_rmd + (float)cfg.gbl_rgn * angle[ROLL] * 0.0625f + rcCommand[ROLL] , cfg.gbl_rmn, cfg.gbl_rmx);
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
servo[0] = cfg.pitch_direction_l * (rcData[PITCH] - cfg.rc_mid) + cfg.roll_direction_l * (rcData[ROLL] - cfg.rc_mid);
servo[1] = cfg.pitch_direction_r * (rcData[PITCH] - cfg.rc_mid) + cfg.roll_direction_r * (rcData[ROLL] - cfg.rc_mid);
}
else
{
// use sensors to correct (gyro only or gyro + acc)
servo[0] = cfg.pitch_direction_l * axisPID[PITCH] + cfg.roll_direction_l * axisPID[ROLL];
servo[1] = cfg.pitch_direction_r * axisPID[PITCH] + cfg.roll_direction_r * axisPID[ROLL];
}
servo[0] = constrain(servo[0] + cfg.wing_left_mid, cfg.wing_left_min, cfg.wing_left_max);
servo[1] = constrain(servo[1] + cfg.wing_right_mid, cfg.wing_right_min, cfg.wing_right_max);
break;
}
// do camstab
if (feature(FEATURE_SERVO_TILT))
{
uint16_t aux[2] = { 0, 0 };
if ((cfg.gbl_flg & GIMBAL_NORMAL) || (cfg.gbl_flg & GIMBAL_TILTONLY)) aux[0] = rcData[AUX3] - cfg.rc_mid;
if (!(cfg.gbl_flg & GIMBAL_DISABLEAUX34)) aux[1] = rcData[AUX4] - cfg.rc_mid;
servo[0] = cfg.gbl_pmd + aux[0];
servo[1] = cfg.gbl_rmd + aux[1];
if (rcOptions[BOXCAMSTAB])
{
if (cfg.gbl_flg & GIMBAL_MIXTILT)
{
servo[0] -= ((float)(-cfg.gbl_pgn) * angle[PITCH] * 0.0625f) - (float)cfg.gbl_rgn * angle[ROLL] * 0.0625f;
servo[1] += ((float)(-cfg.gbl_pgn) * angle[PITCH] * 0.0625f) + (float)cfg.gbl_rgn * angle[ROLL] * 0.0625f;
}
else
{
servo[0] += (float)cfg.gbl_pgn * angle[PITCH] * 0.0625f;
servo[1] += (float)cfg.gbl_rgn * angle[ROLL] * 0.0625f;
}
}
servo[0] = constrain(servo[0], cfg.gbl_pmn, cfg.gbl_pmx);
servo[1] = constrain(servo[1], cfg.gbl_rmn, cfg.gbl_rmx);
}
if (cfg.gbl_flg & GIMBAL_FORWARDAUX)
{
int offset = 0;
if (feature(FEATURE_SERVO_TILT))
offset = 2;
for (i = 0; i < 4; i++)
pwmWriteServo(i + offset, rcData[AUX1 + i]);
}
if (feature(FEATURE_LED) && (cfg.LED_Type == 1))
{
if (feature(FEATURE_SERVO_TILT))
pwmWriteServo(2, LED_Value);
else
pwmWriteServo(0, LED_Value);
}
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.esc_max) motor[i] -= maxMotor - cfg.esc_max; // this is a way to still have good gyro corrections if at least one motor reaches its max.
motor[i] = constrain(motor[i], cfg.esc_min, cfg.esc_max);
if ((rcData[THROTTLE]) < cfg.rc_min)
{
if (!feature(FEATURE_MOTOR_STOP)) motor[i] = cfg.esc_min;
else motor[i] = cfg.esc_moff;
}
if (!f.ARMED) motor[i] = cfg.esc_moff;
}
}
void mixerTable(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]));
}
// motors for non-servo mixes
for (i = 0; i < numberMotor; i++)
motor[i] = currentMixer[i].throttle * rcCommand[THROTTLE] +
currentMixer[i].pitch * axisPID[PITCH] +
currentMixer[i].roll * axisPID[ROLL ] +
currentMixer[i].yaw * axisPID[YAW ] * cfg.yaw_direction;
// airplane / servo mixes
switch (cfg.mixerConfiguration)
{
case MULTITYPE_BI:
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:
servo[5] = constrain(cfg.tri_yaw_middle + cfg.yaw_direction * axisPID[YAW], cfg.tri_yaw_min, cfg.tri_yaw_max); //REAR
break;
case MULTITYPE_GIMBAL:
servo[0] = constrain(cfg.gimbal_pitch_mid + cfg.gimbal_pitch_gain * imu.rpy[PITCH] / 16 + rcCommand[PITCH], cfg.gimbal_pitch_min, cfg.gimbal_pitch_max);
servo[1] = constrain(cfg.gimbal_roll_mid + cfg.gimbal_roll_gain * imu.rpy[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 (flag(FLAG_PASSTHRU_MODE))
{
// do not use sensors for correction, simple 2 channel mixing
servo[0] = cfg.pitch_direction_l * (rcData[PITCH] - cfg.midrc) + cfg.roll_direction_l * (rcData[ROLL] - cfg.midrc);
servo[1] = cfg.pitch_direction_r * (rcData[PITCH] - cfg.midrc) + cfg.roll_direction_r * (rcData[ROLL] - cfg.midrc);
}
else
{
// use sensors to correct (gyro only or gyro + acc)
servo[0] = cfg.pitch_direction_l * axisPID[PITCH] + cfg.roll_direction_l * axisPID[ROLL];
servo[1] = cfg.pitch_direction_r * axisPID[PITCH] + cfg.roll_direction_r * axisPID[ROLL];
}
servo[0] = constrain(servo[0] + cfg.wing_left_mid, cfg.wing_left_min, cfg.wing_left_max);
servo[1] = constrain(servo[1] + cfg.wing_right_mid, cfg.wing_right_min, cfg.wing_right_max);
break;
}
// do camstab
if (cfg.gimbal)
{
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])
{
if (cfg.gimbal_flags & GIMBAL_MIXTILT)
{
servo[0] = (-cfg.gimbal_roll_gain) * imu.rpy[PITCH] / 16 - cfg.gimbal_roll_gain * imu.rpy[ROLL] / 16;
servo[1] = (-cfg.gimbal_roll_gain) * imu.rpy[PITCH] / 16 - cfg.gimbal_roll_gain * imu.rpy[ROLL] / 16;
}
else
{
servo[0] += cfg.gimbal_pitch_gain * imu.rpy[PITCH] / 16;
servo[1] += cfg.gimbal_roll_gain * imu.rpy[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 (cfg.gimbal)
offset = 2;
for (i = 0; i < 4; i++)
pwmWrite(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 (!cfg.motor_stop)
motor[i] = cfg.minthrottle;
else
motor[i] = cfg.mincommand;
}
if (!flag(FLAG_ARMED))
motor[i] = cfg.mincommand;
}
}
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;
}
}
