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

Sonar merge

This commit is contained in:
borisbstyle 2016-02-01 22:32:44 +01:00
parent 75cd1d88eb
commit 35defbb27b
5 changed files with 154 additions and 64 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -19,6 +19,7 @@
#include <stdint.h>
#include "platform.h"
#include "build_config.h"
#include "system.h"
#include "gpio.h"
@ -26,27 +27,27 @@
#include "sonar_hcsr04.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
* When triggered it sends out a series of 40KHz ultrasonic pulses and receives
* echo from an 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 ***
*
*/
#if defined(SONAR)
STATIC_UNIT_TESTED volatile int32_t measurement = -1;
static uint32_t lastMeasurementAt;
static volatile int32_t measurement = -1;
static sonarHardware_t const *sonarHardware;
#if !defined(UNIT_TEST)
static void ECHO_EXTI_IRQHandler(void)
{
static uint32_t timing_start;
uint32_t timing_stop;
if (digitalIn(GPIOB, sonarHardware->echo_pin) != 0) {
if (digitalIn(sonarHardware->echo_gpio, sonarHardware->echo_pin) != 0) {
timing_start = micros();
} else {
timing_stop = micros();
@ -72,14 +73,19 @@ void EXTI9_5_IRQHandler(void)
{
ECHO_EXTI_IRQHandler();
}
#endif
void hcsr04_init(const sonarHardware_t *initialSonarHardware)
void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sonarRange)
{
sonarHardware = initialSonarHardware;
sonarRange->maxRangeCm = HCSR04_MAX_RANGE_CM;
sonarRange->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
#if !defined(UNIT_TEST)
gpio_config_t gpio;
EXTI_InitTypeDef EXTIInit;
sonarHardware = initialSonarHardware;
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
@ -96,12 +102,12 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware)
gpio.pin = sonarHardware->trigger_pin;
gpio.mode = Mode_Out_PP;
gpio.speed = Speed_2MHz;
gpioInit(GPIOB, &gpio);
gpioInit(sonarHardware->trigger_gpio, &gpio);
// echo pin
gpio.pin = sonarHardware->echo_pin;
gpio.mode = Mode_IN_FLOATING;
gpioInit(GPIOB, &gpio);
gpioInit(sonarHardware->echo_gpio, &gpio);
#ifdef STM32F10X
// setup external interrupt on echo pin
@ -129,11 +135,15 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware)
NVIC_Init(&NVIC_InitStructure);
lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
#else
lastMeasurementAt = 0; // to avoid "unused" compiler warning
#endif
}
// measurement reading is done asynchronously, using interrupt
void hcsr04_start_reading(void)
{
#if !defined(UNIT_TEST)
uint32_t now = millis();
if (now < (lastMeasurementAt + 60)) {
@ -144,15 +154,15 @@ void hcsr04_start_reading(void)
lastMeasurementAt = now;
digitalHi(GPIOB, sonarHardware->trigger_pin);
digitalHi(sonarHardware->trigger_gpio, sonarHardware->trigger_pin);
// The width of trig signal must be greater than 10us
delayMicroseconds(11);
digitalLo(GPIOB, sonarHardware->trigger_pin);
digitalLo(sonarHardware->trigger_gpio, sonarHardware->trigger_pin);
#endif
}
/**
* Get the distance that was measured by the last pulse, in centimeters. When the ground is too far away to be
* reliably read by the sonar, -1 is returned instead.
* Get the distance that was measured by the last pulse, in centimeters.
*/
int32_t hcsr04_get_distance(void)
{
@ -163,10 +173,6 @@ int32_t hcsr04_get_distance(void)
// 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
int32_t distance = measurement / 59;
// this sonar range is up to 4meter , but 3meter is the safe working range (+tilted and roll)
if (distance > 300)
distance = -1;
return distance;
}
#endif

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

@ -17,17 +17,31 @@
#pragma once
#include "platform.h"
typedef struct sonarHardware_s {
uint16_t trigger_pin;
GPIO_TypeDef* trigger_gpio;
uint16_t echo_pin;
GPIO_TypeDef* echo_gpio;
uint32_t exti_line;
uint8_t exti_pin_source;
IRQn_Type exti_irqn;
} sonarHardware_t;
typedef struct sonarRange_s {
int16_t maxRangeCm;
// these are full detection cone angles, maximum tilt is half of this
int16_t detectionConeDeciDegrees; // detection cone angle as in HC-SR04 device spec
int16_t detectionConeExtendedDeciDegrees; // device spec is conservative, in practice have slightly larger detection cone
} sonarRange_t;
#define SONAR_GPIO GPIOB
void hcsr04_init(const sonarHardware_t *sonarHardware);
#define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet
#define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet
#define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well
void hcsr04_init(const sonarHardware_t *sonarHardware, sonarRange_t *sonarRange);
void hcsr04_start_reading(void);
int32_t hcsr04_get_distance(void);

37
src/main/flight/altitudehold.c
View File

@ -30,6 +30,7 @@
#include "drivers/sensor.h"
#include "drivers/accgyro.h"
#include "drivers/sonar_hcsr04.h"
#include "sensors/sensors.h"
#include "sensors/acceleration.h"
@ -173,16 +174,6 @@ bool isThrustFacingDownwards(attitudeEulerAngles_t *attitude)
return ABS(attitude->values.roll) < DEGREES_80_IN_DECIDEGREES && ABS(attitude->values.pitch) < DEGREES_80_IN_DECIDEGREES;
}
/*
* This (poorly named) function merely returns whichever is higher, roll inclination or pitch inclination.
* //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft
* (my best interpretation of scalar 'tiltAngle') or rename the function.
*/
int16_t calculateTiltAngle(attitudeEulerAngles_t *attitude)
{
return MAX(ABS(attitude->values.roll), ABS(attitude->values.pitch));
}
int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
{
int32_t result = 0;
@ -228,17 +219,15 @@ void calculateEstimatedAltitude(uint32_t currentTime)
float vel_acc;
int32_t vel_tmp;
float accZ_tmp;
int32_t sonarAlt = -1;
static float accZ_old = 0.0f;
static float vel = 0.0f;
static float accAlt = 0.0f;
static int32_t lastBaroAlt;
#ifdef SONAR
int32_t sonarAlt = SONAR_OUT_OF_RANGE;
static int32_t baroAlt_offset = 0;
float sonarTransition;
#ifdef SONAR
int16_t tiltAngle;
#endif
dTime = currentTime - previousTime;
@ -260,21 +249,22 @@ void calculateEstimatedAltitude(uint32_t currentTime)
#endif
#ifdef SONAR
tiltAngle = calculateTiltAngle(&attitude);
sonarAlt = sonarRead();
sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
#endif
sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle());
if (sonarAlt > 0 && sonarAlt < 200) {
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// just use the SONAR
baroAlt_offset = BaroAlt - sonarAlt;
BaroAlt = sonarAlt;
} else {
BaroAlt -= baroAlt_offset;
if (sonarAlt > 0 && sonarAlt <= 300) {
sonarTransition = (300 - sonarAlt) / 100.0f;
if (sonarAlt > 0 && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
}
}
#endif
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
@ -305,12 +295,16 @@ void calculateEstimatedAltitude(uint32_t currentTime)
}
#endif
if (sonarAlt > 0 && sonarAlt < 200) {
#ifdef SONAR
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// the sonar has the best range
EstAlt = BaroAlt;
} else {
EstAlt = accAlt;
}
#else
EstAlt = accAlt;
#endif
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
@ -337,4 +331,3 @@ int32_t altitudeHoldGetEstimatedAltitude(void)
}
#endif

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

