void writeServos(void) { if (!useServo) return; switch (cfg.mixerConfiguration) { case MULTITYPE_BI: pwmWriteServo(0, servo[4]); pwmWriteServo(1, servo[5]); break; case MULTITYPE_TRI: pwmWriteServo(0, servo[5]); break; case MULTITYPE_AIRPLANE: break; case MULTITYPE_FLYING_WING: break; default: // Two servos for SERVO_TILT, if enabled if (feature(FEATURE_SERVO_TILT)) { pwmWriteServo(0, servo[0]); pwmWriteServo(1, servo[1]); } break; } }
void writeServos(void) { uint8_t servoIndex = 0; bool zeroServoValue = false; /* * in case of tricopters, there might me a need to zero servo output when unarmed */ if ((currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI) && !ARMING_FLAG(ARMED) && !mixerConfig->tri_unarmed_servo) { zeroServoValue = true; } if (mixerUsesServos) { for (int i = minServoIndex; i <= maxServoIndex; i++) { if (zeroServoValue) { pwmWriteServo(servoIndex++, 0); } else { pwmWriteServo(servoIndex++, servo[i]); } } } if (feature(FEATURE_SERVO_TILT)) { pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_PITCH]); pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_ROLL]); } // forward AUX to remaining servo outputs (not constrained) if (feature(FEATURE_CHANNEL_FORWARDING)) { forwardAuxChannelsToServos(servoIndex); servoIndex += MAX_AUX_CHANNEL_COUNT; } }
STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex) { // start forwarding from this channel uint8_t channelOffset = AUX1; uint8_t servoOffset; for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]); } }
void writeServos(void) { uint8_t servoIndex = 0; switch (currentMixerMode) { case MIXER_BICOPTER: pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]); pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]); break; case MIXER_TRI: case MIXER_CUSTOM_TRI: if (mixerConfig->tri_unarmed_servo) { // if unarmed flag set, we always move servo pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); } else { // otherwise, only move servo when copter is armed if (ARMING_FLAG(ARMED)) pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); else pwmWriteServo(servoIndex++, 0); // kill servo signal completely. } break; case MIXER_FLYING_WING: pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]); pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]); break; case MIXER_DUALCOPTER: pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]); pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]); break; case MIXER_CUSTOM_AIRPLANE: case MIXER_AIRPLANE: for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) { pwmWriteServo(servoIndex++, servo[i]); } break; case MIXER_SINGLECOPTER: for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) { pwmWriteServo(servoIndex++, servo[i]); } break; default: break; } // Two servos for SERVO_TILT, if enabled if (feature(FEATURE_SERVO_TILT) || currentMixerMode == MIXER_GIMBAL) { updateGimbalServos(servoIndex); servoIndex += 2; } // forward AUX to remaining servo outputs (not constrained) if (feature(FEATURE_CHANNEL_FORWARDING)) { forwardAuxChannelsToServos(servoIndex); servoIndex += MAX_AUX_CHANNEL_COUNT; } }
static void updateGimbalServos(uint8_t firstServoIndex) { pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]); pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]); }
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 writeServos(void) { static uint32_t yawarmdelaytimer = 0; if (!useServo) return; switch (cfg.mixerConfiguration) { case MULTITYPE_BI: pwmWriteServo(0, servo[4]); pwmWriteServo(1, servo[5]); break; case MULTITYPE_TRI: if (!cfg.tri_ydel) { pwmWriteServo(0, servo[5]); // like always } else { if (f.ARMED) { if (!yawarmdelaytimer) yawarmdelaytimer = currentTimeMS + (uint32_t)cfg.tri_ydel; if (currentTimeMS >= yawarmdelaytimer) pwmWriteServo(0, servo[5]); // like always else pwmWriteServo(0, cfg.tri_ymid); // Give middlesignal to yaw servo when disarmed } else { yawarmdelaytimer = 0; pwmWriteServo(0, cfg.tri_ymid); // Give middlesignal to yaw servo when disarmed } } break; case MULTITYPE_AIRPLANE: break; case MULTITYPE_FLYING_WING: pwmWriteServo(0, servo[3]); pwmWriteServo(1, servo[4]); break; case MULTITYPE_GIMBAL: pwmWriteServo(0, servo[0]); pwmWriteServo(1, servo[1]); break; default: // Two servos for SERVO_TILT, if enabled if (feature(FEATURE_SERVO_TILT)) { pwmWriteServo(0, servo[0]); pwmWriteServo(1, servo[1]); } break; } }
void writeServos(void) { int i; if (!core.useServo) return; // forward AUX1-4 to servo outputs (not constrained) if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) { int offset = core.numServos - 4; // offset servos based off number already used in mixer types // airplane and servo_tilt together can't be used // calculate offset by taking 4 from core.numServos for (i = 0; i < 4; i++) pwmWriteServo(i + offset, rcData[AUX1 + i]); } // apply servos for the specific mixerConfiguration switch (mcfg.mixerConfiguration) { case MULTITYPE_BI: pwmWriteServo(0, servo[4]); pwmWriteServo(1, servo[5]); break; case MULTITYPE_TRI: if (cfg.tri_unarmed_servo) { // if unarmed flag set, we always move servo pwmWriteServo(0, servo[5]); } else { // otherwise, only move servo when copter is armed if (f.ARMED) pwmWriteServo(0, servo[5]); else pwmWriteServo(0, 0); // kill servo signal completely. } break; case MULTITYPE_GIMBAL: pwmWriteServo(0, servo[0]); pwmWriteServo(1, servo[1]); break; case MULTITYPE_DUALCOPTER: pwmWriteServo(0, servo[4]); pwmWriteServo(1, servo[5]); break; case MULTITYPE_FLYING_WING: pwmWriteServo(0, servo[3]); pwmWriteServo(1, servo[4]); break; case MULTITYPE_AIRPLANE: case MULTITYPE_SINGLECOPTER: pwmWriteServo(0, servo[3]); pwmWriteServo(1, servo[4]); pwmWriteServo(2, servo[5]); pwmWriteServo(3, servo[6]); break; case MULTITYPE_CUSTOM_PLANE: pwmWriteServo(0, servo[3]); pwmWriteServo(1, servo[4]); pwmWriteServo(2, servo[5]); pwmWriteServo(3, servo[6]); if (feature(FEATURE_PPM)) { pwmWriteServo(4, servo[0]); pwmWriteServo(5, servo[1]); pwmWriteServo(6, servo[2]); pwmWriteServo(7, servo[7]); } break; default: // otherwise, control the first two servos when SERVO_TILT or SERVO_MIXER is enabled if (feature(FEATURE_SERVO_TILT) || feature(FEATURE_SERVO_MIXER)) { pwmWriteServo(0, servo[0]); pwmWriteServo(1, servo[1]); } break; } }
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; 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 if (numberMotor > 1) for (i = 0; i < numberMotor; i++) motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw; if (f.FIXED_WING) { if (!f.ARMED) motor[0] = mcfg.mincommand; // Kill throttle when disarmed else motor[0] = constrain(rcCommand[THROTTLE], mcfg.minthrottle, mcfg.maxthrottle); } // airplane / servo mixes switch (mcfg.mixerConfiguration) { case MULTITYPE_CUSTOM_PLANE: case MULTITYPE_FLYING_WING: case MULTITYPE_AIRPLANE: case MULTITYPE_BI: case MULTITYPE_TRI: case MULTITYPE_DUALCOPTER: case MULTITYPE_SINGLECOPTER: servoMixer(); break; case MULTITYPE_GIMBAL: servo[0] = (((int32_t)cfg.servoConf[0].rate * angle[PITCH]) / 50) + servoMiddle(0); servo[1] = (((int32_t)cfg.servoConf[1].rate * angle[ROLL]) / 50) + servoMiddle(1); break; } // do camstab if (feature(FEATURE_SERVO_TILT)) { // center at fixed position, or vary either pitch or roll by RC channel servo[0] = servoMiddle(0); servo[1] = servoMiddle(1); if (rcOptions[BOXCAMSTAB]) { if (cfg.gimbal_flags & GIMBAL_MIXTILT) { servo[0] -= (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 - (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; servo[1] += (-(int32_t)cfg.servoConf[0].rate) * angle[PITCH] / 50 + (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; } else { servo[0] += (int32_t)cfg.servoConf[0].rate * angle[PITCH] / 50; servo[1] += (int32_t)cfg.servoConf[1].rate * angle[ROLL] / 50; } } } // constrain servos for (i = 0; i < MAX_SERVOS; i++) servo[i] = constrain(servo[i], cfg.servoConf[i].min, cfg.servoConf[i].max); // limit the values // forward AUX1-4 to servo outputs (not constrained) if (cfg.gimbal_flags & GIMBAL_FORWARDAUX) { int offset = core.numServos - 4; // offset servos based off number already used in mixer types // airplane and servo_tilt together can't be used // calculate offset by taking 4 from core.numServos 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 > mcfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max. motor[i] -= maxMotor - mcfg.maxthrottle; if (feature(FEATURE_3D)) { if ((rcData[THROTTLE]) > mcfg.midrc) { motor[i] = constrain(motor[i], mcfg.deadband3d_high, mcfg.maxthrottle); if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) { servo[5] = constrain(servo[5], cfg.servoConf[5].min, cfg.servoConf[5].max); } } else { motor[i] = constrain(motor[i], mcfg.mincommand, mcfg.deadband3d_low); if ((mcfg.mixerConfiguration) == MULTITYPE_TRI) { servo[5] = constrain(servo[5], cfg.servoConf[5].max, cfg.servoConf[5].min); } } } else { motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); if ((rcData[THROTTLE]) < mcfg.mincheck) { if (!feature(FEATURE_MOTOR_STOP)) motor[i] = mcfg.minthrottle; else { motor[i] = mcfg.mincommand; f.MOTORS_STOPPED = 1; } } else { f.MOTORS_STOPPED = 0; } } if (!f.ARMED) { motor[i] = motor_disarmed[i]; } } }
void writeServos(void) { if (!core.useServo) return; switch (mcfg.mixerConfiguration) { case MULTITYPE_BI: pwmWriteServo(0, servo[4]); pwmWriteServo(1, servo[5]); break; case MULTITYPE_TRI: if (cfg.tri_unarmed_servo) { // if unarmed flag set, we always move servo pwmWriteServo(0, servo[5]); } else { // otherwise, only move servo when copter is armed if (f.ARMED) pwmWriteServo(0, servo[5]); else pwmWriteServo(0, 0); // kill servo signal completely. } break; case MULTITYPE_GIMBAL: pwmWriteServo(0, servo[0]); pwmWriteServo(1, servo[1]); break; case MULTITYPE_DUALCOPTER: pwmWriteServo(0, servo[4]); pwmWriteServo(1, servo[5]); break; case MULTITYPE_FLYING_WING: pwmWriteServo(0, servo[3]); pwmWriteServo(1, servo[4]); break; case MULTITYPE_AIRPLANE: case MULTITYPE_SINGLECOPTER: pwmWriteServo(0, servo[3]); pwmWriteServo(1, servo[4]); pwmWriteServo(2, servo[5]); pwmWriteServo(3, servo[6]); break; case MULTITYPE_CUSTOM_PLANE: pwmWriteServo(0, servo[3]); pwmWriteServo(1, servo[4]); pwmWriteServo(2, servo[5]); pwmWriteServo(3, servo[6]); if (feature(FEATURE_PPM)) { pwmWriteServo(4, servo[0]); pwmWriteServo(5, servo[1]); pwmWriteServo(6, servo[2]); pwmWriteServo(7, servo[7]); } break; default: // Two servos for SERVO_TILT, if enabled if (feature(FEATURE_SERVO_TILT)) { pwmWriteServo(0, servo[0]); pwmWriteServo(1, servo[1]); } break; } }
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])); } // motors for non-servo mixes if (numberMotor > 1) for (i = 0; i < numberMotor; i++) motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + cfg.yaw_direction * axisPID[YAW] * currentMixer[i].yaw; // airplane / servo mixes switch (mcfg.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 * 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 servo[0] = cfg.pitch_direction_l * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_l * (rcData[ROLL] - mcfg.midrc); servo[1] = cfg.pitch_direction_r * (rcData[PITCH] - mcfg.midrc) + cfg.roll_direction_r * (rcData[ROLL] - mcfg.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 (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] - mcfg.midrc; if (!(cfg.gimbal_flags & GIMBAL_DISABLEAUX34)) aux[1] = rcData[AUX4] - mcfg.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_pitch_gain) * angle[PITCH] / 16 - cfg.gimbal_roll_gain * angle[ROLL] / 16; servo[1] += (-cfg.gimbal_pitch_gain) * angle[PITCH] / 16 + cfg.gimbal_roll_gain * angle[ROLL] / 16; } else { 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 > mcfg.maxthrottle) // this is a way to still have good gyro corrections if at least one motor reaches its max. motor[i] -= maxMotor - mcfg.maxthrottle; motor[i] = constrain(motor[i], mcfg.minthrottle, mcfg.maxthrottle); if ((rcData[THROTTLE]) < mcfg.mincheck) { if (!feature(FEATURE_MOTOR_STOP)) motor[i] = mcfg.minthrottle; else motor[i] = mcfg.mincommand; } if (!f.ARMED) motor[i] = mcfg.mincommand; } }