Port iNav's rangefinder
This commit is contained in:
parent
8a9fd29dd3
commit
11c47c631b
|
@ -143,8 +143,8 @@ FC_SRC = \
|
|||
common/gps_conversion.c \
|
||||
drivers/display_ug2864hsweg01.c \
|
||||
drivers/light_ws2811strip.c \
|
||||
drivers/rangefinder/rangefinder_hcsr04.c \
|
||||
drivers/serial_escserial.c \
|
||||
drivers/sonar_hcsr04.c \
|
||||
drivers/vtx_common.c \
|
||||
flight/navigation.c \
|
||||
io/dashboard.c \
|
||||
|
@ -159,8 +159,8 @@ FC_SRC = \
|
|||
io/gps.c \
|
||||
io/ledstrip.c \
|
||||
io/osd.c \
|
||||
sensors/sonar.c \
|
||||
sensors/barometer.c \
|
||||
sensors/rangefinder.c \
|
||||
telemetry/telemetry.c \
|
||||
telemetry/crsf.c \
|
||||
telemetry/srxl.c \
|
||||
|
|
|
@ -69,7 +69,7 @@
|
|||
#include "sensors/battery.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/sonar.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
||||
enum {
|
||||
BLACKBOX_MODE_NORMAL = 0,
|
||||
|
@ -201,8 +201,8 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
|
|||
#ifdef USE_BARO
|
||||
{"BaroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
|
||||
#endif
|
||||
#ifdef USE_SONAR
|
||||
{"sonarRaw", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_SONAR},
|
||||
#ifdef USE_RANGEFINDER
|
||||
{"surfaceRaw", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_RANGEFINDER},
|
||||
#endif
|
||||
{"rssi", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_RSSI},
|
||||
|
||||
|
@ -303,8 +303,8 @@ typedef struct blackboxMainState_s {
|
|||
#ifdef USE_MAG
|
||||
int16_t magADC[XYZ_AXIS_COUNT];
|
||||
#endif
|
||||
#ifdef USE_SONAR
|
||||
int32_t sonarRaw;
|
||||
#ifdef USE_RANGEFINDER
|
||||
int32_t surfaceRaw;
|
||||
#endif
|
||||
uint16_t rssi;
|
||||
} blackboxMainState_t;
|
||||
|
@ -437,9 +437,9 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
|
|||
case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC:
|
||||
return (batteryConfig()->currentMeterSource != CURRENT_METER_NONE) && (batteryConfig()->currentMeterSource != CURRENT_METER_VIRTUAL);
|
||||
|
||||
case FLIGHT_LOG_FIELD_CONDITION_SONAR:
|
||||
#ifdef USE_SONAR
|
||||
return feature(FEATURE_SONAR);
|
||||
case FLIGHT_LOG_FIELD_CONDITION_RANGEFINDER:
|
||||
#ifdef USE_RANGEFINDER
|
||||
return sensors(SENSOR_RANGEFINDER);
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
|
@ -568,9 +568,9 @@ static void writeIntraframe(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_SONAR
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->sonarRaw);
|
||||
#ifdef USE_RANGEFINDER
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RANGEFINDER)) {
|
||||
blackboxWriteSignedVB(blackboxCurrent->surfaceRaw);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -698,9 +698,9 @@ static void writeInterframe(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_SONAR
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
|
||||
deltas[optionalFieldCount++] = blackboxCurrent->sonarRaw - blackboxLast->sonarRaw;
|
||||
#ifdef USE_RANGEFINDER
|
||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_RANGEFINDER)) {
|
||||
deltas[optionalFieldCount++] = blackboxCurrent->surfaceRaw - blackboxLast->surfaceRaw;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
@ -1022,9 +1022,9 @@ static void loadMainState(timeUs_t currentTimeUs)
|
|||
blackboxCurrent->BaroAlt = baro.BaroAlt;
|
||||
#endif
|
||||
|
||||
#ifdef USE_SONAR
|
||||
#ifdef USE_RANGEFINDER
|
||||
// Store the raw sonar value without applying tilt correction
|
||||
blackboxCurrent->sonarRaw = sonarRead();
|
||||
blackboxCurrent->surfaceRaw = rangefinderGetLatestAltitude();
|
||||
#endif
|
||||
|
||||
blackboxCurrent->rssi = getRssi();
|
||||
|
|
|
@ -33,7 +33,7 @@ typedef enum FlightLogFieldCondition {
|
|||
FLIGHT_LOG_FIELD_CONDITION_BARO,
|
||||
FLIGHT_LOG_FIELD_CONDITION_VBAT,
|
||||
FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC,
|
||||
FLIGHT_LOG_FIELD_CONDITION_SONAR,
|
||||
FLIGHT_LOG_FIELD_CONDITION_RANGEFINDER,
|
||||
FLIGHT_LOG_FIELD_CONDITION_RSSI,
|
||||
|
||||
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0,
|
||||
|
|
|
@ -56,4 +56,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
|
|||
"MAX7456_SPICLOCK",
|
||||
"SBUS",
|
||||
"FPORT",
|
||||
"RANGEFINDER",
|
||||
"RANGEFINDER_QUALITY",
|
||||
};
|
||||
|
|
|
@ -74,6 +74,8 @@ typedef enum {
|
|||
DEBUG_MAX7456_SPICLOCK,
|
||||
DEBUG_SBUS,
|
||||
DEBUG_FPORT,
|
||||
DEBUG_RANGEFINDER,
|
||||
DEBUG_RANGEFINDER_QUALITY,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
@ -17,24 +17,40 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include "common/time.h"
|
||||
#include "drivers/io_types.h"
|
||||
|
||||
typedef struct sonarConfig_s {
|
||||
#define RANGEFINDER_OUT_OF_RANGE (-1)
|
||||
#define RANGEFINDER_HARDWARE_FAILURE (-2)
|
||||
#define RANGEFINDER_NO_NEW_DATA (-3)
|
||||
|
||||
struct rangefinderDev_s;
|
||||
|
||||
typedef struct rangefinderHardwarePins_s {
|
||||
ioTag_t triggerTag;
|
||||
ioTag_t echoTag;
|
||||
} sonarConfig_t;
|
||||
} rangefinderHardwarePins_t;
|
||||
|
||||
typedef struct sonarRange_s {
|
||||
struct rangefinderDev_s;
|
||||
|
||||
typedef void (*rangefinderOpInitFuncPtr)(struct rangefinderDev_s * dev);
|
||||
typedef void (*rangefinderOpStartFuncPtr)(struct rangefinderDev_s * dev);
|
||||
typedef int32_t (*rangefinderOpReadFuncPtr)(struct rangefinderDev_s * dev);
|
||||
|
||||
typedef struct rangefinderDev_s {
|
||||
timeMs_t delayMs;
|
||||
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 detectionConeDeciDegrees; // detection cone angle as in device spec
|
||||
int16_t detectionConeExtendedDeciDegrees; // device spec is conservative, in practice have slightly larger detection cone
|
||||
} sonarRange_t;
|
||||
|
||||
#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
|
||||
// function pointers
|
||||
rangefinderOpInitFuncPtr init;
|
||||
rangefinderOpStartFuncPtr update;
|
||||
rangefinderOpReadFuncPtr read;
|
||||
} rangefinderDev_t;
|
||||
|
||||
void hcsr04_init(const sonarConfig_t *sonarConfig, sonarRange_t *sonarRange);
|
||||
void hcsr04_start_reading(void);
|
||||
int32_t hcsr04_get_distance(void);
|
||||
extern int16_t rangefinderMaxRangeCm;
|
||||
extern int16_t rangefinderMaxAltWithTiltCm;
|
||||
extern int16_t rangefinderCfAltCm;
|
|
@ -0,0 +1,226 @@
|
|||
/*
|
||||
* 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 <platform.h>
|
||||
|
||||
#if defined(USE_RANGEFINDER_HCSR04)
|
||||
|
||||
#include "build/build_config.h"
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/time.h"
|
||||
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/rcc.h"
|
||||
|
||||
#include "drivers/rangefinder/rangefinder.h"
|
||||
#include "drivers/rangefinder/rangefinder_hcsr04.h"
|
||||
|
||||
#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
|
||||
|
||||
|
||||
/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
|
||||
* 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 ***
|
||||
*
|
||||
*/
|
||||
|
||||
static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
|
||||
static volatile timeMs_t lastMeasurementReceivedAt;
|
||||
static volatile int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
|
||||
static timeMs_t lastMeasurementStartedAt = 0;
|
||||
|
||||
#ifdef USE_EXTI
|
||||
static extiCallbackRec_t hcsr04_extiCallbackRec;
|
||||
#endif
|
||||
|
||||
static IO_t echoIO;
|
||||
static IO_t triggerIO;
|
||||
|
||||
#if !defined(UNIT_TEST)
|
||||
void hcsr04_extiHandler(extiCallbackRec_t* cb)
|
||||
{
|
||||
static timeUs_t timing_start;
|
||||
UNUSED(cb);
|
||||
|
||||
if (IORead(echoIO) != 0) {
|
||||
timing_start = micros();
|
||||
} else {
|
||||
const timeUs_t timing_stop = micros();
|
||||
if (timing_stop > timing_start) {
|
||||
lastMeasurementReceivedAt = millis();
|
||||
hcsr04SonarPulseTravelTime = timing_stop - timing_start;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void hcsr04_init(rangefinderDev_t *dev)
|
||||
{
|
||||
UNUSED(dev);
|
||||
}
|
||||
|
||||
#define HCSR04_MinimumFiringIntervalMs 60
|
||||
|
||||
/*
|
||||
* Start a range reading
|
||||
* Called periodically by the scheduler
|
||||
* Measurement reading is done asynchronously, using interrupt
|
||||
*/
|
||||
void hcsr04_start_reading(void)
|
||||
{
|
||||
#if !defined(UNIT_TEST)
|
||||
#ifdef RANGEFINDER_HCSR04_TRIG_INVERTED
|
||||
IOLo(triggerIO);
|
||||
delayMicroseconds(11);
|
||||
IOHi(triggerIO);
|
||||
#else
|
||||
IOHi(triggerIO);
|
||||
delayMicroseconds(11);
|
||||
IOLo(triggerIO);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
void hcsr04_update(rangefinderDev_t *dev)
|
||||
{
|
||||
UNUSED(dev);
|
||||
const timeMs_t timeNowMs = millis();
|
||||
|
||||
// the firing interval of the trigger signal should be greater than 60ms
|
||||
// to avoid interference between consecutive measurements
|
||||
if (timeNowMs > lastMeasurementStartedAt + HCSR04_MinimumFiringIntervalMs) {
|
||||
// We should have a valid measurement within 60ms of trigger
|
||||
if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MinimumFiringIntervalMs) {
|
||||
// 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
|
||||
|
||||
lastCalculatedDistance = hcsr04SonarPulseTravelTime / 59;
|
||||
if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) {
|
||||
lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// No measurement within reasonable time - indicate failure
|
||||
lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
|
||||
}
|
||||
|
||||
// Trigger a new measurement
|
||||
lastMeasurementStartedAt = timeNowMs;
|
||||
hcsr04_start_reading();
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the distance that was measured by the last pulse, in centimeters.
|
||||
*/
|
||||
int32_t hcsr04_get_distance(rangefinderDev_t *dev)
|
||||
{
|
||||
UNUSED(dev);
|
||||
return lastCalculatedDistance;
|
||||
}
|
||||
|
||||
bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwarePins)
|
||||
{
|
||||
bool detected = false;
|
||||
|
||||
#ifdef STM32F10X
|
||||
// enable AFIO for EXTI support
|
||||
RCC_ClockCmd(RCC_APB2(AFIO), ENABLE);
|
||||
#endif
|
||||
|
||||
#if defined(STM32F3) || defined(STM32F4)
|
||||
RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); // XXX Do we need this?
|
||||
#endif
|
||||
|
||||
triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag);
|
||||
echoIO = IOGetByTag(rangefinderHardwarePins->echoTag);
|
||||
|
||||
if (IOGetOwner(triggerIO) != OWNER_FREE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (IOGetOwner(echoIO) != OWNER_FREE) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// trigger pin
|
||||
IOInit(triggerIO, OWNER_SONAR_TRIGGER, 0);
|
||||
IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
|
||||
IOLo(triggerIO);
|
||||
delay(100);
|
||||
|
||||
// echo pin
|
||||
IOInit(echoIO, OWNER_SONAR_ECHO, 0);
|
||||
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
|
||||
|
||||
// HC-SR04 echo line should be low by default and should return a response pulse when triggered
|
||||
if (IORead(echoIO) == false) {
|
||||
for (int i = 0; i < 5 && !detected; i++) {
|
||||
timeMs_t requestTime = millis();
|
||||
hcsr04_start_reading();
|
||||
|
||||
while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) {
|
||||
if (IORead(echoIO) == true) {
|
||||
detected = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (detected) {
|
||||
// Hardware detected - configure the driver
|
||||
#ifdef USE_EXTI
|
||||
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
|
||||
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
|
||||
EXTIEnable(echoIO, true);
|
||||
#endif
|
||||
|
||||
dev->delayMs = 100;
|
||||
dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
|
||||
dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
|
||||
dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
|
||||
|
||||
dev->init = &hcsr04_init;
|
||||
dev->update = &hcsr04_update;
|
||||
dev->read = &hcsr04_get_distance;
|
||||
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
// Not detected - free resources
|
||||
IORelease(triggerIO);
|
||||
IORelease(echoIO);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
|
@ -19,19 +19,16 @@
|
|||
|
||||
#include "pg/pg.h"
|
||||
#include "common/time.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/rangefinder/rangefinder.h"
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#define SONAR_OUT_OF_RANGE (-1)
|
||||
#define RANGEFINDER_HCSR04_TASK_PERIOD_MS 70
|
||||
|
||||
extern int16_t sonarMaxRangeCm;
|
||||
extern int16_t sonarCfAltCm;
|
||||
extern int16_t sonarMaxAltWithTiltCm;
|
||||
typedef struct sonarConfig_s {
|
||||
ioTag_t triggerTag;
|
||||
ioTag_t echoTag;
|
||||
} sonarConfig_t;
|
||||
|
||||
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);
|
||||
bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * sonarConfig);
|
|
@ -68,4 +68,5 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
|
|||
"ESCSERIAL",
|
||||
"CAMERA_CONTROL",
|
||||
"TIMUP",
|
||||
"RANGEFINDER",
|
||||
};
|
||||
|
|
|
@ -68,6 +68,7 @@ typedef enum {
|
|||
OWNER_ESCSERIAL,
|
||||
OWNER_CAMERA_CONTROL,
|
||||
OWNER_TIMUP,
|
||||
OWNER_RANGEFINDER,
|
||||
OWNER_TOTAL_COUNT
|
||||
} resourceOwner_e;
|
||||
|
||||
|
|
|
@ -1,142 +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 "platform.h"
|
||||
|
||||
#if defined(USE_SONAR)
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "drivers/exti.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
/* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
|
||||
* 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 ***
|
||||
*
|
||||
*/
|
||||
|
||||
STATIC_UNIT_TESTED volatile int32_t measurement = -1;
|
||||
static uint32_t lastMeasurementAt;
|
||||
|
||||
extiCallbackRec_t hcsr04_extiCallbackRec;
|
||||
|
||||
static IO_t echoIO;
|
||||
static IO_t triggerIO;
|
||||
|
||||
void hcsr04_extiHandler(extiCallbackRec_t* cb)
|
||||
{
|
||||
static uint32_t timing_start;
|
||||
uint32_t timing_stop;
|
||||
UNUSED(cb);
|
||||
|
||||
if (IORead(echoIO) != 0) {
|
||||
timing_start = micros();
|
||||
}
|
||||
else {
|
||||
timing_stop = micros();
|
||||
if (timing_stop > timing_start) {
|
||||
measurement = timing_stop - timing_start;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void hcsr04_init(const sonarConfig_t *sonarConfig, sonarRange_t *sonarRange)
|
||||
{
|
||||
sonarRange->maxRangeCm = HCSR04_MAX_RANGE_CM;
|
||||
sonarRange->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
|
||||
sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
|
||||
|
||||
#if !defined(UNIT_TEST)
|
||||
|
||||
#ifdef STM32F10X
|
||||
// enable AFIO for EXTI support
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
|
||||
#endif
|
||||
|
||||
#if defined(STM32F3) || defined(STM32F4)
|
||||
/* Enable SYSCFG clock otherwise the EXTI irq handlers are not called */
|
||||
RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
|
||||
#endif
|
||||
|
||||
// trigger pin
|
||||
triggerIO = IOGetByTag(sonarConfig->triggerTag);
|
||||
IOInit(triggerIO, OWNER_SONAR_TRIGGER, 0);
|
||||
IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
|
||||
|
||||
// echo pin
|
||||
echoIO = IOGetByTag(sonarConfig->echoTag);
|
||||
IOInit(echoIO, OWNER_SONAR_ECHO, 0);
|
||||
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
|
||||
|
||||
#ifdef USE_EXTI
|
||||
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
|
||||
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
|
||||
EXTIEnable(echoIO, true);
|
||||
#endif
|
||||
|
||||
lastMeasurementAt = millis() - 60; // force 1st measurement in hcsr04_get_distance()
|
||||
#else
|
||||
UNUSED(lastMeasurementAt); // 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)) {
|
||||
// the repeat interval of trig signal should be greater than 60ms
|
||||
// to avoid interference between connective measurements.
|
||||
return;
|
||||
}
|
||||
|
||||
lastMeasurementAt = now;
|
||||
|
||||
IOHi(triggerIO);
|
||||
// The width of trig signal must be greater than 10us
|
||||
delayMicroseconds(11);
|
||||
IOLo(triggerIO);
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the distance that was measured by the last pulse, in centimeters.
|
||||
*/
|
||||
int32_t hcsr04_get_distance(void)
|
||||
{
|
||||
// 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 distance = measurement / 59;
|
||||
|
||||
return distance;
|
||||
}
|
||||
#endif
|
|
@ -53,7 +53,6 @@
|
|||
#include "drivers/rx_spi.h"
|
||||
#include "drivers/sdcard.h"
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/sound_beeper.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/timer.h"
|
||||
|
@ -403,8 +402,8 @@ static void validateAndFixConfig(void)
|
|||
featureClear(FEATURE_GPS);
|
||||
#endif
|
||||
|
||||
#ifndef USE_SONAR
|
||||
featureClear(FEATURE_SONAR);
|
||||
#ifndef USE_RANGEFINDER
|
||||
featureClear(FEATURE_RANGEFINDER);
|
||||
#endif
|
||||
|
||||
#ifndef USE_TELEMETRY
|
||||
|
|
|
@ -38,7 +38,7 @@ typedef enum {
|
|||
FEATURE_SERVO_TILT = 1 << 5,
|
||||
FEATURE_SOFTSERIAL = 1 << 6,
|
||||
FEATURE_GPS = 1 << 7,
|
||||
FEATURE_SONAR = 1 << 9,
|
||||
FEATURE_RANGEFINDER = 1 << 9,
|
||||
FEATURE_TELEMETRY = 1 << 10,
|
||||
FEATURE_3D = 1 << 12,
|
||||
FEATURE_RX_PARALLEL_PWM = 1 << 13,
|
||||
|
|
|
@ -627,8 +627,8 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
|
|||
#if defined(USE_ALT_HOLD)
|
||||
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
|
||||
updateRcCommands();
|
||||
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
|
||||
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
|
||||
if (sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER)) {
|
||||
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_MODE)) {
|
||||
applyAltHold();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -60,7 +60,6 @@
|
|||
#include "drivers/buttons.h"
|
||||
#include "drivers/inverter.h"
|
||||
#include "drivers/flash_m25p16.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/sdcard.h"
|
||||
#include "drivers/usb_io.h"
|
||||
#include "drivers/transponder_ir.h"
|
||||
|
@ -121,7 +120,6 @@
|
|||
#include "sensors/gyro.h"
|
||||
#include "sensors/initialisation.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/sonar.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
|
@ -473,14 +471,16 @@ void init(void)
|
|||
cameraControlInit();
|
||||
#endif
|
||||
|
||||
#if defined(SONAR_SOFTSERIAL2_EXCLUSIVE) && defined(USE_SONAR) && defined(USE_SOFTSERIAL2)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
||||
// XXX These kind of code should goto target/config.c?
|
||||
// XXX And these no longer work properly as FEATURE_RANGEFINDER does control HCSR04 runtime configuration.
|
||||
#if defined(RANGEFINDER_HCSR04_SOFTSERIAL2_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL2)
|
||||
if (feature(FEATURE_RANGEFINDER) && feature(FEATURE_SOFTSERIAL)) {
|
||||
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if defined(SONAR_SOFTSERIAL1_EXCLUSIVE) && defined(USE_SONAR) && defined(USE_SOFTSERIAL1)
|
||||
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
|
||||
#if defined(RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE) && defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1)
|
||||
if (feature(FEATURE_RANGEFINDER) && feature(FEATURE_SOFTSERIAL)) {
|
||||
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
|
||||
}
|
||||
#endif
|
||||
|
|
|
@ -76,8 +76,8 @@
|
|||
#include "sensors/compass.h"
|
||||
#include "sensors/esc_sensor.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/sonar.h"
|
||||
|
||||
#include "scheduler/scheduler.h"
|
||||
|
||||
|
@ -87,10 +87,6 @@
|
|||
void taskBstMasterProcess(timeUs_t currentTimeUs);
|
||||
#endif
|
||||
|
||||
#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
|
||||
#define TASK_PERIOD_MS(ms) ((ms) * 1000)
|
||||
#define TASK_PERIOD_US(us) (us)
|
||||
|
||||
bool taskSerialCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
|
@ -151,9 +147,9 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_SONAR
|
||||
if (sensors(SENSOR_SONAR)) {
|
||||
updateSonarAltHoldState();
|
||||
#ifdef USE_RANGEFINDER
|
||||
if (sensors(SENSOR_RANGEFINDER)) {
|
||||
updateRangefinderAltHoldState();
|
||||
}
|
||||
#endif
|
||||
#endif // USE_ALT_HOLD
|
||||
|
@ -183,20 +179,20 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
static void taskCalculateAltitude(timeUs_t currentTimeUs)
|
||||
{
|
||||
if (false
|
||||
#if defined(USE_BARO)
|
||||
|| (sensors(SENSOR_BARO) && isBaroReady())
|
||||
#endif
|
||||
#if defined(USE_SONAR)
|
||||
|| sensors(SENSOR_SONAR)
|
||||
#if defined(USE_RANGEFINDER)
|
||||
|| sensors(SENSOR_RANGEFINDER)
|
||||
#endif
|
||||
) {
|
||||
calculateEstimatedAltitude(currentTimeUs);
|
||||
}}
|
||||
#endif // USE_BARO || USE_SONAR
|
||||
#endif // USE_BARO || USE_RANGEFINDER
|
||||
|
||||
#ifdef USE_TELEMETRY
|
||||
static void taskTelemetry(timeUs_t currentTimeUs)
|
||||
|
@ -292,11 +288,11 @@ void fcTasksInit(void)
|
|||
#ifdef USE_BARO
|
||||
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
|
||||
#endif
|
||||
#ifdef USE_SONAR
|
||||
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
|
||||
#ifdef USE_RANGEFINDER
|
||||
setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER));
|
||||
#endif
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER));
|
||||
#endif
|
||||
#ifdef USE_DASHBOARD
|
||||
setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
|
||||
|
@ -492,16 +488,16 @@ cfTask_t cfTasks[TASK_COUNT] = {
|
|||
},
|
||||
#endif
|
||||
|
||||
#ifdef USE_SONAR
|
||||
[TASK_SONAR] = {
|
||||
.taskName = "SONAR",
|
||||
.taskFunc = sonarUpdate,
|
||||
.desiredPeriod = TASK_PERIOD_MS(70), // 70ms required so that SONAR pulses do not interfere with each other
|
||||
#ifdef USE_RANGEFINDER
|
||||
[TASK_RANGEFINDER] = {
|
||||
.taskName = "RANGEFINDER",
|
||||
.taskFunc = rangefinderUpdate,
|
||||
.desiredPeriod = TASK_PERIOD_MS(70), // XXX HCSR04 sonar specific value.
|
||||
.staticPriority = TASK_PRIORITY_LOW,
|
||||
},
|
||||
#endif
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
[TASK_ALTITUDE] = {
|
||||
.taskName = "ALTITUDE",
|
||||
.taskFunc = taskCalculateAltitude,
|
||||
|
|
|
@ -19,4 +19,8 @@
|
|||
|
||||
#define LOOPTIME_SUSPEND_TIME 3 // Prevent too long busy wait times
|
||||
|
||||
#define TASK_PERIOD_HZ(hz) (1000000 / (hz))
|
||||
#define TASK_PERIOD_MS(ms) ((ms) * 1000)
|
||||
#define TASK_PERIOD_US(us) (us)
|
||||
|
||||
void fcTasksInit(void);
|
||||
|
|
|
@ -44,7 +44,7 @@ typedef enum {
|
|||
BOXOSD,
|
||||
BOXTELEMETRY,
|
||||
BOXGTUNE,
|
||||
BOXSONAR,
|
||||
BOXRANGEFINDER,
|
||||
BOXSERVO1,
|
||||
BOXSERVO2,
|
||||
BOXSERVO3,
|
||||
|
|
|
@ -75,7 +75,7 @@ typedef enum {
|
|||
HEADFREE_MODE = (1 << 6),
|
||||
UNUSED_MODE = (1 << 7), // old autotune
|
||||
PASSTHRU_MODE = (1 << 8),
|
||||
SONAR_MODE = (1 << 9),
|
||||
RANGEFINDER_MODE= (1 << 9),
|
||||
FAILSAFE_MODE = (1 << 10)
|
||||
} flightModeFlags_e;
|
||||
|
||||
|
@ -90,7 +90,7 @@ extern uint16_t flightModeFlags;
|
|||
// It is much more memory efficient than full map (uint32_t -> uint8_t)
|
||||
#define FLIGHT_MODE_BOXID_MAP_INITIALIZER { \
|
||||
BOXANGLE, BOXHORIZON, BOXMAG, BOXBARO, BOXGPSHOME, BOXGPSHOLD, \
|
||||
BOXHEADFREE, -1, BOXPASSTHRU, BOXSONAR, BOXFAILSAFE} \
|
||||
BOXHEADFREE, -1, BOXPASSTHRU, BOXRANGEFINDER, BOXFAILSAFE} \
|
||||
/**/
|
||||
|
||||
typedef enum {
|
||||
|
|
|
@ -44,7 +44,7 @@
|
|||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/sonar.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
||||
|
||||
int32_t AltHold;
|
||||
|
@ -146,16 +146,16 @@ void updateAltHoldState(void)
|
|||
}
|
||||
}
|
||||
|
||||
void updateSonarAltHoldState(void)
|
||||
void updateRangefinderAltHoldState(void)
|
||||
{
|
||||
// Sonar alt hold activate
|
||||
if (!IS_RC_MODE_ACTIVE(BOXSONAR)) {
|
||||
DISABLE_FLIGHT_MODE(SONAR_MODE);
|
||||
if (!IS_RC_MODE_ACTIVE(BOXRANGEFINDER)) {
|
||||
DISABLE_FLIGHT_MODE(RANGEFINDER_MODE);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!FLIGHT_MODE(SONAR_MODE)) {
|
||||
ENABLE_FLIGHT_MODE(SONAR_MODE);
|
||||
if (!FLIGHT_MODE(RANGEFINDER_MODE)) {
|
||||
ENABLE_FLIGHT_MODE(RANGEFINDER_MODE);
|
||||
AltHold = estimatedAltitude;
|
||||
initialThrottleHold = rcData[THROTTLE];
|
||||
errorVelocityI = 0;
|
||||
|
@ -205,7 +205,7 @@ int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, floa
|
|||
}
|
||||
#endif // USE_ALT_HOLD
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
|
||||
{
|
||||
static timeUs_t previousTimeUs = 0;
|
||||
|
@ -232,14 +232,14 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_SONAR
|
||||
if (sensors(SENSOR_SONAR)) {
|
||||
int32_t sonarAlt = sonarCalculateAltitude(sonarRead(), getCosTiltAngle());
|
||||
if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
|
||||
// SONAR in range, so use complementary filter
|
||||
float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
|
||||
sonarAlt = (float)sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition);
|
||||
estimatedAltitude = sonarAlt;
|
||||
#ifdef USE_RANGEFINDER
|
||||
if (sensors(SENSOR_RANGEFINDER) && rangefinderProcess(getCosTiltAngle())) {
|
||||
int32_t rangefinderAlt = rangefinderGetLatestAltitude();
|
||||
if (rangefinderAlt > 0 && rangefinderAlt >= rangefinderCfAltCm && rangefinderAlt <= rangefinderMaxAltWithTiltCm) {
|
||||
// RANGEFINDER in range, so use complementary filter
|
||||
float rangefinderTransition = (float)(rangefinderMaxAltWithTiltCm - rangefinderAlt) / (rangefinderMaxAltWithTiltCm - rangefinderCfAltCm);
|
||||
rangefinderAlt = (float)rangefinderAlt * rangefinderTransition + baroAlt * (1.0f - rangefinderTransition);
|
||||
estimatedAltitude = rangefinderAlt;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -301,7 +301,7 @@ void calculateEstimatedAltitude(timeUs_t currentTimeUs)
|
|||
UNUSED(accZ_tmp);
|
||||
#endif
|
||||
}
|
||||
#endif // USE_BARO || USE_SONAR
|
||||
#endif // USE_BARO || USE_RANGEFINDER
|
||||
|
||||
int32_t getEstimatedAltitude(void)
|
||||
{
|
||||
|
|
|
@ -33,4 +33,4 @@ int32_t getEstimatedVario(void);
|
|||
|
||||
void applyAltHold(void);
|
||||
void updateAltHoldState(void);
|
||||
void updateSonarAltHoldState(void);
|
||||
void updateRangefinderAltHoldState(void);
|
||||
|
|
|
@ -46,7 +46,6 @@
|
|||
#include "sensors/compass.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/sonar.h"
|
||||
|
||||
#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_MULTITHREAD)
|
||||
#include <stdio.h>
|
||||
|
|
|
@ -72,7 +72,7 @@ extern uint8_t __config_end;
|
|||
#include "drivers/sensor.h"
|
||||
#include "drivers/serial.h"
|
||||
#include "drivers/serial_escserial.h"
|
||||
#include "drivers/sonar_hcsr04.h"
|
||||
#include "drivers/rangefinder/rangefinder_hcsr04.h"
|
||||
#include "drivers/stack_check.h"
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/transponder_ir.h"
|
||||
|
@ -175,7 +175,7 @@ static const char * const mixerNames[] = {
|
|||
static const char * const featureNames[] = {
|
||||
"RX_PPM", "", "INFLIGHT_ACC_CAL", "RX_SERIAL", "MOTOR_STOP",
|
||||
"SERVO_TILT", "SOFTSERIAL", "GPS", "",
|
||||
"SONAR", "TELEMETRY", "", "3D", "RX_PARALLEL_PWM",
|
||||
"RANGEFINDER", "TELEMETRY", "", "3D", "RX_PARALLEL_PWM",
|
||||
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
|
||||
"", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
|
||||
"", "", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", "DYNAMIC_FILTER", NULL
|
||||
|
@ -192,13 +192,13 @@ static const rxFailsafeChannelMode_e rxFailsafeModesTable[RX_FAILSAFE_TYPE_COUNT
|
|||
#if defined(USE_SENSOR_NAMES)
|
||||
// sync this with sensors_e
|
||||
static const char * const sensorTypeNames[] = {
|
||||
"GYRO", "ACC", "BARO", "MAG", "SONAR", "GPS", "GPS+MAG", NULL
|
||||
"GYRO", "ACC", "BARO", "MAG", "RANGEFINDER", "GPS", "GPS+MAG", NULL
|
||||
};
|
||||
|
||||
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG)
|
||||
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_RANGEFINDER)
|
||||
|
||||
static const char * const *sensorHardwareNames[] = {
|
||||
lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware
|
||||
lookupTableGyroHardware, lookupTableAccHardware, lookupTableBaroHardware, lookupTableMagHardware, lookupTableRangefinderHardware
|
||||
};
|
||||
#endif // USE_SENSOR_NAMES
|
||||
|
||||
|
@ -2031,8 +2031,8 @@ static void cliFeature(char *cmdline)
|
|||
break;
|
||||
}
|
||||
#endif
|
||||
#ifndef USE_SONAR
|
||||
if (mask & FEATURE_SONAR) {
|
||||
#ifndef USE_RANGEFINDER
|
||||
if (mask & FEATURE_RANGEFINDER) {
|
||||
cliPrintLine("unavailable");
|
||||
break;
|
||||
}
|
||||
|
@ -3166,7 +3166,7 @@ const cliResourceValue_t resourceTable[] = {
|
|||
{ OWNER_PPMINPUT, PG_PPM_CONFIG, offsetof(ppmConfig_t, ioTag), 0 },
|
||||
{ OWNER_PWMINPUT, PG_PWM_CONFIG, offsetof(pwmConfig_t, ioTags[0]), PWM_INPUT_PORT_COUNT },
|
||||
#endif
|
||||
#ifdef USE_SONAR
|
||||
#ifdef USE_RANGEFINDER_HCSR04
|
||||
{ OWNER_SONAR_TRIGGER, PG_SONAR_CONFIG, offsetof(sonarConfig_t, triggerTag), 0 },
|
||||
{ OWNER_SONAR_ECHO, PG_SONAR_CONFIG, offsetof(sonarConfig_t, echoTag), 0 },
|
||||
#endif
|
||||
|
|
|
@ -107,11 +107,11 @@
|
|||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/esc_sensor.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/sonar.h"
|
||||
#include "sensors/esc_sensor.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
|
@ -733,7 +733,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
|
|||
#else
|
||||
sbufWriteU16(dst, 0);
|
||||
#endif
|
||||
sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4 | sensors(SENSOR_GYRO) << 5);
|
||||
sbufWriteU16(dst, sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4 | sensors(SENSOR_GYRO) << 5);
|
||||
sbufWriteData(dst, &flightModeFlags, 4); // unconditional part of flags, first 32 bits
|
||||
sbufWriteU8(dst, getCurrentPidProfileIndex());
|
||||
sbufWriteU16(dst, constrain(averageSystemLoadPercent, 0, 100));
|
||||
|
@ -849,7 +849,7 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
|
|||
break;
|
||||
|
||||
case MSP_ALTITUDE:
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
sbufWriteU32(dst, getEstimatedAltitude());
|
||||
#else
|
||||
sbufWriteU32(dst, 0);
|
||||
|
@ -858,8 +858,8 @@ static bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst)
|
|||
break;
|
||||
|
||||
case MSP_SONAR_ALTITUDE:
|
||||
#if defined(USE_SONAR)
|
||||
sbufWriteU32(dst, sonarGetLatestAltitude());
|
||||
#if defined(USE_RANGEFINDER)
|
||||
sbufWriteU32(dst, rangefinderGetLatestAltitude());
|
||||
#else
|
||||
sbufWriteU32(dst, 0);
|
||||
#endif
|
||||
|
|
|
@ -64,7 +64,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
|
|||
{ BOXOSD, "OSD DISABLE SW", 19 },
|
||||
{ BOXTELEMETRY, "TELEMETRY", 20 },
|
||||
{ BOXGTUNE, "GTUNE", 21 },
|
||||
{ BOXSONAR, "SONAR", 22 },
|
||||
{ BOXRANGEFINDER, "RANGEFINDER", 22 },
|
||||
{ BOXSERVO1, "SERVO1", 23 },
|
||||
{ BOXSERVO2, "SERVO2", 24 },
|
||||
{ BOXSERVO3, "SERVO3", 25 },
|
||||
|
@ -188,9 +188,9 @@ void initActiveBoxIds(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_SONAR
|
||||
if (feature(FEATURE_SONAR)) {
|
||||
BME(BOXSONAR);
|
||||
#ifdef USE_RANGEFINDER
|
||||
if (feature(FEATURE_RANGEFINDER)) { // XXX && sensors(SENSOR_RANGEFINDER)?
|
||||
BME(BOXRANGEFINDER);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
@ -78,6 +78,7 @@
|
|||
#include "sensors/compass.h"
|
||||
#include "sensors/esc_sensor.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
||||
#include "telemetry/frsky.h"
|
||||
#include "telemetry/telemetry.h"
|
||||
|
@ -109,6 +110,11 @@ const char * const lookupTableMagHardware[] = {
|
|||
"AUTO", "NONE", "HMC5883", "AK8975", "AK8963"
|
||||
};
|
||||
#endif
|
||||
#if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER)
|
||||
const char * const lookupTableRangefinderHardware[] = {
|
||||
"NONE", "HCSR04"
|
||||
};
|
||||
#endif
|
||||
|
||||
static const char * const lookupTableOffOn[] = {
|
||||
"OFF", "ON"
|
||||
|
@ -318,6 +324,9 @@ const lookupTableEntry_t lookupTables[] = {
|
|||
#ifdef USE_MAX7456
|
||||
{ lookupTableMax7456Clock, sizeof(lookupTableMax7456Clock) / sizeof(char *) },
|
||||
#endif
|
||||
#ifdef USE_RANGEFINDER
|
||||
{ lookupTableRangefinderHardware, sizeof(lookupTableRangefinderHardware) / sizeof(char *) },
|
||||
#endif
|
||||
};
|
||||
|
||||
const clivalue_t valueTable[] = {
|
||||
|
@ -829,6 +838,11 @@ const clivalue_t valueTable[] = {
|
|||
{ "camera_control_key_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 500 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, keyDelayMs) },
|
||||
{ "camera_control_internal_resistance", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 1000 }, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, internalResistance) },
|
||||
#endif
|
||||
|
||||
// PG_RANGEFINDER_CONFIG
|
||||
#ifdef USE_RANGEFINDER
|
||||
{ "rangefinder_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RANGEFINDER_HARDWARE }, PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_hardware) },
|
||||
#endif
|
||||
};
|
||||
|
||||
const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);
|
||||
|
|
|
@ -71,8 +71,12 @@ typedef enum {
|
|||
TABLE_BUS_TYPE,
|
||||
#ifdef USE_MAX7456
|
||||
TABLE_MAX7456_CLOCK,
|
||||
#endif
|
||||
#ifdef USE_RANGEFINDER
|
||||
TABLE_RANGEFINDER_HARDWARE,
|
||||
#endif
|
||||
LOOKUP_TABLE_COUNT
|
||||
|
||||
} lookupTableIndex_e;
|
||||
|
||||
typedef struct lookupTableEntry_s {
|
||||
|
@ -153,3 +157,5 @@ extern const char * const lookupTableBaroHardware[];
|
|||
|
||||
extern const char * const lookupTableMagHardware[];
|
||||
//extern const uint8_t lookupTableMagHardwareEntryCount;
|
||||
|
||||
extern const char * const lookupTableRangefinderHardware[];
|
||||
|
|
|
@ -117,7 +117,8 @@
|
|||
#define PG_MAX7456_CONFIG 524
|
||||
#define PG_FLYSKY_CONFIG 525
|
||||
#define PG_TIME_CONFIG 526
|
||||
#define PG_BETAFLIGHT_END 526
|
||||
#define PG_RANGEFINDER_CONFIG 527 // iNav
|
||||
#define PG_BETAFLIGHT_END 527
|
||||
|
||||
|
||||
// OSD configuration (subject to change)
|
||||
|
|
|
@ -71,10 +71,10 @@ typedef enum {
|
|||
#ifdef USE_BARO
|
||||
TASK_BARO,
|
||||
#endif
|
||||
#ifdef USE_SONAR
|
||||
TASK_SONAR,
|
||||
#ifdef USE_RANGEFINDER
|
||||
TASK_RANGEFINDER,
|
||||
#endif
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
TASK_ALTITUDE,
|
||||
#endif
|
||||
#ifdef USE_DASHBOARD
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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
|
||||
|
|
@ -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
|
|
@ -97,9 +97,10 @@
|
|||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
//#define USE_SONAR
|
||||
//#define SONAR_ECHO_PIN PB0
|
||||
//#define SONAR_TRIGGER_PIN PB5
|
||||
//#define USE_RANGEFINDER
|
||||
//#define USE_RANGEFINDER_HCSR04
|
||||
//#define RANGEFINDER_HCSR04_ECHO_PIN PB0
|
||||
//#define RANGEFINDER_HCSR04_TRIGGER_PIN PB5
|
||||
|
||||
#undef USE_MAG
|
||||
|
||||
|
@ -107,7 +108,8 @@
|
|||
#define SKIP_CLI_COMMAND_HELP
|
||||
//#undef USE_SERVOS
|
||||
#undef USE_BARO
|
||||
#undef USE_SONAR
|
||||
#undef USE_RANGEFINDER
|
||||
#undef USE_RANGEFINDER_HCSR04
|
||||
#undef USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
//#undef USE_SERIALRX_SPEKTRUM // SRXL, DSM2 and DSMX protocol
|
||||
//#undef USE_SERIALRX_SBUS // Frsky and Futaba receivers
|
||||
|
|
|
@ -52,11 +52,11 @@
|
|||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sonar.h"
|
||||
#include "sensors/acceleration.h"
|
||||
#include "sensors/barometer.h"
|
||||
#include "sensors/compass.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
|
||||
|
@ -173,7 +173,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
|||
{ BOXOSD, "OSD DISABLE SW;", 19 },
|
||||
{ BOXTELEMETRY, "TELEMETRY;", 20 },
|
||||
{ BOXGTUNE, "GTUNE;", 21 },
|
||||
{ BOXSONAR, "SONAR;", 22 },
|
||||
{ BOXRANGEFINDER, "RANGEFINDER;", 22 },
|
||||
{ BOXSERVO1, "SERVO1;", 23 },
|
||||
{ BOXSERVO2, "SERVO2;", 24 },
|
||||
{ BOXSERVO3, "SERVO3;", 25 },
|
||||
|
@ -294,7 +294,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
#else
|
||||
bstWrite16(0);
|
||||
#endif
|
||||
bstWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
|
||||
bstWrite16(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4);
|
||||
// BST the flags in the order we delivered them, ignoring BOXNAMES and BOXINDEXES
|
||||
// Requires new Multiwii protocol version to fix
|
||||
// It would be preferable to setting the enabled bits based on BOXINDEX.
|
||||
|
@ -319,7 +319,7 @@ static bool bstSlaveProcessFeedbackCommand(uint8_t bstRequest)
|
|||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXOSD)) << BOXOSD |
|
||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTELEMETRY)) << BOXTELEMETRY |
|
||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXGTUNE)) << BOXGTUNE |
|
||||
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
|
||||
IS_ENABLED(FLIGHT_MODE(RANGEFINDER_MODE)) << BOXRANGEFINDER |
|
||||
IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM |
|
||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX |
|
||||
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE;
|
||||
|
@ -628,7 +628,7 @@ static bool bstSlaveProcessWriteCommand(uint8_t bstWriteCommand)
|
|||
static bool bstSlaveUSBCommandFeedback(/*uint8_t bstFeedback*/)
|
||||
{
|
||||
bstWrite8(BST_USB_DEVICE_INFO_FRAME); //Sub CPU Device Info FRAME
|
||||
bstWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
|
||||
bstWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4);
|
||||
bstWrite8(0x00);
|
||||
bstWrite8(0x00);
|
||||
bstWrite8(0x00);
|
||||
|
@ -841,13 +841,13 @@ bool writeFCModeToBST(void)
|
|||
IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << 3 |
|
||||
IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << 4 |
|
||||
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << 5 |
|
||||
IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << 6 |
|
||||
IS_ENABLED(FLIGHT_MODE(RANGEFINDER_MODE)) << 6 |
|
||||
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << 7;
|
||||
|
||||
bstMasterStartBuffer(PUBLIC_ADDRESS);
|
||||
bstMasterWrite8(CLEANFLIGHT_MODE_FRAME_ID);
|
||||
bstMasterWrite8(tmp);
|
||||
bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_SONAR) << 4);
|
||||
bstMasterWrite8(sensors(SENSOR_ACC) | sensors(SENSOR_BARO) << 1 | sensors(SENSOR_MAG) << 2 | sensors(SENSOR_GPS) << 3 | sensors(SENSOR_RANGEFINDER) << 4);
|
||||
|
||||
return bstMasterWrite(masterWriteData);
|
||||
}
|
||||
|
|
|
@ -97,7 +97,8 @@
|
|||
|
||||
#undef USE_GPS
|
||||
#undef USE_MAG
|
||||
#undef USE_SONAR
|
||||
#undef USE_RANGEFINDER
|
||||
#undef USE_RANGEFINDER_HCSR04
|
||||
#undef USE_SERVOS
|
||||
#undef BEEPER
|
||||
|
||||
|
|
|
@ -49,9 +49,10 @@
|
|||
#define USE_FLASHFS
|
||||
#define USE_FLASH_M25P16
|
||||
|
||||
#define USE_SONAR
|
||||
#define SONAR_TRIGGER_PIN PB0
|
||||
#define SONAR_ECHO_PIN PB1
|
||||
#define USE_RANGEFINDER
|
||||
#define USE_RANGEFINDER_HCSR04
|
||||
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
|
||||
#define RANGEFINDER_HCSR04_ECHO_PIN PB1
|
||||
|
||||
#define USE_UART1
|
||||
#define USE_UART2
|
||||
|
|
|
@ -57,9 +57,10 @@
|
|||
//#define USE_FLASHFS
|
||||
//#define USE_FLASH_M25P16
|
||||
|
||||
#define USE_SONAR
|
||||
#define SONAR_TRIGGER_PIN PB0
|
||||
#define SONAR_ECHO_PIN PB1
|
||||
#define USE_RANGEFINDER
|
||||
#define USE_RANGEFINDER_HCSR04
|
||||
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
|
||||
#define RANGEFINDER_HCSR04_ECHO_PIN PB1
|
||||
|
||||
#define USE_UART1
|
||||
#define USE_UART2
|
||||
|
|
|
@ -110,11 +110,12 @@
|
|||
#define MAG_HMC5883_ALIGN CW180_DEG
|
||||
*/
|
||||
|
||||
//#define USE_SONAR
|
||||
//#define SONAR_TRIGGER_PIN PB0
|
||||
//#define SONAR_ECHO_PIN PB1
|
||||
//#define SONAR_TRIGGER_PIN_PWM PB8
|
||||
//#define SONAR_ECHO_PIN_PWM PB9
|
||||
//#define USE_RANGEFINDER
|
||||
//#define USE_RANGEFINDER_HCSR04
|
||||
//#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
|
||||
//#define RANGEFINDER_HCSR04_ECHO_PIN PB1
|
||||
//#define RANGEFINDER_HCSR04_TRIGGER_PIN_PWM PB8
|
||||
//#define RANGEFINDER_HCSR04_ECHO_PIN_PWM PB9
|
||||
|
||||
#define USE_UART1
|
||||
#define USE_UART2
|
||||
|
|
|
@ -59,9 +59,10 @@
|
|||
#define USE_BARO_BMP280
|
||||
#define USE_BARO_SPI_BMP280
|
||||
|
||||
//#define USE_SONAR
|
||||
//#define SONAR_ECHO_PIN PB1
|
||||
//#define SONAR_TRIGGER_PIN PB0
|
||||
//#define USE_RANGEFINDER
|
||||
//#define USE_RANGEFINDER_HCSR04
|
||||
//#define RANGEFINDER_HCSR04_ECHO_PIN PB1
|
||||
//#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
|
||||
|
||||
#define USB_DETECT_PIN PB5
|
||||
|
||||
|
|
|
@ -243,7 +243,10 @@
|
|||
|
||||
#define USE_TRANSPONDER
|
||||
|
||||
#define USE_SONAR
|
||||
#define USE_RANGEFINDER
|
||||
#define USE_RANGEFINDER_HCSR04
|
||||
#define RANGEFINDER_HCSR04_TRIGGER_PIN PA1
|
||||
#define RANGEFINDER_HCSR04_ECHO_PIN PA8
|
||||
|
||||
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL
|
||||
|
||||
|
|
|
@ -52,9 +52,10 @@
|
|||
|
||||
#define MAG_AK8975_ALIGN CW180_DEG
|
||||
|
||||
#define USE_SONAR
|
||||
#define SONAR_TRIGGER_PIN PA6 // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
|
||||
#define SONAR_ECHO_PIN PB1 // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
|
||||
#define USE_RANGEFINDER
|
||||
#define USE_RANGEFINDER_HCSR04
|
||||
#define RANGEFINDER_HCSR04_TRIGGER_PIN PA6 // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
|
||||
#define RANGEFINDER_HCSR04_ECHO_PIN PB1 // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
|
||||
|
||||
#define USE_VCP
|
||||
#define USE_UART1
|
||||
|
|
|
@ -90,9 +90,10 @@
|
|||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
//#define USE_SONAR
|
||||
//#define SONAR_ECHO_PIN PB1
|
||||
//#define SONAR_TRIGGER_PIN PA2
|
||||
//#define USE_RANGEFINDER
|
||||
//#define USE_RANGEFINDER_HCSR04
|
||||
//#define RANGEFINDER_HCSR04_ECHO_PIN PB1
|
||||
//#define RANGEFINDER_HCSR04_TRIGGER_PIN PA2
|
||||
|
||||
// available IO pins (from schematics)
|
||||
//#define TARGET_IO_PORTA (BIT(1)|BIT(2)|BIT(3)|BIT(4)|BIT(6)|BIT(7)|BIT(8)|BIT(9)|BIT(10)|BIT(11)|BIT(12)|BIT(13)|BIT(14)|BIT(15))
|
||||
|
|
|
@ -89,9 +89,10 @@
|
|||
#endif
|
||||
|
||||
#if defined(FLIP32F3OSD)
|
||||
#define USE_SONAR
|
||||
#define SONAR_TRIGGER_PIN PB0
|
||||
#define SONAR_ECHO_PIN PB1
|
||||
#define USE_RANGEFINDER
|
||||
#define USE_RANGEFINDER_HCSR04
|
||||
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
|
||||
#define RANGEFINDER_HCSR04_ECHO_PIN PB1
|
||||
|
||||
#elif defined(RMDO)
|
||||
#undef USE_GPS
|
||||
|
@ -102,9 +103,10 @@
|
|||
|
||||
#else //SPRACINGF3
|
||||
|
||||
#define USE_SONAR
|
||||
#define SONAR_TRIGGER_PIN PB0
|
||||
#define SONAR_ECHO_PIN PB1
|
||||
#define USE_RANGEFINDER
|
||||
#define USE_RANGEFINDER_HCSR04
|
||||
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
|
||||
#define RANGEFINDER_HCSR04_ECHO_PIN PB1
|
||||
|
||||
#define USE_BARO_MS5611
|
||||
#define USE_BARO_BMP085
|
||||
|
@ -137,7 +139,7 @@
|
|||
#define SOFTSERIAL2_RX_PIN PB0 // PWM 7
|
||||
#define SOFTSERIAL2_TX_PIN PB1 // PWM 8
|
||||
|
||||
#define SONAR_SOFTSERIAL2_EXCLUSIVE
|
||||
#define RANGEFINDER_HCSR04_SOFTSERIAL2_EXCLUSIVE
|
||||
#endif
|
||||
|
||||
#define USE_ESCSERIAL
|
||||
|
|
|
@ -82,7 +82,8 @@
|
|||
|
||||
#define MAG_AK8963_ALIGN CW90_DEG_FLIP
|
||||
|
||||
//#define USE_SONAR
|
||||
//#define USE_RANGEFINDER
|
||||
//#define USE_RANGEFINDER_HCSR04
|
||||
|
||||
#define USE_VCP
|
||||
#define USE_UART1
|
||||
|
|
|
@ -74,9 +74,10 @@
|
|||
#define MAG_AK8975_ALIGN CW90_DEG_FLIP
|
||||
#endif
|
||||
|
||||
//#define USE_SONAR
|
||||
//#define SONAR_ECHO_PIN PB1
|
||||
//#define SONAR_TRIGGER_PIN PB0
|
||||
//#define USE_RANGEFINDER
|
||||
//#define USE_RANGEFINDER_HCSR04
|
||||
//#define RANGEFINDER_HCSR04_ECHO_PIN PB1
|
||||
//#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
|
||||
|
||||
#define USE_BRUSHED_ESC_AUTODETECT
|
||||
|
||||
|
@ -122,7 +123,7 @@
|
|||
#define SOFTSERIAL1_TX_PIN PA1 // PA1 / PAD4
|
||||
#endif
|
||||
|
||||
#define SONAR_SOFTSERIAL1_EXCLUSIVE
|
||||
#define RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE
|
||||
|
||||
#define USE_SPI
|
||||
|
||||
|
|
|
@ -194,9 +194,10 @@
|
|||
|
||||
#define USE_ESC_SENSOR
|
||||
|
||||
#define USE_SONAR
|
||||
#define SONAR_TRIGGER_PIN PB0
|
||||
#define SONAR_ECHO_PIN PB1
|
||||
#define USE_RANGEFINDER
|
||||
#define USE_RANGEFINDER_HCSR04
|
||||
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
|
||||
#define RANGEFINDER_HCSR04_ECHO_PIN PB1
|
||||
|
||||
#define USE_SERIAL_4WAY_BLHELI_INTERFACE
|
||||
|
||||
|
|
|
@ -72,7 +72,7 @@
|
|||
|
||||
#define SOFTSERIAL1_RX_PIN PB0 // PWM 5
|
||||
#define SOFTSERIAL1_TX_PIN PB1 // PWM 6
|
||||
#define SONAR_SOFTSERIAL1_EXCLUSIVE
|
||||
#define RANGEFINDER_HCSR04_SOFTSERIAL1_EXCLUSIVE
|
||||
|
||||
#define USE_I2C
|
||||
#define USE_I2C_DEVICE_1
|
||||
|
|
|
@ -334,7 +334,7 @@ static void sendGPSLatLong(void)
|
|||
#endif
|
||||
#endif
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
/*
|
||||
* Send vertical speed for opentx. ID_VERT_SPEED
|
||||
* Unit is cm/s
|
||||
|
@ -545,8 +545,8 @@ void handleFrSkyTelemetry(timeUs_t currentTimeUs)
|
|||
sendAccel();
|
||||
}
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
if (sensors(SENSOR_BARO | SENSOR_SONAR)) {
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
if (sensors(SENSOR_BARO | SENSOR_RANGEFINDER)) {
|
||||
// Sent every 125ms
|
||||
sendVario();
|
||||
|
||||
|
|
|
@ -142,8 +142,8 @@ static void ltm_gframe(void)
|
|||
ltm_serialise_32(gpsSol.llh.lon);
|
||||
ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100));
|
||||
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100;
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
ltm_alt = (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100;
|
||||
#else
|
||||
ltm_alt = gpsSol.llh.alt * 100;
|
||||
#endif
|
||||
|
|
|
@ -329,8 +329,8 @@ void mavlinkSendPosition(void)
|
|||
// alt Altitude in 1E3 meters (millimeters) above MSL
|
||||
gpsSol.llh.alt * 1000,
|
||||
// relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters)
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
(sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000,
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
(sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000,
|
||||
#else
|
||||
gpsSol.llh.alt * 1000,
|
||||
#endif
|
||||
|
@ -396,8 +396,8 @@ void mavlinkSendHUDAndHeartbeat(void)
|
|||
#endif
|
||||
|
||||
// select best source for altitude
|
||||
#if defined(USE_BARO) || defined(USE_SONAR)
|
||||
if (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) {
|
||||
#if defined(USE_BARO) || defined(USE_RANGEFINDER)
|
||||
if (sensors(SENSOR_RANGEFINDER) || sensors(SENSOR_BARO)) {
|
||||
// Baro or sonar generally is a better estimate of altitude than GPS MSL altitude
|
||||
mavAltitude = getEstimatedAltitude() / 100.0;
|
||||
}
|
||||
|
@ -478,7 +478,7 @@ void mavlinkSendHUDAndHeartbeat(void)
|
|||
mavCustomMode = 0; //Stabilize
|
||||
mavModes |= MAV_MODE_FLAG_STABILIZE_ENABLED;
|
||||
}
|
||||
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE))
|
||||
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_MODE))
|
||||
mavCustomMode = 2; //Alt Hold
|
||||
if (FLIGHT_MODE(GPS_HOME_MODE))
|
||||
mavCustomMode = 6; //Return to Launch
|
||||
|
|
|
@ -464,7 +464,7 @@ void processSmartPortTelemetry(smartPortPayload_t *payload, volatile bool *clear
|
|||
tmpi += 100;
|
||||
if (FLIGHT_MODE(BARO_MODE))
|
||||
tmpi += 200;
|
||||
if (FLIGHT_MODE(SONAR_MODE))
|
||||
if (FLIGHT_MODE(RANGEFINDER_MODE))
|
||||
tmpi += 400;
|
||||
|
||||
if (FLIGHT_MODE(GPS_HOLD_MODE))
|
||||
|
|
Loading…
Reference in New Issue