diff --git a/Makefile b/Makefile
index e5c2cf45f3..b78f00f8ab 100644
--- a/Makefile
+++ b/Makefile
@@ -404,6 +404,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_eeprom.c b/src/main/config/config_eeprom.c
index 8e6f8f1c52..f806a91a86 100755
--- a/src/main/config/config_eeprom.c
+++ b/src/main/config/config_eeprom.c
@@ -56,6 +56,7 @@
#include "telemetry/telemetry.h"
#include "flight/mixer.h"
+#include "flight/servos.h"
#include "flight/pid.h"
#include "flight/failsafe.h"
#include "flight/navigation_rewrite.h"
diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h
index d13a0420d3..76e4d502f3 100644
--- a/src/main/config/config_master.h
+++ b/src/main/config/config_master.h
@@ -81,6 +81,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 f03a71a38f..bd8506526f 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..d67fbaffe3
--- /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++) {
+ biquadFilterInitLPF(&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 ebf563d2dd..5f6077aafa 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -81,6 +81,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"
@@ -754,9 +755,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 be87fc01f8..7fbc4b1f99 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"