From ddb60edfdd644c70ff1cf03e66b934a30bf913ed Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sat, 10 Sep 2016 11:42:45 +0100 Subject: [PATCH] Split servo code into servos.c --- Makefile | 1 + src/main/blackbox/blackbox.c | 1 + src/main/blackbox/blackbox_io.c | 1 + src/main/config/config.c | 23 +- src/main/config/config_master.h | 1 + src/main/config/feature.c | 1 + src/main/fc/msp_server_fc.c | 1 + src/main/fc/mw.c | 3 +- src/main/flight/mixer.c | 426 +------------------------- src/main/flight/mixer.h | 117 ------- src/main/flight/servos.c | 470 +++++++++++++++++++++++++++++ src/main/flight/servos.h | 132 ++++++++ src/main/io/ledstrip.c | 1 + src/main/io/serial_cli.c | 7 +- src/main/main.c | 1 + src/main/target/BLUEJAYF4/config.c | 1 + src/main/telemetry/mavlink.c | 1 + 17 files changed, 638 insertions(+), 550 deletions(-) create mode 100755 src/main/flight/servos.c create mode 100644 src/main/flight/servos.h diff --git a/Makefile b/Makefile index 041fb27026..ed0c98917c 100644 --- a/Makefile +++ b/Makefile @@ -403,6 +403,7 @@ COMMON_SRC = \ flight/imu.c \ flight/hil.c \ flight/mixer.c \ + flight/servos.c \ flight/pid.c \ io/beeper.c \ fc/rc_controls.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b423d382c1..1766cc00a4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -70,6 +70,7 @@ #include "telemetry/telemetry.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/navigation_rewrite.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index ba4e455a46..9bd797df90 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -67,6 +67,7 @@ #include "common/printf.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/navigation_rewrite.h" diff --git a/src/main/config/config.c b/src/main/config/config.c index b01d9db8b2..4cd0879b51 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -62,6 +62,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" @@ -347,16 +348,21 @@ void resetRcControlsConfig(rcControlsConfig_t *rcControlsConfig) { rcControlsConfig->alt_hold_deadband = 50; } -void resetMixerConfig(mixerConfig_t *mixerConfig) { +static void resetMixerConfig(mixerConfig_t *mixerConfig) +{ mixerConfig->yaw_motor_direction = 1; mixerConfig->yaw_jump_prevention_limit = 200; -#ifdef USE_SERVOS - mixerConfig->tri_unarmed_servo = 1; - mixerConfig->servo_lowpass_freq = 400; - mixerConfig->servo_lowpass_enable = 0; -#endif } +#ifdef USE_SERVOS +static void resetServoConfig(servoConfig_t *servoConfig) +{ + servoConfig->tri_unarmed_servo = 1; + servoConfig->servo_lowpass_freq = 400; + servoConfig->servo_lowpass_enable = 0; +} +#endif + uint8_t getCurrentProfile(void) { return masterConfig.current_profile_index; @@ -470,6 +476,9 @@ static void resetConf(void) masterConfig.small_angle = 25; resetMixerConfig(&masterConfig.mixerConfig); +#ifdef USE_SERVOS + resetServoConfig(&masterConfig.servoConfig); +#endif // Motor/ESC/Servo resetEscAndServoConfig(&masterConfig.escAndServoConfig); @@ -716,7 +725,7 @@ void activateConfig(void) mixerUseConfigs(&masterConfig.flight3DConfig, &masterConfig.escAndServoConfig, &masterConfig.mixerConfig, &masterConfig.rxConfig); #ifdef USE_SERVOS - servosUseConfigs(currentProfile->servoConf, ¤tProfile->gimbalConfig); + servosUseConfigs(&masterConfig.servoConfig, currentProfile->servoConf, ¤tProfile->gimbalConfig); #endif imuRuntimeConfig.dcm_kp_acc = masterConfig.dcm_kp_acc / 10000.0f; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 88b4d9a675..59465b983c 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -80,6 +80,7 @@ typedef struct master_s { // mixer-related configuration mixerConfig_t mixerConfig; + servoConfig_t servoConfig; #ifdef GPS gpsConfig_t gpsConfig; diff --git a/src/main/config/feature.c b/src/main/config/feature.c index d6260403f2..b539bf9fe3 100755 --- a/src/main/config/feature.c +++ b/src/main/config/feature.c @@ -60,6 +60,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" diff --git a/src/main/fc/msp_server_fc.c b/src/main/fc/msp_server_fc.c index b6434fbc8c..a8aae48cbb 100644 --- a/src/main/fc/msp_server_fc.c +++ b/src/main/fc/msp_server_fc.c @@ -75,6 +75,7 @@ #include "sensors/gyro.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/hil.h" diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index a251fd8cc1..f738f2bbcc 100755 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -77,6 +77,7 @@ #include "blackbox/blackbox.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/hil.h" @@ -565,7 +566,7 @@ void taskMainPidLoop(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.servoConfig.tri_unarmed_servo) #endif && masterConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerMode != MIXER_FLYING_WING diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 964896d5ea..1dff72b5ba 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -41,7 +41,6 @@ #include "rx/rx.h" #include "io/gimbal.h" -#include "io/escservo.h" #include "fc/rc_controls.h" @@ -50,6 +49,7 @@ #include "sensors/gyro.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/failsafe.h" #include "flight/pid.h" #include "flight/imu.h" @@ -70,31 +70,15 @@ int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; bool motorLimitReached = false; -static mixerConfig_t *mixerConfig; +mixerConfig_t *mixerConfig; static flight3DConfig_t *flight3DConfig; static escAndServoConfig_t *escAndServoConfig; -static rxConfig_t *rxConfig; +rxConfig_t *rxConfig; -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 servoOutputEnabled; - -static uint8_t mixerUsesServos; -static uint8_t minServoIndex; -static uint8_t maxServoIndex; - -static servoParam_t *servoConf; -static biquadFilter_t servoFitlerState[MAX_SUPPORTED_SERVOS]; -static bool servoFilterIsSet; -#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 @@ -282,63 +266,6 @@ const mixer_t mixers[] = { }; #endif // USE_QUAD_MIXER_ONLY -#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 servoMixerTri[] = { - { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, -}; - -const mixerRules_t servoMixers[] = { - { 0, 0, 0, NULL }, // entry 0 - { COUNT_SERVO_RULES(servoMixerTri), SERVO_RUDDER, SERVO_RUDDER, servoMixerTri }, // MULTITYPE_TRI - { 0, 0, 0, NULL }, // MULTITYPE_QUADP - { 0, 0, 0, NULL }, // MULTITYPE_QUADX - { 0, 0, 0, NULL }, // MULTITYPE_BI - { 0, 0, 0, NULL }, // MULTITYPE_GIMBAL / MIXER_GIMBAL -> disabled - { 0, 0, 0, NULL }, // MULTITYPE_Y6 - { 0, 0, 0, NULL }, // MULTITYPE_HEX6 - { COUNT_SERVO_RULES(servoMixerFlyingWing), SERVO_FLAPPERONS_MIN, SERVO_FLAPPERONS_MAX, servoMixerFlyingWing },// * MULTITYPE_FLYING_WING - { 0, 0, 0, NULL }, // MULTITYPE_Y4 - { 0, 0, 0, NULL }, // MULTITYPE_HEX6X - { 0, 0, 0, NULL }, // MULTITYPE_OCTOX8 - { 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATP - { 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATX - { COUNT_SERVO_RULES(servoMixerAirplane), SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, servoMixerAirplane }, // * MULTITYPE_AIRPLANE - { 0, 0, 0, NULL }, // * MIXER_HELI_120_CCPM -> disabled, never fully implemented in CF - { 0, 0, 0, NULL }, // * MIXER_HELI_90_DEG -> disabled, never fully implemented in CF - { 0, 0, 0, NULL }, // MULTITYPE_VTAIL4 - { 0, 0, 0, NULL }, // MULTITYPE_HEX6H - { 0, 0, 0, NULL }, // * MULTITYPE_PPM_TO_SERVO - { 0, 0, 0, NULL }, // MULTITYPE_DUALCOPTER - { 0, 0, 0, NULL }, // MULTITYPE_SINGLECOPTER - { 0, 0, 0, NULL }, // MULTITYPE_ATAIL4 - { 0, 2, 5, NULL }, // MULTITYPE_CUSTOM - { 0, SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, NULL }, // MULTITYPE_CUSTOM_PLANE - { 0, SERVO_RUDDER, SERVO_RUDDER, NULL }, // MULTITYPE_CUSTOM_TRI -}; - -static servoMixer_t *customServoMixers; -#endif - - static motorMixer_t *customMixers; void mixerUseConfigs( @@ -353,43 +280,6 @@ void mixerUseConfigs( rxConfig = rxConfigToUse; } -#ifdef USE_SERVOS -void servosUseConfigs(servoParam_t *servoConfToUse, gimbalConfig_t *gimbalConfigToUse) -{ - servoConf = servoConfToUse; - gimbalConfig = gimbalConfigToUse; -} - -int16_t getFlaperonDirection(uint8_t servoPin) { - if (servoPin == SERVO_FLAPPERON_2) { - return -1; - } else { - return 1; - } -} - -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 - bool isMixerEnabled(mixerMode_e mixerMode) { #ifdef USE_QUAD_MIXER_ONLY @@ -400,74 +290,6 @@ bool isMixerEnabled(mixerMode_e mixerMode) #endif } -#ifdef USE_SERVOS -void servosInit(servoMixer_t *initialCustomServoMixers) -{ - int 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); - } else { - DISABLE_STATE(FIXED_WING); - } - - if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { - ENABLE_STATE(FLAPERON_AVAILABLE); - } else { - DISABLE_STATE(FLAPERON_AVAILABLE); - } - - customServoMixers = initialCustomServoMixers; - - minServoIndex = servoMixers[currentMixerMode].minServoIndex; - maxServoIndex = servoMixers[currentMixerMode].maxServoIndex; - - // enable servos for mixes that require them. note, this shifts motor counts. - mixerUsesServos = mixers[currentMixerMode].useServo || feature(FEATURE_SERVO_TILT); - - // if we want camstab/trig, that also enables servos, even if mixer doesn't - servoOutputEnabled = mixerUsesServos || feature(FEATURE_CHANNEL_FORWARDING); - - // give all servos a default command - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = DEFAULT_SERVO_MIDDLE; - } - - /* - * If mixer has predefined servo mixes, load them - */ - if (mixerUsesServos) { - servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; - if (servoMixers[currentMixerMode].rule) { - for (i = 0; i < servoRuleCount; i++) - currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; - } - } - - /* - * When we are dealing with CUSTOM mixers, load mixes defined with - * smix and update internal variables - */ - if (currentMixerMode == MIXER_CUSTOM_AIRPLANE || - currentMixerMode == MIXER_CUSTOM_TRI || - currentMixerMode == MIXER_CUSTOM) - { - loadCustomServoMixer(); - - // If there are servo rules after all, update variables - if (servoRuleCount > 0) { - servoOutputEnabled = 1; - mixerUsesServos = 1; - } - - } -} -#endif - // mixerInit must be called before servosInit void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers) { @@ -527,53 +349,6 @@ void mixerUsePWMIOConfiguration(void) #ifndef USE_QUAD_MIXER_ONLY -#ifdef USE_SERVOS -void loadCustomServoMixer(void) -{ - int i; - - // reset settings - servoRuleCount = 0; - minServoIndex = 255; - maxServoIndex = 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; - - if (customServoMixers[i].targetChannel < minServoIndex) { - minServoIndex = customServoMixers[i].targetChannel; - } - - if (customServoMixers[i].targetChannel > maxServoIndex) { - maxServoIndex = customServoMixers[i].targetChannel; - } - - currentServoMixer[i] = customServoMixers[i]; - servoRuleCount++; - } -} - -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]; - } -} -#endif - - void mixerLoadMix(int index, motorMixer_t *customMixers) { int i; @@ -601,57 +376,6 @@ void mixerResetDisarmedMotors(void) 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; - - int servoOffset; - for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { - pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]); - } -} - -void writeServos(void) -{ - int servoIndex = 0; - - bool zeroServoValue = false; - - /* - * in case of tricopters, there might me a need to zero servo output when unarmed - */ - if ((currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI) && !ARMING_FLAG(ARMED) && !mixerConfig->tri_unarmed_servo) { - zeroServoValue = true; - } - - // Write mixer servo outputs - // mixerUsesServos might indicate SERVO_TILT, servoRuleCount indicate if mixer alone uses servos - if (mixerUsesServos && servoRuleCount) { - for (int i = minServoIndex; i <= maxServoIndex; i++) { - if (zeroServoValue) { - pwmWriteServo(servoIndex++, 0); - } else { - pwmWriteServo(servoIndex++, servo[i]); - } - } - } - - if (feature(FEATURE_SERVO_TILT)) { - pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_PITCH]); - pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_ROLL]); - } - - // forward AUX to remaining servo outputs (not constrained) - if (feature(FEATURE_CHANNEL_FORWARDING)) { - forwardAuxChannelsToServos(servoIndex); - servoIndex += MAX_AUX_CHANNEL_COUNT; - } -} -#endif - void writeMotors(void) { int i; @@ -796,145 +520,3 @@ void mixTable(void) } } -#ifdef USE_SERVOS - -void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted) -{ - int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500] - static int16_t currentOutput[MAX_SERVO_RULES]; - int 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]); - } - } - - /* - Flaperon fligh mode - */ - if (FLIGHT_MODE(FLAPERON) && (target == SERVO_FLAPPERON_1 || target == SERVO_FLAPPERON_2)) { - int8_t multiplier = 1; - - if (flaperon_throw_inverted == 1) { - multiplier = -1; - } - currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier; - } - - 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 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; - } - } -} - -bool isServoOutputEnabled(void) -{ - return servoOutputEnabled; -} - -bool isMixerUsingServos(void) { - return mixerUsesServos; -} - -void filterServos(void) -{ - int servoIdx; - - if (mixerConfig->servo_lowpass_enable) { - // Initialize servo lowpass filter (servos are calculated at looptime rate) - if (!servoFilterIsSet) { - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - biquadFilterInitLPF(&servoFitlerState[servoIdx], mixerConfig->servo_lowpass_freq, gyro.targetLooptime); - } - - servoFilterIsSet = true; - } - - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - // Apply servo lowpass filter and do sanity cheching - servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]); - } - } - - for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { - servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); - } -} -#endif diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 1691317a2e..0d127096d1 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -90,11 +90,6 @@ typedef struct mixer_s { typedef struct mixerConfig_s { int8_t yaw_motor_direction; uint16_t yaw_jump_prevention_limit; // make limit configurable (original fixed value was 100) -#ifdef USE_SERVOS - uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed - int16_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 { @@ -106,111 +101,6 @@ typedef struct flight3DConfig_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_ELEVATOR = 2, - SERVO_FLAPPERON_1 = 3, - SERVO_FLAPPERON_2 = 4, - SERVO_RUDDER = 5, - SERVO_THROTTLE = 6, // for internal combustion (IC) planes - SERVO_FLAPS = 7, - - 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_ELEVATOR -#define SERVO_PLANE_INDEX_MAX SERVO_FLAPS - -#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 - -#define FLAPERON_THROW_DEFAULT 250 -#define FLAPERON_THROW_MIN 100 -#define FLAPERON_THROW_MAX 400 - -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; - uint8_t minServoIndex; - uint8_t maxServoIndex; - 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 escAndServoConfig_s; -struct rxConfig_s; - -extern int16_t servo[MAX_SUPPORTED_SERVOS]; -bool isServoOutputEnabled(void); -bool isMixerUsingServos(void); -void writeServos(void); -void filterServos(void); -#endif extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; @@ -227,13 +117,6 @@ void mixerUseConfigs( void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); -#ifdef USE_SERVOS -void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); -void loadCustomServoMixer(void); -int servoDirection(int servoIndex, int fromChannel); -void servosUseConfigs(servoParam_t *servoConfToUse, struct gimbalConfig_s *gimbalConfigToUse); -void servosInit(servoMixer_t *customServoMixers); -#endif void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); void mixerUsePWMIOConfiguration(void); void mixerResetDisarmedMotors(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c new file mode 100755 index 0000000000..b515a80b9b --- /dev/null +++ b/src/main/flight/servos.c @@ -0,0 +1,470 @@ +/* + * 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/debug.h" +#include "build/build_config.h" + +#include "common/axis.h" +#include "common/maths.h" +#include "common/filter.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 "fc/rc_controls.h" +#include "sensors/sensors.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" + +#include "flight/mixer.h" +#include "flight/servos.h" +#include "flight/failsafe.h" +#include "flight/pid.h" +#include "flight/imu.h" +#include "flight/navigation_rewrite.h" + +#include "fc/runtime_config.h" + +#include "config/config.h" +#include "config/config_profile.h" +#include "config/feature.h" + +extern mixerMode_e currentMixerMode; +extern const mixer_t mixers[]; +extern mixerConfig_t *mixerConfig; +extern rxConfig_t *rxConfig; + +servoConfig_t *servoConfig; + +static uint8_t servoRuleCount = 0; +static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; +static gimbalConfig_t *gimbalConfig; +int16_t servo[MAX_SUPPORTED_SERVOS]; +static int servoOutputEnabled; + +static uint8_t mixerUsesServos; +static uint8_t minServoIndex; +static uint8_t maxServoIndex; + +static servoParam_t *servoConf; +static biquadFilter_t servoFitlerState[MAX_SUPPORTED_SERVOS]; +static bool servoFilterIsSet; + +#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 servoMixerTri[] = { + { SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0, 0, 100, 0 }, +}; + +const mixerRules_t servoMixers[] = { + { 0, 0, 0, NULL }, // entry 0 + { COUNT_SERVO_RULES(servoMixerTri), SERVO_RUDDER, SERVO_RUDDER, servoMixerTri }, // MULTITYPE_TRI + { 0, 0, 0, NULL }, // MULTITYPE_QUADP + { 0, 0, 0, NULL }, // MULTITYPE_QUADX + { 0, 0, 0, NULL }, // MULTITYPE_BI + { 0, 0, 0, NULL }, // MULTITYPE_GIMBAL / MIXER_GIMBAL -> disabled + { 0, 0, 0, NULL }, // MULTITYPE_Y6 + { 0, 0, 0, NULL }, // MULTITYPE_HEX6 + { COUNT_SERVO_RULES(servoMixerFlyingWing), SERVO_FLAPPERONS_MIN, SERVO_FLAPPERONS_MAX, servoMixerFlyingWing },// * MULTITYPE_FLYING_WING + { 0, 0, 0, NULL }, // MULTITYPE_Y4 + { 0, 0, 0, NULL }, // MULTITYPE_HEX6X + { 0, 0, 0, NULL }, // MULTITYPE_OCTOX8 + { 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATP + { 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATX + { COUNT_SERVO_RULES(servoMixerAirplane), SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, servoMixerAirplane }, // * MULTITYPE_AIRPLANE + { 0, 0, 0, NULL }, // * MIXER_HELI_120_CCPM -> disabled, never fully implemented in CF + { 0, 0, 0, NULL }, // * MIXER_HELI_90_DEG -> disabled, never fully implemented in CF + { 0, 0, 0, NULL }, // MULTITYPE_VTAIL4 + { 0, 0, 0, NULL }, // MULTITYPE_HEX6H + { 0, 0, 0, NULL }, // * MULTITYPE_PPM_TO_SERVO + { 0, 0, 0, NULL }, // MULTITYPE_DUALCOPTER + { 0, 0, 0, NULL }, // MULTITYPE_SINGLECOPTER + { 0, 0, 0, NULL }, // MULTITYPE_ATAIL4 + { 0, 2, 5, NULL }, // MULTITYPE_CUSTOM + { 0, SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, NULL }, // MULTITYPE_CUSTOM_PLANE + { 0, SERVO_RUDDER, SERVO_RUDDER, NULL }, // MULTITYPE_CUSTOM_TRI +}; + +static servoMixer_t *customServoMixers; + +void servosUseConfigs(servoConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, gimbalConfig_t *gimbalConfigToUse) +{ + servoConfig = servoConfigToUse; + servoConf = servoParamsToUse; + gimbalConfig = gimbalConfigToUse; +} + +int16_t getFlaperonDirection(uint8_t servoPin) { + if (servoPin == SERVO_FLAPPERON_2) { + return -1; + } else { + return 1; + } +} + +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 servosInit(servoMixer_t *initialCustomServoMixers) +{ + int 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); + } else { + DISABLE_STATE(FIXED_WING); + } + + if (currentMixerMode == MIXER_AIRPLANE || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { + ENABLE_STATE(FLAPERON_AVAILABLE); + } else { + DISABLE_STATE(FLAPERON_AVAILABLE); + } + + customServoMixers = initialCustomServoMixers; + + minServoIndex = servoMixers[currentMixerMode].minServoIndex; + maxServoIndex = servoMixers[currentMixerMode].maxServoIndex; + + // enable servos for mixes that require them. note, this shifts motor counts. + mixerUsesServos = mixers[currentMixerMode].useServo || feature(FEATURE_SERVO_TILT); + + // if we want camstab/trig, that also enables servos, even if mixer doesn't + servoOutputEnabled = mixerUsesServos || feature(FEATURE_CHANNEL_FORWARDING); + + // give all servos a default command + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servo[i] = DEFAULT_SERVO_MIDDLE; + } + + /* + * If mixer has predefined servo mixes, load them + */ + if (mixerUsesServos) { + servoRuleCount = servoMixers[currentMixerMode].servoRuleCount; + if (servoMixers[currentMixerMode].rule) { + for (i = 0; i < servoRuleCount; i++) + currentServoMixer[i] = servoMixers[currentMixerMode].rule[i]; + } + } + + /* + * When we are dealing with CUSTOM mixers, load mixes defined with + * smix and update internal variables + */ + if (currentMixerMode == MIXER_CUSTOM_AIRPLANE || + currentMixerMode == MIXER_CUSTOM_TRI || + currentMixerMode == MIXER_CUSTOM) + { + loadCustomServoMixer(); + + // If there are servo rules after all, update variables + if (servoRuleCount > 0) { + servoOutputEnabled = 1; + mixerUsesServos = 1; + } + + } +} +void loadCustomServoMixer(void) +{ + int i; + + // reset settings + servoRuleCount = 0; + minServoIndex = 255; + maxServoIndex = 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; + + if (customServoMixers[i].targetChannel < minServoIndex) { + minServoIndex = customServoMixers[i].targetChannel; + } + + if (customServoMixers[i].targetChannel > maxServoIndex) { + maxServoIndex = customServoMixers[i].targetChannel; + } + + currentServoMixer[i] = customServoMixers[i]; + servoRuleCount++; + } +} + +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; + + int servoOffset; + for (servoOffset = 0; servoOffset < MAX_AUX_CHANNEL_COUNT && channelOffset < MAX_SUPPORTED_RC_CHANNEL_COUNT; servoOffset++) { + pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]); + } +} + +void writeServos(void) +{ + int servoIndex = 0; + + bool zeroServoValue = false; + + /* + * in case of tricopters, there might me a need to zero servo output when unarmed + */ + if ((currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI) && !ARMING_FLAG(ARMED) && !servoConfig->tri_unarmed_servo) { + zeroServoValue = true; + } + + // Write mixer servo outputs + // mixerUsesServos might indicate SERVO_TILT, servoRuleCount indicate if mixer alone uses servos + if (mixerUsesServos && servoRuleCount) { + for (int i = minServoIndex; i <= maxServoIndex; i++) { + if (zeroServoValue) { + pwmWriteServo(servoIndex++, 0); + } else { + pwmWriteServo(servoIndex++, servo[i]); + } + } + } + + if (feature(FEATURE_SERVO_TILT)) { + pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_PITCH]); + pwmWriteServo(servoIndex++, servo[SERVO_GIMBAL_ROLL]); + } + + // forward AUX to remaining servo outputs (not constrained) + if (feature(FEATURE_CHANNEL_FORWARDING)) { + forwardAuxChannelsToServos(servoIndex); + servoIndex += MAX_AUX_CHANNEL_COUNT; + } +} + +void servoMixer(uint16_t flaperon_throw_offset, uint8_t flaperon_throw_inverted) +{ + int16_t input[INPUT_SOURCE_COUNT]; // Range [-500:+500] + static int16_t currentOutput[MAX_SERVO_RULES]; + int 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]); + } + } + + /* + Flaperon fligh mode + */ + if (FLIGHT_MODE(FLAPERON) && (target == SERVO_FLAPPERON_1 || target == SERVO_FLAPPERON_2)) { + int8_t multiplier = 1; + + if (flaperon_throw_inverted == 1) { + multiplier = -1; + } + currentOutput[i] += flaperon_throw_offset * getFlaperonDirection(target) * multiplier; + } + + 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 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; + } + } +} + +bool isServoOutputEnabled(void) +{ + return servoOutputEnabled; +} + +bool isMixerUsingServos(void) { + return mixerUsesServos; +} + +void filterServos(void) +{ + int servoIdx; + + if (servoConfig->servo_lowpass_enable) { + // Initialize servo lowpass filter (servos are calculated at looptime rate) + if (!servoFilterIsSet) { + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + biquadFilterInit(&servoFitlerState[servoIdx], servoConfig->servo_lowpass_freq, gyro.targetLooptime); + } + + servoFilterIsSet = true; + } + + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + // Apply servo lowpass filter and do sanity cheching + servo[servoIdx] = (int16_t) biquadFilterApply(&servoFitlerState[servoIdx], (float)servo[servoIdx]); + } + } + + for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { + servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max); + } +} + +#endif diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h new file mode 100644 index 0000000000..cc8fd26e56 --- /dev/null +++ b/src/main/flight/servos.h @@ -0,0 +1,132 @@ +/* + * 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 + +// 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_ELEVATOR = 2, + SERVO_FLAPPERON_1 = 3, + SERVO_FLAPPERON_2 = 4, + SERVO_RUDDER = 5, + SERVO_THROTTLE = 6, // for internal combustion (IC) planes + SERVO_FLAPS = 7, + + 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_ELEVATOR +#define SERVO_PLANE_INDEX_MAX SERVO_FLAPS + +#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 + +#define FLAPERON_THROW_DEFAULT 250 +#define FLAPERON_THROW_MIN 100 +#define FLAPERON_THROW_MAX 400 + +typedef struct servoConfig_s { + uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed + int16_t servo_lowpass_freq; // lowpass servo filter frequency selection; 1/1000ths of loop freq + int8_t servo_lowpass_enable; // enable/disable lowpass filter +} servoConfig_t; + +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; + uint8_t minServoIndex; + uint8_t maxServoIndex; + 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; + +extern int16_t servo[MAX_SUPPORTED_SERVOS]; + +bool isServoOutputEnabled(void); +bool isMixerUsingServos(void); +void writeServos(void); +void filterServos(void); +void servoMixerLoadMix(int index, servoMixer_t *customServoMixers); +void loadCustomServoMixer(void); +int servoDirection(int servoIndex, int fromChannel); +void servosUseConfigs(servoConfig_t *servoConfigToUse, servoParam_t *servoParamsToUse, gimbalConfig_t *gimbalConfigToUse); +void servosInit(servoMixer_t *customServoMixers); + diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index fc05ab1cec..d1627986f7 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -64,6 +64,7 @@ #include "flight/failsafe.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/navigation_rewrite.h" diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 4e657b3e97..acbafe8c82 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -80,6 +80,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/navigation_rewrite.h" #include "flight/failsafe.h" @@ -752,9 +753,9 @@ const clivalue_t valueTable[] = { #ifdef USE_SERVOS { "flaperon_throw_offset", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].flaperon_throw_offset, .config.minmax = { FLAPERON_THROW_MIN, FLAPERON_THROW_MAX}, 0 }, { "flaperon_throw_inverted", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].flaperon_throw_inverted, .config.lookup = { TABLE_OFF_ON }, 0 }, - { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON }, 0 }, - { "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.servo_lowpass_freq, .config.minmax = { 10, 400}, 0 }, - { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON }, 0 }, + { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoConfig.tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON }, 0 }, + { "servo_lowpass_freq", VAR_INT16 | MASTER_VALUE, &masterConfig.servoConfig.servo_lowpass_freq, .config.minmax = { 10, 400}, 0 }, + { "servo_lowpass_enable", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.servoConfig.servo_lowpass_enable, .config.lookup = { TABLE_OFF_ON }, 0 }, { "gimbal_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].gimbalConfig.mode, .config.lookup = { TABLE_GIMBAL_MODE }, 0 }, { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &masterConfig.escAndServoConfig.servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX }, 0 }, { "servo_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.servo_pwm_rate, .config.minmax = { 50, 498 }, 0 }, diff --git a/src/main/main.c b/src/main/main.c index 7c163c5a0f..7f7fc680bd 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -100,6 +100,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/failsafe.h" #include "flight/navigation_rewrite.h" diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index 443763440f..ed2c4cc1d4 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -60,6 +60,7 @@ #include "telemetry/telemetry.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/failsafe.h" diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index ac48e9caf0..1e10aa66e5 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -59,6 +59,7 @@ #include "rx/rx.h" #include "flight/mixer.h" +#include "flight/servos.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h"