Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

overhaul - step1

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2017-07-01 22:01:10 +02:00
parent 76f077fa28
commit bdba06aac1
50 changed files with 190 additions and 174 deletions
Download patch file Download diff file Expand all files Collapse all files

4
Makefile
View file

@ -649,8 +649,8 @@ HIGHEND_SRC = \
common/colorconversion.c \
common/gps_conversion.c \
drivers/display_ug2864hsweg01.c \
drivers/sonar_hcsr04.c \
drivers/sonar_srf10.c \
drivers/rangefinder_hcsr04.c \
drivers/rangefinder_srf10.c \
io/dashboard.c \
io/displayport_max7456.c \
io/displayport_msp.c \

2
docs/Board - CC3D.md
View file

@ -24,7 +24,7 @@ The 8 pin RC_Input connector has the following pinouts when used in RX_PPM/RX_SE
| 2 | +5V | |
| 3 | Unused | |
| 4 | SoftSerial1 TX / Sonar trigger | |
| 5 | SoftSerial1 RX / Sonar Echo / RSSI\_ADC | Used either for SOFTSERIAL, SONAR or RSSI\_ADC*. Only one feature can be enabled at any time. |
| 5 | SoftSerial1 RX / Sonar Echo / RSSI\_ADC | Used either for SOFTSERIAL, HC-SR04 Rangefinder or RSSI\_ADC*. Only one feature can be enabled at any time. |
| 6 | Current | Enable `feature CURRENT_METER`. Connect to the output of a current sensor, 0v-3.3v input |
| 7 | Battery Voltage sensor | Enable `feature VBAT`. Connect to main battery using a voltage divider, 0v-3.3v input |
| 8 | PPM Input | Enable `feature RX_PPM` |

4
docs/Board - OMNIBUS.md
View file

@ -58,9 +58,9 @@ I2C is available on J22 PWM7 and PWM8
|SCL | J22 (PWM7) | J3 (SCL) |
|SDA | J22 (PWM8) | J3 (SDA) |
### SONAR
### RANGEFINDER
SONAR is supported when NOT using PPM.
HC-SR04 rangefinder is supported when NOT using PPM.
|signal | Location |
|-------|------------|

2
docs/Sonar.md
View file

@ -1,6 +1,6 @@
# Sonar
A sonar sensor can be used to measure altitude for use with BARO and SONAR altitude
A sonar sensor can be used to measure altitude for use with BARO and RANGEFINDER altitude
hold modes.
The sonar sensor is used instead of the pressure sensor (barometer) at low altitudes.

14
src/main/blackbox/blackbox.c
View file

@ -213,7 +213,7 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
#ifdef PITOT
{"AirSpeed", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_PITOT},
#endif
#ifdef SONAR
#ifdef RANGEFINDER
{"sonarRaw", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_SONAR},
#endif
{"rssi", -1, UNSIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_RSSI},
@ -352,7 +352,7 @@ typedef struct blackboxMainState_s {
#ifdef MAG
int16_t magADC[XYZ_AXIS_COUNT];
#endif
#ifdef SONAR
#ifdef RANGEFINDER
int32_t sonarRaw;
#endif
uint16_t rssi;
@ -500,8 +500,8 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
return feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC;
case FLIGHT_LOG_FIELD_CONDITION_SONAR:
#ifdef SONAR
return sensors(SENSOR_SONAR);
#ifdef RANGEFINDER
return sensors(SENSOR_RANGEFINDER);
#else
return false;
#endif
@ -634,7 +634,7 @@ static void writeIntraframe(void)
}
#endif
#ifdef SONAR
#ifdef RANGEFINDER
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
blackboxWriteSignedVB(blackboxCurrent->sonarRaw);
}
@ -803,7 +803,7 @@ static void writeInterframe(void)
}
#endif
#ifdef SONAR
#ifdef RANGEFINDER
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
deltas[optionalFieldCount++] = blackboxCurrent->sonarRaw - blackboxLast->sonarRaw;
}
@ -1144,7 +1144,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->airSpeed = pitot.airSpeed;
#endif
#ifdef SONAR
#ifdef RANGEFINDER
// Store the raw sonar value without applying tilt correction
blackboxCurrent->sonarRaw = rangefinderRead();
#endif

2
src/main/drivers/io.c
View file

