void servoTable(void) { // airplane / servo mixes switch (currentMixerMode) { case MIXER_CUSTOM_AIRPLANE: case MIXER_FLYING_WING: case MIXER_AIRPLANE: case MIXER_BICOPTER: case MIXER_CUSTOM_TRI: case MIXER_TRI: case MIXER_DUALCOPTER: case MIXER_SINGLECOPTER: case MIXER_GIMBAL: servoMixer(); break; /* case MIXER_GIMBAL: servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); break; */ default: break; } // camera stabilization if (feature(FEATURE_SERVO_TILT)) { // center at fixed position, or vary either pitch or roll by RC channel servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } } } // constrain servos for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values } }
void processServoTilt(void) { // center at fixed position, or vary either pitch or roll by RC channel servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } } }
void mixTable(void) { uint32_t i; if (motorCount >= 4 && mixerConfig->yaw_jump_prevention_limit < YAW_JUMP_PREVENTION_LIMIT_HIGH) { // prevent "yaw jump" during yaw correction axisPID[YAW] = constrain(axisPID[YAW], -mixerConfig->yaw_jump_prevention_limit - ABS(rcCommand[YAW]), mixerConfig->yaw_jump_prevention_limit + ABS(rcCommand[YAW])); } // motors for non-servo mixes for (i = 0; i < motorCount; i++) { motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; } if (ARMING_FLAG(ARMED)) { bool isFailsafeActive = failsafeIsActive(); // Find the maximum motor output. int16_t maxMotor = motor[0]; for (i = 1; i < motorCount; i++) { // If one motor is above the maxthrottle threshold, we reduce the value // of all motors by the amount of overshoot. That way, only one motor // is at max and the relative power of each motor is preserved. if (motor[i] > maxMotor) { maxMotor = motor[i]; } } int16_t maxThrottleDifference = 0; if (maxMotor > escAndServoConfig->maxthrottle) { maxThrottleDifference = maxMotor - escAndServoConfig->maxthrottle; } for (i = 0; i < motorCount; i++) { // this is a way to still have good gyro corrections if at least one motor reaches its max. motor[i] -= maxThrottleDifference; if (feature(FEATURE_3D)) { if (mixerConfig->pid_at_min_throttle || rcData[THROTTLE] <= rxConfig->midrc - flight3DConfig->deadband3d_throttle || rcData[THROTTLE] >= rxConfig->midrc + flight3DConfig->deadband3d_throttle) { if (rcData[THROTTLE] > rxConfig->midrc) { motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); } else { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, flight3DConfig->deadband3d_low); } } else { if (rcData[THROTTLE] > rxConfig->midrc) { motor[i] = flight3DConfig->deadband3d_high; } else { motor[i] = flight3DConfig->deadband3d_low; } } } else { if (isFailsafeActive) { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); } else { // If we're at minimum throttle and FEATURE_MOTOR_STOP enabled, // do not spin the motors. motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); if ((rcData[THROTTLE]) < rxConfig->mincheck) { if (feature(FEATURE_MOTOR_STOP)) { motor[i] = escAndServoConfig->mincommand; } else if (mixerConfig->pid_at_min_throttle == 0) { motor[i] = escAndServoConfig->minthrottle; } } } } } } else { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } // motor outputs are used as sources for servo mixing, so motors must be calculated before servos. #if !defined(USE_QUAD_MIXER_ONLY) && defined(USE_SERVOS) // airplane / servo mixes switch (currentMixerMode) { case MIXER_CUSTOM_AIRPLANE: case MIXER_FLYING_WING: case MIXER_AIRPLANE: case MIXER_BICOPTER: case MIXER_CUSTOM_TRI: case MIXER_TRI: case MIXER_DUALCOPTER: case MIXER_SINGLECOPTER: case MIXER_GIMBAL: servoMixer(); break; /* case MIXER_GIMBAL: servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); break; */ default: break; } // camera stabilization if (feature(FEATURE_SERVO_TILT)) { // center at fixed position, or vary either pitch or roll by RC channel servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } } } // constrain servos for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values } #endif }
STATIC_UNIT_TESTED void servoMixer(void) { int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500] static int16_t currentOutput[MAX_SERVO_RULES]; uint8_t i; if (FLIGHT_MODE(PASSTHRU_MODE)) { // Direct passthru from RX input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH]; input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; } else { // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui input[INPUT_STABILIZED_ROLL] = axisPID[ROLL]; input[INPUT_STABILIZED_PITCH] = axisPID[PITCH]; input[INPUT_STABILIZED_YAW] = axisPID[YAW]; // Reverse yaw servo when inverted in 3D mode if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { input[INPUT_STABILIZED_YAW] *= -1; } } input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500); input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] // center the RC input value around the RC middle value // by subtracting the RC middle value from the RC input value, we get: // data - middle = input // 2000 - 1500 = +500 // 1500 - 1500 = 0 // 1000 - 1500 = -500 input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc; input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc; input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc; input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc; input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc; input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc; input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc; input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc; for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) servo[i] = 0; // mix servos according to rules for (i = 0; i < servoRuleCount; i++) { // consider rule if no box assigned or box is active if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) { uint8_t target = currentServoMixer[i].targetChannel; uint8_t from = currentServoMixer[i].inputSource; uint16_t servo_width = servoConf[target].max - servoConf[target].min; int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; if (currentServoMixer[i].speed == 0) currentOutput[i] = input[from]; else { if (currentOutput[i] < input[from]) currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]); else if (currentOutput[i] > input[from]) currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]); } servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max); } else { currentOutput[i] = 0; } } for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; servo[i] += determineServoMiddleOrForwardFromChannel(i); } }
void mixTable(void *pidProfilePtr) { uint32_t i = 0; fix12_t vbatCompensationFactor = 0; static fix12_t mixReduction; bool use_vbat_compensation = false; pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr; if (batteryConfig && pidProfile->vbatPidCompensation) { use_vbat_compensation = true; vbatCompensationFactor = calculateVbatPidCompensation(); } bool isFailsafeActive = failsafeIsActive(); // TODO - Find out if failsafe checks are really needed here in mixer code // Initial mixer concept by bdoiron74 reused and optimized for Air Mode int16_t rollPitchYawMix[MAX_SUPPORTED_MOTORS]; int16_t rollPitchYawMixMax = 0; // assumption: symetrical about zero. int16_t rollPitchYawMixMin = 0; if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation // Find roll/pitch/yaw desired output for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_motor_direction * axisPID[YAW] * currentMixer[i].yaw; if (use_vbat_compensation) rollPitchYawMix[i] = qMultiply(vbatCompensationFactor, rollPitchYawMix[i]); // Add voltage compensation if (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i]; if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i]; if (debugMode == DEBUG_MIXER && i < 5) debug[i] = rollPitchYawMix[i]; } // Scale roll/pitch/yaw uniformly to fit within throttle range int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin; int16_t throttleRange, throttle; int16_t throttleMin, throttleMax; static int16_t throttlePrevious = 0; // Store the last throttle direction for deadband transitions // Find min and max throttle based on condition. if (feature(FEATURE_3D)) { if (!ARMING_FLAG(ARMED)) throttlePrevious = rxConfig->midrc; // When disarmed set to mid_rc. It always results in positive direction after arming. if ((rcCommand[THROTTLE] <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Out of band handling throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; throttlePrevious = throttle = rcCommand[THROTTLE]; } else if (rcCommand[THROTTLE] >= (rxConfig->midrc + flight3DConfig->deadband3d_throttle)) { // Positive handling throttleMax = escAndServoConfig->maxthrottle; throttleMin = flight3DConfig->deadband3d_high; throttlePrevious = throttle = rcCommand[THROTTLE]; } else if ((throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle))) { // Deadband handling from negative to positive throttle = throttleMax = flight3DConfig->deadband3d_low; throttleMin = escAndServoConfig->minthrottle; } else { // Deadband handling from positive to negative throttleMax = escAndServoConfig->maxthrottle; throttle = throttleMin = flight3DConfig->deadband3d_high; } } else { throttle = rcCommand[THROTTLE]; throttleMin = escAndServoConfig->minthrottle; throttleMax = escAndServoConfig->maxthrottle; } throttleRange = throttleMax - throttleMin; if (rollPitchYawMixRange > throttleRange) { mixReduction = qConstruct(throttleRange, rollPitchYawMixRange); for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = qMultiply(mixReduction,rollPitchYawMix[i]); } // Get the maximum correction by setting offset to center throttleMin = throttleMax = throttleMin + (throttleRange / 2); } else { throttleMin = throttleMin + (rollPitchYawMixRange / 2); throttleMax = throttleMax - (rollPitchYawMixRange / 2); } // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. for (i = 0; i < motorCount; i++) { motor[i] = rollPitchYawMix[i] + constrain(throttle * currentMixer[i].throttle, throttleMin, throttleMax); if (isFailsafeActive) { motor[i] = constrain(motor[i], escAndServoConfig->mincommand, escAndServoConfig->maxthrottle); } else if (feature(FEATURE_3D)) { if (throttlePrevious <= (rxConfig->midrc - flight3DConfig->deadband3d_throttle)) { motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, flight3DConfig->deadband3d_low); } else { motor[i] = constrain(motor[i], flight3DConfig->deadband3d_high, escAndServoConfig->maxthrottle); } } else { motor[i] = constrain(motor[i], escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle); } // Motor stop handling if (feature(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) && !feature(FEATURE_3D) && !isAirmodeActive()) { if (((rcData[THROTTLE]) < rxConfig->mincheck)) { motor[i] = escAndServoConfig->mincommand; } } } // Disarmed mode if (!ARMING_FLAG(ARMED)) { for (i = 0; i < motorCount; i++) { motor[i] = motor_disarmed[i]; } } // motor outputs are used as sources for servo mixing, so motors must be calculated before servos. #if !defined(USE_QUAD_MIXER_ONLY) || defined(USE_SERVOS) // airplane / servo mixes switch (currentMixerMode) { case MIXER_CUSTOM_AIRPLANE: case MIXER_FLYING_WING: case MIXER_AIRPLANE: case MIXER_BICOPTER: case MIXER_CUSTOM_TRI: case MIXER_TRI: case MIXER_DUALCOPTER: case MIXER_SINGLECOPTER: case MIXER_GIMBAL: servoMixer(); break; /* case MIXER_GIMBAL: servo[SERVO_GIMBAL_PITCH] = (((int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = (((int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll) / 50) + determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); break; */ default: break; } // camera stabilization if (feature(FEATURE_SERVO_TILT)) { // center at fixed position, or vary either pitch or roll by RC channel servo[SERVO_GIMBAL_PITCH] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_PITCH); servo[SERVO_GIMBAL_ROLL] = determineServoMiddleOrForwardFromChannel(SERVO_GIMBAL_ROLL); if (IS_RC_MODE_ACTIVE(BOXCAMSTAB)) { if (gimbalConfig->mode == GIMBAL_MODE_MIXTILT) { servo[SERVO_GIMBAL_PITCH] -= (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 - (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; servo[SERVO_GIMBAL_ROLL] += (-(int32_t)servoConf[SERVO_GIMBAL_PITCH].rate) * attitude.values.pitch / 50 + (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } else { servo[SERVO_GIMBAL_PITCH] += (int32_t)servoConf[SERVO_GIMBAL_PITCH].rate * attitude.values.pitch / 50; servo[SERVO_GIMBAL_ROLL] += (int32_t)servoConf[SERVO_GIMBAL_ROLL].rate * attitude.values.roll / 50; } } } // constrain servos for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values } #endif }