diff --git a/Makefile b/Makefile index 0316ae1c7..4117bbda9 100644 --- a/Makefile +++ b/Makefile @@ -421,7 +421,6 @@ COMMON_SRC = \ drivers/rx_nrf24l01.c \ drivers/rx_spi.c \ drivers/rx_xn297.c \ - drivers/pwm_mapping.c \ drivers/pwm_output.c \ drivers/pwm_rx.c \ drivers/rcc.c \ diff --git a/src/main/build/build_config.c b/src/main/build/build_config.c index bd1352fd4..6c876a4ed 100644 --- a/src/main/build/build_config.c +++ b/src/main/build/build_config.c @@ -22,15 +22,6 @@ #include "drivers/gpio.h" #include "drivers/timer.h" -#include "drivers/pwm_mapping.h" #include "flight/mixer.h" #include "build_config.h" - -#if MAX_PWM_MOTORS != MAX_SUPPORTED_MOTORS -#error Motor configuration mismatch -#endif - -#if MAX_PWM_SERVOS != MAX_SUPPORTED_SERVOS -#error Servo configuration mismatch -#endif diff --git a/src/main/config/config.c b/src/main/config/config.c index 0f7534b81..0b67ef60f 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -41,6 +41,7 @@ #include "drivers/serial.h" #include "drivers/pwm_output.h" #include "drivers/max7456.h" +#include "drivers/sound_beeper.h" #include "sensors/sensors.h" #include "sensors/gyro.h" @@ -243,6 +244,14 @@ void resetSensorAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig) sensorAlignmentConfig->mag_align = ALIGN_DEFAULT; } +#ifdef USE_SERVOS +void resetServoConfig(servoConfig_t *servoConfig) +{ + servoConfig->servoCenterPulse = 1500; + servoConfig->servoPwmRate = 50; +} +#endif + void resetMotorConfig(motorConfig_t *motorConfig) { #ifdef BRUSHED_MOTORS @@ -257,15 +266,60 @@ void resetMotorConfig(motorConfig_t *motorConfig) #endif motorConfig->maxthrottle = 2000; motorConfig->mincommand = 1000; - motorConfig->maxEscThrottleJumpMs = 0; + uint8_t motorIndex = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < MAX_SUPPORTED_MOTORS; i++) { + if ((timerHardware[i].output & TIMER_OUTPUT_ENABLED) == TIMER_OUTPUT_ENABLED) { + motorConfig->ioTags[motorIndex] = timerHardware[i].tag; + motorIndex++; + } + } } -#ifdef USE_SERVOS -void resetServoConfig(servoConfig_t *servoConfig) +#ifdef SONAR +void resetSonarConfig(sonarConfig_t *sonarConfig) { - servoConfig->servoCenterPulse = 1500; - servoConfig->servoPwmRate = 50; +#if defined(SONAR_TRIGGER_PIN) && defined(SONAR_ECHO_PIN) + sonarConfig->triggerTag = IO_TAG(SONAR_TRIGGER_PIN); + sonarConfig->echoTag = IO_TAG(SONAR_ECHO_PIN); +#else +#error Sonar not defined for target +#endif +} +#endif + +#ifdef BEEPER +void resetBeeperConfig(beeperConfig_t *beeperConfig) +{ +#ifdef BEEPER_INVERTED + beeperConfig->isInverted = true; +#else + beeperConfig->isInverted = false; +#endif + beeperConfig->isOD = false; + beeperConfig->ioTag = IO_TAG(BEEPER); +} +#endif + +#ifndef SKIP_RX_PWM_PPM +void resetPpmConfig(ppmConfig_t *ppmConfig) +{ +#ifdef PPM_PIN + ppmConfig->ioTag = IO_TAG(PPM_PIN); +#else + ppmConfig->ioTag = IO_TAG_NONE; +#endif +} + +void resetPwmConfig(pwmConfig_t *pwmConfig) +{ + uint8_t inputIndex = 0; + for (int i = 0; i < USABLE_TIMER_CHANNEL_COUNT && i < PWM_INPUT_PORT_COUNT; i++) { + if ((timerHardware[i].output == TIMER_INPUT_ENABLED)) { + pwmConfig->ioTags[inputIndex] = timerHardware[i].tag; + inputIndex++; + } + } } #endif @@ -461,10 +515,23 @@ void createDefaultConfig(master_t *config) resetBatteryConfig(&config->batteryConfig); +#ifndef SKIP_RX_PWM_PPM + resetPpmConfig(&config->ppmConfig); + resetPwmConfig(&config->pwmConfig); +#endif + #ifdef TELEMETRY resetTelemetryConfig(&config->telemetryConfig); #endif +#ifdef BEEPER + resetBeeperConfig(&config->beeperConfig); +#endif + +#ifdef SONAR + resetSonarConfig(&config->sonarConfig); +#endif + #ifdef SERIALRX_PROVIDER config->rxConfig.serialrx_provider = SERIALRX_PROVIDER; #else @@ -515,10 +582,6 @@ void createDefaultConfig(master_t *config) #endif resetFlight3DConfig(&config->flight3DConfig); -#ifdef CC3D - config->use_buzzer_p6 = 0; -#endif - #ifdef GPS // gps/nav stuff config->gpsConfig.provider = GPS_NMEA; diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 59d5a39fe..64132e301 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -22,6 +22,8 @@ #include "config/config_profile.h" #include "drivers/pwm_rx.h" +#include "drivers/sound_beeper.h" +#include "drivers/sonar_hcsr04.h" #include "fc/rc_controls.h" @@ -74,10 +76,6 @@ typedef struct master_s { gimbalConfig_t gimbalConfig; #endif -#ifdef CC3D - uint8_t use_buzzer_p6; -#endif - // global sensor-related stuff sensorAlignmentConfig_t sensorAlignmentConfig; boardAlignment_t boardAlignment; @@ -147,6 +145,19 @@ typedef struct master_s { serialConfig_t serialConfig; telemetryConfig_t telemetryConfig; +#ifndef SKIP_RX_PWM_PPM + ppmConfig_t ppmConfig; + pwmConfig_t pwmConfig; +#endif + +#ifdef BEEPER + beeperConfig_t beeperConfig; +#endif + +#ifdef SONAR + sonarConfig_t sonarConfig; +#endif + #ifdef LED_STRIP ledConfig_t ledConfigs[LED_MAX_STRIP_LENGTH]; hsvColor_t colors[LED_CONFIGURABLE_COLOR_COUNT]; diff --git a/src/main/drivers/io.c b/src/main/drivers/io.c index d371d38e5..25ceb82ed 100644 --- a/src/main/drivers/io.c +++ b/src/main/drivers/io.c @@ -55,8 +55,8 @@ const struct ioPortDef_s ioPortDefs[] = { const char * const ownerNames[OWNER_TOTAL_COUNT] = { "FREE", "PWM", "PPM", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "DEBUG", "TIMER", - "SONAR", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", - "BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER" + "SONAR_TRIGGER", "SONAR_ECHO", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD", + "BARO", "MPU", "INVERTER", "LED_STRIP", "LED", "RX", "TX", "SOFT_SPI", "RX_SPI" }; const char * const resourceNames[RESOURCE_TOTAL_COUNT] = { diff --git a/src/main/drivers/io_types.h b/src/main/drivers/io_types.h index ba80cd3bc..78a3158e3 100644 --- a/src/main/drivers/io_types.h +++ b/src/main/drivers/io_types.h @@ -8,7 +8,7 @@ typedef uint8_t ioTag_t; // packet tag to specify IO pin typedef void* IO_t; // type specifying IO pin. Currently ioRec_t pointer, but this may change // NONE initializer for ioTag_t variables -#define IOTAG_NONE ((ioTag_t)0) +#define IO_TAG_NONE ((ioTag_t)0) // NONE initializer for IO_t variable #define IO_NONE ((IO_t)0) diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 5603ce072..bc77455e6 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -23,35 +23,19 @@ #include "io.h" #include "timer.h" -#include "pwm_mapping.h" #include "pwm_output.h" #define MULTISHOT_5US_PW (MULTISHOT_TIMER_MHZ * 5) #define MULTISHOT_20US_MULT (MULTISHOT_TIMER_MHZ * 20 / 1000.0f) - -typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors - -typedef struct { - volatile timCCR_t *ccr; - TIM_TypeDef *tim; - uint16_t period; - pwmWriteFuncPtr pwmWritePtr; -} pwmOutputPort_t; - -static pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; - -static pwmOutputPort_t *motors[MAX_PWM_MOTORS]; +static pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; #ifdef USE_SERVOS -static pwmOutputPort_t *servos[MAX_PWM_SERVOS]; +static pwmOutputPort_t servos[MAX_SUPPORTED_SERVOS]; #endif -static uint8_t allocatedOutputPortCount = 0; - static bool pwmMotorsEnabled = true; - static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) { TIM_OCInitTypeDef TIM_OCInitStructure; @@ -89,16 +73,9 @@ static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8 } } -static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) +static void pwmOutConfig(pwmOutputPort_t *port, const timerHardware_t *timerHardware, uint8_t mhz, uint16_t period, uint16_t value) { - pwmOutputPort_t *p = &pwmOutputPorts[allocatedOutputPortCount++]; - configTimeBase(timerHardware->tim, period, mhz); - - const IO_t io = IOGetByTag(timerHardware->tag); - IOInit(io, OWNER_MOTOR, RESOURCE_OUTPUT, allocatedOutputPortCount); - IOConfigGPIO(io, IOCFG_AF_PP); - pwmOCConfig(timerHardware->tim, timerHardware->channel, value, timerHardware->output); if (timerHardware->output & TIMER_OUTPUT_ENABLED) { @@ -108,55 +85,53 @@ static pwmOutputPort_t *pwmOutConfig(const timerHardware_t *timerHardware, uint8 switch (timerHardware->channel) { case TIM_Channel_1: - p->ccr = &timerHardware->tim->CCR1; + port->ccr = &timerHardware->tim->CCR1; break; case TIM_Channel_2: - p->ccr = &timerHardware->tim->CCR2; + port->ccr = &timerHardware->tim->CCR2; break; case TIM_Channel_3: - p->ccr = &timerHardware->tim->CCR3; + port->ccr = &timerHardware->tim->CCR3; break; case TIM_Channel_4: - p->ccr = &timerHardware->tim->CCR4; + port->ccr = &timerHardware->tim->CCR4; break; } - p->period = period; - p->tim = timerHardware->tim; + port->period = period; + port->tim = timerHardware->tim; - *p->ccr = 0; - - return p; + *port->ccr = 0; } static void pwmWriteBrushed(uint8_t index, uint16_t value) { - *motors[index]->ccr = (value - 1000) * motors[index]->period / 1000; + *motors[index].ccr = (value - 1000) * motors[index].period / 1000; } static void pwmWriteStandard(uint8_t index, uint16_t value) { - *motors[index]->ccr = value; + *motors[index].ccr = value; } static void pwmWriteOneShot125(uint8_t index, uint16_t value) { - *motors[index]->ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); + *motors[index].ccr = lrintf((float)(value * ONESHOT125_TIMER_MHZ/8.0f)); } static void pwmWriteOneShot42(uint8_t index, uint16_t value) { - *motors[index]->ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); + *motors[index].ccr = lrintf((float)(value * ONESHOT42_TIMER_MHZ/24.0f)); } static void pwmWriteMultiShot(uint8_t index, uint16_t value) { - *motors[index]->ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); + *motors[index].ccr = lrintf(((float)(value-1000) * MULTISHOT_20US_MULT) + MULTISHOT_5US_PW); } void pwmWriteMotor(uint8_t index, uint16_t value) { - if (motors[index] && index < MAX_MOTORS && pwmMotorsEnabled) { - motors[index]->pwmWritePtr(index, value); + if (index < MAX_SUPPORTED_MOTORS && pwmMotorsEnabled && motors[index].pwmWritePtr) { + motors[index].pwmWritePtr(index, value); } } @@ -164,7 +139,7 @@ void pwmShutdownPulsesForAllMotors(uint8_t motorCount) { for (int index = 0; index < motorCount; index++) { // Set the compare register to 0, which stops the output pulsing if the timer overflows - *motors[index]->ccr = 0; + *motors[index].ccr = 0; } } @@ -184,40 +159,26 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) bool overflowed = false; // If we have not already overflowed this timer for (int j = 0; j < index; j++) { - if (motors[j]->tim == motors[index]->tim) { + if (motors[j].tim == motors[index].tim) { overflowed = true; break; } } if (!overflowed) { - timerForceOverflow(motors[index]->tim); + timerForceOverflow(motors[index].tim); } // Set the compare register to 0, which stops the output pulsing if the timer overflows before the main loop completes again. // This compare register will be set to the output value on the next main loop. - *motors[index]->ccr = 0; + *motors[index].ccr = 0; } } -void pwmBrushedMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate) -{ - const uint32_t hz = PWM_BRUSHED_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, PWM_BRUSHED_TIMER_MHZ, hz / motorPwmRate, 0); - motors[motorIndex]->pwmWritePtr = pwmWriteBrushed; -} - -void pwmBrushlessMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse) -{ - const uint32_t hz = PWM_TIMER_MHZ * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, hz / motorPwmRate, idlePulse); - motors[motorIndex]->pwmWritePtr = pwmWriteStandard; -} - -void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType) +void motorInit(motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount) { uint32_t timerMhzCounter; pwmWriteFuncPtr pwmWritePtr; - - switch (fastPwmProtocolType) { + + switch (motorConfig->motorPwmProtocol) { default: case (PWM_TYPE_ONESHOT125): timerMhzCounter = ONESHOT125_TIMER_MHZ; @@ -231,27 +192,87 @@ void pwmFastPwmMotorConfig(const timerHardware_t *timerHardware, uint8_t motorIn timerMhzCounter = MULTISHOT_TIMER_MHZ; pwmWritePtr = pwmWriteMultiShot; break; + case (PWM_TYPE_BRUSHED): + timerMhzCounter = PWM_BRUSHED_TIMER_MHZ; + pwmWritePtr = pwmWriteBrushed; + motorConfig->useUnsyncedPwm = true; + idlePulse = 0; + break; + case (PWM_TYPE_STANDARD): + timerMhzCounter = PWM_TIMER_MHZ; + pwmWritePtr = pwmWriteStandard; + motorConfig->useUnsyncedPwm = true; + idlePulse = 0; + break; } + + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) { + ioTag_t tag = motorConfig->ioTags[motorIndex]; + + if (DEFIO_TAG_ISEMPTY(tag)) { + break; + } - if (motorPwmRate > 0) { - const uint32_t hz = timerMhzCounter * 1000000; - motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, hz / motorPwmRate, idlePulse); - } else { - motors[motorIndex] = pwmOutConfig(timerHardware, timerMhzCounter, 0xFFFF, 0); + motors[motorIndex].io = IOGetByTag(tag); + + IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_OUTPUT, RESOURCE_INDEX(motorIndex)); + IOConfigGPIO(motors[motorIndex].io, IOCFG_AF_PP); + + const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED); + + if (timer == NULL) { + /* flag failure and disable ability to arm */ + break; + } + + motors[motorIndex].pwmWritePtr = pwmWritePtr; + if (motorConfig->useUnsyncedPwm) { + const uint32_t hz = timerMhzCounter * 1000000; + pwmOutConfig(&motors[motorIndex], timer, timerMhzCounter, hz / motorConfig->motorPwmProtocol, idlePulse); + } else { + pwmOutConfig(&motors[motorIndex], timer, timerMhzCounter, 0xFFFF, 0); + } + motors[motorIndex].enabled = true; } - motors[motorIndex]->pwmWritePtr = pwmWritePtr; +} + +pwmOutputPort_t *pwmGetMotors() +{ + return motors; } #ifdef USE_SERVOS -void pwmServoConfig(const timerHardware_t *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse) -{ - servos[servoIndex] = pwmOutConfig(timerHardware, PWM_TIMER_MHZ, 1000000 / servoPwmRate, servoCenterPulse); -} - void pwmWriteServo(uint8_t index, uint16_t value) { - if (servos[index] && index < MAX_SERVOS) { - *servos[index]->ccr = value; + if (index < MAX_SUPPORTED_SERVOS && servos[index].ccr) { + *servos[index].ccr = value; } } + +void servoInit(servoConfig_t *servoConfig) +{ + for (uint8_t servoIndex = 0; servoIndex < MAX_SUPPORTED_SERVOS; servoIndex++) { + ioTag_t tag = servoConfig->ioTags[servoIndex]; + + if (DEFIO_TAG_ISEMPTY(tag)) { + break; + } + + servos[servoIndex].io = IOGetByTag(tag); + + IOInit(servos[servoIndex].io, OWNER_SERVO, RESOURCE_OUTPUT, RESOURCE_INDEX(servoIndex)); + IOConfigGPIO(servos[servoIndex].io, IOCFG_AF_PP); + + const timerHardware_t *timer = timerGetByTag(tag, TIMER_OUTPUT_ENABLED); + + if (timer == NULL) { + /* flag failure and disable ability to arm */ + break; + } + + pwmOutConfig(&servos[servoIndex], timer, PWM_TIMER_MHZ, 1000000 / servoConfig->servoPwmRate, servoConfig->servoCenterPulse); + servos[servoIndex].enabled = true; + } +} + #endif diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index b8443ac40..a2c20aa47 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -17,14 +17,20 @@ #pragma once +#include "io/motors.h" +#include "io/servos.h" +#include "drivers/timer.h" + typedef enum { - PWM_TYPE_CONVENTIONAL = 0, + PWM_TYPE_STANDARD = 0, PWM_TYPE_ONESHOT125, PWM_TYPE_ONESHOT42, PWM_TYPE_MULTISHOT, PWM_TYPE_BRUSHED } motorPwmProtocolTypes_e; +#define PWM_TIMER_MHZ 1 + #if defined(STM32F40_41xxx) // must be multiples of timer clock #define ONESHOT125_TIMER_MHZ 12 #define ONESHOT42_TIMER_MHZ 21 @@ -38,9 +44,20 @@ typedef enum { #endif struct timerHardware_s; -void pwmBrushedMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate); -void pwmBrushlessMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse); -void pwmFastPwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIndex, uint16_t motorPwmRate, uint16_t idlePulse, uint8_t fastPwmProtocolType); +typedef void(*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors + +typedef struct { + volatile timCCR_t *ccr; + TIM_TypeDef *tim; + uint16_t period; + pwmWriteFuncPtr pwmWritePtr; + bool enabled; + IO_t io; +} pwmOutputPort_t; + +void motorInit(motorConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount); +void servoInit(servoConfig_t *servoConfig); + void pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse); void pwmWriteMotor(uint8_t index, uint16_t value); @@ -49,5 +66,7 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount); void pwmWriteServo(uint8_t index, uint16_t value); +pwmOutputPort_t *pwmGetMotors(); void pwmDisableMotors(void); void pwmEnableMotors(void); + diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index 36a101d2c..791c5f25d 100644 --- a/src/main/drivers/pwm_rx.c +++ b/src/main/drivers/pwm_rx.c @@ -34,14 +34,11 @@ #include "timer.h" #include "pwm_output.h" -#include "pwm_mapping.h" - #include "pwm_rx.h" #define DEBUG_PPM_ISR #define PPM_CAPTURE_COUNT 12 -#define PWM_INPUT_PORT_COUNT 8 #if PPM_CAPTURE_COUNT > PWM_INPUT_PORT_COUNT #define PWM_PORTS_OR_PPM_CAPTURE_COUNT PPM_CAPTURE_COUNT @@ -127,7 +124,7 @@ void resetPPMDataReceivedState(void) #define MIN_CHANNELS_BEFORE_PPM_FRAME_CONSIDERED_VALID 4 -void pwmRxInit(inputFilteringMode_e initialInputFilteringMode) +void pwmRxSetInputFilteringMode(inputFilteringMode_e initialInputFilteringMode) { inputFilteringMode = initialInputFilteringMode; } @@ -157,7 +154,7 @@ void ppmISREvent(eventSource_e source, uint32_t capture) void ppmISREvent(eventSource_e source, uint32_t capture) {} #endif -static void ppmInit(void) +static void ppmResetDevice(void) { ppmDev.pulseIndex = 0; ppmDev.currentCapture = 0; @@ -357,35 +354,50 @@ void pwmICConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t polarity) TIM_ICInit(tim, &TIM_ICInitStructure); } -void pwmInConfig(const timerHardware_t *timerHardwarePtr, uint8_t channel) +void pwmRxInit(const pwmConfig_t *pwmConfig) { - pwmInputPort_t *self = &pwmInputPorts[channel]; + for (int channel = 0; channel < PWM_INPUT_PORT_COUNT; channel++) { + + pwmInputPort_t *port = &pwmInputPorts[channel]; - self->state = 0; - self->missedEvents = 0; - self->channel = channel; - self->mode = INPUT_MODE_PWM; - self->timerHardware = timerHardwarePtr; + const timerHardware_t *timer = timerGetByTag(pwmConfig->ioTags[channel], TIMER_INPUT_ENABLED); + + if (!timer) { + /* TODO: maybe fail here if not enough channels? */ + continue; + } + + port->state = 0; + port->missedEvents = 0; + port->channel = channel; + port->mode = INPUT_MODE_PWM; + port->timerHardware = timer; - IO_t io = IOGetByTag(timerHardwarePtr->tag); - IOInit(io, OWNER_PWMINPUT, RESOURCE_INPUT, RESOURCE_INDEX(channel)); - IOConfigGPIO(io, timerHardwarePtr->ioMode); + IO_t io = IOGetByTag(pwmConfig->ioTags[channel]); + IOInit(io, OWNER_PWMINPUT, RESOURCE_INPUT, RESOURCE_INDEX(channel)); + IOConfigGPIO(io, timer->ioMode); - pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); + pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising); - timerConfigure(timerHardwarePtr, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ); + timerConfigure(timer, (uint16_t)PWM_TIMER_PERIOD, PWM_TIMER_MHZ); - timerChCCHandlerInit(&self->edgeCb, pwmEdgeCallback); - timerChOvrHandlerInit(&self->overflowCb, pwmOverflowCallback); - timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb); + timerChCCHandlerInit(&port->edgeCb, pwmEdgeCallback); + timerChOvrHandlerInit(&port->overflowCb, pwmOverflowCallback); + timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb); + } } #define UNUSED_PPM_TIMER_REFERENCE 0 #define FIRST_PWM_PORT 0 -void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer, uint8_t pwmProtocol) +void ppmAvoidPWMTimerClash(TIM_TypeDef *pwmTimer, uint8_t pwmProtocol) { - if (timerHardwarePtr->tim == sharedPwmTimer) { + pwmOutputPort_t *motors = pwmGetMotors(); + for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS; motorIndex++) { + if (!motors[motorIndex].enabled || motors[motorIndex].tim != pwmTimer) { + continue; + } + switch (pwmProtocol) { case PWM_TYPE_ONESHOT125: @@ -401,29 +413,38 @@ void ppmAvoidPWMTimerClash(const timerHardware_t *timerHardwarePtr, TIM_TypeDef ppmCountDivisor = PWM_BRUSHED_TIMER_MHZ; break; } - } + return; + } } -void ppmInConfig(const timerHardware_t *timerHardwarePtr) +void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol) { - ppmInit(); + ppmResetDevice(); - pwmInputPort_t *self = &pwmInputPorts[FIRST_PWM_PORT]; + pwmInputPort_t *port = &pwmInputPorts[FIRST_PWM_PORT]; - self->mode = INPUT_MODE_PPM; - self->timerHardware = timerHardwarePtr; + const timerHardware_t *timer = timerGetByTag(ppmConfig->ioTag, TIMER_INPUT_ENABLED); + if (!timer) { + /* TODO: fail here? */ + return; + } + + ppmAvoidPWMTimerClash(timer->tim, pwmProtocol); + + port->mode = INPUT_MODE_PPM; + port->timerHardware = timer; - IO_t io = IOGetByTag(timerHardwarePtr->tag); + IO_t io = IOGetByTag(ppmConfig->ioTag); IOInit(io, OWNER_PPMINPUT, RESOURCE_INPUT, 0); - IOConfigGPIO(io, timerHardwarePtr->ioMode); + IOConfigGPIO(io, timer->ioMode); - pwmICConfig(timerHardwarePtr->tim, timerHardwarePtr->channel, TIM_ICPolarity_Rising); + pwmICConfig(timer->tim, timer->channel, TIM_ICPolarity_Rising); - timerConfigure(timerHardwarePtr, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ); + timerConfigure(timer, (uint16_t)PPM_TIMER_PERIOD, PWM_TIMER_MHZ); - timerChCCHandlerInit(&self->edgeCb, ppmEdgeCallback); - timerChOvrHandlerInit(&self->overflowCb, ppmOverflowCallback); - timerChConfigCallbacks(timerHardwarePtr, &self->edgeCb, &self->overflowCb); + timerChCCHandlerInit(&port->edgeCb, ppmEdgeCallback); + timerChOvrHandlerInit(&port->overflowCb, ppmOverflowCallback); + timerChConfigCallbacks(timer, &port->edgeCb, &port->overflowCb); } uint16_t ppmRead(uint8_t channel) diff --git a/src/main/drivers/pwm_rx.h b/src/main/drivers/pwm_rx.h index b30279617..bcb0b60ec 100644 --- a/src/main/drivers/pwm_rx.h +++ b/src/main/drivers/pwm_rx.h @@ -17,7 +17,7 @@ #pragma once -#include "drivers/timer.h" +#include "drivers/io.h" typedef enum { INPUT_FILTERING_DISABLED = 0, @@ -25,17 +25,25 @@ typedef enum { } inputFilteringMode_e; #define PPM_RCVR_TIMEOUT 0 +#define PWM_INPUT_PORT_COUNT 8 -void ppmInConfig(const struct timerHardware_s *timerHardwarePtr); -void ppmAvoidPWMTimerClash(const struct timerHardware_s *timerHardwarePtr, TIM_TypeDef *sharedPwmTimer, uint8_t pwmProtocol); +typedef struct ppmConfig_s { + ioTag_t ioTag; +} ppmConfig_t; + +typedef struct pwmConfig_s { + ioTag_t ioTags[PWM_INPUT_PORT_COUNT]; +} pwmConfig_t; + +void ppmRxInit(const ppmConfig_t *ppmConfig, uint8_t pwmProtocol); +void pwmRxInit(const pwmConfig_t *pwmConfig); -void pwmInConfig(const struct timerHardware_s *timerHardwarePtr, uint8_t channel); uint16_t pwmRead(uint8_t channel); uint16_t ppmRead(uint8_t channel); bool isPPMDataBeingReceived(void); void resetPPMDataReceivedState(void); -void pwmRxInit(inputFilteringMode_e initialInputFilteringMode); +void pwmRxSetInputFilteringMode(inputFilteringMode_e initialInputFilteringMode); bool isPWMDataBeingReceived(void); diff --git a/src/main/drivers/resource.h b/src/main/drivers/resource.h index 636f5db14..7d1649c5f 100644 --- a/src/main/drivers/resource.h +++ b/src/main/drivers/resource.h @@ -14,7 +14,8 @@ typedef enum { OWNER_SERIAL, OWNER_PINDEBUG, OWNER_TIMER, - OWNER_SONAR, + OWNER_SONAR_TRIGGER, + OWNER_SONAR_ECHO, OWNER_SYSTEM, OWNER_SPI, OWNER_I2C, diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index dd0e9b684..4f4581b49 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -40,7 +40,6 @@ #if defined(SONAR) STATIC_UNIT_TESTED volatile int32_t measurement = -1; static uint32_t lastMeasurementAt; -sonarHardware_t sonarHardwareHCSR04; extiCallbackRec_t hcsr04_extiCallbackRec; @@ -64,7 +63,7 @@ void hcsr04_extiHandler(extiCallbackRec_t* cb) } } -void hcsr04_init(sonarRange_t *sonarRange) +void hcsr04_init(sonarRange_t *sonarRange, sonarConfig_t *sonarConfig) { sonarRange->maxRangeCm = HCSR04_MAX_RANGE_CM; sonarRange->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES; @@ -83,13 +82,13 @@ void hcsr04_init(sonarRange_t *sonarRange) #endif // trigger pin - triggerIO = IOGetByTag(sonarHardwareHCSR04.triggerTag); - IOInit(triggerIO, OWNER_SONAR, RESOURCE_OUTPUT, 0); + triggerIO = IOGetByTag(sonarConfig->triggerTag); + IOInit(triggerIO, OWNER_SONAR_TRIGGER, RESOURCE_OUTPUT, 0); IOConfigGPIO(triggerIO, IOCFG_OUT_PP); // echo pin - echoIO = IOGetByTag(sonarHardwareHCSR04.echoTag); - IOInit(echoIO, OWNER_SONAR, RESOURCE_INPUT, 0); + echoIO = IOGetByTag(sonarConfig->echoTag); + IOInit(echoIO, OWNER_SONAR_ECHO, RESOURCE_INPUT, 0); IOConfigGPIO(echoIO, IOCFG_IN_FLOATING); #ifdef USE_EXTI diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index cb3f38c98..397803580 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -19,10 +19,10 @@ #include "io_types.h" -typedef struct sonarHardware_s { +typedef struct sonarConfig_s { ioTag_t triggerTag; ioTag_t echoTag; -} sonarHardware_t; +} sonarConfig_t; typedef struct sonarRange_s { int16_t maxRangeCm; @@ -37,6 +37,6 @@ typedef struct sonarRange_s { #define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet #define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well -void hcsr04_init(sonarRange_t *sonarRange); +void hcsr04_init(sonarRange_t *sonarRange, sonarConfig_t *sonarConfig); void hcsr04_start_reading(void); int32_t hcsr04_get_distance(void); diff --git a/src/main/drivers/sound_beeper.h b/src/main/drivers/sound_beeper.h index 9caad4f55..25bbad244 100644 --- a/src/main/drivers/sound_beeper.h +++ b/src/main/drivers/sound_beeper.h @@ -31,8 +31,8 @@ typedef struct beeperConfig_s { ioTag_t ioTag; - unsigned isInverted : 1; - unsigned isOD : 1; + uint8_t isInverted; + uint8_t isOD; } beeperConfig_t; void systemBeep(bool on); diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index da2889ac9..5b5267023 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -784,3 +784,18 @@ void timerForceOverflow(TIM_TypeDef *tim) tim->EGR |= TIM_EGR_UG; } } + +const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag) +{ + for (uint8_t i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) { + if (timerHardware[i].tag == tag) { + if (flag && (timerHardware[i].output & flag) == flag) { + return &timerHardware[i]; + } else if (!flag && timerHardware[i].output == flag) { + // TODO: shift flag by one so not to be 0 + return &timerHardware[i]; + } + } + } + return NULL; +} \ No newline at end of file diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 00410a9c6..5ffee4373 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -85,11 +85,12 @@ typedef struct timerHardware_s { #endif } timerHardware_t; -enum { - TIMER_OUTPUT_ENABLED = 0x01, - TIMER_OUTPUT_INVERTED = 0x02, - TIMER_OUTPUT_N_CHANNEL= 0x04 -}; +typedef enum { + TIMER_INPUT_ENABLED = 0x00, + TIMER_OUTPUT_ENABLED = 0x01, + TIMER_OUTPUT_INVERTED = 0x02, + TIMER_OUTPUT_N_CHANNEL = 0x04 +} timerFlag_e; #ifdef STM32F1 #if defined(STM32F10X_XL) || defined(STM32F10X_HD_VL) @@ -105,7 +106,6 @@ enum { #define HARDWARE_TIMER_DEFINITION_COUNT 14 #endif - extern const timerHardware_t timerHardware[]; extern const timerDef_t timerDefinitions[]; @@ -159,3 +159,5 @@ rccPeriphTag_t timerRCC(TIM_TypeDef *tim); #if defined(STM32F3) || defined(STM32F4) uint8_t timerGPIOAF(TIM_TypeDef *tim); #endif + +const timerHardware_t *timerGetByTag(ioTag_t tag, timerFlag_e flag); \ No newline at end of file diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 09fcbe1f9..77e627540 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -32,7 +32,6 @@ #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" @@ -68,7 +67,7 @@ static flight3DConfig_t *flight3DConfig; static motorConfig_t *motorConfig; static airplaneConfig_t *airplaneConfig; static rxConfig_t *rxConfig; -static bool syncPwmWithPidLoop = false; +static bool syncMotorOutputWithPidLoop = false; static mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; @@ -79,7 +78,6 @@ static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; static gimbalConfig_t *gimbalConfig; int16_t servo[MAX_SUPPORTED_SERVOS]; static int useServo; -STATIC_UNIT_TESTED uint8_t servoCount; static servoParam_t *servoConf; #endif @@ -378,7 +376,7 @@ int servoDirection(int servoIndex, int inputSource) return 1; } -void servoInit(servoMixer_t *initialCustomServoMixers) +void servoMixerInit(servoMixer_t *initialCustomServoMixers) { customServoMixers = initialCustomServoMixers; @@ -423,14 +421,13 @@ void loadCustomServoMixer(void) } } -void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm) +void mixerUsePWMOutputConfiguration(bool use_unsyncedPwm) { int i; motorCount = 0; - servoCount = pwmOutputConfiguration->servoCount; - syncPwmWithPidLoop = !use_unsyncedPwm; + syncMotorOutputWithPidLoop = !use_unsyncedPwm; if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { // load custom mixer into currentMixer @@ -524,16 +521,12 @@ void mixerLoadMix(int index, motorMixer_t *customMixers) #else -void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration, bool use_unsyncedPwm) +void mixerUsePWMOutputConfiguration(bool use_unsyncedPwm) { - UNUSED(pwmOutputConfiguration); - syncPwmWithPidLoop = !use_unsyncedPwm; + syncMotorOutputWithPidLoop = !use_unsyncedPwm; motorCount = 4; -#ifdef USE_SERVOS - servoCount = 0; -#endif uint8_t i; for (i = 0; i < motorCount; i++) { @@ -643,7 +636,7 @@ void writeMotors(void) for (i = 0; i < motorCount; i++) pwmWriteMotor(i, motor[i]); - if (syncPwmWithPidLoop) { + if (syncMotorOutputWithPidLoop) { pwmCompleteOneshotMotorUpdate(motorCount); } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 10ab08a08..a6a7cee6d 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -187,6 +187,7 @@ void writeServos(void); void filterServos(void); #endif +extern const mixer_t mixers[]; extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; struct motorConfig_s; @@ -202,7 +203,7 @@ void mixerUseConfigs( void writeAllMotors(int16_t mc); void mixerLoadMix(int index, motorMixer_t *customMixers); #ifdef USE_SERVOS -void servoInit(servoMixer_t *customServoMixers); +void servoMixerInit(servoMixer_t *customServoMixers); struct servoParam_s; struct gimbalConfig_s; void servoUseConfigs(struct servoParam_s *servoConfToUse, struct gimbalConfig_s *gimbalConfigToUse); @@ -211,8 +212,9 @@ void loadCustomServoMixer(void); int servoDirection(int servoIndex, int fromChannel); #endif void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers); -struct pwmOutputConfiguration_s; -void mixerUsePWMOutputConfiguration(struct pwmOutputConfiguration_s *pwmOutputConfiguration, bool use_unsyncedPwm); + +void mixerUsePWMOutputConfiguration(bool use_unsyncedPwm); + void mixerResetDisarmedMotors(void); void mixTable(void *pidProfilePtr); void syncMotors(bool enabled); diff --git a/src/main/io/motors.h b/src/main/io/motors.h index 04148a5fd..918c478f7 100644 --- a/src/main/io/motors.h +++ b/src/main/io/motors.h @@ -17,8 +17,10 @@ #pragma once +#include "drivers/io.h" +#include "flight/mixer.h" + typedef struct motorConfig_s { - // PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms) uint16_t minthrottle; // Set the minimum throttle command sent to the ESC (Electronic Speed Controller). This is the minimum value that allow motors to run at a idle speed. uint16_t maxthrottle; // This is the maximum value for the ESCs at full power this value can be increased up to 2000 uint16_t mincommand; // This is the value for the ESCs when they are not armed. In some cases, this value must be lowered down to 900 for some specific ESCs @@ -26,4 +28,5 @@ typedef struct motorConfig_s { uint16_t motorPwmRate; // The update rate of motor outputs (50-498Hz) uint8_t motorPwmProtocol; // Pwm Protocol uint8_t useUnsyncedPwm; -} motorConfig_t; + ioTag_t ioTags[MAX_SUPPORTED_MOTORS]; +} motorConfig_t; \ No newline at end of file diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index 0c4139074..712c5b47c 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -28,7 +28,6 @@ #include "drivers/io.h" #include "drivers/serial.h" #include "drivers/timer.h" -#include "drivers/pwm_mapping.h" #include "drivers/pwm_output.h" #include "drivers/light_led.h" #include "drivers/system.h" @@ -84,7 +83,7 @@ static uint8_t escCount; -escHardware_t escHardware[MAX_PWM_MOTORS]; +escHardware_t escHardware[MAX_SUPPORTED_MOTORS]; uint8_t selected_esc; @@ -132,11 +131,11 @@ uint8_t esc4wayInit(void) pwmDisableMotors(); escCount = 0; memset(&escHardware, 0, sizeof(escHardware)); - pwmOutputConfiguration_t *pwmOutputConfiguration = pwmGetOutputConfiguration(); - for (volatile uint8_t i = 0; i < pwmOutputConfiguration->outputCount; i++) { - if ((pwmOutputConfiguration->portConfigurations[i].flags & PWM_PF_MOTOR) == PWM_PF_MOTOR) { - if(motor[pwmOutputConfiguration->portConfigurations[i].index] > 0) { - escHardware[escCount].io = IOGetByTag(pwmOutputConfiguration->portConfigurations[i].timerHardware->tag); + pwmOutputPort_t *pwmMotors = pwmGetMotors(); + for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + if (pwmMotors[i].enabled) { + if (pwmMotors[i].io != IO_NONE) { + escHardware[escCount].io = pwmMotors[i].io; setEscInput(escCount); setEscHi(escCount); escCount++; diff --git a/src/main/io/serial_4way_avrootloader.c b/src/main/io/serial_4way_avrootloader.c index bfdf35cfb..aa27ac125 100644 --- a/src/main/io/serial_4way_avrootloader.c +++ b/src/main/io/serial_4way_avrootloader.c @@ -30,7 +30,6 @@ #include "drivers/system.h" #include "drivers/serial.h" #include "drivers/timer.h" -#include "drivers/pwm_mapping.h" #include "io/serial.h" #include "io/serial_4way.h" diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 272a6f193..be183c7f0 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -699,9 +699,6 @@ const clivalue_t valueTable[] = { { "3d_neutral", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.neutral3d, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "3d_deadband_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.flight3DConfig.deadband3d_throttle, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, -#ifdef CC3D - { "enable_buzzer_p6", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.use_buzzer_p6, .config.lookup = { TABLE_OFF_ON } }, -#endif { "use_unsynced_pwm", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motorConfig.useUnsyncedPwm, .config.lookup = { TABLE_OFF_ON } }, { "motor_pwm_protocol", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.motorConfig.motorPwmProtocol, .config.lookup = { TABLE_MOTOR_PWM_PROTOCOL } }, { "motor_pwm_rate", VAR_UINT16 | MASTER_VALUE, &masterConfig.motorConfig.motorPwmRate, .config.minmax = { 200, 32000 } }, @@ -736,6 +733,7 @@ const clivalue_t valueTable[] = { { "nav_speed_max", VAR_UINT16 | MASTER_VALUE, &masterConfig.gpsProfile.nav_speed_max, .config.minmax = { 10, 2000 } }, { "nav_slew_rate", VAR_UINT8 | MASTER_VALUE, &masterConfig.gpsProfile.nav_slew_rate, .config.minmax = { 0, 100 } }, #endif + #ifdef GTUNE { "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 } }, { "gtune_loP_ptch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_PITCH], .config.minmax = { 10, 200 } }, @@ -748,10 +746,17 @@ const clivalue_t valueTable[] = { { "gtune_average_cycles", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_average_cycles, .config.minmax = { 8, 128 } }, #endif +#ifdef BEEPER + { "beeper_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.beeperConfig.isInverted, .config.lookup = { TABLE_OFF_ON } }, + { "beeper_od", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.beeperConfig.isOD, .config.lookup = { TABLE_OFF_ON } }, +#endif + #ifdef SERIAL_RX { "serialrx_provider", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.serialrx_provider, .config.lookup = { TABLE_SERIAL_RX } }, #endif + { "sbus_inversion", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.sbus_inversion, .config.lookup = { TABLE_OFF_ON } }, + #ifdef SPEKTRUM_BIND { "spektrum_sat_bind", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind, .config.minmax = { SPEKTRUM_SAT_BIND_DISABLED, SPEKTRUM_SAT_BIND_MAX} }, { "spektrum_sat_bind_autoreset",VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.spektrum_sat_bind_autoreset, .config.minmax = { 0, 1} }, @@ -3667,23 +3672,134 @@ void cliProcess(void) } #if (FLASH_SIZE > 64) && !defined(CLI_MINIMAL_VERBOSITY) + +typedef struct { + const uint8_t owner; + ioTag_t *ptr; + const uint8_t maxIndex; +} cliResourceValue_t; + +const cliResourceValue_t resourceTable[] = { +#ifdef BEEPER + { OWNER_BEEPER, &masterConfig.beeperConfig.ioTag, 0 }, +#endif + { OWNER_MOTOR, &masterConfig.motorConfig.ioTags[0], MAX_SUPPORTED_MOTORS }, +#ifdef USE_SERVOS + { OWNER_SERVO, &masterConfig.servoConfig.ioTags[0], MAX_SUPPORTED_SERVOS }, +#endif +#ifndef SKIP_RX_PWM_PPM + { OWNER_PPMINPUT, &masterConfig.ppmConfig.ioTag, 0 }, + { OWNER_PWMINPUT, &masterConfig.pwmConfig.ioTags[0], PWM_INPUT_PORT_COUNT }, +#endif +}; + static void cliResource(char *cmdline) { - UNUSED(cmdline); - cliPrintf("IO:\r\n----------------------\r\n"); - for (uint32_t i = 0; i < DEFIO_IO_USED_COUNT; i++) { - const char* owner; - owner = ownerNames[ioRecs[i].owner]; + int len; + len = strlen(cmdline); - const char* resource; - resource = resourceNames[ioRecs[i].resource]; + if (len == 0) { + cliPrintf("IO:\r\n----------------------\r\n"); + for (uint32_t i = 0; i < DEFIO_IO_USED_COUNT; i++) { + const char* owner; + owner = ownerNames[ioRecs[i].owner]; - if (ioRecs[i].index > 0) { - cliPrintf("%c%02d: %s%d %s\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner, ioRecs[i].index, resource); - } else { - cliPrintf("%c%02d: %s %s\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner, resource); + const char* resource; + resource = resourceNames[ioRecs[i].resource]; + + if (ioRecs[i].index > 0) { + cliPrintf("%c%02d: %s%d %s\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner, ioRecs[i].index, resource); + } else { + cliPrintf("%c%02d: %s %s\r\n", IO_GPIOPortIdx(ioRecs + i) + 'A', IO_GPIOPinIdx(ioRecs + i), owner, resource); + } + } + cliPrintf("\r\nUse: 'resource list' to see how to change resources.\r\n"); + return; + } else if (strncasecmp(cmdline, "list", len) == 0) { + for (uint8_t i = 0; i < ARRAYLEN(resourceTable); i++) { + const char* owner; + owner = ownerNames[resourceTable[i].owner]; + + if (resourceTable[i].maxIndex > 0) { + for (int index = 0; index < resourceTable[i].maxIndex; index++) { + + if (DEFIO_TAG_ISEMPTY(*(resourceTable[i].ptr + index))) { + continue; + } + + IO_t io = IOGetByTag(*(resourceTable[i].ptr + index)); + if (!io) { + continue; + } + cliPrintf("resource %s %d %c%02d\r\n", owner, RESOURCE_INDEX(index), IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); + } + } else { + if (DEFIO_TAG_ISEMPTY(*(resourceTable[i].ptr))) { + continue; + } + IO_t io = IOGetByTag(*resourceTable[i].ptr); + cliPrintf("resource %s %c%02d\r\n", owner, IO_GPIOPortIdx(io) + 'A', IO_GPIOPinIdx(io)); + } + } + return; + } + + uint8_t resourceIndex = 0; + int index = 0; + char *pch = NULL; + char *saveptr; + + pch = strtok_r(cmdline, " ", &saveptr); + for (resourceIndex = 0; ; resourceIndex++) { + if (resourceIndex >= ARRAYLEN(resourceTable)) { + cliPrint("Invalid resource\r\n"); + return; + } + + if (strncasecmp(pch, ownerNames[resourceTable[resourceIndex].owner], len) == 0) { + break; } } + + if (resourceTable[resourceIndex].maxIndex > 0) { + pch = strtok_r(NULL, " ", &saveptr); + index = atoi(pch); + + if (index <= 0 || index > resourceTable[resourceIndex].maxIndex) { + cliShowArgumentRangeError("index", 1, resourceTable[resourceIndex].maxIndex); + return; + } + } + + pch = strtok_r(NULL, " ", &saveptr); + ioTag_t *tag = (ioTag_t*)(resourceTable[resourceIndex].ptr + (index == 0 ? 0 : index - 1)); + + uint8_t pin = 0; + if (strlen(pch) > 0) { + if (strcasecmp(pch, "NONE") == 0) { + *tag = IO_TAG_NONE; + cliPrintf("Resource is freed!"); + return; + } else { + uint8_t port = (*pch)-'A'; + if (port < 8) { + pch++; + pin = atoi(pch); + if (pin < 16) { + ioRec_t *rec = IO_Rec(IOGetByTag(DEFIO_TAG_MAKE(port, pin))); + if (rec) { + *tag = DEFIO_TAG_MAKE(port, pin); + cliPrintf("Resource is set to %c%02d!", port + 'A', pin); + } else { + cliPrintf("Resource is invalid!"); + } + return; + } + } + } + } + + cliShowParseError(); } #endif diff --git a/src/main/io/servos.h b/src/main/io/servos.h index 237a96b6c..311b0c579 100644 --- a/src/main/io/servos.h +++ b/src/main/io/servos.h @@ -17,8 +17,12 @@ #pragma once +#include "drivers/io.h" +#include "flight/mixer.h" + typedef struct servoConfig_s { // PWM values, in milliseconds, common range is 1000-2000 (1 to 2ms) uint16_t servoCenterPulse; // This is the value for servos when they should be in the middle. e.g. 1500. uint16_t servoPwmRate; // The update rate of servo outputs (50-498Hz) + ioTag_t ioTags[MAX_SUPPORTED_SERVOS]; } servoConfig_t; diff --git a/src/main/main.c b/src/main/main.c index eb98cf01d..b46f85204 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -42,7 +42,6 @@ #include "drivers/serial_uart.h" #include "drivers/accgyro.h" #include "drivers/compass.h" -#include "drivers/pwm_mapping.h" #include "drivers/pwm_rx.h" #include "drivers/pwm_output.h" #include "drivers/adc.h" @@ -247,116 +246,46 @@ void init(void) mixerInit(masterConfig.mixerMode, masterConfig.customMotorMixer); #ifdef USE_SERVOS - servoInit(masterConfig.customServoMixer); + servoMixerInit(masterConfig.customServoMixer); #endif - drv_pwm_config_t pwm_params; - memset(&pwm_params, 0, sizeof(pwm_params)); - -#ifdef SONAR - if (feature(FEATURE_SONAR)) { - const sonarHardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType); - if (sonarHardware) { - pwm_params.useSonar = true; - pwm_params.sonarIOConfig.triggerTag = sonarHardware->triggerTag; - pwm_params.sonarIOConfig.echoTag = sonarHardware->echoTag; - } + uint16_t idlePulse = masterConfig.motorConfig.mincommand; + if (feature(FEATURE_3D)) { + idlePulse = masterConfig.flight3DConfig.neutral3d; } -#endif - - // when using airplane/wing mixer, servo/motor outputs are remapped - if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_CUSTOM_AIRPLANE) - pwm_params.airplane = true; - else - pwm_params.airplane = false; -#if defined(USE_UART2) && defined(STM32F10X) - pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); -#endif -#ifdef STM32F303xC - pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); - pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_USART3); -#endif -#if defined(USE_UART2) && defined(STM32F40_41xxx) - pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_USART2); -#endif -#if defined(USE_UART6) && defined(STM32F40_41xxx) - pwm_params.useUART6 = doesConfigurationUsePort(SERIAL_PORT_USART6); -#endif - pwm_params.useVbat = feature(FEATURE_VBAT); - pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL); - pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM); - pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC); - pwm_params.useCurrentMeterADC = feature(FEATURE_CURRENT_METER) - && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC; - pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); - pwm_params.usePPM = feature(FEATURE_RX_PPM); - pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); - -#ifdef USE_SERVOS - pwm_params.useServos = isMixerUsingServos(); - pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); - pwm_params.servoCenterPulse = masterConfig.servoConfig.servoCenterPulse; - pwm_params.servoPwmRate = masterConfig.servoConfig.servoPwmRate; -#endif - - bool use_unsyncedPwm = masterConfig.motorConfig.useUnsyncedPwm || masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_CONVENTIONAL || masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED; - - // Configurator feature abused for enabling Fast PWM - pwm_params.useFastPwm = (masterConfig.motorConfig.motorPwmProtocol != PWM_TYPE_CONVENTIONAL && masterConfig.motorConfig.motorPwmProtocol != PWM_TYPE_BRUSHED); - pwm_params.pwmProtocolType = masterConfig.motorConfig.motorPwmProtocol; - pwm_params.motorPwmRate = use_unsyncedPwm ? masterConfig.motorConfig.motorPwmRate : 0; - pwm_params.idlePulse = masterConfig.motorConfig.mincommand; - if (feature(FEATURE_3D)) - pwm_params.idlePulse = masterConfig.flight3DConfig.neutral3d; if (masterConfig.motorConfig.motorPwmProtocol == PWM_TYPE_BRUSHED) { featureClear(FEATURE_3D); - pwm_params.idlePulse = 0; // brushed motors + idlePulse = 0; // brushed motors } -#ifdef CC3D - pwm_params.useBuzzerP6 = masterConfig.use_buzzer_p6 ? true : false; +#ifdef USE_QUAD_MIXER_ONLY + motorInit(&masterConfig.motorConfig, idlePulse, 4); +#else + motorInit(&masterConfig.motorConfig, idlePulse, mixers[masterConfig.mixerMode].motorCount); #endif + +#ifdef USE_SERVOS + if (isMixerUsingServos()) { + //pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING); + servoInit(&masterConfig.servoConfig); + } +#endif + #ifndef SKIP_RX_PWM_PPM - pwmRxInit(masterConfig.inputFilteringMode); + if (feature(FEATURE_RX_PPM)) { + ppmRxInit(&masterConfig.ppmConfig, masterConfig.motorConfig.motorPwmProtocol); + } else if (feature(FEATURE_RX_PARALLEL_PWM)) { + pwmRxInit(&masterConfig.pwmConfig); + } + pwmRxSetInputFilteringMode(masterConfig.inputFilteringMode); #endif - // pwmInit() needs to be called as soon as possible for ESC compatibility reasons - pwmOutputConfiguration_t *pwmOutputConfiguration = pwmInit(&pwm_params); - - mixerUsePWMOutputConfiguration(pwmOutputConfiguration, use_unsyncedPwm); + mixerUsePWMOutputConfiguration(masterConfig.motorConfig.useUnsyncedPwm); systemState |= SYSTEM_STATE_MOTORS_READY; #ifdef BEEPER - beeperConfig_t beeperConfig = { - .ioTag = IO_TAG(BEEPER), -#ifdef BEEPER_INVERTED - .isOD = false, - .isInverted = true -#else - .isOD = true, - .isInverted = false -#endif - }; -#ifdef NAZE - if (hardwareRevision >= NAZE32_REV5) { - // naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN. - beeperConfig.isOD = false; - beeperConfig.isInverted = true; - } -#endif -/* temp until PGs are implemented. */ -#ifdef BLUEJAYF4 - if (hardwareRevision <= BJF4_REV2) { - beeperConfig.ioTag = IO_TAG(BEEPER_OPT); - } -#endif -#ifdef CC3D - if (masterConfig.use_buzzer_p6 == 1) - beeperConfig.ioTag = IO_TAG(BEEPER_OPT); -#endif - - beeperInit(&beeperConfig); + beeperInit(&masterConfig.beeperConfig); #endif #ifdef INVERTER @@ -535,7 +464,7 @@ void init(void) #ifdef SONAR if (feature(FEATURE_SONAR)) { - sonarInit(); + sonarInit(&masterConfig.sonarConfig); } #endif @@ -569,10 +498,10 @@ void init(void) #ifdef USE_FLASHFS #ifdef NAZE if (hardwareRevision == NAZE32_REV5) { - m25p16_init(IOTAG_NONE); + m25p16_init(IO_TAG_NONE); } #elif defined(USE_FLASH_M25P16) - m25p16_init(IOTAG_NONE); + m25p16_init(IO_TAG_NONE); #endif flashfsInit(); diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 2e11e64d9..f5c327ab5 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -42,8 +42,6 @@ // Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range. // Inclination is adjusted by imu -extern sonarHardware_t sonarHardwareHCSR04; - int16_t sonarMaxRangeCm; int16_t sonarMaxAltWithTiltCm; int16_t sonarCfAltCm; // Complimentary Filter altitude @@ -52,38 +50,12 @@ float sonarMaxTiltCos; static int32_t calculatedAltitude; -const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor) -{ -#if defined(SONAR_TRIGGER_PIN_PWM) && defined(SONAR_ECHO_PIN_PWM) - // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins for sonar, otherwise use RC pins - if (feature(FEATURE_SOFTSERIAL) - || feature(FEATURE_RX_PARALLEL_PWM ) - || (feature(FEATURE_CURRENT_METER) && currentSensor == CURRENT_SENSOR_ADC)) { - sonarHardwareHCSR04.triggerTag = IO_TAG(SONAR_TRIGGER_PIN_PWM); - sonarHardwareHCSR04.echoTag = IO_TAG(SONAR_ECHO_PIN_PWM); - } else { - sonarHardwareHCSR04.triggerTag = IO_TAG(SONAR_TRIGGER_PIN); - sonarHardwareHCSR04.echoTag = IO_TAG(SONAR_ECHO_PIN); - } - return &sonarHardwareHCSR04; -#elif defined(SONAR_TRIGGER_PIN) && defined(SONAR_ECHO_PIN) - UNUSED(currentSensor); - sonarHardwareHCSR04.triggerTag = IO_TAG(SONAR_TRIGGER_PIN); - sonarHardwareHCSR04.echoTag = IO_TAG(SONAR_ECHO_PIN); - return &sonarHardwareHCSR04; -#elif defined(UNIT_TEST) - UNUSED(currentSensor); - return NULL; -#else -#error Sonar not defined for target -#endif -} - -void sonarInit(void) +void sonarInit(sonarConfig_t *sonarConfig) { sonarRange_t sonarRange; - hcsr04_init(&sonarRange); + hcsr04_init(&sonarRange, sonarConfig); + sensorsSet(SENSOR_SONAR); sonarMaxRangeCm = sonarRange.maxRangeCm; sonarCfAltCm = sonarMaxRangeCm / 2; diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index 0c0849e32..6a91ce144 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -18,6 +18,7 @@ #pragma once #include "sensors/battery.h" +#include "drivers/sonar_hcsr04.h" #define SONAR_OUT_OF_RANGE (-1) @@ -27,7 +28,7 @@ extern int16_t sonarMaxAltWithTiltCm; struct sonarHardware_s; const struct sonarHardware_s *sonarGetHardwareConfiguration(currentSensor_e currentSensor); -void sonarInit(void); +void sonarInit(sonarConfig_t *sonarConfig); void sonarUpdate(void); int32_t sonarRead(void); int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle); diff --git a/src/main/target/BLUEJAYF4/config.c b/src/main/target/BLUEJAYF4/config.c index 3d285c678..34391fae5 100644 --- a/src/main/target/BLUEJAYF4/config.c +++ b/src/main/target/BLUEJAYF4/config.c @@ -16,6 +16,7 @@ */ #include +#include #include @@ -23,12 +24,12 @@ #include "hardware_revision.h" - // alternative defaults settings for BlueJayF4 targets void targetConfiguration(master_t *config) { if (hardwareRevision == BJF4_REV1 || hardwareRevision == BJF4_REV2) { config->sensorAlignmentConfig.gyro_align = CW180_DEG; config->sensorAlignmentConfig.acc_align = CW180_DEG; + config->beeperConfig.ioTag = IO_TAG(BEEPER_OPT); } } diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index 72a5b5ba7..a725c47a3 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -19,53 +19,8 @@ #include #include "drivers/io.h" -#include "drivers/pwm_mapping.h" #include "drivers/timer.h" -const uint16_t multiPPM[] = { - PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - 0xFFFF -}; - -const uint16_t multiPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - 0xFFFF -}; - -const uint16_t airPPM[] = { - PWM1 | (MAP_TO_PPM_INPUT << 8), // PPM input - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM4 | (MAP_TO_SERVO_OUTPUT << 8), - PWM5 | (MAP_TO_SERVO_OUTPUT << 8), - PWM6 | (MAP_TO_SERVO_OUTPUT << 8), - PWM7 | (MAP_TO_SERVO_OUTPUT << 8), - 0xFFFF -}; - -const uint16_t airPWM[] = { - PWM1 | (MAP_TO_PWM_INPUT << 8), - PWM2 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM3 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM4 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM5 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM6 | (MAP_TO_MOTOR_OUTPUT << 8), - PWM7 | (MAP_TO_MOTOR_OUTPUT << 8), - 0xFFFF -}; - const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { { TIM8, IO_TAG(PC7), TIM_Channel_2, TIM8_CC_IRQn, 0, IOCFG_AF_PP, GPIO_AF_TIM8 }, // PPM IN { TIM5, IO_TAG(PA0), TIM_Channel_1, TIM5_IRQn, 1, IOCFG_AF_PP, GPIO_AF_TIM5 }, // S1_OUT