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

Merge pull request #4753 from jflyper/bfdev-port-inav-rangefinder 

Port iNav's rangefinder abstraction
This commit is contained in:
Michael Keller 2017-12-20 19:01:07 +13:00 committed by GitHub
parent 8a9fd29dd3 11c47c631b
commit 03a4426fe1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
53 changed files with 898 additions and 492 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
make/source.mk
View File

@ -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 \

32
src/main/blackbox/blackbox.c
View File

@ -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();

2
src/main/blackbox/blackbox_fielddefs.h
View File

@ -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,

2
src/main/build/debug.c
View File

@ -56,4 +56,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"MAX7456_SPICLOCK",
"SBUS",
"FPORT",
"RANGEFINDER",
"RANGEFINDER_QUALITY",
};

2
src/main/build/debug.h
View File

@ -74,6 +74,8 @@ typedef enum {
DEBUG_MAX7456_SPICLOCK,
DEBUG_SBUS,
DEBUG_FPORT,
DEBUG_RANGEFINDER,
DEBUG_RANGEFINDER_QUALITY,
DEBUG_COUNT
} debugType_e;

38
src/main/drivers/sonar_hcsr04.h → src/main/drivers/rangefinder/rangefinder.h
View File

@ -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;

226
src/main/drivers/rangefinder/rangefinder_hcsr04.c Normal file
View File

@ -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

17
src/main/sensors/sonar.h → src/main/drivers/rangefinder/rangefinder_hcsr04.h
View File

@ -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);

1
src/main/drivers/resource.c
View File

@ -68,4 +68,5 @@ const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"ESCSERIAL",
"CAMERA_CONTROL",
"TIMUP",
"RANGEFINDER",
};

1
src/main/drivers/resource.h
View File

@ -68,6 +68,7 @@ typedef enum {
OWNER_ESCSERIAL,
OWNER_CAMERA_CONTROL,
OWNER_TIMUP,
OWNER_RANGEFINDER,
OWNER_TOTAL_COUNT
} resourceOwner_e;

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

@ -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

5
src/main/fc/config.c
View File

@ -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

2
src/main/fc/config.h
View File

@ -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,

4
src/main/fc/fc_core.c
View File

@ -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();
}
}

12
src/main/fc/fc_init.c
View File

@ -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

40
src/main/fc/fc_tasks.c
View File

@ -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,

4
src/main/fc/fc_tasks.h
View File

@ -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);

2
src/main/fc/rc_modes.h
View File

@ -44,7 +44,7 @@ typedef enum {
BOXOSD,
BOXTELEMETRY,
BOXGTUNE,
BOXSONAR,
BOXRANGEFINDER,
BOXSERVO1,
BOXSERVO2,
BOXSERVO3,

4
src/main/fc/runtime_config.h
View File

@ -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 {

32
src/main/flight/altitude.c
View File

@ -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)
{

2
src/main/flight/altitude.h
View File

@ -33,4 +33,4 @@ int32_t getEstimatedVario(void);
void applyAltHold(void);
void updateAltHoldState(void);
void updateSonarAltHoldState(void);
void updateRangefinderAltHoldState(void);

1
src/main/flight/imu.c
View File

@ -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>

16
src/main/interface/cli.c
View File

@ -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

12
src/main/interface/msp.c
View File

@ -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

8
src/main/interface/msp_box.c
View File

@ -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

14
src/main/interface/settings.c
View File

@ -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);

6
src/main/interface/settings.h
View File

@ -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[];

3
src/main/pg/pg_ids.h
View File

@ -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)

6
src/main/scheduler/scheduler.h
View File

@ -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

26
src/main/sensors/initialisation.c
View File

@ -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
View 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
View 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);

3
src/main/sensors/sensors.h
View File

@ -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;

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

@ -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

10
src/main/target/CC3D/target.h
View File

@ -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

14
src/main/target/COLIBRI_RACE/i2c_bst.c
View File

@ -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);
}

3
src/main/target/EACHIF3/target.h
View File

@ -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

7
src/main/target/ISHAPEDF3/target.h
View File

@ -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

7
src/main/target/MULTIFLITEPICO/target.h
View File

@ -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

11
src/main/target/NAZE/target.h
View File

@ -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

7
src/main/target/OMNIBUS/target.h
View File

@ -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

5
src/main/target/OMNIBUSF4/target.h
View File

@ -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

7
src/main/target/RCEXPLORERF3/target.h
View File

@ -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

7
src/main/target/SPARKY/target.h
View File

@ -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))

16
src/main/target/SPRACINGF3/target.h
View File

@ -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

3
src/main/target/SPRACINGF3EVO/target.h
View File

@ -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

9
src/main/target/SPRACINGF3MINI/target.h
View File

@ -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

7
src/main/target/STM32F3DISCOVERY/target.h
View File

@ -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

2
src/main/target/X_RACERSPI/target.h
View File

@ -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

6
src/main/telemetry/frsky.c
View File

@ -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();

4
src/main/telemetry/ltm.c
View File

@ -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

10
src/main/telemetry/mavlink.c
View File

@ -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

2
src/main/telemetry/smartport.c
View File

@ -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))