rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Cleanup sonar support for Naze/Flip32/Olimexino. Add Sonar 

documentation.
This commit is contained in:
Dominic Clifton 2014-08-13 22:01:39 +01:00
parent 74ef19bc34
commit d0338eac19
8 changed files with 108 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Makefile
View File

@ -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 \

29
docs/Sonar.md Normal file
View File

@ -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.

14
src/main/config/config.c
View File

@ -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);

55
src/main/drivers/sonar_hcsr04.c
View File

@ -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

14
src/main/drivers/sonar_hcsr04.h
View File

@ -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);

2
src/main/mw.c
View File

@ -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)) {

40
src/main/sensors/sonar.c
View File

@ -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;

1
src/main/target/OLIMEXINO/target.h
View File

@ -39,6 +39,7 @@
#define BARO
#define GYRO
#define MAG
#define SONAR
#define USE_USART1
#define USE_USART2