mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 17:25:20 +03:00
rename/removed global var sonarAngle to tiltAngle , fix spaces/lines
Conflicts: src/imu.c src/main/drivers/sonar_hcsr04.c
This commit is contained in:
treymarc
Dominic Clifton
parent
5253064b46
commit
ae0f842266
4 changed files with 666 additions and 660 deletions
|
|
@ -1,157 +1,155 @@
|
||||||
/*
|
/*
|
||||||
* This file is part of Cleanflight.
|
* This file is part of Cleanflight.
|
||||||
*
|
*
|
||||||
* Cleanflight is free software: you can redistribute it and/or modify
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
* it under the terms of the GNU General Public License as published by
|
* it under the terms of the GNU General Public License as published by
|
||||||
* the Free Software Foundation, either version 3 of the License, or
|
* the Free Software Foundation, either version 3 of the License, or
|
||||||
* (at your option) any later version.
|
* (at your option) any later version.
|
||||||
*
|
*
|
||||||
* Cleanflight is distributed in the hope that it will be useful,
|
* Cleanflight is distributed in the hope that it will be useful,
|
||||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
* GNU General Public License for more details.
|
* GNU General Public License for more details.
|
||||||
*
|
*
|
||||||
* You should have received a copy of the GNU General Public License
|
* You should have received a copy of the GNU General Public License
|
||||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
#include "system.h"
|
#include "system.h"
|
||||||
#include "gpio.h"
|
#include "gpio.h"
|
||||||
|
|
||||||
#include "sonar_hcsr04.h"
|
#include "sonar_hcsr04.h"
|
||||||
|
|
||||||
#ifdef SONAR
|
#ifdef SONAR
|
||||||
|
|
||||||
/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
|
/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
|
||||||
* When trigged it sends out a series of 40KHz ultrasonic pulses and receives
|
* 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
|
* 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.
|
* by measuring the traveling time of sound and output it as the width of a TTL pulse.
|
||||||
*
|
*
|
||||||
* *** Warning: HC-SR04 operates at +5V ***
|
* *** Warning: HC-SR04 operates at +5V ***
|
||||||
*
|
*
|
||||||
*/
|
*/
|
||||||
|
|
||||||
static uint16_t trigger_pin;
|
static uint16_t trigger_pin;
|
||||||
static uint16_t echo_pin;
|
static uint16_t echo_pin;
|
||||||
static uint32_t exti_line;
|
static uint32_t exti_line;
|
||||||
static uint8_t exti_pin_source;
|
static uint8_t exti_pin_source;
|
||||||
static IRQn_Type exti_irqn;
|
static IRQn_Type exti_irqn;
|
||||||
|
|
||||||
static uint32_t last_measurement;
|
static uint32_t last_measurement;
|
||||||
static volatile int32_t *distance_ptr;
|
static volatile int32_t *distance_ptr;
|
||||||
|
|
||||||
void ECHO_EXTI_IRQHandler(void)
|
void ECHO_EXTI_IRQHandler(void)
|
||||||
{
|
{
|
||||||
static uint32_t timing_start;
|
static uint32_t timing_start;
|
||||||
uint32_t timing_stop;
|
uint32_t timing_stop;
|
||||||
if (digitalIn(GPIOB, echo_pin) != 0) {
|
if (digitalIn(GPIOB, echo_pin) != 0) {
|
||||||
timing_start = micros();
|
timing_start = micros();
|
||||||
} else {
|
} else {
|
||||||
timing_stop = micros();
|
timing_stop = micros();
|
||||||
if (timing_stop > timing_start) {
|
if (timing_stop > timing_start) {
|
||||||
// The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
|
// 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
|
// The ping travels out and back, so to find the distance of the
|
||||||
// object we take half of the distance traveled.
|
// object we take half of the distance traveled.
|
||||||
//
|
//
|
||||||
// 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
|
// 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
|
||||||
int32_t distance = (timing_stop - timing_start) / 59;
|
int32_t distance = (timing_stop - timing_start) / 59;
|
||||||
|
// this sonar range is up to 4meter , but 3meter is the safe working range (+tilted and roll)
|
||||||
// this sonar range is up to 4meter , but 3meter is accurate enough (+tilted and roll)
|
if (distance > 300)
|
||||||
if (distance > 300)
|
distance = -1;
|
||||||
distance = -1;
|
|
||||||
|
*distance_ptr = distance;
|
||||||
*distance_ptr = distance;
|
}
|
||||||
|
}
|
||||||
}
|
|
||||||
}
|
EXTI_ClearITPendingBit(exti_line);
|
||||||
|
}
|
||||||
EXTI_ClearITPendingBit(exti_line);
|
|
||||||
}
|
void EXTI1_IRQHandler(void)
|
||||||
|
{
|
||||||
void EXTI1_IRQHandler(void)
|
ECHO_EXTI_IRQHandler();
|
||||||
{
|
}
|
||||||
ECHO_EXTI_IRQHandler();
|
|
||||||
}
|
void EXTI9_5_IRQHandler(void)
|
||||||
|
{
|
||||||
void EXTI9_5_IRQHandler(void)
|
ECHO_EXTI_IRQHandler();
|
||||||
{
|
}
|
||||||
ECHO_EXTI_IRQHandler();
|
|
||||||
}
|
void hcsr04_init(sonar_config_t config)
|
||||||
|
{
|
||||||
void hcsr04_init(sonar_config_t config)
|
gpio_config_t gpio;
|
||||||
{
|
EXTI_InitTypeDef EXTIInit;
|
||||||
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);
|
||||||
// enable AFIO for EXTI support - already done is drv_system.c
|
|
||||||
// RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph, ENABLE);
|
switch (config) {
|
||||||
|
case sonar_pwm56:
|
||||||
switch (config) {
|
trigger_pin = Pin_8; // PWM5 (PB8) - 5v tolerant
|
||||||
case sonar_pwm56:
|
echo_pin = Pin_9; // PWM6 (PB9) - 5v tolerant
|
||||||
trigger_pin = Pin_8; // PWM5 (PB8) - 5v tolerant
|
exti_line = EXTI_Line9;
|
||||||
echo_pin = Pin_9; // PWM6 (PB9) - 5v tolerant
|
exti_pin_source = GPIO_PinSource9;
|
||||||
exti_line = EXTI_Line9;
|
exti_irqn = EXTI9_5_IRQn;
|
||||||
exti_pin_source = GPIO_PinSource9;
|
break;
|
||||||
exti_irqn = EXTI9_5_IRQn;
|
case sonar_rc78:
|
||||||
break;
|
trigger_pin = Pin_0; // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
|
||||||
case sonar_rc78:
|
echo_pin = Pin_1; // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
|
||||||
trigger_pin = Pin_0; // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
|
exti_line = EXTI_Line1;
|
||||||
echo_pin = Pin_1; // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
|
exti_pin_source = GPIO_PinSource1;
|
||||||
exti_line = EXTI_Line1;
|
exti_irqn = EXTI1_IRQn;
|
||||||
exti_pin_source = GPIO_PinSource1;
|
break;
|
||||||
exti_irqn = EXTI1_IRQn;
|
}
|
||||||
break;
|
|
||||||
}
|
// tp - trigger pin
|
||||||
|
gpio.pin = trigger_pin;
|
||||||
// tp - trigger pin
|
gpio.mode = Mode_Out_PP;
|
||||||
gpio.pin = trigger_pin;
|
gpio.speed = Speed_2MHz;
|
||||||
gpio.mode = Mode_Out_PP;
|
gpioInit(GPIOB, &gpio);
|
||||||
gpio.speed = Speed_2MHz;
|
|
||||||
gpioInit(GPIOB, &gpio);
|
// ep - echo pin
|
||||||
|
gpio.pin = echo_pin;
|
||||||
// ep - echo pin
|
gpio.mode = Mode_IN_FLOATING;
|
||||||
gpio.pin = echo_pin;
|
gpioInit(GPIOB, &gpio);
|
||||||
gpio.mode = Mode_IN_FLOATING;
|
|
||||||
gpioInit(GPIOB, &gpio);
|
// setup external interrupt on echo pin
|
||||||
|
gpioExtiLineConfig(GPIO_PortSourceGPIOB, exti_pin_source);
|
||||||
// setup external interrupt on echo pin
|
|
||||||
gpioExtiLineConfig(GPIO_PortSourceGPIOB, exti_pin_source);
|
EXTI_ClearITPendingBit(exti_line);
|
||||||
|
|
||||||
EXTI_ClearITPendingBit(exti_line);
|
EXTIInit.EXTI_Line = exti_line;
|
||||||
|
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
|
||||||
EXTIInit.EXTI_Line = exti_line;
|
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
|
||||||
EXTIInit.EXTI_Mode = EXTI_Mode_Interrupt;
|
EXTIInit.EXTI_LineCmd = ENABLE;
|
||||||
EXTIInit.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
|
EXTI_Init(&EXTIInit);
|
||||||
EXTIInit.EXTI_LineCmd = ENABLE;
|
|
||||||
EXTI_Init(&EXTIInit);
|
NVIC_EnableIRQ(exti_irqn);
|
||||||
|
|
||||||
NVIC_EnableIRQ(exti_irqn);
|
last_measurement = millis() - 60; // force 1st measurement in hcsr04_get_distance()
|
||||||
|
}
|
||||||
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)
|
||||||
// distance calculation is done asynchronously, using interrupt
|
{
|
||||||
void hcsr04_get_distance(volatile int32_t *distance)
|
uint32_t current_time = millis();
|
||||||
{
|
|
||||||
uint32_t current_time = millis();
|
if (current_time < (last_measurement + 60)) {
|
||||||
|
// the repeat interval of trig signal should be greater than 60ms
|
||||||
if (current_time < (last_measurement + 60)) {
|
// to avoid interference between connective measurements.
|
||||||
// the repeat interval of trig signal should be greater than 60ms
|
return;
|
||||||
// to avoid interference between connective measurements.
|
}
|
||||||
return;
|
|
||||||
}
|
last_measurement = current_time;
|
||||||
|
distance_ptr = distance;
|
||||||
last_measurement = current_time;
|
|
||||||
distance_ptr = distance;
|
digitalHi(GPIOB, trigger_pin);
|
||||||
|
// The width of trig signal must be greater than 10us
|
||||||
digitalHi(GPIOB, trigger_pin);
|
delayMicroseconds(11);
|
||||||
// The width of trig signal must be greater than 10us
|
digitalLo(GPIOB, trigger_pin);
|
||||||
delayMicroseconds(11);
|
}
|
||||||
digitalLo(GPIOB, trigger_pin);
|
#endif
|
||||||
}
|
|
||||||
#endif
|
|
||||||
|
|
|
||||||
File diff suppressed because it is too large
Load diff
|
|
@ -47,15 +47,13 @@ void Sonar_update(void)
|
||||||
hcsr04_get_distance(&sonarAlt);
|
hcsr04_get_distance(&sonarAlt);
|
||||||
}
|
}
|
||||||
|
|
||||||
int16_t sonarCalculateAltitude(int32_t sonarAlt, rollAndPitchInclination_t *angle)
|
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
|
||||||
{
|
{
|
||||||
// calculate sonar altitude
|
// calculate sonar altitude only if the sonar is facing downwards(<25deg)
|
||||||
int16_t sonarAnglecorrection = max(abs(angle->values.rollDeciDegrees), abs(angle->values.pitchDeciDegrees));
|
if (tiltAngle > 250)
|
||||||
if (sonarAnglecorrection > 250)
|
return -1;
|
||||||
return -1;
|
|
||||||
|
|
||||||
return sonarAlt * (900.0f - sonarAnglecorrection) / 900.0f;
|
return sonarAlt * (900.0f - tiltAngle) / 900.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
||||||
|
|
@ -22,4 +22,4 @@ extern int32_t sonarAlt;
|
||||||
void Sonar_init(void);
|
void Sonar_init(void);
|
||||||
void Sonar_update(void);
|
void Sonar_update(void);
|
||||||
|
|
||||||
int16_t sonarCalculateAltitude(int32_t sonarAlt, rollAndPitchInclination_t *angle);
|
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue