diff --git a/Makefile b/Makefile index 7fce78ce6..7000534af 100644 --- a/Makefile +++ b/Makefile @@ -220,6 +220,7 @@ OLIMEXINO_SRC = startup_stm32f10x_md_gcc.S \ 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/Sonar.md b/docs/Sonar.md new file mode 100644 index 000000000..743a4d339 --- /dev/null +++ b/docs/Sonar.md @@ -0,0 +1,29 @@ +# Sonar + +A sonar sensor can be used to measure altitude for use with altitude hold modes. + +The current sensor takes over from the pressure sensor at low altitudes, but only when +the angle of the aircraft is small. + +## Hardware + +Currently the only supported sensor is the HCSR04 sensor. + +## Connections + +### Naze + +| Mode | Trigger | Echo | Inline 1k resistors | +| ------------- | ------------- | ------------- | ------------------- | +| Parallel PWM | PB8 / Motor 5 | PB9 / Motor 6 | NO (5v tolerant) | +| PPM/Serial RX | PB0 / RC7 | PB1 / RC8 | YES (3.3v input) | + +Current meter cannot be used in conjunction with Parallel PWM and Sonar. + +### Olimexino + +| Trigger | Echo | Inline 1k resistors | +| ------------- | ------------- | ------------------- | +| PB0 / RC7 | PB1 / RC8 | YES (3.3v input) | + +Current meter cannot be used in conjunction sonar. diff --git a/src/main/config/config.c b/src/main/config/config.c index 0b3062af5..593ca9556 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -458,7 +458,7 @@ void validateAndFixConfig(void) featureClear(FEATURE_RX_PPM); } - if (feature(FEATURE_RX_PARALLEL_PWM)) { + if (feature(FEATURE_CURRENT_METER)) { #if defined(STM32F10X_MD) // rssi adc needs the same ports featureClear(FEATURE_RSSI_ADC); @@ -471,7 +471,6 @@ void validateAndFixConfig(void) featureClear(FEATURE_LED_STRIP); #endif - // software serial needs free PWM ports featureClear(FEATURE_SOFTSERIAL); } @@ -484,6 +483,17 @@ void validateAndFixConfig(void) } #endif +#if defined(NAZE) && defined(SONAR) + if (feature(FEATURE_RX_PARALLEL_PWM) && feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER)) { + featureClear(FEATURE_CURRENT_METER); + } +#endif + +#if defined(OLIMEXINO) && defined(SONAR) + if (feature(FEATURE_SONAR) && feature(FEATURE_CURRENT_METER)) { + featureClear(FEATURE_CURRENT_METER); + } +#endif useRxConfig(&masterConfig.rxConfig); diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index 97f8f551d..ba853075e 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -36,20 +36,18 @@ * */ -static uint16_t trigger_pin; -static uint16_t echo_pin; -static uint32_t exti_line; -static uint8_t exti_pin_source; -static IRQn_Type exti_irqn; - static uint32_t last_measurement; static volatile int32_t *distance_ptr = 0; +extern int16_t debug[4]; + +static sonarHardware_t const *sonarHardware; + void ECHO_EXTI_IRQHandler(void) { static uint32_t timing_start; uint32_t timing_stop; - if (digitalIn(GPIOB, echo_pin) != 0) { + if (digitalIn(GPIOB, sonarHardware->echo_pin) != 0) { timing_start = micros(); } else { timing_stop = micros(); @@ -70,7 +68,7 @@ void ECHO_EXTI_IRQHandler(void) } } - EXTI_ClearITPendingBit(exti_line); + EXTI_ClearITPendingBit(sonarHardware->exti_line); } void EXTI1_IRQHandler(void) @@ -83,54 +81,39 @@ void EXTI9_5_IRQHandler(void) ECHO_EXTI_IRQHandler(); } -void hcsr04_init(sonar_config_t config) +void hcsr04_init(const sonarHardware_t *initialSonarHardware) { gpio_config_t gpio; EXTI_InitTypeDef EXTIInit; + sonarHardware = initialSonarHardware; + // enable AFIO for EXTI support RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); - switch (config) { - case sonar_pwm56: - trigger_pin = Pin_8; // PWM5 (PB8) - 5v tolerant - echo_pin = Pin_9; // PWM6 (PB9) - 5v tolerant - exti_line = EXTI_Line9; - exti_pin_source = GPIO_PinSource9; - exti_irqn = EXTI9_5_IRQn; - break; - case sonar_rc78: - trigger_pin = Pin_0; // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) - echo_pin = Pin_1; // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) - exti_line = EXTI_Line1; - exti_pin_source = GPIO_PinSource1; - exti_irqn = EXTI1_IRQn; - break; - } - // tp - trigger pin - gpio.pin = trigger_pin; + gpio.pin = sonarHardware->trigger_pin; gpio.mode = Mode_Out_PP; gpio.speed = Speed_2MHz; gpioInit(GPIOB, &gpio); // ep - echo pin - gpio.pin = echo_pin; + gpio.pin = sonarHardware->echo_pin; gpio.mode = Mode_IN_FLOATING; gpioInit(GPIOB, &gpio); // setup external interrupt on echo pin - gpioExtiLineConfig(GPIO_PortSourceGPIOB, exti_pin_source); + gpioExtiLineConfig(GPIO_PortSourceGPIOB, sonarHardware->exti_pin_source); - EXTI_ClearITPendingBit(exti_line); + EXTI_ClearITPendingBit(sonarHardware->exti_line); - EXTIInit.EXTI_Line = exti_line; + EXTIInit.EXTI_Line = sonarHardware->exti_line; EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt; EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling; EXTIInit.EXTI_LineCmd = ENABLE; EXTI_Init(&EXTIInit); - NVIC_EnableIRQ(exti_irqn); + NVIC_EnableIRQ(sonarHardware->exti_irqn); last_measurement = millis() - 60; // force 1st measurement in hcsr04_get_distance() } @@ -149,9 +132,13 @@ void hcsr04_get_distance(volatile int32_t *distance) last_measurement = current_time; distance_ptr = distance; - digitalHi(GPIOB, trigger_pin); +#if 1 + debug[0] = *distance; +#endif + + digitalHi(GPIOB, sonarHardware->trigger_pin); // The width of trig signal must be greater than 10us delayMicroseconds(11); - digitalLo(GPIOB, trigger_pin); + digitalLo(GPIOB, sonarHardware->trigger_pin); } #endif diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index 869d0400c..6950c8367 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -17,10 +17,14 @@ #pragma once -typedef enum { - sonar_pwm56, - sonar_rc78, -} sonar_config_t; +typedef struct sonarHardware_s { + uint16_t trigger_pin; + uint16_t echo_pin; + uint32_t exti_line; + uint8_t exti_pin_source; + IRQn_Type exti_irqn; +} sonarHardware_t; + +void hcsr04_init(const sonarHardware_t *sonarHardware); -void hcsr04_init(sonar_config_t config); void hcsr04_get_distance(volatile int32_t *distance); diff --git a/src/main/mw.c b/src/main/mw.c index 494e542bd..421fd4625 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -612,8 +612,8 @@ void loop(void) if (f.BARO_MODE) { updateAltHold(); } - debug[0] = rcCommand[THROTTLE]; } + #endif if (currentProfile.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) { diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index eb8039f0b..e13fc7b78 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -24,6 +24,7 @@ #include "common/axis.h" #include "drivers/sonar_hcsr04.h" +#include "drivers/gpio.h" #include "config/runtime_config.h" #include "config/config.h" @@ -38,12 +39,39 @@ int32_t sonarAlt = -1; // in cm , -1 indicate sonar is not in range void Sonar_init(void) { - // If we are using parallel PWM for our receiver, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8 - if (feature(FEATURE_RX_PARALLEL_PWM)) { - hcsr04_init(sonar_pwm56); - } else { - hcsr04_init(sonar_rc78); - } +#if defined(NAZE) + static const sonarHardware_t const sonarPWM56 = { + .trigger_pin = Pin_8, // PWM5 (PB8) - 5v tolerant + .echo_pin = Pin_9, // PWM6 (PB9) - 5v tolerant + .exti_line = EXTI_Line9, + .exti_pin_source = GPIO_PinSource9, + .exti_irqn = EXTI9_5_IRQn + }; + static const sonarHardware_t const sonarRC78 = { + .trigger_pin = Pin_0, // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) + .echo_pin = Pin_1, // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) + .exti_line = EXTI_Line1, + .exti_pin_source = GPIO_PinSource1, + .exti_irqn = EXTI1_IRQn + }; + // If we are using parallel PWM for our receiver, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8 + if (feature(FEATURE_RX_PARALLEL_PWM)) { + hcsr04_init(&sonarPWM56); + } else { + hcsr04_init(&sonarRC78); + } +#elif defined(OLIMEXINO) + static const sonarHardware_t const sonarHardware = { + .trigger_pin = Pin_0, // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) + .echo_pin = Pin_1, // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) + .exti_line = EXTI_Line1, + .exti_pin_source = GPIO_PinSource1, + .exti_irqn = EXTI1_IRQn + }; + hcsr04_init(&sonarHardware); +#else +#error Sonar not defined for target +#endif sensorsSet(SENSOR_SONAR); sonarAlt = 0; diff --git a/src/main/target/OLIMEXINO/target.h b/src/main/target/OLIMEXINO/target.h index eaceee9e9..f69fc39a0 100644 --- a/src/main/target/OLIMEXINO/target.h +++ b/src/main/target/OLIMEXINO/target.h @@ -39,6 +39,7 @@ #define BARO #define GYRO #define MAG +#define SONAR #define USE_USART1 #define USE_USART2