mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 17:25:20 +03:00
Port iNav's rangefinder
This commit is contained in:
jflyper
parent
8a9fd29dd3
commit
11c47c631b
53 changed files with 898 additions and 492 deletions
|
|
@ -35,24 +35,12 @@
|
|||
#include "sensors/barometer.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/sonar.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/initialisation.h"
|
||||
|
||||
uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE };
|
||||
|
||||
|
||||
#ifdef USE_SONAR
|
||||
static bool sonarDetect(void)
|
||||
{
|
||||
if (feature(FEATURE_SONAR)) {
|
||||
// the user has set the sonar feature, so assume they have an HC-SR04 plugged in,
|
||||
// since there is no way to detect it
|
||||
sensorsSet(SENSOR_SONAR);
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
// requestedSensors is not actually used
|
||||
uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
|
||||
uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
|
||||
|
||||
bool sensorsAutodetect(void)
|
||||
{
|
||||
|
|
@ -73,10 +61,8 @@ bool sensorsAutodetect(void)
|
|||
baroDetect(&baro.dev, barometerConfig()->baro_hardware);
|
||||
#endif
|
||||
|
||||
#ifdef USE_SONAR
|
||||
if (sonarDetect()) {
|
||||
sonarInit(sonarConfig());
|
||||
}
|
||||
#ifdef USE_RANGEFINDER
|
||||
rangefinderInit();
|
||||
#endif
|
||||
|
||||
return gyroDetected;
|
||||
|
|
|
|||
370
src/main/sensors/rangefinder.c
Normal file
370
src/main/sensors/rangefinder.c
Normal file
|
|
@ -0,0 +1,370 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <platform.h>
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
#include "common/time.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/rangefinder/rangefinder_hcsr04.h"
|
||||
#include "drivers/rangefinder/rangefinder.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "fc/fc_tasks.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
//#include "uav_interconnect/uav_interconnect.h"
|
||||
|
||||
// XXX Interface to CF/BF legacy(?) altitude estimation code.
|
||||
// XXX Will be gone once iNav's estimator is ported.
|
||||
int16_t rangefinderMaxRangeCm;
|
||||
int16_t rangefinderMaxAltWithTiltCm;
|
||||
int16_t rangefinderCfAltCm; // Complimentary Filter altitude
|
||||
|
||||
rangefinder_t rangefinder;
|
||||
|
||||
#define RANGEFINDER_HARDWARE_TIMEOUT_MS 500 // Accept 500ms of non-responsive sensor, report HW failure otherwise
|
||||
|
||||
#define RANGEFINDER_DYNAMIC_THRESHOLD 600 //Used to determine max. usable rangefinder disatance
|
||||
#define RANGEFINDER_DYNAMIC_FACTOR 75
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 0);
|
||||
|
||||
PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig,
|
||||
.rangefinder_hardware = RANGEFINDER_NONE,
|
||||
);
|
||||
|
||||
#ifdef USE_RANGEFINDER_HCSR04
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(sonarConfig_t, sonarConfig, PG_SONAR_CONFIG, 1);
|
||||
|
||||
#ifndef RANGEFINDER_HCSR04_TRIGGER_PIN
|
||||
#define RANGEFINDER_HCSR04_TRIGGER_PIN NONE
|
||||
#endif
|
||||
#ifndef RANGEFINDER_HCSR04_ECHO_PIN
|
||||
#define RANGEFINDER_HCSR04_ECHO_PIN NONE
|
||||
#endif
|
||||
|
||||
PG_RESET_TEMPLATE(sonarConfig_t, sonarConfig,
|
||||
.triggerTag = IO_TAG(RANGEFINDER_HCSR04_TRIGGER_PIN),
|
||||
.echoTag = IO_TAG(RANGEFINDER_HCSR04_ECHO_PIN),
|
||||
);
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Detect which rangefinder is present
|
||||
*/
|
||||
static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwareToUse)
|
||||
{
|
||||
rangefinderType_e rangefinderHardware = RANGEFINDER_NONE;
|
||||
requestedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardwareToUse;
|
||||
|
||||
switch (rangefinderHardwareToUse) {
|
||||
case RANGEFINDER_HCSR04:
|
||||
#ifdef USE_RANGEFINDER_HCSR04
|
||||
{
|
||||
if (hcsr04Detect(dev, sonarConfig())) { // FIXME: Do actual detection if HC-SR04 is plugged in
|
||||
rangefinderHardware = RANGEFINDER_HCSR04;
|
||||
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_HCSR04_TASK_PERIOD_MS));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case RANGEFINDER_SRF10:
|
||||
#ifdef USE_RANGEFINDER_SRF10
|
||||
if (srf10Detect(dev)) {
|
||||
rangefinderHardware = RANGEFINDER_SRF10;
|
||||
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_SRF10_TASK_PERIOD_MS));
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case RANGEFINDER_HCSR04I2C:
|
||||
#ifdef USE_RANGEFINDER_HCSR04_I2C
|
||||
if (hcsr04i2c0Detect(dev)) {
|
||||
rangefinderHardware = RANGEFINDER_HCSR04I2C;
|
||||
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_HCSR04_i2C_TASK_PERIOD_MS));
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case RANGEFINDER_VL53L0X:
|
||||
#if defined(USE_RANGEFINDER_VL53L0X)
|
||||
if (vl53l0xDetect(dev)) {
|
||||
rangefinderHardware = RANGEFINDER_VL53L0X;
|
||||
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VL53L0X_TASK_PERIOD_MS));
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case RANGEFINDER_UIB:
|
||||
#if defined(USE_RANGEFINDER_UIB)
|
||||
if (uibRangefinderDetect(dev)) {
|
||||
rangefinderHardware = RANGEFINDER_UIB;
|
||||
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_UIB_TASK_PERIOD_MS));
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case RANGEFINDER_NONE:
|
||||
rangefinderHardware = RANGEFINDER_NONE;
|
||||
break;
|
||||
}
|
||||
|
||||
if (rangefinderHardware == RANGEFINDER_NONE) {
|
||||
sensorsClear(SENSOR_RANGEFINDER);
|
||||
return false;
|
||||
}
|
||||
|
||||
detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardware;
|
||||
sensorsSet(SENSOR_RANGEFINDER);
|
||||
return true;
|
||||
}
|
||||
|
||||
void rangefinderResetDynamicThreshold(void)
|
||||
{
|
||||
rangefinder.snrThresholdReached = false;
|
||||
rangefinder.dynamicDistanceThreshold = 0;
|
||||
}
|
||||
|
||||
bool rangefinderInit(void)
|
||||
{
|
||||
if (!rangefinderDetect(&rangefinder.dev, rangefinderConfig()->rangefinder_hardware)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
rangefinder.dev.init(&rangefinder.dev);
|
||||
rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
rangefinder.maxTiltCos = cos_approx(DECIDEGREES_TO_RADIANS(rangefinder.dev.detectionConeExtendedDeciDegrees / 2.0f));
|
||||
rangefinder.lastValidResponseTimeMs = millis();
|
||||
rangefinder.snr = 0;
|
||||
|
||||
rangefinderResetDynamicThreshold();
|
||||
|
||||
// XXX Interface to CF/BF legacy(?) altitude estimation code.
|
||||
// XXX Will be gone once iNav's estimator is ported.
|
||||
rangefinderMaxRangeCm = rangefinder.dev.maxRangeCm;
|
||||
rangefinderMaxAltWithTiltCm = rangefinderMaxRangeCm * rangefinder.maxTiltCos;
|
||||
rangefinderCfAltCm = rangefinder.dev.maxRangeCm / 2 ; // Complimentary Filter altitude
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static int32_t applyMedianFilter(int32_t newReading)
|
||||
{
|
||||
#define DISTANCE_SAMPLES_MEDIAN 5
|
||||
static int32_t filterSamples[DISTANCE_SAMPLES_MEDIAN];
|
||||
static int filterSampleIndex = 0;
|
||||
static bool medianFilterReady = false;
|
||||
|
||||
if (newReading > RANGEFINDER_OUT_OF_RANGE) {// only accept samples that are in range
|
||||
filterSamples[filterSampleIndex] = newReading;
|
||||
++filterSampleIndex;
|
||||
if (filterSampleIndex == DISTANCE_SAMPLES_MEDIAN) {
|
||||
filterSampleIndex = 0;
|
||||
medianFilterReady = true;
|
||||
}
|
||||
}
|
||||
return medianFilterReady ? quickMedianFilter5(filterSamples) : newReading;
|
||||
}
|
||||
|
||||
static int16_t computePseudoSnr(int32_t newReading) {
|
||||
#define SNR_SAMPLES 5
|
||||
static int16_t snrSamples[SNR_SAMPLES];
|
||||
static uint8_t snrSampleIndex = 0;
|
||||
static int32_t previousReading = RANGEFINDER_OUT_OF_RANGE;
|
||||
static bool snrReady = false;
|
||||
int16_t pseudoSnr = 0;
|
||||
|
||||
snrSamples[snrSampleIndex] = constrain((int)(pow(newReading - previousReading, 2) / 10), 0, 6400);
|
||||
++snrSampleIndex;
|
||||
if (snrSampleIndex == SNR_SAMPLES) {
|
||||
snrSampleIndex = 0;
|
||||
snrReady = true;
|
||||
}
|
||||
|
||||
previousReading = newReading;
|
||||
|
||||
if (snrReady) {
|
||||
|
||||
for (uint8_t i = 0; i < SNR_SAMPLES; i++) {
|
||||
pseudoSnr += snrSamples[i];
|
||||
}
|
||||
|
||||
return constrain(pseudoSnr, 0, 32000);
|
||||
} else {
|
||||
return RANGEFINDER_OUT_OF_RANGE;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* This is called periodically by the scheduler
|
||||
*/
|
||||
// XXX Returns timeDelta_t for iNav for pseudo-RT scheduling.
|
||||
void rangefinderUpdate(timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
|
||||
if (rangefinder.dev.update) {
|
||||
rangefinder.dev.update(&rangefinder.dev);
|
||||
}
|
||||
|
||||
// return rangefinder.dev.delayMs * 1000; // to microseconds XXX iNav only
|
||||
}
|
||||
|
||||
bool isSurfaceAltitudeValid() {
|
||||
|
||||
/*
|
||||
* Preconditions: raw and calculated altidude > 0
|
||||
* SNR lower than threshold
|
||||
*/
|
||||
if (
|
||||
rangefinder.calculatedAltitude > 0 &&
|
||||
rangefinder.rawAltitude > 0 &&
|
||||
rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD
|
||||
) {
|
||||
|
||||
/*
|
||||
* When critical altitude was determined, distance reported by rangefinder
|
||||
* has to be lower than it to assume healthy readout
|
||||
*/
|
||||
if (rangefinder.snrThresholdReached) {
|
||||
return (rangefinder.rawAltitude < rangefinder.dynamicDistanceThreshold);
|
||||
} else {
|
||||
return true;
|
||||
}
|
||||
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned.
|
||||
*/
|
||||
bool rangefinderProcess(float cosTiltAngle)
|
||||
{
|
||||
if (rangefinder.dev.read) {
|
||||
const int32_t distance = rangefinder.dev.read(&rangefinder.dev);
|
||||
|
||||
// If driver reported no new measurement - don't do anything
|
||||
if (distance == RANGEFINDER_NO_NEW_DATA) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (distance >= 0) {
|
||||
rangefinder.lastValidResponseTimeMs = millis();
|
||||
rangefinder.rawAltitude = applyMedianFilter(distance);
|
||||
}
|
||||
else if (distance == RANGEFINDER_OUT_OF_RANGE) {
|
||||
rangefinder.lastValidResponseTimeMs = millis();
|
||||
rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
}
|
||||
else {
|
||||
// Invalid response / hardware failure
|
||||
rangefinder.rawAltitude = RANGEFINDER_HARDWARE_FAILURE;
|
||||
}
|
||||
|
||||
rangefinder.snr = computePseudoSnr(distance);
|
||||
|
||||
if (rangefinder.snrThresholdReached == false && rangefinder.rawAltitude > 0) {
|
||||
|
||||
if (rangefinder.snr < RANGEFINDER_DYNAMIC_THRESHOLD && rangefinder.dynamicDistanceThreshold < rangefinder.rawAltitude) {
|
||||
rangefinder.dynamicDistanceThreshold = rangefinder.rawAltitude * RANGEFINDER_DYNAMIC_FACTOR / 100;
|
||||
}
|
||||
|
||||
if (rangefinder.snr >= RANGEFINDER_DYNAMIC_THRESHOLD) {
|
||||
rangefinder.snrThresholdReached = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_RANGEFINDER, 3, rangefinder.snr);
|
||||
|
||||
DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 0, rangefinder.rawAltitude);
|
||||
DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 1, rangefinder.snrThresholdReached);
|
||||
DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 2, rangefinder.dynamicDistanceThreshold);
|
||||
DEBUG_SET(DEBUG_RANGEFINDER_QUALITY, 3, isSurfaceAltitudeValid());
|
||||
|
||||
}
|
||||
else {
|
||||
// Bad configuration
|
||||
rangefinder.rawAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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 large, RANGEFINDER_OUT_OF_RANGE is returned.
|
||||
*/
|
||||
if (cosTiltAngle < rangefinder.maxTiltCos || rangefinder.rawAltitude < 0) {
|
||||
rangefinder.calculatedAltitude = RANGEFINDER_OUT_OF_RANGE;
|
||||
} else {
|
||||
rangefinder.calculatedAltitude = rangefinder.rawAltitude * cosTiltAngle;
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_RANGEFINDER, 1, rangefinder.rawAltitude);
|
||||
DEBUG_SET(DEBUG_RANGEFINDER, 2, rangefinder.calculatedAltitude);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the latest altitude that was computed, or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude
|
||||
* has never been called.
|
||||
*/
|
||||
int32_t rangefinderGetLatestAltitude(void)
|
||||
{
|
||||
return rangefinder.calculatedAltitude;
|
||||
}
|
||||
|
||||
int32_t rangefinderGetLatestRawAltitude(void) {
|
||||
return rangefinder.rawAltitude;
|
||||
}
|
||||
|
||||
bool rangefinderIsHealthy(void)
|
||||
{
|
||||
return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS;
|
||||
}
|
||||
#endif
|
||||
|
||||
61
src/main/sensors/rangefinder.h
Normal file
61
src/main/sensors/rangefinder.h
Normal file
|
|
@ -0,0 +1,61 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include "pg/pg.h"
|
||||
#include "drivers/rangefinder/rangefinder.h"
|
||||
|
||||
typedef enum {
|
||||
RANGEFINDER_NONE = 0,
|
||||
RANGEFINDER_HCSR04 = 1,
|
||||
RANGEFINDER_SRF10 = 2,
|
||||
RANGEFINDER_HCSR04I2C = 3,
|
||||
RANGEFINDER_VL53L0X = 4,
|
||||
RANGEFINDER_UIB = 5,
|
||||
} rangefinderType_e;
|
||||
|
||||
typedef struct rangefinderConfig_s {
|
||||
uint8_t rangefinder_hardware;
|
||||
} rangefinderConfig_t;
|
||||
|
||||
PG_DECLARE(rangefinderConfig_t, rangefinderConfig);
|
||||
|
||||
typedef struct rangefinder_s {
|
||||
rangefinderDev_t dev;
|
||||
float maxTiltCos;
|
||||
int32_t rawAltitude;
|
||||
int32_t calculatedAltitude;
|
||||
timeMs_t lastValidResponseTimeMs;
|
||||
|
||||
bool snrThresholdReached;
|
||||
int32_t dynamicDistanceThreshold;
|
||||
int16_t snr;
|
||||
} rangefinder_t;
|
||||
|
||||
extern rangefinder_t rangefinder;
|
||||
|
||||
void rangefinderResetDynamicThreshold(void);
|
||||
bool rangefinderInit(void);
|
||||
|
||||
int32_t rangefinderGetLatestAltitude(void);
|
||||
int32_t rangefinderGetLatestRawAltitude(void);
|
||||
|
||||
void rangefinderUpdate(timeUs_t currentTimeUs);
|
||||
bool rangefinderProcess(float cosTiltAngle);
|
||||
bool rangefinderIsHealthy(void);
|
||||
|
|
@ -22,9 +22,11 @@ typedef enum {
|
|||
SENSOR_INDEX_ACC,
|
||||
SENSOR_INDEX_BARO,
|
||||
SENSOR_INDEX_MAG,
|
||||
SENSOR_INDEX_RANGEFINDER,
|
||||
SENSOR_INDEX_COUNT
|
||||
} sensorIndex_e;
|
||||
|
||||
extern uint8_t requestedSensors[SENSOR_INDEX_COUNT];
|
||||
extern uint8_t detectedSensors[SENSOR_INDEX_COUNT];
|
||||
|
||||
typedef struct int16_flightDynamicsTrims_s {
|
||||
|
|
@ -48,6 +50,7 @@ typedef enum {
|
|||
SENSOR_BARO = 1 << 2,
|
||||
SENSOR_MAG = 1 << 3,
|
||||
SENSOR_SONAR = 1 << 4,
|
||||
SENSOR_RANGEFINDER = 1 << 4,
|
||||
SENSOR_GPS = 1 << 5,
|
||||
SENSOR_GPSMAG = 1 << 6
|
||||
} sensors_e;
|
||||
|
|
|
|||
|
|
@ -1,153 +0,0 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#ifdef USE_SONAR
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
#include "drivers/io.h"
|
||||
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sonar.h"
|
||||
|
||||
// Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range.
|
||||
// Inclination is adjusted by imu
|
||||
|
||||
int16_t sonarMaxRangeCm;
|
||||
int16_t sonarMaxAltWithTiltCm;
|
||||
int16_t sonarCfAltCm; // Complimentary Filter altitude
|
||||
STATIC_UNIT_TESTED int16_t sonarMaxTiltDeciDegrees;
|
||||
float sonarMaxTiltCos;
|
||||
|
||||
static int32_t calculatedAltitude;
|
||||
|
||||
PG_REGISTER_WITH_RESET_TEMPLATE(sonarConfig_t, sonarConfig, PG_SONAR_CONFIG, 0);
|
||||
|
||||
#ifndef SONAR_TRIGGER_PIN
|
||||
#define SONAR_TRIGGER_PIN NONE
|
||||
#endif
|
||||
#ifndef SONAR_ECHO_PIN
|
||||
#define SONAR_ECHO_PIN NONE
|
||||
#endif
|
||||
|
||||
PG_RESET_TEMPLATE(sonarConfig_t, sonarConfig,
|
||||
.triggerTag = IO_TAG(SONAR_TRIGGER_PIN),
|
||||
.echoTag = IO_TAG(SONAR_ECHO_PIN)
|
||||
);
|
||||
|
||||
void sonarInit(const sonarConfig_t *sonarConfig)
|
||||
{
|
||||
sonarRange_t sonarRange;
|
||||
|
||||
hcsr04_init(sonarConfig, &sonarRange);
|
||||
|
||||
sensorsSet(SENSOR_SONAR);
|
||||
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(timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
hcsr04_start_reading();
|
||||
}
|
||||
|
||||
/**
|
||||
* 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)
|
||||
{
|
||||
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 large, SONAR_OUT_OF_RANGE is returned.
|
||||
*/
|
||||
int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle)
|
||||
{
|
||||
// calculate sonar altitude only if the ground is in the sonar cone
|
||||
if (cosTiltAngle <= sonarMaxTiltCos)
|
||||
calculatedAltitude = SONAR_OUT_OF_RANGE;
|
||||
else
|
||||
// altitude = distance * cos(tiltAngle), use approximation
|
||||
calculatedAltitude = sonarDistance * cosTiltAngle;
|
||||
return calculatedAltitude;
|
||||
}
|
||||
|
||||
/**
|
||||
* 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)
|
||||
{
|
||||
return calculatedAltitude;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
@ -1,37 +0,0 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "common/time.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#define SONAR_OUT_OF_RANGE (-1)
|
||||
|
||||
extern int16_t sonarMaxRangeCm;
|
||||
extern int16_t sonarCfAltCm;
|
||||
extern int16_t sonarMaxAltWithTiltCm;
|
||||
|
||||
PG_DECLARE(sonarConfig_t, sonarConfig);
|
||||
|
||||
void sonarInit(const sonarConfig_t *sonarConfig);
|
||||
void sonarUpdate(timeUs_t currentTimeUs);
|
||||
int32_t sonarRead(void);
|
||||
int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle);
|
||||
int32_t sonarGetLatestAltitude(void);
|
||||
Loading…
Add table
Add a link
Reference in a new issue