@ -17,6 +17,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#include "build_config.h"
@ -30,33 +31,48 @@
#include "config/config.h"
#include "sensors/sensors.h"
#include "sensors/battery.h"
#include "sensors/sonar.h"
// in cm , -1 indicate sonar is not in range - inclination adjusted by imu
// Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range.
// Inclination is adjusted by imu
float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
float baro_cf_alt; // apply CF to use ACC for height estimation
#ifdef SONAR
int16_t sonarMaxRangeCm;
int16_t sonarMaxAltWithTiltCm;
int16_t sonarCfAltCm; // Complimentary Filter altitude
STATIC_UNIT_TESTED int16_t sonarMaxTiltDeciDegrees;
float sonarMaxTiltCos;
static int32_t calculatedAltitude;
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig)
const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig)
{
#if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
static const sonarHardware_t sonarPWM56 = {
static const sonarHardware_t const sonarPWM56 = {
.trigger_pin = Pin_8, // PWM5 (PB8) - 5v tolerant
.trigger_gpio = GPIOB,
.echo_pin = Pin_9, // PWM6 (PB9) - 5v tolerant
.echo_gpio = GPIOB,
.exti_line = EXTI_Line9,
.exti_pin_source = GPIO_PinSource9,
.exti_irqn = EXTI9_5_IRQn
};
static const sonarHardware_t sonarRC78 = {
.trigger_pin = Pin_0, // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
.trigger_gpio = GPIOB,
.echo_pin = Pin_1, // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB,
.exti_line = EXTI_Line1,
.exti_pin_source = GPIO_PinSource1,
.exti_irqn = EXTI1_IRQn
};
// 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) ) {
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
if (feature(FEATURE_SOFTSERIAL)
|| feature(FEATURE_RX_PARALLEL_PWM )
|| (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) {
return &sonarPWM56;
} else {
return &sonarRC78;
@ -65,7 +81,9 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
UNUSED(batteryConfig);
static const sonarHardware_t const sonarHardware = {
.trigger_pin = Pin_0, // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
.trigger_gpio = GPIOB,
.echo_pin = Pin_1, // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB,
.exti_line = EXTI_Line1,
.exti_pin_source = GPIO_PinSource1,
.exti_irqn = EXTI1_IRQn
@ -75,22 +93,41 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
UNUSED(batteryConfig);
static const sonarHardware_t const sonarHardware = {
.trigger_pin = Pin_5, // (PB5)
.trigger_gpio = GPIOB,
.echo_pin = Pin_0, // (PB0) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB,
.exti_line = EXTI_Line0,
.exti_pin_source = GPIO_PinSource0,
.exti_irqn = EXTI0_IRQn
};
return &sonarHardware;
#elif defined(SPRACINGF3)
#elif defined(SPRACINGF3) || defined(SPRACINGF3MINI)
UNUSED(batteryConfig);
static const sonarHardware_t const sonarHardware = {
.trigger_pin = Pin_0, // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
.trigger_gpio = GPIOB,
.echo_pin = Pin_1, // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB,
.exti_line = EXTI_Line1,
.exti_pin_source = EXTI_PinSource1,
.exti_irqn = EXTI1_IRQn
};
return &sonarHardware;
#elif defined(SPARKY)
UNUSED(batteryConfig);
static const sonarHardware_t const sonarHardware = {
.trigger_pin = Pin_2, // PWM6 (PA2) - only 3.3v ( add a 1K Ohms resistor )
.trigger_gpio = GPIOA,
.echo_pin = Pin_1, // PWM7 (PB1) - only 3.3v ( add a 1K Ohms resistor )
.echo_gpio = GPIOB,
.exti_line = EXTI_Line1,
.exti_pin_source = EXTI_PinSource1,
.exti_irqn = EXTI1_IRQn
};
return &sonarHardware;
#elif defined(UNIT_TEST)
UNUSED(batteryConfig);
return 0;
#else
#error Sonar not defined for target
#endif
@ -98,9 +135,42 @@ const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryCon
void sonarInit(const sonarHardware_t *sonarHardware)
{
hcsr04_init(sonarHardware);
sonarRange_t sonarRange;
hcsr04_init(sonarHardware, &sonarRange);
sensorsSet(SENSOR_SONAR);
calculatedAltitude = -1;
sonarMaxRangeCm = sonarRange.maxRangeCm;
sonarCfAltCm = sonarMaxRangeCm / 2;
sonarMaxTiltDeciDegrees = sonarRange.detectionConeExtendedDeciDegrees / 2;
sonarMaxTiltCos = cos_approx(sonarMaxTiltDeciDegrees / 10.0f * RAD);
sonarMaxAltWithTiltCm = sonarMaxRangeCm * sonarMaxTiltCos;
calculatedAltitude = SONAR_OUT_OF_RANGE;
}
#define DISTANCE_SAMPLES_MEDIAN 5
static int32_t applySonarMedianFilter(int32_t newSonarReading)
{
static int32_t sonarFilterSamples[DISTANCE_SAMPLES_MEDIAN];
static int currentFilterSampleIndex = 0;
static bool medianFilterReady = false;
int nextSampleIndex;
if (newSonarReading > SONAR_OUT_OF_RANGE) // only accept samples that are in range
{
nextSampleIndex = (currentFilterSampleIndex + 1);
if (nextSampleIndex == DISTANCE_SAMPLES_MEDIAN) {
nextSampleIndex = 0;
medianFilterReady = true;
}
sonarFilterSamples[currentFilterSampleIndex] = newSonarReading;
currentFilterSampleIndex = nextSampleIndex;
}
if (medianFilterReady)
return quickMedianFilter5(sonarFilterSamples);
else
return newSonarReading;
}
void sonarUpdate(void)
@ -109,32 +179,36 @@ void sonarUpdate(void)
}
/**
* Get the last distance measured by the sonar in centimeters. When the ground is too far away, -1 is returned instead.
* Get the last distance measured by the sonar in centimeters. When the ground is too far away, SONAR_OUT_OF_RANGE is returned.
*/
int32_t sonarRead(void)
{
return hcsr04_get_distance();
int32_t distance = hcsr04_get_distance();
if (distance > HCSR04_MAX_RANGE_CM)
distance = SONAR_OUT_OF_RANGE;
return applySonarMedianFilter(distance);
}
/**
* Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
* the altitude. Returns the computed altitude in centimeters.
*
* When the ground is too far away or the tilt is too strong, -1 is returned instead.
* When the ground is too far away or the tilt is too large, SONAR_OUT_OF_RANGE is returned.
*/
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle)
{
// calculate sonar altitude only if the sonar is facing downwards(<25deg)
if (tiltAngle > 250)
calculatedAltitude = -1;
// calculate sonar altitude only if the ground is in the sonar cone
if (cosTiltAngle <= sonarMaxTiltCos)
calculatedAltitude = SONAR_OUT_OF_RANGE;
else
calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f;
// altitude = distance * cos(tiltAngle), use approximation
calculatedAltitude = sonarDistance * cosTiltAngle;
return calculatedAltitude;
}
/**
* Get the latest altitude that was computed by a call to sonarCalculateAltitude(), or -1 if sonarCalculateAltitude
* Get the latest altitude that was computed by a call to sonarCalculateAltitude(), or SONAR_OUT_OF_RANGE if sonarCalculateAltitude
* has never been called.
*/
int32_t sonarGetLatestAltitude(void)

11
src/main/sensors/sonar.h
View File

@ -16,11 +16,14 @@
*/
#pragma once
#include "sensors/battery.h"
#define SONAR_OUT_OF_RANGE (-1)
extern int16_t sonarMaxRangeCm;
extern int16_t sonarCfAltCm;
extern int16_t sonarMaxAltWithTiltCm;
void sonarUpdate(void);
int32_t sonarRead(void);
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle);
int32_t sonarGetLatestAltitude(void);