diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index d1e11a8a52..6a96005a36 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -17,7 +17,7 @@ #include #include - +#include #include #include "platform.h" @@ -143,14 +143,16 @@ static const uint16_t * const hardwareMaps[] = { airPPM, }; -void pwmInit(drv_pwm_config_t *init) +pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) { int i = 0; const uint16_t *setup; int channelIndex = 0; - int servoIndex = 0; - int motorIndex = 0; + + static pwmOutputConfiguration_t pwmOutputConfiguration; + + memset(&pwmOutputConfiguration, 0, sizeof(pwmOutputConfiguration)); // this is pretty hacky shit, but it will do for now. array of 4 config maps, [ multiPWM multiPPM airPWM airPPM ] if (init->airplane) @@ -265,14 +267,16 @@ void pwmInit(drv_pwm_config_t *init) channelIndex++; } else if (type == TYPE_M) { if (init->motorPwmRate > 500) { - pwmBrushedMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse); + pwmBrushedMotorConfig(&timerHardware[timerIndex], pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); } else { - pwmBrushlessMotorConfig(&timerHardware[timerIndex], motorIndex, init->motorPwmRate, init->idlePulse); + pwmBrushlessMotorConfig(&timerHardware[timerIndex], pwmOutputConfiguration.motorCount, init->motorPwmRate, init->idlePulse); } - motorIndex++; + pwmOutputConfiguration.motorCount++; } else if (type == TYPE_S) { - pwmServoConfig(&timerHardware[timerIndex], servoIndex, init->servoPwmRate, init->servoCenterPulse); - servoIndex++; + pwmServoConfig(&timerHardware[timerIndex], pwmOutputConfiguration.servoCount, init->servoPwmRate, init->servoCenterPulse); + pwmOutputConfiguration.servoCount++; } } + + return &pwmOutputConfiguration; } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 100901f42b..4ef366556e 100755 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -54,6 +54,12 @@ typedef struct drv_pwm_config_t { uint16_t servoCenterPulse; } drv_pwm_config_t; + +typedef struct pwmOutputConfiguration_s { + uint8_t servoCount; + uint8_t motorCount; +} pwmOutputConfiguration_t; + // This indexes into the read-only hardware definition structure, timerHardware_t, as well as into pwmPorts structure with dynamic data. enum { PWM1 = 0, diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index cf3404905a..6569d296ce 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -27,6 +27,7 @@ #include "drivers/gpio.h" #include "drivers/timer.h" #include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" #include "rx/rx.h" #include "io/gimbal.h" @@ -39,6 +40,7 @@ #include "config/runtime_config.h" #include "config/config.h" +#define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4 static uint8_t numberMotor = 0; int16_t motor[MAX_SUPPORTED_MOTORS]; @@ -47,6 +49,8 @@ int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500 static int useServo; +static uint8_t servoCount; + static servoParam_t *servoConf; static mixerConfig_t *mixerConfig; static flight3DConfig_t *flight3DConfig; @@ -239,12 +243,14 @@ int servoDirection(int nr, int lr) return 1; } -void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers) +void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers, pwmOutputConfiguration_t *pwmOutputConfiguration) { int i; currentMixerConfiguration = mixerConfiguration; + servoCount = pwmOutputConfiguration->servoCount; + // enable servos for mixes that require them. note, this shifts motor counts. useServo = mixers[mixerConfiguration].useServo; // if we want camstab/trig, that also enables servos, even if mixer doesn't @@ -536,15 +542,20 @@ void mixTable(void) // forward AUX1-4 to servo outputs (not constrained) if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) { - int offset = 0; // offset servos based off number already used in mixer types // airplane and servo_tilt together can't be used - if (currentMixerConfiguration == MULTITYPE_AIRPLANE || currentMixerConfiguration == MULTITYPE_FLYING_WING) - offset = 4; - else if (mixers[currentMixerConfiguration].useServo) - offset = 2; - for (i = 0; i < 4; i++) - pwmWriteServo(i + offset, rcData[AUX1 + i]); + int8_t firstServo = servoCount - AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; + + // start forwarding from this channel + uint8_t channelOffset = AUX1; + + int8_t servoOffset; + for (servoOffset = 0; servoOffset < AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; servoOffset++) { + if (firstServo + servoOffset < 0) { + continue; // there are not enough servos to forward all the AUX channels. + } + pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]); + } } maxMotor = motor[0]; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 4b4ce7b635..285ed46489 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -94,7 +94,6 @@ extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int16_t servo[MAX_SUPPORTED_SERVOS]; bool isMixerUsingServos(void); -void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers); void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); void mixerResetMotors(void); diff --git a/src/main/main.c b/src/main/main.c index ee7c4f84d1..071c955239 100755 --- a/src/main/main.c +++ b/src/main/main.c @@ -78,7 +78,8 @@ void timerInit(void); void initTelemetry(void); void serialInit(serialConfig_t *initialSerialConfig); failsafe_t* failsafeInit(rxConfig_t *intialRxConfig); -void pwmInit(drv_pwm_config_t *init); +pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init); +void mixerInit(MultiType mixerConfiguration, motorMixer_t *customMixers, pwmOutputConfiguration_t *pwmOutputConfiguration); void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe); void buzzerInit(failsafe_t *initialFailsafe); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig, gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); @@ -163,8 +164,6 @@ void init(void) compassInit(); #endif - mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer); - timerInit(); serialInit(&masterConfig.serialConfig); @@ -195,7 +194,9 @@ void init(void) pwm_params.idlePulse = 0; // brushed motors pwm_params.servoCenterPulse = masterConfig.rxConfig.midrc; - pwmInit(&pwm_params); + pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); + + mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer, pwmOutputConfiguration); failsafe = failsafeInit(&masterConfig.rxConfig); buzzerInit(failsafe);