/* * This file is part of Cleanflight. * * Cleanflight is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * Cleanflight is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with Cleanflight. If not, see . */ #include #include #include #include #include "platform.h" #include "debug.h" #include "build_config.h" #include "common/axis.h" #include "common/maths.h" #include "drivers/system.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" #include "drivers/system.h" #include "rx/rx.h" #include "io/gimbal.h" #include "io/escservo.h" #include "io/rc_controls.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" #include "flight/mixer.h" #include "flight/failsafe.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/lowpass.h" #include "config/runtime_config.h" #include "config/config.h" //#define MIXER_DEBUG uint8_t motorCount; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; static mixerConfig_t *mixerConfig; static flight3DConfig_t *flight3DConfig; static escAndServoConfig_t *escAndServoConfig; static airplaneConfig_t *airplaneConfig; static rxConfig_t *rxConfig; static mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; bool motorLimitReached = false; #ifdef USE_SERVOS static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; static gimbalConfig_t *gimbalConfig; int16_t servo[MAX_SUPPORTED_SERVOS]; static int useServo; STATIC_UNIT_TESTED uint8_t servoCount; static servoParam_t *servoConf; static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS]; #endif static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L }; #ifndef USE_QUAD_MIXER_ONLY static const motorMixer_t mixerTricopter[] = { { 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR { 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT { 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT }; static const motorMixer_t mixerQuadP[] = { { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR { 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT }; static const motorMixer_t mixerBicopter[] = { { 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT { 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT }; static const motorMixer_t mixerY6[] = { { 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR { 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT { 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT { 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR { 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT { 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT }; static const motorMixer_t mixerHex6P[] = { { 1.0f, -0.866025f, 0.5f, 1.0f }, // REAR_R { 1.0f, -0.866025f, -0.5f, -1.0f }, // FRONT_R { 1.0f, 0.866025f, 0.5f, 1.0f }, // REAR_L { 1.0f, 0.866025f, -0.5f, -1.0f }, // FRONT_L { 1.0f, 0.0f, -1.0f, 1.0f }, // FRONT { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR }; static const motorMixer_t mixerY4[] = { { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW { 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW }; static const motorMixer_t mixerHex6X[] = { { 1.0f, -0.5f, 0.866025f, 1.0f }, // REAR_R { 1.0f, -0.5f, -0.866025f, 1.0f }, // FRONT_R { 1.0f, 0.5f, 0.866025f, -1.0f }, // REAR_L { 1.0f, 0.5f, -0.866025f, -1.0f }, // FRONT_L { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT }; static const motorMixer_t mixerOctoX8[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L { 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R { 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R { 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L { 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L }; static const motorMixer_t mixerOctoFlatP[] = { { 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L { 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R { 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R { 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR { 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT }; static const motorMixer_t mixerOctoFlatX[] = { { 1.0f, 1.0f, -0.414178f, 1.0f }, // MIDFRONT_L { 1.0f, -0.414178f, -1.0f, 1.0f }, // FRONT_R { 1.0f, -1.0f, 0.414178f, 1.0f }, // MIDREAR_R { 1.0f, 0.414178f, 1.0f, 1.0f }, // REAR_L { 1.0f, 0.414178f, -1.0f, -1.0f }, // FRONT_L { 1.0f, -1.0f, -0.414178f, -1.0f }, // MIDFRONT_R { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R { 1.0f, 1.0f, 0.414178f, -1.0f }, // MIDREAR_L }; static const motorMixer_t mixerVtail4[] = { { 1.0f, -0.58f, 0.58f, 1.0f }, // REAR_R { 1.0f, -0.46f, -0.39f, -0.5f }, // FRONT_R { 1.0f, 0.58f, 0.58f, -1.0f }, // REAR_L { 1.0f, 0.46f, -0.39f, 0.5f }, // FRONT_L }; static const motorMixer_t mixerAtail4[] = { { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L { 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L }; static const motorMixer_t mixerHex6H[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L { 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT { 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT }; static const motorMixer_t mixerDualcopter[] = { { 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT { 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT }; static const motorMixer_t mixerSingleProp[] = { { 1.0f, 0.0f, 0.0f, 0.0f }, }; static const motorMixer_t mixerQuadX1234[] = { { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L }; // Keep synced with mixerMode_e const mixer_t mixers[] = { // motors, use servo, motor mixer { 0, false, NULL }, // entry 0 { 3, true, mixerTricopter }, // MIXER_TRI { 4, false, mixerQuadP }, // MIXER_QUADP { 4, false, mixerQuadX }, // MIXER_QUADX { 2, true, mixerBicopter }, // MIXER_BICOPTER { 0, true, NULL }, // * MIXER_GIMBAL { 6, false, mixerY6 }, // MIXER_Y6 { 6, false, mixerHex6P }, // MIXER_HEX6 { 1, true, mixerSingleProp }, // * MIXER_FLYING_WING { 4, false, mixerY4 }, // MIXER_Y4 { 6, false, mixerHex6X }, // MIXER_HEX6X { 8, false, mixerOctoX8 }, // MIXER_OCTOX8 { 8, false, mixerOctoFlatP }, // MIXER_OCTOFLATP { 8, false, mixerOctoFlatX }, // MIXER_OCTOFLATX { 1, true, mixerSingleProp }, // * MIXER_AIRPLANE { 0, true, NULL }, // * MIXER_HELI_120_CCPM { 0, true, NULL }, // * MIXER_HELI_90_DEG { 4, false, mixerVtail4 }, // MIXER_VTAIL4 { 6, false, mixerHex6H }, // MIXER_HEX6H { 0, true, NULL }, // * MIXER_PPM_TO_SERVO { 2, true, mixerDualcopter }, // MIXER_DUALCOPTER { 1, true, NULL }, // MIXER_SINGLECOPTER { 4, false, mixerAtail4 }, // MIXER_ATAIL4 { 0, false, NULL }, // MIXER_CUSTOM { 2, true, NULL }, // MIXER_CUSTOM_AIRPLANE { 3, true, NULL }, // MIXER_CUSTOM_TRI { 4, false, mixerQuadX1234 }, }; #endif #ifdef USE_SERVOS #define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t)) // mixer rule format servo, input, rate, speed, min, max, box static const servoMixer_t servoMixerAirplane[] = { { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, { SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 }, }; static const servoMixer_t servoMixerFlyingWing[] = { { SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, { SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, { SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -100, 0, 0, 100, 0 }, { SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, { SERVO_THROTTLE, INPUT_STABILIZED_THROTTLE, 100, 0, 0, 100, 0 }, }; static const servoMixer_t servoMixerBI[] = { { SERVO_BICOPTER_LEFT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, { SERVO_BICOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, { SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, { SERVO_BICOPTER_RIGHT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, }; static const servoMixer_t servoMixerTri[] = { { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, }; static const servoMixer_t servoMixerDual[] = { { SERVO_DUALCOPTER_LEFT, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, { SERVO_DUALCOPTER_RIGHT, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, }; static const servoMixer_t servoMixerSingle[] = { { SERVO_SINGLECOPTER_1, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, { SERVO_SINGLECOPTER_1, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, { SERVO_SINGLECOPTER_2, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, { SERVO_SINGLECOPTER_2, INPUT_STABILIZED_PITCH, 100, 0, 0, 100, 0 }, { SERVO_SINGLECOPTER_3, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, { SERVO_SINGLECOPTER_3, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, { SERVO_SINGLECOPTER_4, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, { SERVO_SINGLECOPTER_4, INPUT_STABILIZED_ROLL, 100, 0, 0, 100, 0 }, }; static const servoMixer_t servoMixerGimbal[] = { { SERVO_GIMBAL_PITCH, INPUT_GIMBAL_PITCH, 125, 0, 0, 100, 0 }, { SERVO_GIMBAL_ROLL, INPUT_GIMBAL_ROLL, 125, 0, 0, 100, 0 }, }; const mixerRules_t servoMixers[] = { { 0, NULL }, // entry 0 { COUNT_SERVO_RULES(servoMixerTri), servoMixerTri }, // MULTITYPE_TRI { 0, NULL }, // MULTITYPE_QUADP { 0, NULL }, // MULTITYPE_QUADX { COUNT_SERVO_RULES(servoMixerBI), servoMixerBI }, // MULTITYPE_BI { COUNT_SERVO_RULES(servoMixerGimbal), servoMixerGimbal }, // * MULTITYPE_GIMBAL { 0, NULL }, // MULTITYPE_Y6 { 0, NULL }, // MULTITYPE_HEX6 { COUNT_SERVO_RULES(servoMixerFlyingWing), servoMixerFlyingWing },// * MULTITYPE_FLYING_WING { 0, NULL }, // MULTITYPE_Y4 { 0, NULL }, // MULTITYPE_HEX6X { 0, NULL }, // MULTITYPE_OCTOX8 { 0, NULL }, // MULTITYPE_OCTOFLATP { 0, NULL }, // MULTITYPE_OCTOFLATX { COUNT_SERVO_RULES(servoMixerAirplane), servoMixerAirplane }, // * MULTITYPE_AIRPLANE { 0, NULL }, // * MULTITYPE_HELI_120_CCPM { 0, NULL }, // * MULTITYPE_HELI_90_DEG { 0, NULL }, // MULTITYPE_VTAIL4 { 0, NULL }, // MULTITYPE_HEX6H { 0, NULL }, // * MULTITYPE_PPM_TO_SERVO { COUNT_SERVO_RULES(servoMixerDual), servoMixerDual }, // MULTITYPE_DUALCOPTER { COUNT_SERVO_RULES(servoMixerSingle), servoMixerSingle }, // MULTITYPE_SINGLECOPTER { 0, NULL }, // MULTITYPE_ATAIL4 { 0, NULL }, // MULTITYPE_CUSTOM { 0, NULL }, // MULTITYPE_CUSTOM_PLANE { 0, NULL }, // MULTITYPE_CUSTOM_TRI { 0, NULL }, }; static servoMixer_t *customServoMixers; #endif static motorMixer_t *customMixers; void mixerUseConfigs( #ifdef USE_SERVOS servoParam_t *servoConfToUse, gimbalConfig_t *gimbalConfigToUse, #endif flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse) { #ifdef USE_SERVOS servoConf = servoConfToUse; gimbalConfig = gimbalConfigToUse; #endif flight3DConfig = flight3DConfigToUse; escAndServoConfig = escAndServoConfigToUse; mixerConfig = mixerConfigToUse; airplaneConfig = airplaneConfigToUse; rxConfig = rxConfigToUse; } #ifdef USE_SERVOS int16_t determineServoMiddleOrForwardFromChannel(servoIndex_e servoIndex) { uint8_t channelToForwardFrom = servoConf[servoIndex].forwardFromChannel; if (channelToForwardFrom != CHANNEL_FORWARDING_DISABLED && channelToForwardFrom < rxRuntimeConfig.channelCount) { return rcData[channelToForwardFrom]; } return servoConf[servoIndex].middle; } int servoDirection(int servoIndex, int inputSource) { // determine the direction (reversed or not) from the direction bitfield of the servo if (servoConf[servoIndex].reversedSources & (1 << inputSource)) return -1; else return 1; } #endif #ifndef USE_QUAD_MIXER_ONLY void loadCustomServoMixer(void) { uint8_t i; // reset settings servoRuleCount = 0; memset(currentServoMixer, 0, sizeof(currentServoMixer)); // load custom mixer into currentServoMixer for (i = 0; i < MAX_SERVO_RULES; i++) { // check if done if (customServoMixers[i].rate == 0) break; currentServoMixer[i] = customServoMixers[i]; servoRuleCount++; } } void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMotorMixers, servoMixer_t *initialCustomServoMixers) { currentMixerMode = mixerMode; customMixers = initialCustomMotorMixers; customServoMixers = initialCustomServoMixers; // enable servos for mixes that require them. note, this shifts motor counts. useServo = mixers[currentMixerMode].useServo; // if we want camstab/trig, that also enables servos, even if mixer doesn't if (feature(FEATURE_SERVO_TILT)) useServo = 1; // give all servos a default command for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servo[i] = DEFAULT_SERVO_MIDDLE; } } void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration) { int i; motorCount = 0; servoCount = pwmOutputConfiguration->servoCount; if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { // load custom mixer into currentMixer for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) { // check if done if (customMixers[i].throttle == 0.0f) break; currentMixer[i] = customMixers[i]; motorCount++; } } else { motorCount = mixers[currentMixerMode].motorCount; // copy motor-based mixers if (mixers[currentMixerMode].motor) { for (i = 0; i < motorCount; i++) currentMixer[i] = mixers[currentMixerMode].motor[i]; } } if (useServo) { servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; if (servoMixers[currentMixerMode].rule) { for (i = 0; i < servoRuleCount; i++) currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; } } // in 3D mode, mixer gain has to be halved if (feature(FEATURE_3D)) { if (motorCount > 1) { for (i = 0; i < motorCount; i++) { currentMixer[i].pitch *= 0.5f; currentMixer[i].roll *= 0.5f; currentMixer[i].yaw *= 0.5f; } } } // set flag that we're on something with wings if (currentMixerMode == MIXER_FLYING_WING || currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE ) { ENABLE_STATE(FIXED_WING); if (currentMixerMode == MIXER_CUSTOM_AIRPLANE) { loadCustomServoMixer(); } } else { DISABLE_STATE(FIXED_WING); if (currentMixerMode == MIXER_CUSTOM_TRI) { loadCustomServoMixer(); } } mixerResetDisarmedMotors(); } void servoMixerLoadMix(int index, servoMixer_t *customServoMixers) { int i; // we're 1-based index++; // clear existing for (i = 0; i < MAX_SERVO_RULES; i++) customServoMixers[i].targetChannel = customServoMixers[i].inputSource = customServoMixers[i].rate = customServoMixers[i].box = 0; for (i = 0; i < servoMixers[index].servoRuleCount; i++) customServoMixers[i] = servoMixers[index].rule[i]; } void mixerLoadMix(int index, motorMixer_t *customMixers) { int i; // we're 1-based index++; // clear existing for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) customMixers[i].throttle = 0.0f; // do we have anything here to begin with? if (mixers[index].motor != NULL) { for (i = 0; i < mixers[index].motorCount; i++) customMixers[i] = mixers[index].motor[i]; } } #else void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) { currentMixerMode = mixerMode; customMixers = initialCustomMixers; } void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration) { UNUSED(pwmOutputConfiguration); motorCount = 4; #ifdef USE_SERVOS servoCount = 0; #endif uint8_t i; for (i = 0; i < motorCount; i++) { currentMixer[i] = mixerQuadX[i]; } mixerResetDisarmedMotors(); } #endif void mixerResetDisarmedMotors(void) { int i; // set disarmed motor values for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand; } #ifdef USE_SERVOS 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++]); } } static void updateGimbalServos(uint8_t firstServoIndex) { pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]); pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]); } 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; } } #endif void writeMotors(void) { uint8_t i; for (i = 0; i < motorCount; i++) pwmWriteMotor(i, motor[i]); if (feature(FEATURE_ONESHOT125)) { pwmCompleteOneshotMotorUpdate(motorCount); } } void writeAllMotors(int16_t mc) { uint8_t i; // Sends commands to all motors for (i = 0; i < motorCount; i++) motor[i] = mc; writeMotors(); } void stopMotors(void) { writeAllMotors(feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand); delay(50); // give the timers and ESCs a chance to react. } void StopPwmAllMotors() { pwmShutdownPulsesForAllMotors(motorCount); } #ifndef USE_QUAD_MIXER_ONLY 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); } } #endif 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])); } if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) { motorLimitReached = false; // It always needs to be reset so it can't get stuck when flipping back and fourth // 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; } } else { // 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; // 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 (rollPitchYawMix[i] > rollPitchYawMixMax) rollPitchYawMixMax = rollPitchYawMix[i]; if (rollPitchYawMix[i] < rollPitchYawMixMin) rollPitchYawMixMin = rollPitchYawMix[i]; } // Scale roll/pitch/yaw uniformly to fit within throttle range int16_t rollPitchYawMixRange = rollPitchYawMixMax - rollPitchYawMixMin; int16_t throttleRange = escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle; int16_t throttleMin, throttleMax; if (rollPitchYawMixRange > throttleRange) { motorLimitReached = true; for (i = 0; i < motorCount; i++) { rollPitchYawMix[i] = (rollPitchYawMix[i] * throttleRange) / rollPitchYawMixRange; } throttleMin = escAndServoConfig->minthrottle; throttleMax = escAndServoConfig->maxthrottle; } else { motorLimitReached = false; throttleMin = escAndServoConfig->minthrottle + (rollPitchYawMixRange / 2); throttleMax = escAndServoConfig->maxthrottle - (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] + constrainf(rcCommand[THROTTLE] * currentMixer[i].throttle, throttleMin, throttleMax); } } if (ARMING_FLAG(ARMED)) { bool isFailsafeActive = failsafeIsActive(); int16_t maxThrottleDifference = 0; if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) { // 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]; } } if (maxMotor > escAndServoConfig->maxthrottle) { maxThrottleDifference = maxMotor - escAndServoConfig->maxthrottle; } } for (i = 0; i < motorCount; i++) { if (!(IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !(feature(FEATURE_3D))) { // 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 ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) || 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 { 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 } #ifdef USE_SERVOS bool isMixerUsingServos(void) { return useServo; } #endif void filterServos(void) { #ifdef USE_SERVOS int16_t servoIdx; #if defined(MIXER_DEBUG) uint32_t startTime = micros(); #endif if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { servo[servoIdx] = (int16_t)lowpassFixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq); // Sanity check servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); } } #if defined(MIXER_DEBUG) debug[0] = (int16_t)(micros() - startTime); #endif #endif }