From 31828873fa5ec10663d224f08d17dfaee6b365db Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Fri, 14 Oct 2016 10:47:04 +0100 Subject: [PATCH] Split mixer and servo code --- Makefile | 1 + src/main/config/config_master.h | 4 +- src/main/fc/config.c | 21 +- src/main/fc/mw.c | 5 +- src/main/flight/mixer.c | 471 +----------------------------- src/main/flight/mixer.h | 117 +------- src/main/flight/servos.c | 490 ++++++++++++++++++++++++++++++++ src/main/flight/servos.h | 129 +++++++++ src/main/io/motors.h | 2 +- src/main/io/serial_cli.c | 6 +- src/main/io/servos.h | 4 +- src/main/main.c | 2 +- 12 files changed, 654 insertions(+), 598 deletions(-) create mode 100755 src/main/flight/servos.c create mode 100644 src/main/flight/servos.h diff --git a/Makefile b/Makefile index fa0c53e5a..d234823bc 100644 --- a/Makefile +++ b/Makefile @@ -508,6 +508,7 @@ COMMON_SRC = \ flight/imu.c \ flight/mixer.c \ flight/pid.c \ + flight/servos.c \ io/beeper.c \ io/serial.c \ io/serial_4way.c \ diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 64132e301..2e7444196 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -29,6 +29,7 @@ #include "flight/failsafe.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/imu.h" #include "flight/navigation.h" @@ -65,10 +66,11 @@ typedef struct master_s { // motor/esc/servo related stuff motorMixer_t customMotorMixer[MAX_SUPPORTED_MOTORS]; motorConfig_t motorConfig; - servoConfig_t servoConfig; flight3DConfig_t flight3DConfig; #ifdef USE_SERVOS + servoConfig_t servoConfig; + servoMixerConfig_t servoMixerConfig; servoMixer_t customServoMixer[MAX_SERVO_RULES]; // Servo-related stuff servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; // servo configuration diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 92c9b27a5..adf21ae91 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -72,6 +72,7 @@ #include "telemetry/telemetry.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" @@ -407,13 +408,17 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) void resetMixerConfig(mixerConfig_t *mixerConfig) { mixerConfig->yaw_motor_direction = 1; -#ifdef USE_SERVOS - mixerConfig->tri_unarmed_servo = 1; - mixerConfig->servo_lowpass_freq = 400; - mixerConfig->servo_lowpass_enable = 0; -#endif } +#ifdef USE_SERVOS +void resetServoMixerConfig(servoMixerConfig_t *servoMixerConfig) +{ + servoMixerConfig->tri_unarmed_servo = 1; + servoMixerConfig->servo_lowpass_freq = 400; + servoMixerConfig->servo_lowpass_enable = 0; +} +#endif + uint8_t getCurrentProfile(void) { return masterConfig.current_profile_index; @@ -573,13 +578,13 @@ void createDefaultConfig(master_t *config) config->auto_disarm_delay = 5; config->small_angle = 25; - resetMixerConfig(&config->mixerConfig); - config->airplaneConfig.fixedwing_althold_dir = 1; // Motor/ESC/Servo + resetMixerConfig(&config->mixerConfig); resetMotorConfig(&config->motorConfig); #ifdef USE_SERVOS + resetServoMixerConfig(&config->servoMixerConfig); resetServoConfig(&config->servoConfig); #endif resetFlight3DConfig(&config->flight3DConfig); @@ -774,7 +779,7 @@ void activateConfig(void) ); #ifdef USE_SERVOS - servoUseConfigs(masterConfig.servoConf, &masterConfig.gimbalConfig); + servoUseConfigs(&masterConfig.servoMixerConfig, masterConfig.servoConf, &masterConfig.gimbalConfig); #endif imuRuntimeConfig.dcm_kp = masterConfig.dcm_kp / 10000.0f; diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 583b74619..48d234fe3 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -65,6 +65,7 @@ #include "scheduler/scheduler.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/failsafe.h" #include "flight/gtune.h" @@ -740,7 +741,7 @@ void subTaskMainSubprocesses(void) if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS - && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo) + && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.servoMixerConfig.tri_unarmed_servo) #endif && masterConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerMode != MIXER_FLYING_WING @@ -794,6 +795,8 @@ void subTaskMotorUpdate(void) mixTable(¤tProfile->pidProfile); #ifdef USE_SERVOS + // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos. + servoTable(); filterServos(); writeServos(); #endif diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index c31f958a8..093cf1749 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -17,14 +17,12 @@ #include #include -#include #include #include #include "platform.h" #include "build/build_config.h" -#include "build/debug.h" #include "common/axis.h" #include "common/maths.h" @@ -32,18 +30,11 @@ #include "drivers/system.h" #include "drivers/pwm_output.h" -#include "drivers/sensor.h" -#include "drivers/accgyro.h" -#include "drivers/system.h" + +#include "io/motors.h" #include "rx/rx.h" -#include "io/gimbal.h" -#include "io/motors.h" -#include "io/servos.h" - -#include "sensors/sensors.h" -#include "sensors/acceleration.h" #include "sensors/battery.h" #include "flight/mixer.h" @@ -66,20 +57,12 @@ static mixerConfig_t *mixerConfig; static flight3DConfig_t *flight3DConfig; static motorConfig_t *motorConfig; static airplaneConfig_t *airplaneConfig; -static rxConfig_t *rxConfig; +rxConfig_t *rxConfig; static bool syncMotorOutputWithPidLoop = false; -static mixerMode_e currentMixerMode; +mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; -#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 servoParam_t *servoConf; -#endif static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R @@ -245,92 +228,6 @@ const mixer_t mixers[] = { }; #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; static uint16_t disarmMotorOutput, minMotorOutputNormal, maxMotorOutputNormal, deadbandMotor3dHigh, deadbandMotor3dLow; @@ -370,51 +267,6 @@ void mixerUseConfigs( initEscEndpoints(); } -#ifdef USE_SERVOS -void servoUseConfigs(servoParam_t *servoConfToUse, gimbalConfig_t *gimbalConfigToUse) -{ - servoConf = servoConfToUse; - gimbalConfig = gimbalConfigToUse; -} - -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; -} - -void servoMixerInit(servoMixer_t *initialCustomServoMixers) -{ - 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; - } -} -#endif - void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) { currentMixerMode = mixerMode; @@ -424,23 +276,6 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) #ifndef USE_QUAD_MIXER_ONLY -void loadCustomServoMixer(void) -{ - // reset settings - servoRuleCount = 0; - memset(currentServoMixer, 0, sizeof(currentServoMixer)); - - // load custom mixer into currentServoMixer - for (uint8_t i = 0; i < MAX_SERVO_RULES; i++) { - // check if done - if (customServoMixers[i].rate == 0) - break; - - currentServoMixer[i] = customServoMixers[i]; - servoRuleCount++; - } -} - void mixerConfigureOutput(void) { int i; @@ -467,14 +302,6 @@ void mixerConfigureOutput(void) } } - 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) { @@ -486,42 +313,9 @@ void mixerConfigureOutput(void) } } - // 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; @@ -561,90 +355,6 @@ void mixerResetDisarmedMotors(void) motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : motorConfig->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) { for (uint8_t i = 0; i < motorCount; i++) { @@ -679,91 +389,10 @@ void stopPwmAllMotors(void) delayMicroseconds(1500); } -#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] = axisPIDf[ROLL]; - input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH]; - input[INPUT_STABILIZED_YAW] = axisPIDf[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 *pidProfilePtr) +void mixTable(pidProfile_t *pidProfile) { uint32_t i = 0; float vbatCompensationFactor = 1; - pidProfile_t *pidProfile = (pidProfile_t *) pidProfilePtr; // Scale roll/pitch/yaw uniformly to fit within throttle range // Initial mixer concept by bdoiron74 reused and optimized for Air Mode @@ -885,94 +514,4 @@ void mixTable(void *pidProfilePtr) } } } - - // 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 - static int16_t servoIdx; - static bool servoFilterIsSet; - static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS]; - -#if defined(MIXER_DEBUG) - uint32_t startTime = micros(); -#endif - - if (mixerConfig->servo_lowpass_enable) { - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - if (!servoFilterIsSet) { - biquadFilterInitLPF(&servoFilter[servoIdx], mixerConfig->servo_lowpass_freq, targetPidLooptime); - servoFilterIsSet = true; - } - - servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); - // 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 -} - diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 57f2ce0fc..cf9c37fd8 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -18,7 +18,6 @@ #pragma once #define MAX_SUPPORTED_MOTORS 12 -#define MAX_SUPPORTED_SERVOS 8 #define QUAD_MOTOR_COUNT 4 @@ -77,11 +76,6 @@ typedef struct mixer_s { typedef struct mixerConfig_s { int8_t yaw_motor_direction; -#ifdef USE_SERVOS - uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq - int8_t servo_lowpass_enable; // enable/disable lowpass filter -#endif } mixerConfig_t; typedef struct flight3DConfig_s { @@ -97,105 +91,6 @@ typedef struct airplaneConfig_s { #define CHANNEL_FORWARDING_DISABLED (uint8_t)0xFF -#ifdef USE_SERVOS - -// These must be consecutive, see 'reversedSources' -enum { - INPUT_STABILIZED_ROLL = 0, - INPUT_STABILIZED_PITCH, - INPUT_STABILIZED_YAW, - INPUT_STABILIZED_THROTTLE, - INPUT_RC_ROLL, - INPUT_RC_PITCH, - INPUT_RC_YAW, - INPUT_RC_THROTTLE, - INPUT_RC_AUX1, - INPUT_RC_AUX2, - INPUT_RC_AUX3, - INPUT_RC_AUX4, - INPUT_GIMBAL_PITCH, - INPUT_GIMBAL_ROLL, - - INPUT_SOURCE_COUNT -} inputSource_e; - -// target servo channels -typedef enum { - SERVO_GIMBAL_PITCH = 0, - SERVO_GIMBAL_ROLL = 1, - SERVO_FLAPS = 2, - SERVO_FLAPPERON_1 = 3, - SERVO_FLAPPERON_2 = 4, - SERVO_RUDDER = 5, - SERVO_ELEVATOR = 6, - SERVO_THROTTLE = 7, // for internal combustion (IC) planes - - SERVO_BICOPTER_LEFT = 4, - SERVO_BICOPTER_RIGHT = 5, - - SERVO_DUALCOPTER_LEFT = 4, - SERVO_DUALCOPTER_RIGHT = 5, - - SERVO_SINGLECOPTER_1 = 3, - SERVO_SINGLECOPTER_2 = 4, - SERVO_SINGLECOPTER_3 = 5, - SERVO_SINGLECOPTER_4 = 6, - -} servoIndex_e; // FIXME rename to servoChannel_e - -#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS -#define SERVO_PLANE_INDEX_MAX SERVO_THROTTLE - -#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT -#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT - -#define SERVO_SINGLECOPTER_INDEX_MIN SERVO_SINGLECOPTER_1 -#define SERVO_SINGLECOPTER_INDEX_MAX SERVO_SINGLECOPTER_4 - -#define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1 -#define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2 - -typedef struct servoMixer_s { - uint8_t targetChannel; // servo that receives the output of the rule - uint8_t inputSource; // input channel for this rule - int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction - uint8_t speed; // reduces the speed of the rule, 0=unlimited speed - int8_t min; // lower bound of rule range [0;100]% of servo max-min - int8_t max; // lower bound of rule range [0;100]% of servo max-min - uint8_t box; // active rule if box is enabled, range [0;3], 0=no box, 1=BOXSERVO1, 2=BOXSERVO2, 3=BOXSERVO3 -} servoMixer_t; - -#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) -#define MAX_SERVO_SPEED UINT8_MAX -#define MAX_SERVO_BOXES 3 - -// Custom mixer configuration -typedef struct mixerRules_s { - uint8_t servoRuleCount; - const servoMixer_t *rule; -} mixerRules_t; - -typedef struct servoParam_s { - int16_t min; // servo min - int16_t max; // servo max - int16_t middle; // servo middle - int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction - uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value. - uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value. - int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED - uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted -} __attribute__ ((__packed__)) servoParam_t; - -struct gimbalConfig_s; -struct motorConfig_s; -struct rxConfig_s; - -extern int16_t servo[MAX_SUPPORTED_SERVOS]; -bool isMixerUsingServos(void); -void writeServos(void); -void filterServos(void); -#endif - extern const mixer_t mixers[]; extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; @@ -211,21 +106,13 @@ void mixerUseConfigs( void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); -#ifdef USE_SERVOS -void servoMixerInit(servoMixer_t *customServoMixers); -struct servoParam_s; -struct gimbalConfig_s; -void servoUseConfigs(struct servoParam_s *servoConfToUse, struct gimbalConfig_s *gimbalConfigToUse); -void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); -void loadCustomServoMixer(void); -int servoDirection(int servoIndex, int fromChannel); -#endif void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); void mixerConfigureOutput(void); void mixerResetDisarmedMotors(void); -void mixTable(void *pidProfilePtr); +struct pidProfile_s; +void mixTable(struct pidProfile_s *pidProfile); void syncMotors(bool enabled); void writeMotors(void); void stopMotors(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c new file mode 100755 index 000000000..0469238cb --- /dev/null +++ b/src/main/flight/servos.c @@ -0,0 +1,490 @@ +/* + * 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" + +#ifdef USE_SERVOS + +#include "build/build_config.h" + +#include "common/filter.h" + +#include "drivers/pwm_output.h" +#include "drivers/system.h" + +#include "rx/rx.h" + +#include "io/gimbal.h" +#include "io/servos.h" + +#include "flight/mixer.h" +#include "flight/pid.h" +#include "flight/imu.h" + +#include "fc/config.h" +#include "fc/rc_controls.h" +#include "fc/runtime_config.h" + +#include "config/feature.h" + +extern mixerMode_e currentMixerMode; +extern rxConfig_t *rxConfig; + +static servoMixerConfig_t *servoMixerConfig; + +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 servoParam_t *servoConf; + + +#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; + +void servoUseConfigs(servoMixerConfig_t *servoMixerConfigToUse, servoParam_t *servoParamsToUse, struct gimbalConfig_s *gimbalConfigToUse) +{ + servoMixerConfig = servoMixerConfigToUse; + servoConf = servoParamsToUse; + gimbalConfig = gimbalConfigToUse; +} + +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; +} + +void servoMixerInit(servoMixer_t *initialCustomServoMixers) +{ + 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 loadCustomServoMixer(void) +{ + // reset settings + servoRuleCount = 0; + memset(currentServoMixer, 0, sizeof(currentServoMixer)); + + // load custom mixer into currentServoMixer + for (uint8_t i = 0; i < MAX_SERVO_RULES; i++) { + // check if done + if (customServoMixers[i].rate == 0) + break; + + currentServoMixer[i] = customServoMixers[i]; + servoRuleCount++; + } +} + +void servoConfigureOutput(void) +{ + if (useServo) { + servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; + if (servoMixers[currentMixerMode].rule) { + for (int i = 0; i < servoRuleCount; i++) + currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; + } + } + + // 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(); + } + } +} + + +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]; +} + +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 (servoMixerConfig->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_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] = axisPIDf[ROLL]; + input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH]; + input[INPUT_STABILIZED_YAW] = axisPIDf[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 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 + } +} + +bool isMixerUsingServos(void) +{ + return useServo; +} + +void filterServos(void) +{ + static int16_t servoIdx; + static bool servoFilterIsSet; + static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS]; + +#if defined(MIXER_DEBUG) + uint32_t startTime = micros(); +#endif + + if (servoMixerConfig->servo_lowpass_enable) { + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + if (!servoFilterIsSet) { + biquadFilterInitLPF(&servoFilter[servoIdx], servoMixerConfig->servo_lowpass_freq, targetPidLooptime); + servoFilterIsSet = true; + } + + servo[servoIdx] = lrintf(biquadFilterApply(&servoFilter[servoIdx], (float)servo[servoIdx])); + // 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 diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h new file mode 100644 index 000000000..66f86955b --- /dev/null +++ b/src/main/flight/servos.h @@ -0,0 +1,129 @@ +/* + * 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 . + */ + +#pragma once + +#define MAX_SUPPORTED_SERVOS 8 + +// These must be consecutive, see 'reversedSources' +enum { + INPUT_STABILIZED_ROLL = 0, + INPUT_STABILIZED_PITCH, + INPUT_STABILIZED_YAW, + INPUT_STABILIZED_THROTTLE, + INPUT_RC_ROLL, + INPUT_RC_PITCH, + INPUT_RC_YAW, + INPUT_RC_THROTTLE, + INPUT_RC_AUX1, + INPUT_RC_AUX2, + INPUT_RC_AUX3, + INPUT_RC_AUX4, + INPUT_GIMBAL_PITCH, + INPUT_GIMBAL_ROLL, + + INPUT_SOURCE_COUNT +} inputSource_e; + +// target servo channels +typedef enum { + SERVO_GIMBAL_PITCH = 0, + SERVO_GIMBAL_ROLL = 1, + SERVO_FLAPS = 2, + SERVO_FLAPPERON_1 = 3, + SERVO_FLAPPERON_2 = 4, + SERVO_RUDDER = 5, + SERVO_ELEVATOR = 6, + SERVO_THROTTLE = 7, // for internal combustion (IC) planes + + SERVO_BICOPTER_LEFT = 4, + SERVO_BICOPTER_RIGHT = 5, + + SERVO_DUALCOPTER_LEFT = 4, + SERVO_DUALCOPTER_RIGHT = 5, + + SERVO_SINGLECOPTER_1 = 3, + SERVO_SINGLECOPTER_2 = 4, + SERVO_SINGLECOPTER_3 = 5, + SERVO_SINGLECOPTER_4 = 6, + +} servoIndex_e; // FIXME rename to servoChannel_e + +#define SERVO_PLANE_INDEX_MIN SERVO_FLAPS +#define SERVO_PLANE_INDEX_MAX SERVO_THROTTLE + +#define SERVO_DUALCOPTER_INDEX_MIN SERVO_DUALCOPTER_LEFT +#define SERVO_DUALCOPTER_INDEX_MAX SERVO_DUALCOPTER_RIGHT + +#define SERVO_SINGLECOPTER_INDEX_MIN SERVO_SINGLECOPTER_1 +#define SERVO_SINGLECOPTER_INDEX_MAX SERVO_SINGLECOPTER_4 + +#define SERVO_FLAPPERONS_MIN SERVO_FLAPPERON_1 +#define SERVO_FLAPPERONS_MAX SERVO_FLAPPERON_2 + +typedef struct servoMixer_s { + uint8_t targetChannel; // servo that receives the output of the rule + uint8_t inputSource; // input channel for this rule + int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction + uint8_t speed; // reduces the speed of the rule, 0=unlimited speed + int8_t min; // lower bound of rule range [0;100]% of servo max-min + int8_t max; // lower bound of rule range [0;100]% of servo max-min + uint8_t box; // active rule if box is enabled, range [0;3], 0=no box, 1=BOXSERVO1, 2=BOXSERVO2, 3=BOXSERVO3 +} servoMixer_t; + +#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS) +#define MAX_SERVO_SPEED UINT8_MAX +#define MAX_SERVO_BOXES 3 + +// Custom mixer configuration +typedef struct mixerRules_s { + uint8_t servoRuleCount; + const servoMixer_t *rule; +} mixerRules_t; + +typedef struct servoParam_s { + int16_t min; // servo min + int16_t max; // servo max + int16_t middle; // servo middle + int8_t rate; // range [-125;+125] ; can be used to adjust a rate 0-125% and a direction + uint8_t angleAtMin; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'min' value. + uint8_t angleAtMax; // range [0;180] the measured angle in degrees from the middle when the servo is at the 'max' value. + int8_t forwardFromChannel; // RX channel index, 0 based. See CHANNEL_FORWARDING_DISABLED + uint32_t reversedSources; // the direction of servo movement for each input source of the servo mixer, bit set=inverted +} __attribute__ ((__packed__)) servoParam_t; + +typedef struct servoMixerConfig_s{ + uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed + uint16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq + int8_t servo_lowpass_enable; // enable/disable lowpass filter +} servoMixerConfig_t; + +extern int16_t servo[MAX_SUPPORTED_SERVOS]; + +void servoTable(void); +bool isMixerUsingServos(void); +void writeServos(void); +void filterServos(void); + +void servoMixerInit(servoMixer_t *customServoMixers); +struct gimbalConfig_s; +void servoUseConfigs(servoMixerConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, struct gimbalConfig_s *gimbalConfigToUse); +void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); +void loadCustomServoMixer(void); +void servoConfigureOutput(void); +int servoDirection(int servoIndex, int fromChannel); + diff --git a/src/main/io/motors.h b/src/main/io/motors.h index d7fedf85c..d90c0a9e1 100644 --- a/src/main/io/motors.h +++ b/src/main/io/motors.h @@ -17,7 +17,7 @@ #pragma once -#include "drivers/io.h" +#include "drivers/io_types.h" #include "flight/mixer.h" typedef struct motorConfig_s { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 70ba4fbda..76c2a0164 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -823,9 +823,9 @@ const clivalue_t valueTable[] = { { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } }, #ifdef USE_SERVOS { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, - { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, - { "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} }, - { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, + { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, + { "servo_lowpass_freq", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoMixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400} }, + { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoMixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON } }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servoConfig.servoPwmRate, .config.minmax = { 50, 498 } }, { "gimbal_mode", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE } }, #endif diff --git a/src/main/io/servos.h b/src/main/io/servos.h index 311b0c579..711ceec75 100644 --- a/src/main/io/servos.h +++ b/src/main/io/servos.h @@ -17,8 +17,8 @@ #pragma once -#include "drivers/io.h" -#include "flight/mixer.h" +#include "drivers/io_types.h" +#include "flight/servos.h" typedef struct servoConfig_s { // PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms) diff --git a/src/main/main.c b/src/main/main.c index ae8e88d17..e84a14db5 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -288,7 +288,7 @@ void init(void) #endif mixerConfigureOutput(); - // pwmInit() needs to be called as soon as possible for ESC compatibility reasons + servoConfigureOutput(); systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER