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; } }