diff --git a/src/build_config.c b/src/build_config.c index 9b8ab6f5da..a9c11a4717 100644 --- a/src/build_config.c +++ b/src/build_config.c @@ -17,11 +17,11 @@ #include "serial_common.h" -#if MAX_MOTORS != MAX_SUPPORTED_MOTORS +#if MAX_PWM_MOTORS != MAX_SUPPORTED_MOTORS #error Motor configuration mismatch #endif -#if MAX_SERVOS != MAX_SUPPORTED_SERVOS +#if MAX_PWM_SERVOS != MAX_SUPPORTED_SERVOS #error Servo configuration mismatch #endif diff --git a/src/drivers/pwm_common.c b/src/drivers/pwm_common.c index 2c260c890f..f9573cc976 100755 --- a/src/drivers/pwm_common.c +++ b/src/drivers/pwm_common.c @@ -65,10 +65,10 @@ enum { typedef void (* pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors -static pwmPortData_t pwmPorts[MAX_PORTS]; -static uint16_t captures[MAX_INPUTS]; -static pwmPortData_t *motors[MAX_MOTORS]; -static pwmPortData_t *servos[MAX_SERVOS]; +static pwmPortData_t pwmPorts[USABLE_TIMER_CHANNEL_COUNT]; +static uint16_t captures[MAX_PPM_AND_PWM_INPUTS]; +static pwmPortData_t *motors[MAX_PWM_MOTORS]; +static pwmPortData_t *servos[MAX_PWM_SERVOS]; static pwmWriteFuncPtr pwmWritePtr = NULL; static uint8_t numMotors = 0; static uint8_t numServos = 0; @@ -269,7 +269,7 @@ static void ppmCallback(uint8_t port, uint16_t capture) if (diff > 2700) { // Per http://www.rcgroups.com/forums/showpost.php?p=21996147&postcount=3960 "So, if you use 2.5ms or higher as being the reset for the PPM stream start, you will be fine. I use 2.7ms just to be safe." chan = 0; } else { - if (diff > 750 && diff < 2250 && chan < MAX_INPUTS) { // 750 to 2250 ms is our 'valid' channel range + if (diff > 750 && diff < 2250 && chan < MAX_PPM_AND_PWM_INPUTS) { // 750 to 2250 ms is our 'valid' channel range captures[chan] = diff; if (chan < 4 && diff > failsafeThreshold) GoodPulses |= (1 << chan); // if signal is valid - mark channel as OK @@ -330,7 +330,7 @@ void pwmInit(drv_pwm_config_t *init, failsafe_t *initialFailsafe) setup = hardwareMaps[i]; - for (i = 0; i < MAX_PORTS; i++) { + for (i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { uint8_t port = setup[i] & 0x0F; uint8_t mask = setup[i] & 0xF0; diff --git a/src/drivers/pwm_common.h b/src/drivers/pwm_common.h index 5e5babfa93..71f63f5a3a 100755 --- a/src/drivers/pwm_common.h +++ b/src/drivers/pwm_common.h @@ -1,8 +1,8 @@ #pragma once -#define MAX_MOTORS 12 -#define MAX_SERVOS 8 -#define MAX_INPUTS 8 +#define MAX_PWM_MOTORS 12 +#define MAX_PWM_SERVOS 8 +#define MAX_PPM_AND_PWM_INPUTS 8 #define PULSE_1MS (1000) // 1ms pulse width @@ -23,7 +23,7 @@ typedef struct drv_pwm_config_t { uint16_t failsafeThreshold; } drv_pwm_config_t; -// This indexes into the read-only hardware definition structure in drv_pwm.c, as well as into pwmPorts[] structure with dynamic data. +// This indexes into the read-only hardware definition structure, timerHardware_t, as well as into pwmPorts structure with dynamic data. enum { PWM1 = 0, PWM2, @@ -38,8 +38,7 @@ enum { PWM11, PWM12, PWM13, - PWM14, - MAX_PORTS + PWM14 }; void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity); diff --git a/src/drivers/timer_common.c b/src/drivers/timer_common.c index 369e8ea61f..2edd4fad84 100644 --- a/src/drivers/timer_common.c +++ b/src/drivers/timer_common.c @@ -49,7 +49,7 @@ TIM4 4 channels */ -const timerHardware_t timerHardware[] = { +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM2, GPIOA, Pin_0, TIM_Channel_1, TIM2_IRQn, 0, }, // PWM1 { TIM2, GPIOA, Pin_1, TIM_Channel_2, TIM2_IRQn, 0, }, // PWM2 { TIM2, GPIOA, Pin_2, TIM_Channel_3, TIM2_IRQn, 0, }, // PWM3 diff --git a/src/drivers/timer_common.h b/src/drivers/timer_common.h index c198b7d227..6b73163213 100644 --- a/src/drivers/timer_common.h +++ b/src/drivers/timer_common.h @@ -1,5 +1,7 @@ #pragma once +#define USABLE_TIMER_CHANNEL_COUNT 14 + typedef void timerCCCallbackPtr(uint8_t port, uint16_t capture); typedef struct { diff --git a/src/flight_mixer.c b/src/flight_mixer.c index b3e7ad0f2b..97808c71af 100755 --- a/src/flight_mixer.c +++ b/src/flight_mixer.c @@ -10,7 +10,7 @@ static uint8_t numberMotor = 0; int16_t motor[MAX_SUPPORTED_MOTORS]; int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; -int16_t servo[MAX_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; +int16_t servo[MAX_SUPPORTED_SERVOS] = { 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500 }; static int useServo; @@ -165,7 +165,7 @@ int16_t determineServoMiddleOrForwardFromChannel(int nr) return rcData[channelToForwardFrom]; } - if (nr < MAX_SERVOS) { + if (nr < MAX_SUPPORTED_SERVOS) { return currentProfile.servoConf[nr].middle; } @@ -480,7 +480,7 @@ void mixTable(void) } // constrain servos - for (i = 0; i < MAX_SERVOS; i++) + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) servo[i] = constrain(servo[i], currentProfile.servoConf[i].min, currentProfile.servoConf[i].max); // limit the values // forward AUX1-4 to servo outputs (not constrained) diff --git a/src/mw.h b/src/mw.h index 3d7108a4c7..956827bbe1 100755 --- a/src/mw.h +++ b/src/mw.h @@ -52,7 +52,7 @@ extern int32_t vario; extern int16_t throttleAngleCorrection; extern int16_t headFreeModeHold; extern int16_t motor[MAX_SUPPORTED_MOTORS]; -extern int16_t servo[MAX_SERVOS]; +extern int16_t servo[MAX_SUPPORTED_SERVOS]; extern uint16_t rssi; // range: [0;1023] extern int16_t telemTemperature1; // gyro sensor temperature extern uint8_t toggleBeep; diff --git a/src/serial_msp.c b/src/serial_msp.c index 0aead53c35..5d50ef1b5e 100755 --- a/src/serial_msp.c +++ b/src/serial_msp.c @@ -425,7 +425,7 @@ static void evaluateCommand(void) break; case MSP_SERVO_CONF: headSerialReply(56); - for (i = 0; i < MAX_SERVOS; i++) { + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { serialize16(currentProfile.servoConf[i].min); serialize16(currentProfile.servoConf[i].max); serialize16(currentProfile.servoConf[i].middle); @@ -434,12 +434,12 @@ static void evaluateCommand(void) break; case MSP_SET_SERVO_CONF: headSerialReply(0); - for (i = 0; i < MAX_SERVOS; i++) { + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { currentProfile.servoConf[i].min = read16(); currentProfile.servoConf[i].max = read16(); // provide temporary support for old clients that try and send a channel index instead of a servo middle uint16_t potentialServoMiddleOrChannelToForward = read16(); - if (potentialServoMiddleOrChannelToForward < MAX_SERVOS) { + if (potentialServoMiddleOrChannelToForward < MAX_SUPPORTED_SERVOS) { currentProfile.servoConf[i].forwardFromChannel = potentialServoMiddleOrChannelToForward; } if (potentialServoMiddleOrChannelToForward >= PWM_RANGE_MIN && potentialServoMiddleOrChannelToForward <= PWM_RANGE_MAX) { @@ -450,13 +450,13 @@ static void evaluateCommand(void) break; case MSP_CHANNEL_FORWARDING: headSerialReply(8); - for (i = 0; i < MAX_SERVOS; i++) { + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { serialize8(currentProfile.servoConf[i].forwardFromChannel); } break; case MSP_SET_CHANNEL_FORWARDING: headSerialReply(0); - for (i = 0; i < MAX_SERVOS; i++) { + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { currentProfile.servoConf[i].forwardFromChannel = read8(); } break;