From 4ed6fdfea5823c00bf23645609af933e29e63c7e Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Thu, 29 Jan 2015 16:33:45 +0100 Subject: [PATCH] CC3D - Add sonar. This also ensures that the PWM mapping does not use the sonar pins when sonar is enabled in a board agnostic way. Conflicts: src/main/config/config.c src/main/drivers/pwm_mapping.h src/main/main.c src/main/target/CC3D/target.h --- Makefile | 1 + docs/Board - CC3D.md | 4 ++-- docs/Sonar.md | 16 +++++++++++++++- src/main/config/config.c | 6 ++++++ src/main/drivers/pwm_mapping.c | 34 +++++++++++---------------------- src/main/drivers/pwm_mapping.h | 7 +++++++ src/main/drivers/sonar_hcsr04.c | 5 +++++ src/main/drivers/sonar_hcsr04.h | 2 ++ src/main/drivers/timer.h | 2 +- src/main/main.c | 20 ++++++++++++++++++- src/main/sensors/sonar.c | 24 ++++++++++++++++++----- src/main/sensors/sonar.h | 2 +- src/main/target/CC3D/target.h | 2 ++ 13 files changed, 91 insertions(+), 34 deletions(-) diff --git a/Makefile b/Makefile index f88fe8e680..3d06d4a1d2 100644 --- a/Makefile +++ b/Makefile @@ -444,6 +444,7 @@ CC3D_SRC = \ drivers/serial_softserial.c \ drivers/serial_uart.c \ drivers/serial_uart_stm32f10x.c \ + drivers/sonar_hcsr04.c \ drivers/sound_beeper_stm32f10x.c \ drivers/system_stm32f10x.c \ drivers/timer.c \ diff --git a/docs/Board - CC3D.md b/docs/Board - CC3D.md index 62de80018e..95a7d3dfac 100644 --- a/docs/Board - CC3D.md +++ b/docs/Board - CC3D.md @@ -23,8 +23,8 @@ The 8 pin RC_Input connector has the following pinouts when used in RX_PPM/RX_SE | 1 | Ground | | | 2 | +5V | | | 3 | PPM Input | Enable `feature RX_PPM` | -| 4 | SoftSerial1 TX | Enable `feature SOFTSERIAL` | -| 5 | SoftSerial1 RX | Enable `feature SOFTSERIAL` | +| 4 | SoftSerial1 TX / Sonar trigger | | +| 5 | SoftSerial1 RX / Sonar Echo | | | 6 | Current | Enable `feature CURRENT_METER`. Connect to the output of a current sensor, 0v-3.3v input | | 7 | Battery Voltage sensor | Enable `feature VBAT`. Connect to main battery using a voltage divider, 0v-3.3v input | | 8 | RSSI | Enable `feature RSSI_ADC`. Connect to the output of a PWM-RSSI conditioner, 0v-3.3v input | diff --git a/docs/Sonar.md b/docs/Sonar.md index 6def6928f5..f42fdc935b 100644 --- a/docs/Sonar.md +++ b/docs/Sonar.md @@ -19,6 +19,8 @@ Currently the only supported sensor is the HCSR04 sensor. | Parallel PWM/ADC current sensor | PB8 / Motor 5 | PB9 / Motor 6 | NO (5v tolerant) | | PPM/Serial RX | PB0 / RC7 | PB1 / RC8 | YES (3.3v input) | +#### Constraints + Current meter cannot be used in conjunction with Parallel PWM and Sonar. ### Olimexino @@ -27,4 +29,16 @@ Current meter cannot be used in conjunction with Parallel PWM and Sonar. | ------------- | ------------- | ------------------- | | PB0 / RC7 | PB1 / RC8 | YES (3.3v input) | -Current meter cannot be used in conjunction with sonar. +#### Constraints + +Current meter cannot be used in conjunction with Sonar. + +### CC3D + +| Trigger | Echo | Inline 1k resistors | +| ------------- | ------------- | ------------------- | +| PB5 | PB0 | YES (3.3v input) | + +#### Constraints + +Sonar cannot be used in conjuction with SoftSerial or Parallel PWM. diff --git a/src/main/config/config.c b/src/main/config/config.c index 40163d080f..afe3d069e0 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -804,6 +804,12 @@ void validateAndFixConfig(void) masterConfig.mixerConfig.pid_at_min_throttle = 0; } +#if defined(CC3D) && defined(SONAR) && defined(USE_SOFTSERIAL1) + if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) { + featureClear(FEATURE_SONAR); + } +#endif + useRxConfig(&masterConfig.rxConfig); serialConfig_t *serialConfig = &masterConfig.serialConfig; diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 2ce4481731..828daecb47 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -408,29 +408,6 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) continue; #endif -#ifdef SONAR - // skip Sonar pins - // FIXME - Hack - See sonar.c sonarInit() and sonarHardware_t - if (init->useSonar && timerHardwarePtr->gpio == GPIOB) { -#if defined(SPRACINGF3) || defined(OLIMEXINO) - if (timerHardwarePtr->pin == GPIO_Pin_0 || timerHardwarePtr->pin == GPIO_Pin_1) { - continue; - } -#endif -#if defined(NAZE) - if (init->useParallelPWM) { - if (timerHardwarePtr->pin == GPIO_Pin_8 || timerHardwarePtr->pin == GPIO_Pin_9) { - continue; - } - } else { - if (timerHardwarePtr->pin == GPIO_Pin_0 || timerHardwarePtr->pin == GPIO_Pin_1) { - continue; - } - } -#endif - } -#endif - #ifdef SOFTSERIAL_1_TIMER if (init->useSoftSerial && timerHardwarePtr->tim == SOFTSERIAL_1_TIMER) continue; @@ -471,6 +448,17 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) } #endif +#ifdef SONAR + if (init->sonarGPIOConfig && timerHardwarePtr->gpio == init->sonarGPIOConfig->gpio && + ( + timerHardwarePtr->pin == init->sonarGPIOConfig->triggerPin || + timerHardwarePtr->pin == init->sonarGPIOConfig->echoPin + ) + ) { + continue; + } +#endif + // hacks to allow current functionality if (type == MAP_TO_PWM_INPUT && !init->useParallelPWM) continue; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 08d318168d..4e11e88f0e 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -36,6 +36,12 @@ #define PWM_TIMER_MHZ 1 #define ONESHOT125_TIMER_MHZ 8 +typedef struct sonarGPIOConfig_s { + GPIO_TypeDef *gpio; + uint16_t triggerPin; + uint16_t echoPin; +} sonarGPIOConfig_t; + typedef struct drv_pwm_config_t { bool useParallelPWM; bool usePPM; @@ -65,6 +71,7 @@ typedef struct drv_pwm_config_t { uint16_t motorPwmRate; uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), // some higher value (used by 3d mode), or 0, for brushed pwm drivers. + sonarGPIOConfig_t *sonarGPIOConfig; } drv_pwm_config_t; diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index 13216ee20d..663e1c3531 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -58,6 +58,11 @@ static void ECHO_EXTI_IRQHandler(void) EXTI_ClearITPendingBit(sonarHardware->exti_line); } +void EXTI0_IRQHandler(void) +{ + ECHO_EXTI_IRQHandler(); +} + void EXTI1_IRQHandler(void) { ECHO_EXTI_IRQHandler(); diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index 9ca57d397f..9315668a75 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -25,6 +25,8 @@ typedef struct sonarHardware_s { IRQn_Type exti_irqn; } sonarHardware_t; +#define SONAR_GPIO GPIOB + void hcsr04_init(const sonarHardware_t *sonarHardware); void hcsr04_start_reading(void); diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 8edbd88e5c..89317c507d 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -60,7 +60,7 @@ typedef struct timerOvrHandlerRec_s { typedef struct { TIM_TypeDef *tim; GPIO_TypeDef *gpio; - uint32_t pin; + uint16_t pin; uint8_t channel; uint8_t irq; uint8_t outputEnable; diff --git a/src/main/main.c b/src/main/main.c index 035546d984..cec2935a0c 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -47,6 +47,7 @@ #include "drivers/bus_spi.h" #include "drivers/inverter.h" #include "drivers/flash_m25p16.h" +#include "drivers/sonar_hcsr04.h" #include "rx/rx.h" @@ -114,6 +115,8 @@ void displayInit(rxConfig_t *intialRxConfig); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); void loop(void); void spektrumBind(rxConfig_t *rxConfig); +const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig); +void sonarInit(const sonarHardware_t *sonarHardware); #ifdef STM32F303xC // from system_stm32f30x.c @@ -193,6 +196,21 @@ void init(void) mixerInit(masterConfig.mixerMode, masterConfig.customMixer); memset(&pwm_params, 0, sizeof(pwm_params)); + +#ifdef SONAR + const sonarHardware_t *sonarHardware = NULL; + + if (feature(FEATURE_SONAR)) { + sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig); + sonarGPIOConfig_t sonarGPIOConfig = { + .echoPin = sonarHardware->trigger_pin, + .triggerPin = sonarHardware->echo_pin, + .gpio = SONAR_GPIO + }; + pwm_params.sonarGPIOConfig = &sonarGPIOConfig; + } +#endif + // when using airplane/wing mixer, servo/motor outputs are remapped if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING) pwm_params.airplane = true; @@ -383,7 +401,7 @@ void init(void) #ifdef SONAR if (feature(FEATURE_SONAR)) { - sonarInit(&masterConfig.batteryConfig); + sonarInit(sonarHardware); } #endif diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index cb76432246..439bbcffce 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -38,7 +38,7 @@ static int32_t calculatedAltitude; -void sonarInit(batteryConfig_t *batteryConfig) +const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig) { #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R) static const sonarHardware_t const sonarPWM56 = { @@ -57,9 +57,9 @@ void sonarInit(batteryConfig_t *batteryConfig) }; // If we are using parallel PWM for our receiver or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8 if (feature(FEATURE_RX_PARALLEL_PWM ) || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC) ) { - hcsr04_init(&sonarPWM56); + return &sonarPWM56; } else { - hcsr04_init(&sonarRC78); + return &sonarRC78; } #elif defined(OLIMEXINO) UNUSED(batteryConfig); @@ -70,7 +70,17 @@ void sonarInit(batteryConfig_t *batteryConfig) .exti_pin_source = GPIO_PinSource1, .exti_irqn = EXTI1_IRQn }; - hcsr04_init(&sonarHardware); + return &sonarHardware; +#elif defined(CC3D) + UNUSED(batteryConfig); + static const sonarHardware_t const sonarHardware = { + .trigger_pin = Pin_5, // (PB5) + .echo_pin = Pin_0, // (PB0) - only 3.3v ( add a 1K Ohms resistor ) + .exti_line = EXTI_Line0, + .exti_pin_source = GPIO_PinSource0, + .exti_irqn = EXTI0_IRQn + }; + return &sonarHardware; #elif defined(SPRACINGF3) UNUSED(batteryConfig); static const sonarHardware_t const sonarHardware = { @@ -80,11 +90,15 @@ void sonarInit(batteryConfig_t *batteryConfig) .exti_pin_source = EXTI_PinSource1, .exti_irqn = EXTI1_IRQn }; - hcsr04_init(&sonarHardware); + return &sonarHardware; #else #error Sonar not defined for target #endif +} +void sonarInit(const sonarHardware_t *sonarHardware) +{ + hcsr04_init(sonarHardware); sensorsSet(SENSOR_SONAR); calculatedAltitude = -1; } diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index 695ca8442d..333d2842d8 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -18,9 +18,9 @@ #pragma once #include "sensors/battery.h" -void sonarInit(batteryConfig_t *batteryConfig); void sonarUpdate(void); int32_t sonarRead(void); int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle); int32_t sonarGetLatestAltitude(void); + diff --git a/src/main/target/CC3D/target.h b/src/main/target/CC3D/target.h index 22db9a574c..8c760cf610 100644 --- a/src/main/target/CC3D/target.h +++ b/src/main/target/CC3D/target.h @@ -111,6 +111,7 @@ #define BLACKBOX #define TELEMETRY #define SERIAL_RX +#define SONAR #define AUTOTUNE #define USE_SERVOS #define USE_CLI @@ -119,6 +120,7 @@ #undef AUTOTUNE // disabled for OPBL build due to code size. #endif + #define SPEKTRUM_BIND // USART3, PB11 (Flexport) #define BIND_PORT GPIOB