@ -66,7 +66,7 @@ const struct ioPortDef_s ioPortDefs[] = {
const char * const ownerNames[OWNER_TOTAL_COUNT] = {
"FREE", "PWM", "PPM", "MOTOR", "SERVO", "SOFTSERIAL", "ADC", "SERIAL", "DEBUG", "TIMER",
"SONAR", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
"RANGEFINDER", "SYSTEM", "SPI", "I2C", "SDCARD", "FLASH", "USB", "BEEPER", "OSD",
"BARO", "MPU", "INVERTER", "LED STRIP", "LED", "RECEIVER", "TRANSMITTER",
"SOFTSPI", "NRF24"
};

2
src/main/drivers/pwm_mapping.c
View file

@ -154,7 +154,7 @@ pwmIOConfiguration_t *pwmInit(drv_pwm_config_t *init)
}
#endif
#ifdef SONAR
#ifdef USE_RANGEFINDER_HCSR04
if (init->useSonar &&
(
timerHardwarePtr->tag == init->sonarIOConfig.triggerTag ||

2
src/main/drivers/pwm_mapping.h
View file

@ -71,7 +71,7 @@ typedef struct drv_pwm_config_s {
bool useFastPwm;
bool useSoftSerial;
bool useLEDStrip;
#ifdef SONAR
#ifdef RANGEFINDER
bool useSonar;
#endif
#ifdef USE_SERVOS

14
src/main/drivers/sonar_hcsr04.c → src/main/drivers/rangefinder_hcsr04.c
View file

@ -20,7 +20,7 @@
#include <platform.h>
#if defined(SONAR)
#if defined(USE_RANGEFINDER_HCSR04)
#include "build/build_config.h"
@ -36,7 +36,7 @@
#include "drivers/logging.h"
#include "drivers/rangefinder.h"
#include "drivers/sonar_hcsr04.h"
#include "drivers/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
@ -96,7 +96,7 @@ void hcsr04_init(void)
void hcsr04_start_reading(void)
{
#if !defined(UNIT_TEST)
#ifdef SONAR_TRIG_INVERTED
#ifdef RANGEFINDER_HCSR04_TRIG_INVERTED
IOLo(triggerIO);
delayMicroseconds(11);
IOHi(triggerIO);
@ -164,23 +164,23 @@ bool hcsr04Detect(rangefinderDev_t *dev, const rangefinderHardwarePins_t * sonar
echoIO = IOGetByTag(sonarHardwarePins->echoTag);
if (IOGetOwner(triggerIO) != OWNER_FREE) {
addBootlogEvent4(BOOT_EVENT_HARDWARE_IO_CONFLICT, BOOT_EVENT_FLAGS_WARNING, IOGetOwner(triggerIO), OWNER_SONAR);
addBootlogEvent4(BOOT_EVENT_HARDWARE_IO_CONFLICT, BOOT_EVENT_FLAGS_WARNING, IOGetOwner(triggerIO), OWNER_RANGEFINDER);
return false;
}
if (IOGetOwner(echoIO) != OWNER_FREE) {
addBootlogEvent4(BOOT_EVENT_HARDWARE_IO_CONFLICT, BOOT_EVENT_FLAGS_WARNING, IOGetOwner(echoIO), OWNER_SONAR);
addBootlogEvent4(BOOT_EVENT_HARDWARE_IO_CONFLICT, BOOT_EVENT_FLAGS_WARNING, IOGetOwner(echoIO), OWNER_RANGEFINDER);
return false;
}
// trigger pin
IOInit(triggerIO, OWNER_SONAR, RESOURCE_OUTPUT, 0);
IOInit(triggerIO, OWNER_RANGEFINDER, RESOURCE_OUTPUT, 0);
IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
IOLo(triggerIO);
delay(100);
// echo pin
IOInit(echoIO, OWNER_SONAR, RESOURCE_INPUT, 0);
IOInit(echoIO, OWNER_RANGEFINDER, RESOURCE_INPUT, 0);
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
// HC-SR04 echo line should be low by default and should return a response pulse when triggered

0
src/main/drivers/sonar_hcsr04.h → src/main/drivers/rangefinder_hcsr04.h
View file

4
src/main/drivers/sonar_srf10.c → src/main/drivers/rangefinder_srf10.c
View file

@ -20,7 +20,7 @@
#include "platform.h"
#if defined(SONAR) && defined(USE_SONAR_SRF10)
#if defined(RANGEFINDER) && defined(USE_RANGEFINDER_SRF10)
#include "build/build_config.h"
@ -29,7 +29,7 @@
#include "drivers/bus_i2c.h"
#include "drivers/rangefinder.h"
#include "drivers/sonar_srf10.h"
#include "drivers/rangefinder_srf10.h"
#ifndef SRF10_I2C_INSTANCE
#define SRF10_I2C_INSTANCE I2CDEV_1

0
src/main/drivers/sonar_srf10.h → src/main/drivers/rangefinder_srf10.h
View file

2
src/main/drivers/resource.h
View file

@ -14,7 +14,7 @@ typedef enum {
OWNER_SERIAL,
OWNER_PINDEBUG,
OWNER_TIMER,
OWNER_SONAR,
OWNER_RANGEFINDER,
OWNER_SYSTEM,
OWNER_SPI,
OWNER_I2C,

16
src/main/fc/cli.c
View file

@ -173,10 +173,10 @@ static const char * const lookupTablePitotHardware[] = { "NONE", "AUTO", "MS4525
// sync this with sensors_e
static const char * const sensorTypeNames[] = {
"GYRO", "ACC", "BARO", "MAG", "SONAR", "PITOT", "GPS", "GPS+MAG", NULL
"GYRO", "ACC", "BARO", "MAG", "RANGEFINDER", "PITOT", "GPS", "GPS+MAG", NULL
};
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_SONAR | SENSOR_PITOT)
#define SENSOR_NAMES_MASK (SENSOR_GYRO | SENSOR_ACC | SENSOR_BARO | SENSOR_MAG | SENSOR_RANGEFINDER | SENSOR_PITOT)
static const char * const hardwareSensorStatusNames[] = {
"NONE", "OK", "UNAVAILABLE", "FAILING"
@ -371,7 +371,7 @@ typedef enum {
#ifdef MAG
TABLE_MAG_HARDWARE,
#endif
#ifdef SONAR
#ifdef RANGEFINDER
TABLE_RANGEFINDER_HARDWARE, // currently not used
#endif
#ifdef PITOT
@ -428,7 +428,7 @@ static const lookupTableEntry_t lookupTables[] = {
#ifdef MAG
{ lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) },
#endif
#ifdef SONAR
#ifdef RANGEFINDER
{ lookupTableRangefinderHardware, sizeof(lookupTableRangefinderHardware) / sizeof(char *) },
#endif
#ifdef PITOT
@ -548,7 +548,7 @@ static const clivalue_t valueTable[] = {
{ "accgain_z", VAR_INT16 | MASTER_VALUE, .config.minmax = { 1, 8192 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accGain.raw[Z]) },
// PG_RANGEFINDER_CONFIG
#ifdef SONAR
#ifdef RANGEFINDER
{ "rangefinder_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RANGEFINDER_HARDWARE }, PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_hardware) },
#endif
@ -817,10 +817,10 @@ static const clivalue_t valueTable[] = {
{ "inav_gps_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 500 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, gps_delay_ms) },
{ "inav_reset_altitude", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_NAV_RESET_ALTITUDE }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, reset_altitude_type) },
{ "inav_max_sonar_altitude", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, max_sonar_altitude) },
{ "inav_max_surface_altitude", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, max_surface_altitude) },
{ "inav_w_z_sonar_p", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_sonar_p) },
{ "inav_w_z_sonar_v", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_sonar_v) },
{ "inav_w_z_surface_p", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_surface_p) },
{ "inav_w_z_surface_v", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_surface_v) },
{ "inav_w_z_baro_p", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_baro_p) },
{ "inav_w_z_gps_p", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_gps_p) },
{ "inav_w_z_gps_v", VAR_FLOAT | MASTER_VALUE, .config.minmax = { 0, 10 }, PG_POSITION_ESTIMATION_CONFIG, offsetof(positionEstimationConfig_t, w_z_gps_v) },

8
src/main/fc/config.c
View file

@ -337,13 +337,13 @@ void validateAndFixConfig(void)
}
#endif
#if defined(NAZE) && defined(SONAR)
#if defined(NAZE) && defined(USE_RANGEFINDER_HCSR04)
if (featureConfigured(FEATURE_RX_PARALLEL_PWM) && (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && featureConfigured(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
#endif
#if defined(OLIMEXINO) && defined(SONAR)
#if defined(OLIMEXINO) && defined(USE_RANGEFINDER_HCSR04)
if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC) {
featureClear(FEATURE_CURRENT_METER);
}
@ -357,13 +357,13 @@ void validateAndFixConfig(void)
#if defined(CC3D)
#if defined(CC3D_PPM1)
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
#if defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1)
if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) {
rangefinderConfigMutable()->rangefinder_hardware = RANGEFINDER_NONE;
}
#endif
#else
#if defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
#if defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
// shared pin
if (((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
rangefinderConfigMutable()->rangefinder_hardware = RANGEFINDER_NONE;

6
src/main/fc/fc_init.c
View file

@ -249,7 +249,7 @@ void init(void)
drv_pwm_config_t pwm_params;
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
#ifdef USE_RANGEFINDER_HCSR04
// HC-SR04 has a dedicated connection to FC and require two pins
if (rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) {
const rangefinderHardwarePins_t *sonarHardware = sonarGetHardwarePins();
@ -392,7 +392,7 @@ void init(void)
updateHardwareRevision();
#endif
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
#if defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL1)
#if defined(FURYF3) || defined(OMNIBUS) || defined(SPRACINGF3MINI)
if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
@ -400,7 +400,7 @@ void init(void)
#endif
#endif
#if defined(SONAR) && defined(USE_SOFTSERIAL2) && defined(SPRACINGF3)
#if defined(USE_RANGEFINDER_HCSR04) && defined(USE_SOFTSERIAL2) && defined(SPRACINGF3)
if ((rangefinderConfig()->rangefinder_hardware == RANGEFINDER_HCSR04) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}

8
src/main/fc/fc_msp.c
View file

@ -389,7 +389,7 @@ static uint16_t packSensorStatus(void)
IS_ENABLED(sensors(SENSOR_BARO)) << 1 |
IS_ENABLED(sensors(SENSOR_MAG)) << 2 |
IS_ENABLED(sensors(SENSOR_GPS)) << 3 |
IS_ENABLED(sensors(SENSOR_SONAR)) << 4 |
IS_ENABLED(sensors(SENSOR_RANGEFINDER)) << 4 |
//IS_ENABLED(sensors(SENSOR_OPFLOW)) << 5 |
IS_ENABLED(sensors(SENSOR_PITOT)) << 6;
@ -658,7 +658,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
break;
case MSP_SONAR_ALTITUDE:
#if defined(SONAR)
#if defined(RANGEFINDER)
sbufWriteU32(dst, rangefinderGetLatestAltitude());
#else
sbufWriteU32(dst, 0);
@ -1152,7 +1152,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
#else
sbufWriteU8(dst, 0);
#endif
#ifdef SONAR
#ifdef RANGEFINDER
sbufWriteU8(dst, rangefinderConfig()->rangefinder_hardware);
#else
sbufWriteU8(dst, 0);
@ -1663,7 +1663,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#else
sbufReadU8(src);
#endif
#ifdef SONAR
#ifdef RANGEFINDER
rangefinderConfigMutable()->rangefinder_hardware = sbufReadU8(src);
#else
sbufReadU8(src); // rangefinder hardware

22
src/main/fc/fc_tasks.c
View file

@ -173,12 +173,12 @@ void taskUpdatePitot(timeUs_t currentTimeUs)
}
#endif
#ifdef SONAR
void taskUpdateSonar(timeUs_t currentTimeUs)
#ifdef RANGEFINDER
void taskUpdateRangefinder(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
if (!sensors(SENSOR_SONAR))
if (!sensors(SENSOR_RANGEFINDER))
return;
// Update and adjust task to update at required rate
@ -187,7 +187,7 @@ void taskUpdateSonar(timeUs_t currentTimeUs)
rescheduleTask(TASK_SELF, newDeadline);
}
updatePositionEstimator_SonarTopic(currentTimeUs);
updatePositionEstimator_SurfaceTopic(currentTimeUs);
}
#endif
@ -302,8 +302,8 @@ void fcTasksInit(void)
#ifdef PITOT
setTaskEnabled(TASK_PITOT, sensors(SENSOR_PITOT));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#ifdef RANGEFINDER
setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER));
#endif
#ifdef USE_DASHBOARD
setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
@ -451,11 +451,11 @@ cfTask_t cfTasks[TASK_COUNT] = {
},
#endif
#ifdef SONAR
[TASK_SONAR] = {
.taskName = "SONAR",
.taskFunc = taskUpdateSonar,
.desiredPeriod = TASK_PERIOD_MS(50), // every 70 ms, approximately 20 Hz
#ifdef RANGEFINDER
[TASK_RANGEFINDER] = {
.taskName = "RANGEFINDER",
.taskFunc = taskUpdateRangefinder,
.desiredPeriod = TASK_PERIOD_MS(50),
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif

8
src/main/navigation/navigation.h
View file

@ -72,12 +72,12 @@ typedef struct positionEstimationConfig_s {
uint8_t use_gps_velned;
uint16_t gps_delay_ms;
uint16_t max_sonar_altitude;
uint16_t max_surface_altitude;
float w_z_baro_p; // Weight (cutoff frequency) for barometer altitude measurements
float w_z_sonar_p; // Weight (cutoff frequency) for sonar altitude measurements
float w_z_sonar_v; // Weight (cutoff frequency) for sonar velocity measurements
float w_z_surface_p; // Weight (cutoff frequency) for surface altitude measurements
float w_z_surface_v; // Weight (cutoff frequency) for surface velocity measurements
float w_z_gps_p; // GPS altitude data is very noisy and should be used only on airplanes
float w_z_gps_v; // Weight (cutoff frequency) for GPS climb rate measurements
@ -248,7 +248,7 @@ void navigationInit(void);
/* Position estimator update functions */
void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs);
void updatePositionEstimator_SonarTopic(timeUs_t currentTimeUs);
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs);
/* Navigation system updates */
void updateWaypointsAndNavigationMode(void);

24
src/main/navigation/navigation_pos_estimator.c
View file

@ -72,9 +72,7 @@
#define INAV_GPS_GLITCH_ACCEL 1000.0f // 10m/s/s max possible acceleration for GPS glitch detection
#define INAV_POSITION_PUBLISH_RATE_HZ 50 // Publish position updates at this rate
#define INAV_BARO_UPDATE_RATE 20
#define INAV_PITOT_UPDATE_RATE 10
#define INAV_SONAR_UPDATE_RATE 15 // Sonar is limited to 1/60ms update rate, go lower that that
#define INAV_GPS_TIMEOUT_MS 1500 // GPS timeout
#define INAV_BARO_TIMEOUT_MS 200 // Baro timeout
@ -178,12 +176,12 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
.gravity_calibration_tolerance = 5, // 5 cm/s/s calibration error accepted (0.5% of gravity)
.use_gps_velned = 1, // "Disabled" is mandatory with gps_dyn_model = Pedestrian
.max_sonar_altitude = 200,
.max_surface_altitude = 200,
.w_z_baro_p = 0.35f,
.w_z_sonar_p = 3.500f,
.w_z_sonar_v = 6.100f,
.w_z_surface_p = 3.500f,
.w_z_surface_v = 6.100f,
.w_z_gps_p = 0.2f,
.w_z_gps_v = 0.5f,
@ -472,17 +470,17 @@ static void updatePitotTopic(timeUs_t currentTimeUs)
#endif
#if defined(SONAR)
#if defined(RANGEFINDER)
/**
* Read sonar and update alt/vel topic
* Function is called from TASK_SONAR at arbitrary rate - as soon as new measurements are available
* Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
*/
void updatePositionEstimator_SonarTopic(timeUs_t currentTimeUs)
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs)
{
float newSonarAlt = rangefinderRead();
newSonarAlt = rangefinderCalculateAltitude(newSonarAlt, calculateCosTiltAngle());
if (newSonarAlt > 0 && newSonarAlt <= positionEstimationConfig()->max_sonar_altitude) {
if (newSonarAlt > 0 && newSonarAlt <= positionEstimationConfig()->max_surface_altitude) {
posEstimator.sonar.alt = newSonarAlt;
posEstimator.sonar.lastUpdateTime = currentTimeUs;
}
@ -603,7 +601,7 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
((currentTimeUs - posEstimator.gps.lastUpdateTime) <= MS2US(INAV_GPS_TIMEOUT_MS)) &&
(posEstimator.gps.eph < positionEstimationConfig()->max_eph_epv); // EPV is checked later
bool isBaroValid = sensors(SENSOR_BARO) && ((currentTimeUs - posEstimator.baro.lastUpdateTime) <= MS2US(INAV_BARO_TIMEOUT_MS));
bool isSonarValid = sensors(SENSOR_SONAR) && ((currentTimeUs - posEstimator.sonar.lastUpdateTime) <= MS2US(INAV_SONAR_TIMEOUT_MS));
bool isSonarValid = sensors(SENSOR_RANGEFINDER) && ((currentTimeUs - posEstimator.sonar.lastUpdateTime) <= MS2US(INAV_SONAR_TIMEOUT_MS));
/* Do some preparations to data */
if (isBaroValid) {
@ -812,15 +810,15 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
}
/* Surface offset */
#if defined(SONAR)
#if defined(RANGEFINDER)
posEstimator.est.surface = posEstimator.est.surface + posEstimator.est.surfaceVel * dt;
if (isSonarValid) {
const float sonarResidual = posEstimator.sonar.alt - posEstimator.est.surface;
const float bellCurveScaler = scaleRangef(bellCurve(sonarResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f);
posEstimator.est.surface += sonarResidual * positionEstimationConfig()->w_z_sonar_p * bellCurveScaler * dt;
posEstimator.est.surfaceVel += sonarResidual * positionEstimationConfig()->w_z_sonar_v * sq(bellCurveScaler) * dt;
posEstimator.est.surface += sonarResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * dt;
posEstimator.est.surfaceVel += sonarResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * dt;
posEstimator.est.surfaceValid = true;
}
else {

1
src/main/navigation/navigation_private.h
View file

@ -33,7 +33,6 @@
#define NAV_DTERM_CUT_HZ 10
#define NAV_ACCELERATION_XY_MAX 980.0f // cm/s/s // approx 45 deg lean angle
#define INAV_SONAR_MAX_DISTANCE 55 // Sonar is unreliable above 50cm due to noise from propellers
#define INAV_SURFACE_MAX_DISTANCE 40
#define HZ2US(hz) (1000000 / (hz))

4
src/main/scheduler/scheduler.h
View file

@ -77,8 +77,8 @@ typedef enum {
#ifdef PITOT
TASK_PITOT,
#endif
#ifdef SONAR
TASK_SONAR,
#ifdef RANGEFINDER
TASK_RANGEFINDER,
#endif
#ifdef USE_DASHBOARD
TASK_DASHBOARD,

2
src/main/sensors/diagnostics.c
View file

@ -113,7 +113,7 @@ hardwareSensorStatus_e getHwBarometerStatus(void)
hardwareSensorStatus_e getHwRangefinderStatus(void)
{
#if defined(SONAR)
#if defined(RANGEFINDER)
if (detectedSensors[SENSOR_INDEX_RANGEFINDER] != RANGEFINDER_NONE) {
if (rangefinderIsHealthy()) {
return HW_SENSOR_OK;

2
src/main/sensors/initialisation.c
View file

@ -66,7 +66,7 @@ bool sensorsAutodetect(void)
compassInit();
#endif
#ifdef SONAR
#ifdef RANGEFINDER
rangefinderInit();
#endif

32
src/main/sensors/rangefinder.c
View file

@ -35,8 +35,8 @@
#include "drivers/io.h"
#include "drivers/logging.h"
#include "drivers/time.h"
#include "drivers/sonar_hcsr04.h"
#include "drivers/sonar_srf10.h"
#include "drivers/rangefinder_hcsr04.h"
#include "drivers/rangefinder_srf10.h"
#include "drivers/rangefinder.h"
#include "fc/config.h"
@ -50,7 +50,7 @@ rangefinder_t rangefinder;
#define RANGEFINDER_HARDWARE_TIMEOUT_MS 500 // Accept 500ms of non-responsive sensor, report HW failure otherwise
#ifdef SONAR
#ifdef RANGEFINDER
PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 0);
PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig,
@ -61,22 +61,22 @@ const rangefinderHardwarePins_t * sonarGetHardwarePins(void)
{
static rangefinderHardwarePins_t rangefinderHardwarePins;
#if defined(SONAR_PWM_TRIGGER_PIN)
#if defined(RANGEFINDER_HCSR04_PWM_TRIGGER_PIN)
// If we are using softserial, parallel PWM or ADC current sensor, then use motor pins for sonar, otherwise use RC pins
if (feature(FEATURE_SOFTSERIAL)
|| feature(FEATURE_RX_PARALLEL_PWM )
|| (feature(FEATURE_CURRENT_METER) && batteryConfig()->currentMeterType == CURRENT_SENSOR_ADC)) {
rangefinderHardwarePins.triggerTag = IO_TAG(SONAR_TRIGGER_PIN_PWM);
rangefinderHardwarePins.echoTag = IO_TAG(SONAR_ECHO_PIN_PWM);
rangefinderHardwarePins.triggerTag = IO_TAG(RANGEFINDER_HCSR04_TRIGGER_PIN_PWM);
rangefinderHardwarePins.echoTag = IO_TAG(RANGEFINDER_HCSR04_ECHO_PIN_PWM);
} else {
rangefinderHardwarePins.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
rangefinderHardwarePins.echoTag = IO_TAG(SONAR_ECHO_PIN);
rangefinderHardwarePins.triggerTag = IO_TAG(RANGEFINDER_HCSR04_TRIGGER_PIN);
rangefinderHardwarePins.echoTag = IO_TAG(RANGEFINDER_HCSR04_ECHO_PIN);
}
#elif defined(SONAR_TRIGGER_PIN)
rangefinderHardwarePins.triggerTag = IO_TAG(SONAR_TRIGGER_PIN);
rangefinderHardwarePins.echoTag = IO_TAG(SONAR_ECHO_PIN);
#elif defined(RANGEFINDER_HCSR04_TRIGGER_PIN)
rangefinderHardwarePins.triggerTag = IO_TAG(RANGEFINDER_HCSR04_TRIGGER_PIN);
rangefinderHardwarePins.echoTag = IO_TAG(RANGEFINDER_HCSR04_ECHO_PIN);
#else
#error Sonar not defined for target
#error Rangefinder not defined for target
#endif
return &rangefinderHardwarePins;
}
@ -91,16 +91,18 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
switch (rangefinderHardwareToUse) {
case RANGEFINDER_HCSR04:
#ifdef USE_RANGEFINDER_HCSR04
{
const rangefinderHardwarePins_t *sonarHardwarePins = sonarGetHardwarePins();
if (hcsr04Detect(dev, sonarHardwarePins)) { // FIXME: Do actual detection if HC-SR04 is plugged in
rangefinderHardware = RANGEFINDER_HCSR04;
}
}
#endif
break;
case RANGEFINDER_SRF10:
#ifdef USE_SONAR_SRF10
#ifdef USE_RANGEFINDER_SRF10
if (srf10Detect(dev)) {
rangefinderHardware = RANGEFINDER_SRF10;
}
@ -115,12 +117,12 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
addBootlogEvent6(BOOT_EVENT_RANGEFINDER_DETECTION, BOOT_EVENT_FLAGS_NONE, rangefinderHardware, 0, 0, 0);
if (rangefinderHardware == RANGEFINDER_NONE) {
sensorsClear(SENSOR_SONAR);
sensorsClear(SENSOR_RANGEFINDER);
return false;
}
detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderHardware;
sensorsSet(SENSOR_SONAR);
sensorsSet(SENSOR_RANGEFINDER);
return true;
}

2
src/main/sensors/sensors.h
View file

@ -47,7 +47,7 @@ typedef enum {
SENSOR_ACC = 1 << 1,
SENSOR_BARO = 1 << 2,
SENSOR_MAG = 1 << 3,
SENSOR_SONAR = 1 << 4,
SENSOR_RANGEFINDER = 1 << 4,
SENSOR_PITOT = 1 << 5,
SENSOR_GPS = 1 << 6,
SENSOR_GPSMAG = 1 << 7,

2
src/main/target/AIRHEROF3/target.mk
View file

@ -13,7 +13,7 @@ TARGET_SRC = \
drivers/compass/compass_mag3110.c \
drivers/light_ws2811strip.c \
drivers/light_ws2811strip_stdperiph.c \
drivers/sonar_hcsr04.c \
drivers/rangefinder_hcsr04.c \
drivers/serial_softserial.c \
drivers/pitotmeter_ms4525.c \
drivers/pitotmeter_adc.c

9
src/main/target/CC3D/target.h
View file

@ -160,10 +160,11 @@
//#define USE_SERIAL_4WAY_BLHELI_INTERFACE
//#define SONAR
//#define USE_SONAR_SRF10
#define SONAR_ECHO_PIN PB0
#define SONAR_TRIGGER_PIN PB5
// #define RANGEFINDER
//#define USE_RANGEFINDER_HCSR04
//#define USE_RANGEFINDER_SRF10
#define RANGEFINDER_HCSR04_ECHO_PIN PB0
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB5
//#define NAV_AUTO_MAG_DECLINATION
//#define NAV_GPS_GLITCH_DETECTION

7
src/main/target/COLIBRI/target.h
View file

@ -124,9 +124,10 @@
#define I2C3_SCL PA8
#define I2C3_SDA PC9
#define SONAR
#define SONAR_TRIGGER_PIN PB8
#define SONAR_ECHO_PIN PB9
#define RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB8
#define RANGEFINDER_HCSR04_ECHO_PIN PB9
#define TARGET_CONFIG

11
src/main/target/EUSTM32F103RC/target.h
View file

@ -63,11 +63,12 @@
#define USE_MAG_AK8975
#define MAG_AK8975_ALIGN CW180_DEG_FLIP
// #define SONAR
#define SONAR_TRIGGER_PIN PB0
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN_PWM PB8
#define SONAR_ECHO_PIN_PWM PB9
// #define 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_DASHBOARD

2
src/main/target/F4BY/target.mk
View file

@ -5,5 +5,5 @@ TARGET_SRC = \
drivers/accgyro/accgyro_spi_mpu6000.c \
drivers/barometer/barometer_ms56xx.c \
drivers/compass/compass_hmc5883l.c \
drivers/sonar_hcsr04.c \
drivers/rangefinder_hcsr04.c \
drivers/max7456.c

7
src/main/target/FALCORE/target.h
View file

@ -124,9 +124,10 @@
// USART2, PA3
#define BIND_PIN PA3
#define SONAR
#define SONAR_TRIGGER_PIN PA7
#define SONAR_ECHO_PIN PA2
#define RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_TRIGGER_PIN PA7
#define RANGEFINDER_HCSR04_ECHO_PIN PA2
#define DEFAULT_FEATURES FEATURE_BLACKBOX
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM

11
src/main/target/FISHDRONEF4/target.h
View file

@ -159,11 +159,12 @@
#define RSSI_ADC_CHANNEL ADC_CHN_3
#define VBAT_SCALE_DEFAULT 103
// *************** SONAR *****************************
// #define SONAR
// #define SONAR_TRIGGER_PIN PB10
// #define SONAR_ECHO_PIN PB11
// #define USE_SONAR_SRF10
// *************** RANGEFINDER *****************************
// #define RANGEFINDER
// #define USE_RANGEFINDER_HCSR04
// #define RANGEFINDER_HCSR04_TRIGGER_PIN PB10
// #define RANGEFINDER_HCSR04_ECHO_PIN PB11
// #define USE_RANGEFINDER_SRF10
// *************** NAV *****************************
#define NAV

7
src/main/target/FURYF3/target.h
View file

@ -154,9 +154,10 @@
#define WS2811_DMA_TC_FLAG DMA1_FLAG_TC2
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define SONAR
#define SONAR_TRIGGER_PIN PB0 // 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 RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 // 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 DEFAULT_FEATURES FEATURE_BLACKBOX
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM

13
src/main/target/NAZE/target.h
View file

@ -104,12 +104,13 @@
#define USE_MAG_HMC5883
#define MAG_HMC5883_ALIGN CW180_DEG
// #define SONAR
//#define USE_SONAR_SRF10
#define SONAR_TRIGGER_PIN PB0
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN_PWM PB8
#define SONAR_ECHO_PIN_PWM PB9
// #define RANGEFINDER
// #define USE_RANGEFINDER_HCSR04
// #define USE_RANGEFINDER_SRF10
#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 SOFTSERIAL_1_RX_PIN PA6
#define SOFTSERIAL_1_TX_PIN PA7

7
src/main/target/OLIMEXINO/target.h
View file

@ -58,9 +58,10 @@
#define MAG
#define USE_MAG_HMC5883
// #define SONAR
#define SONAR_TRIGGER_PIN PB0 // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
#define SONAR_ECHO_PIN PB1 // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
// #define RANGEFINDER
// #define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
#define RANGEFINDER_HCSR04_ECHO_PIN PB1 // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
#define USE_UART1
#define USE_UART2

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

@ -63,9 +63,10 @@
#define USE_MAG_HMC5883 // External
#define USE_MAG_MAG3110 // External
#define SONAR
#define SONAR_ECHO_PIN PB2 // Has 1K series resistor
#define SONAR_TRIGGER_PIN PB4 // FT
#define RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_ECHO_PIN PB2 // Has 1K series resistor
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB4 // FT
#define USB_IO
#define USB_CABLE_DETECTION

11
src/main/target/PORT103R/target.h
View file

@ -78,11 +78,12 @@
#define USE_FLASHTOOLS
#define USE_FLASH_M25P16
//#define SONAR
#define SONAR_TRIGGER_PIN PB0 // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
#define SONAR_ECHO_PIN PB1 // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
#define SONAR_TRIGGER_PIN_PWM PB8 // PWM5 (PB8) - 5v tolerant
#define SONAR_ECHO_PIN_PWM PB9 // PWM6 (PB9) - 5v tolerant
// #define RANGEFINDER
// #define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
#define RANGEFINDER_HCSR04_ECHO_PIN PB1 // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
#define RANGEFINDER_HCSR04_TRIGGER_PIN_PWM PB8 // PWM5 (PB8) - 5v tolerant
#define RANGEFINDER_HCSR04_ECHO_PIN_PWM PB9 // PWM6 (PB9) - 5v tolerant
#define USE_UART1
#define USE_UART2

8
src/main/target/RCEXPLORERF3/target.h
View file

@ -99,10 +99,10 @@
#define WS2811_DMA_STREAM DMA1_Channel3
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH3_HANDLER
#define 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 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 SENSORS_SET (SENSOR_ACC | SENSOR_BARO | SENSOR_GPS | SENSOR_MAG)

7
src/main/target/RMDO/target.h
View file

@ -50,9 +50,10 @@
#define USE_FLASHFS
#define USE_FLASH_M25P16
#define SONAR
#define SONAR_TRIGGER_PIN PB0 // 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 RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 // 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_UART1
#define USE_UART2

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

@ -94,9 +94,10 @@
// USART2, PA3
#define BIND_PIN PA3
//#define SONAR
//#define SONAR_TRIGGER_PIN PA2 // PWM6 (PA2) - only 3.3v ( add a 1K Ohms resistor )
//#define SONAR_ECHO_PIN PB1 // PWM7 (PB1) - only 3.3v ( add a 1K Ohms resistor )
// #define RANGEFINDER
// #define USE_RANGEFINDER_HCSR04
// #define RANGEFINDER_HCSR04_TRIGGER_PIN PA2 // PWM6 (PA2) - only 3.3v ( add a 1K Ohms resistor )
// #define RANGEFINDER_HCSR04_ECHO_PIN PB1 // PWM7 (PB1) - only 3.3v ( add a 1K Ohms resistor )
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM

11
src/main/target/SPARKY2/target.h
View file

@ -110,7 +110,7 @@
//#define I2C_DEVICE_EXT (I2CDEV_2)
#define USE_ADC
// PC2 shared with SONAR
// PC2 shared with HC-SR04
#define ADC_CHANNEL_1_PIN PC2
#define ADC_CHANNEL_2_PIN PC1
#define VBAT_ADC_CHANNEL ADC_CHN_2
@ -122,10 +122,11 @@
#define LED_STRIP
#define LED_STRIP_TIMER TIM5
//#define SONAR
//#define SONAR_TRIGGER_PIN PC2
//#define SONAR_ECHO_PIN PC3
//#define USE_SONAR_SRF10
// #define RANGEFINDER
// #define USE_RANGEFINDER_HCSR04
// #define RANGEFINDER_HCSR04_TRIGGER_PIN PC2
// #define RANGEFINDER_HCSR04_ECHO_PIN PC3
// #define USE_RANGEFINDER_SRF10
#define DEFAULT_FEATURES FEATURE_BLACKBOX
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL

7
src/main/target/SPRACINGF3/target.h
View file

@ -97,9 +97,10 @@
#define WS2811_DMA_STREAM DMA1_Channel2
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define SONAR
#define SONAR_TRIGGER_PIN PB0
#define SONAR_ECHO_PIN PB1
#define RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
#define RANGEFINDER_HCSR04_ECHO_PIN PB1
#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT

9
src/main/target/SPRACINGF3EVO/target.h
View file

@ -134,10 +134,11 @@
#define WS2811_DMA_STREAM DMA1_Channel2
#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER
#define SONAR
#define SONAR_TRIGGER_PIN PB0
#define SONAR_ECHO_PIN PB1
#define USE_SONAR_SRF10
#define RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
#define RANGEFINDER_HCSR04_ECHO_PIN PB1
#define USE_RANGEFINDER_SRF10
#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT

7
src/main/target/SPRACINGF3MINI/target.h
View file

@ -56,9 +56,10 @@
#define USE_MAG_HMC5883 // External
#define MAG_AK8963_ALIGN CW270_DEG_FLIP
#define SONAR
#define SONAR_ECHO_PIN PB1
#define SONAR_TRIGGER_PIN PB0
#define RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_ECHO_PIN PB1
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
#define USB_IO
#define USB_CABLE_DETECTION

2
src/main/target/SPRACINGF3MINI/target.mk
View file

@ -12,4 +12,4 @@ TARGET_SRC = \
drivers/light_ws2811strip_stdperiph.c \
drivers/serial_softserial.c \
drivers/serial_usb_vcp.c \
drivers/sonar_hcsr04.c \
drivers/rangefinder_hcsr04.c \

9
src/main/target/STM32F3DISCOVERY/target.h
View file

@ -130,10 +130,11 @@
#define SPEKTRUM_BIND
#define BIND_PIN PA3 // USART2, PA3
#define SONAR
#define SONAR_TRIGGER_PIN PB0
#define SONAR_ECHO_PIN PB1
#define USE_SONAR_SRF10
#define RANGEFINDER
#define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0
#define RANGEFINDER_HCSR04_ECHO_PIN PB1
#define USE_RANGEFINDER_SRF10
#define USE_SERIAL_4WAY_BLHELI_INTERFACE

12
src/test/Makefile
View file

@ -520,12 +520,12 @@ $(OBJECT_DIR)/baro_bmp280_unittest : \
$(CXX) $(CXX_FLAGS) $^ -o $(OBJECT_DIR)/$@
$(OBJECT_DIR)/drivers/sonar_hcsr04.o : \
$(USER_DIR)/drivers/sonar_hcsr04.c \
$(USER_DIR)/drivers/sonar_hcsr04.h \
$(OBJECT_DIR)/drivers/rangefinder_hcsr04.o : \
$(USER_DIR)/drivers/rangefinder_hcsr04.c \
$(USER_DIR)/drivers/rangefinder_hcsr04.h \
$(GTEST_HEADERS)
@mkdir -p $(dir $@)
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -DSONAR -c $(USER_DIR)/drivers/sonar_hcsr04.c -o $@
$(CC) $(C_FLAGS) $(TEST_CFLAGS) -DSONAR -c $(USER_DIR)/drivers/rangefinder_hcsr04.c -o $@
$(OBJECT_DIR)/sensors/sonar.o : \
$(USER_DIR)/sensors/sonar.c \
@ -537,7 +537,7 @@ $(OBJECT_DIR)/sensors/sonar.o : \
$(OBJECT_DIR)/sonar_unittest.o : \
$(TEST_DIR)/sonar_unittest.cc \
$(USER_DIR)/drivers/sonar_hcsr04.h \
$(USER_DIR)/drivers/rangefinder_hcsr04.h \
$(USER_DIR)/sensors/sonar.h \
$(GTEST_HEADERS)
@ -546,7 +546,7 @@ $(OBJECT_DIR)/sonar_unittest.o : \
$(OBJECT_DIR)/sonar_unittest : \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/drivers/sonar_hcsr04.o \
$(OBJECT_DIR)/drivers/rangefinder_hcsr04.o \
$(OBJECT_DIR)/sensors/sonar.o \
$(OBJECT_DIR)/sonar_unittest.o \
$(OBJECT_DIR)/gtest_main.a

2
src/test/unit/scheduler_unittest.cc.txt
View file

@ -65,7 +65,7 @@ extern "C" {
void taskProcessGPS(void) {simulatedTime+=processGPSTime;}
void taskUpdateCompass(void) {simulatedTime+=updateCompassTime;}
void taskUpdateBaro(void) {simulatedTime+=updateBaroTime;}
void taskUpdateSonar(void) {simulatedTime+=updateSonarTime;}
void taskUpdateRangefinder(void) {simulatedTime+=updateSonarTime;}
void taskCalculateAltitude(void) {simulatedTime+=calculateAltitudeTime;}
void taskUpdateDisplay(void) {simulatedTime+=updateDisplayTime;}
void taskTelemetry(void) {simulatedTime+=telemetryTime;}

2
src/test/unit/sonar_unittest.cc.txt
View file

@ -20,7 +20,7 @@
extern "C" {
#include "build_config.h"
#include "drivers/sonar_hcsr04.h"
#include "drivers/rangefinder_hcsr04.h"
#include "sensors/sonar.h"
extern int32_t measurement;
extern int16_t sonarMaxTiltDeciDegrees;