mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 08:15:30 +03:00
5107130ac0
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@465 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
127 lines
3.8 KiB
C
127 lines
3.8 KiB
C
#include "board.h"
|
|
#include "mw.h"
|
|
|
|
#ifdef SONAR
|
|
|
|
/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
|
|
* When trigged it sends out a series of 40KHz ultrasonic pulses and receives
|
|
* echo froman object. The distance between the unit and the object is calculated
|
|
* by measuring the traveling time of sound and output it as the width of a TTL pulse.
|
|
*
|
|
* *** Warning: HC-SR04 operates at +5V ***
|
|
*
|
|
*/
|
|
|
|
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;
|
|
|
|
void ECHO_EXTI_IRQHandler(void)
|
|
{
|
|
static uint32_t timing_start;
|
|
uint32_t timing_stop;
|
|
if (digitalIn(GPIOB, echo_pin) != 0) {
|
|
timing_start = micros();
|
|
} else {
|
|
timing_stop = micros();
|
|
if (timing_stop > timing_start) {
|
|
// The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
|
|
// The ping travels out and back, so to find the distance of the
|
|
// object we take half of the distance traveled.
|
|
//
|
|
// 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
|
|
int32_t pulse_duration = timing_stop - timing_start;
|
|
*distance_ptr = pulse_duration / 59;
|
|
}
|
|
}
|
|
|
|
EXTI_ClearITPendingBit(exti_line);
|
|
}
|
|
|
|
void EXTI1_IRQHandler(void)
|
|
{
|
|
ECHO_EXTI_IRQHandler();
|
|
}
|
|
|
|
void EXTI9_5_IRQHandler(void)
|
|
{
|
|
ECHO_EXTI_IRQHandler();
|
|
}
|
|
|
|
void hcsr04_init(sonar_config_t config)
|
|
{
|
|
gpio_config_t gpio;
|
|
EXTI_InitTypeDef EXTIInit;
|
|
|
|
// enable AFIO for EXTI support - already done is drv_system.c
|
|
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, 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.mode = Mode_Out_PP;
|
|
gpio.speed = Speed_2MHz;
|
|
gpioInit(GPIOB, &gpio);
|
|
|
|
// ep - echo pin
|
|
gpio.pin = echo_pin;
|
|
gpio.mode = Mode_IN_FLOATING;
|
|
gpioInit(GPIOB, &gpio);
|
|
|
|
// setup external interrupt on echo pin
|
|
gpioExtiLineConfig(GPIO_PortSourceGPIOB, exti_pin_source);
|
|
|
|
EXTI_ClearITPendingBit(exti_line);
|
|
|
|
EXTIInit.EXTI_Line = 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);
|
|
|
|
last_measurement = millis() - 60; // force 1st measurement in hcsr04_get_distance()
|
|
}
|
|
|
|
// distance calculation is done asynchronously, using interrupt
|
|
void hcsr04_get_distance(volatile int32_t *distance)
|
|
{
|
|
uint32_t current_time = millis();
|
|
|
|
if (current_time < (last_measurement + 60)) {
|
|
// the repeat interval of trig signal should be greater than 60ms
|
|
// to avoid interference between connective measurements.
|
|
return;
|
|
}
|
|
|
|
last_measurement = current_time;
|
|
distance_ptr = distance;
|
|
|
|
digitalHi(GPIOB, trigger_pin);
|
|
// The width of trig signal must be greater than 10us
|
|
delayMicroseconds(11);
|
|
digitalLo(GPIOB, trigger_pin);
|
|
}
|
|
#endif
|