diff --git a/Makefile b/Makefile index 9054c0da3b..61c4b3bdf2 100755 --- a/Makefile +++ b/Makefile @@ -312,13 +312,15 @@ COMMON_SRC = build_config.c \ $(DEVICE_STDPERIPH_SRC) HIGHEND_SRC = \ + blackbox/blackbox.c \ + blackbox/blackbox_io.c + common/colorconversion.c \ flight/navigation_rewrite.c \ flight/navigation_rewrite_multicopter.c \ flight/navigation_rewrite_fixedwing.c \ flight/navigation_rewrite_pos_estimator.c \ flight/navigation_rewrite_geo.c \ flight/gps_conversion.c \ - common/colorconversion.c \ io/gps.c \ io/gps_ublox.c \ io/gps_nmea.c \ @@ -326,15 +328,13 @@ HIGHEND_SRC = \ io/gps_i2cnav.c \ io/ledstrip.c \ io/display.c \ + sensors/rangefinder.c \ + sensors/barometer.c \ telemetry/telemetry.c \ telemetry/frsky.c \ telemetry/hott.c \ telemetry/smartport.c \ telemetry/ltm.c \ - sensors/sonar.c \ - sensors/barometer.c \ - blackbox/blackbox.c \ - blackbox/blackbox_io.c VCP_SRC = \ vcp/hw_config.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index d0149ed374..b4efe3ddad 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -42,7 +42,7 @@ #include "sensors/sensors.h" #include "sensors/boardalignment.h" -#include "sensors/sonar.h" +#include "sensors/rangefinder.h" #include "sensors/compass.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" @@ -1060,7 +1060,7 @@ static void loadMainState(void) #ifdef SONAR // Store the raw sonar value without applying tilt correction - blackboxCurrent->sonarRaw = sonarRead(); + blackboxCurrent->sonarRaw = rangefinderRead(); #endif blackboxCurrent->rssi = rssi; diff --git a/src/main/drivers/sonar.h b/src/main/drivers/rangefinder.h similarity index 92% rename from src/main/drivers/sonar.h rename to src/main/drivers/rangefinder.h index 811b8e3a7c..b0b6ee7074 100644 --- a/src/main/drivers/sonar.h +++ b/src/main/drivers/rangefinder.h @@ -17,12 +17,12 @@ #pragma once -#define SONAR_OUT_OF_RANGE (-1) +#define RANGEFINDER_OUT_OF_RANGE (-1) -typedef struct sonarRange_s { +typedef struct rangefinder_s { int16_t maxRangeCm; // these are full detection cone angles, maximum tilt is half of this 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; +} rangefinder_t; diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index d8cb81120d..e63daffd7d 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -21,11 +21,13 @@ #include #include "build_config.h" +#if defined(SONAR) + #include "drivers/system.h" #include "drivers/gpio.h" #include "drivers/nvic.h" -#include "drivers/sonar.h" +#include "drivers/rangefinder.h" #include "drivers/sonar_hcsr04.h" #define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet @@ -42,7 +44,6 @@ * */ -#if defined(SONAR) STATIC_UNIT_TESTED volatile int32_t hcsr04SonarPulseTravelTime = 0; sonarHcsr04Hardware_t sonarHcsr04Hardware; @@ -133,11 +134,11 @@ void hcsr04_set_sonar_hardware(void) #endif } -void hcsr04_init(sonarRange_t *sonarRange) +void hcsr04_init(rangefinder_t *rangefinder) { - sonarRange->maxRangeCm = HCSR04_MAX_RANGE_CM; - sonarRange->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES; - sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES; + rangefinder->maxRangeCm = HCSR04_MAX_RANGE_CM; + rangefinder->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES; + rangefinder->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES; } /* @@ -175,7 +176,7 @@ int32_t hcsr04_get_distance(void) // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59 int32_t distance = hcsr04SonarPulseTravelTime / 59; if (distance > HCSR04_MAX_RANGE_CM) { - distance = SONAR_OUT_OF_RANGE; + distance = RANGEFINDER_OUT_OF_RANGE; } return distance; } diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index 9236a3a683..0321856b76 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -17,9 +17,6 @@ #pragma once -#include -#include "drivers/sonar.h" - typedef struct sonarHcsr04Hardware_s { uint16_t trigger_pin; GPIO_TypeDef* trigger_gpio; @@ -30,7 +27,8 @@ typedef struct sonarHcsr04Hardware_s { IRQn_Type exti_irqn; } sonarHcsr04Hardware_t; +struct rangefinder_s; void hcsr04_set_sonar_hardware(void); -void hcsr04_init(sonarRange_t *sonarRange); +void hcsr04_init(struct rangefinder_s *rangefinder); void hcsr04_start_reading(void); int32_t hcsr04_get_distance(void); diff --git a/src/main/drivers/sonar_srf10.c b/src/main/drivers/sonar_srf10.c index 703357d88d..d629e6a1d8 100644 --- a/src/main/drivers/sonar_srf10.c +++ b/src/main/drivers/sonar_srf10.c @@ -21,13 +21,14 @@ #include "platform.h" #include "build_config.h" +#if defined(SONAR) && defined(USE_SONAR_SRF10) + #include "drivers/system.h" #include "drivers/bus_i2c.h" -#include "drivers/sonar.h" +#include "drivers/rangefinder.h" #include "drivers/sonar_srf10.h" -#if defined(SONAR) && defined(USE_SONAR_SRF10) // Technical specification is at: http://robot-electronics.co.uk/htm/srf10tech.htm #define SRF10_MAX_RANGE_CM 600 // 6m, from SFR10 spec sheet @@ -82,7 +83,7 @@ #define SRF10_RangeValue6m 139 // maximum range #define SRF10_RangeValue11m 0xFF // exceeds actual maximum range -STATIC_UNIT_TESTED volatile int32_t srf10measurementCm = SONAR_OUT_OF_RANGE; +STATIC_UNIT_TESTED volatile int32_t srf10measurementCm = RANGEFINDER_OUT_OF_RANGE; static int16_t minimumFiringIntervalMs; static uint32_t timeOfLastMeasurementMs; @@ -117,11 +118,11 @@ bool srf10_detect() return false; } -void srf10_init(sonarRange_t *sonarRange) +void srf10_init(rangefinder_t *rangefinder) { - sonarRange->maxRangeCm = SRF10_MAX_RANGE_CM; - sonarRange->detectionConeDeciDegrees = SRF10_DETECTION_CONE_DECIDEGREES; - sonarRange->detectionConeExtendedDeciDegrees = SRF10_DETECTION_CONE_EXTENDED_DECIDEGREES; + rangefinder->maxRangeCm = SRF10_MAX_RANGE_CM; + rangefinder->detectionConeDeciDegrees = SRF10_DETECTION_CONE_DECIDEGREES; + rangefinder->detectionConeExtendedDeciDegrees = SRF10_DETECTION_CONE_EXTENDED_DECIDEGREES; // set up the SRF10 hardware for a range of 6m minimumFiringIntervalMs = SRF10_MinimumFiringIntervalFor600cmRangeMs; i2c_srf10_send_byte(SRF10_WRITE_MaxGainRegister, SRF10_COMMAND_SetGain_Max); @@ -145,7 +146,7 @@ void srf10_start_reading(void) const uint8_t highByte = i2c_srf10_read_byte(SRF10_READ_RangeHighByte); srf10measurementCm = highByte << 8 | lowByte; if (srf10measurementCm > SRF10_MAX_RANGE_CM) { - srf10measurementCm = SONAR_OUT_OF_RANGE; + srf10measurementCm = RANGEFINDER_OUT_OF_RANGE; } } const uint32_t timeNowMs = millis(); diff --git a/src/main/drivers/sonar_srf10.h b/src/main/drivers/sonar_srf10.h index 914430148c..e64040b94d 100644 --- a/src/main/drivers/sonar_srf10.h +++ b/src/main/drivers/sonar_srf10.h @@ -17,11 +17,9 @@ #pragma once -#include "platform.h" -#include "drivers/sonar.h" - +struct rangefinder_s; bool srf10_detect(); -void srf10_init(sonarRange_t *sonarRange); +void srf10_init(struct rangefinder_s *rangefinder); void srf10_start_reading(void); int32_t srf10_get_distance(void); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index f8c87974e5..838c202020 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -41,7 +41,6 @@ #include "sensors/compass.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" -#include "sensors/sonar.h" #include "flight/mixer.h" #include "flight/pid.h" diff --git a/src/main/flight/navigation_rewrite_pos_estimator.c b/src/main/flight/navigation_rewrite_pos_estimator.c index 10937464ab..c9e7416f2d 100755 --- a/src/main/flight/navigation_rewrite_pos_estimator.c +++ b/src/main/flight/navigation_rewrite_pos_estimator.c @@ -32,7 +32,7 @@ #include "drivers/accgyro.h" #include "sensors/sensors.h" -#include "sensors/sonar.h" +#include "sensors/rangefinder.h" #include "sensors/barometer.h" #include "sensors/acceleration.h" #include "sensors/boardalignment.h" @@ -382,8 +382,8 @@ static void updateSonarTopic(uint32_t currentTime) if (updateTimer(&sonarUpdateTimer, HZ2US(INAV_SONAR_UPDATE_RATE), currentTime)) { if (sensors(SENSOR_SONAR)) { /* Read sonar */ - float newSonarAlt = sonarRead(); - newSonarAlt = sonarCalculateAltitude(newSonarAlt, calculateCosTiltAngle()); + float newSonarAlt = rangefinderRead(); + newSonarAlt = rangefinderCalculateAltitude(newSonarAlt, calculateCosTiltAngle()); /* Apply predictive filter to sonar readings (inspired by PX4Flow) */ if (newSonarAlt > 0 && newSonarAlt <= INAV_SONAR_MAX_DISTANCE) { @@ -742,4 +742,4 @@ void updatePositionEstimator(void) publishEstimatedTopic(currentTime); } -#endif \ No newline at end of file +#endif diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 0a9ea03304..77c68c5106 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -94,6 +94,7 @@ uint8_t cliMode = 0; #ifdef USE_CLI extern uint16_t cycleTime; // FIXME dependency on mw.c +extern uint8_t detectedSensors[SENSOR_INDEX_COUNT]; void gpsEnablePassthrough(serialPort_t *gpsPassthroughPort); @@ -201,16 +202,32 @@ static const char * const sensorTypeNames[] = { "GYRO", "ACC", "BARO", "MAG", "SONAR", "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_SONAR) +#ifndef XXXX +// sync with gyroSensor_e +static const char * const gyroNames[] = { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE"}; +// sync with accelerationSensor_e +static const char * const accNames[] = { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE"}; +// sync with baroSensor_e +static const char * const baroNames[] = { "", "None", "BMP085", "MS5611", "BMP280", "FAKE"}; +// sync with magSensor_e +static const char * const magNames[] = { "", "None", "HMC5883", "AK8975", "FAKE"}; +// sycn with rangefinderType_e +static const char * const rangefinderNames[] = { "None", "HCSR04", "SRF10"}; -static const char * const sensorHardwareNames[4][11] = { +static const char * const *sensorHardwareNames[] = {gyroNames, accNames, baroNames, magNames, rangefinderNames}; +#else +static const char * const sensorHardwareNames[5][11] = { { "", "None", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "FAKE", NULL }, { "", "None", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "FAKE", NULL }, { "", "None", "BMP085", "MS5611", "BMP280", "FAKE", NULL }, - { "", "None", "HMC5883", "AK8975", "FAKE", NULL } + { "", "None", "HMC5883", "AK8975", "FAKE", NULL }, + { "", "None", "HCSR04", "SRF10", NULL } }; #endif +#endif // CJMCU + typedef struct { const char *name; #ifndef SKIP_CLI_COMMAND_HELP @@ -2485,19 +2502,14 @@ static void cliStatus(char *cmdline) cliPrintf("CPU Clock=%dMHz", (SystemCoreClock / 1000000)); #ifndef CJMCU - uint8_t i; - uint32_t mask; - uint32_t detectedSensorsMask = sensorsMask(); + const uint32_t detectedSensorsMask = sensorsMask(); - for (i = 0; ; i++) { + for (int i = 0; i < SENSOR_INDEX_COUNT; i++) { - if (sensorTypeNames[i] == NULL) - break; - - mask = (1 << i); + const uint32_t mask = (1 << i); if ((detectedSensorsMask & mask) && (mask & SENSOR_NAMES_MASK)) { const char *sensorHardware; - uint8_t sensorHardwareIndex = detectedSensors[i]; + const int sensorHardwareIndex = detectedSensors[i]; sensorHardware = sensorHardwareNames[i][sensorHardwareIndex]; cliPrintf(", %s=%s", sensorTypeNames[i], sensorHardware); @@ -2511,9 +2523,9 @@ static void cliStatus(char *cmdline) cliPrint("\r\n"); #ifdef USE_I2C - uint16_t i2cErrorCounter = i2cGetErrorCounter(); + const uint16_t i2cErrorCounter = i2cGetErrorCounter(); #else - uint16_t i2cErrorCounter = 0; + const uint16_t i2cErrorCounter = 0; #endif cliPrintf("Cycle Time: %d, I2C Errors: %d, config size: %d\r\n", cycleTime, i2cErrorCounter, sizeof(master_t)); diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index bae6e51274..6bdfc07821 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -63,7 +63,7 @@ #include "sensors/boardalignment.h" #include "sensors/sensors.h" #include "sensors/battery.h" -#include "sensors/sonar.h" +#include "sensors/rangefinder.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/compass.h" @@ -711,7 +711,7 @@ static bool processOutCommand(uint8_t cmdMSP) case MSP_SONAR_ALTITUDE: headSerialReply(4); #if defined(SONAR) - serialize32(sonarGetLatestAltitude()); + serialize32(rangefinderGetLatestAltitude()); #else serialize32(0); #endif diff --git a/src/main/main.c b/src/main/main.c index 320bd5ada8..5b6eaf5cb3 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -65,7 +65,6 @@ #include "io/display.h" #include "sensors/sensors.h" -#include "sensors/sonar.h" #include "sensors/barometer.h" #include "sensors/compass.h" #include "sensors/acceleration.h" @@ -122,6 +121,7 @@ void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); void spektrumBind(rxConfig_t *rxConfig); +const sonarHcsr04Hardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor); #ifdef STM32F303xC // from system_stm32f30x.c @@ -452,12 +452,6 @@ void init(void) ); #endif -#ifdef SONAR - if (feature(FEATURE_SONAR)) { - sonarInit(); - } -#endif - #ifdef LED_STRIP ledStripInit(masterConfig.ledConfigs, masterConfig.colors); diff --git a/src/main/mw.c b/src/main/mw.c index ab3a55d05c..6f26054b0e 100755 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -45,7 +45,7 @@ #include "sensors/sensors.h" #include "sensors/boardalignment.h" -#include "sensors/sonar.h" +#include "sensors/rangefinder.h" #include "sensors/compass.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" @@ -731,7 +731,7 @@ void taskUpdateBaro(void) void taskUpdateSonar(void) { if (sensors(SENSOR_SONAR)) { - sonarUpdate(); + rangefinderUpdate(); } //updatePositionEstimator_SonarTopic(currentTime); diff --git a/src/main/scheduler/scheduler_tasks.c b/src/main/scheduler/scheduler_tasks.c index 78784a0039..a00a64c217 100755 --- a/src/main/scheduler/scheduler_tasks.c +++ b/src/main/scheduler/scheduler_tasks.c @@ -112,7 +112,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_SONAR] = { .taskName = "SONAR", .taskFunc = taskUpdateSonar, - .desiredPeriod = 1000000 / 20, + .desiredPeriod = 70000, // every 70 ms, approximately 14 Hz .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c index 2ccc6b3d0d..cbf092c0b4 100755 --- a/src/main/sensors/initialisation.c +++ b/src/main/sensors/initialisation.c @@ -58,15 +58,17 @@ #include "drivers/compass_mag3110.h" #include "drivers/sonar_hcsr04.h" +#include "drivers/sonar_srf10.h" #include "config/runtime_config.h" +#include "config/config.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" #include "sensors/gyro.h" #include "sensors/compass.h" -#include "sensors/sonar.h" +#include "sensors/rangefinder.h" #include "sensors/initialisation.h" #ifdef NAZE @@ -83,7 +85,7 @@ extern gyro_t gyro; extern baro_t baro; extern acc_t acc; -uint8_t detectedSensors[MAX_SENSORS_TO_DETECT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE }; +uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE }; const extiConfig_t *selectMPUIntExtiConfig(void) @@ -731,6 +733,30 @@ retry: sensorsSet(SENSOR_MAG); } +#ifdef SONAR +/* + * Detect which rangefinder is present + */ +static rangefinderType_e detectRangefinder(void) +{ + rangefinderType_e rangefinderType = RANGEFINDER_NONE; + 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 + rangefinderType = RANGEFINDER_HCSR04; + } +#ifdef USE_SONAR_SRF10 + if (srf10_detect()) { + // if an SFR10 sonar rangefinder is detected then use it in preference to the assumed HC-SR04 + rangefinderType = RANGEFINDER_SRF10; + } +#endif + detectedSensors[SENSOR_INDEX_RANGEFINDER] = rangefinderType; + sensorsSet(SENSOR_SONAR); + return rangefinderType; +} +#endif + static void reconfigureAlignment(const sensorAlignmentConfig_t *sensorAlignmentConfig) { if (sensorAlignmentConfig->gyro_align != ALIGN_DEFAULT) { @@ -783,6 +809,11 @@ bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint8_t g magneticDeclination = 0.0f; // TODO investigate if this is actually needed if there is no mag sensor or if the value stored in the config should be used. } +#ifdef SONAR + const rangefinderType_e rangefinderType = detectRangefinder(); + rangefinderInit(rangefinderType); +#endif + return true; } diff --git a/src/main/sensors/sonar.c b/src/main/sensors/rangefinder.c similarity index 57% rename from src/main/sensors/sonar.c rename to src/main/sensors/rangefinder.c index db6c894b6a..732c5acfca 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/rangefinder.c @@ -17,11 +17,14 @@ #include #include +#include #include #include #include "build_config.h" +#ifdef SONAR + #include "common/maths.h" #include "config/config.h" @@ -30,25 +33,23 @@ #include "drivers/gpio.h" #include "drivers/sonar_hcsr04.h" #include "drivers/sonar_srf10.h" -#include "drivers/sonar.h" +#include "drivers/rangefinder.h" #include "sensors/sensors.h" -#include "sensors/sonar.h" +#include "sensors/rangefinder.h" #include "sensors/battery.h" -// Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range. +// Sonar measurements are in cm, a value of RANGEFINDER_OUT_OF_RANGE indicates sonar is not in range. // Inclination is adjusted by imu -#ifdef SONAR - extern sonarHcsr04Hardware_t sonarHcsr04Hardware; -static int16_t sonarMaxRangeCm; -static int16_t sonarMaxAltWithTiltCm; -static int16_t sonarCfAltCm; // Complimentary Filter altitude -STATIC_UNIT_TESTED int16_t sonarMaxTiltDeciDegrees; -static sonarFunctionPointers_t sonarFunctionPointers; -static float sonarMaxTiltCos; +static int16_t rangefinderMaxRangeCm; +static int16_t rangefinderMaxAltWithTiltCm; +static int16_t rangefinderCfAltCm; // Complimentary Filter altitude +STATIC_UNIT_TESTED int16_t rangefinderMaxTiltDeciDegrees; +static rangefinderFunctionPointers_t rangefinderFunctionPointers; +static float rangefinderMaxTiltCos; static int32_t calculatedAltitude; @@ -100,23 +101,23 @@ static const sonarHcsr04Hardware_t *sonarGetHardwareConfigurationForHCSR04(curre return &sonarHcsr04Hardware; } -STATIC_UNIT_TESTED void sonarSetFunctionPointers(sonarHardwareType_e sonarHardwareType) +STATIC_UNIT_TESTED void rangefinderSetFunctionPointers(rangefinderType_e rangefinderType) { - switch (sonarHardwareType) { + switch (rangefinderType) { default: - case SONAR_NONE: + case RANGEFINDER_NONE: break; - case SONAR_HCSR04: - sonarFunctionPointers.init = hcsr04_init; - sonarFunctionPointers.update = hcsr04_start_reading; - sonarFunctionPointers.read = hcsr04_get_distance; + case RANGEFINDER_HCSR04: + rangefinderFunctionPointers.init = hcsr04_init; + rangefinderFunctionPointers.update = hcsr04_start_reading; + rangefinderFunctionPointers.read = hcsr04_get_distance; break; #ifdef USE_SONAR_SRF10 - case SONAR_SRF10: - sonarFunctionPointers.init = srf10_init; - sonarFunctionPointers.update = srf10_start_reading; - sonarFunctionPointers.read = srf10_get_distance; + case RANGEFINDER_SRF10: + rangefinderFunctionPointers.init = srf10_init; + rangefinderFunctionPointers.update = srf10_start_reading; + rangefinderFunctionPointers.read = srf10_get_distance; break; #endif } @@ -134,82 +135,67 @@ const sonarHcsr04Hardware_t *sonarGetHardwareConfiguration(currentSensor_e curre return sonarGetHardwareConfigurationForHCSR04(currentSensor); } -/* - * Detect what sonar hardware is present and set the function pointers accordingly - */ -static sonarHardwareType_e sonarDetect(void) +void rangefinderInit(rangefinderType_e rangefinderType) { - // the user has set the sonar feature, so assume they have an HC-SR04 plugged in, - // since there is no way to detect it - sonarHardwareType_e sonarHardwareType = SONAR_HCSR04; -#ifdef USE_SONAR_SRF10 - if (srf10_detect()) { - sonarHardwareType = SONAR_SRF10; - } -#endif - return sonarHardwareType; -} + calculatedAltitude = RANGEFINDER_OUT_OF_RANGE; -void sonarInit(void) -{ - const sonarHardwareType_e sonarHardwareType = sonarDetect(); - if (sonarHardwareType == SONAR_HCSR04) { + if (rangefinderType == RANGEFINDER_HCSR04) { hcsr04_set_sonar_hardware(); } #ifndef UNIT_TEST - sonarSetFunctionPointers(sonarHardwareType); + rangefinderSetFunctionPointers(rangefinderType); #endif - sonarRange_t sonarRange; - sonarFunctionPointers.init(&sonarRange); - 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; - - sensorsSet(SENSOR_SONAR); + rangefinder_t rangefinder; + if (rangefinderType == RANGEFINDER_NONE) { + memset(&rangefinder, 0, sizeof(rangefinder)); + } else { + rangefinderFunctionPointers.init(&rangefinder); + } + rangefinderMaxRangeCm = rangefinder.maxRangeCm; + rangefinderCfAltCm = rangefinderMaxRangeCm / 2; + rangefinderMaxTiltDeciDegrees = rangefinder.detectionConeExtendedDeciDegrees / 2; + rangefinderMaxTiltCos = cos_approx(rangefinderMaxTiltDeciDegrees / 10.0f * RAD); + rangefinderMaxAltWithTiltCm = rangefinderMaxRangeCm * rangefinderMaxTiltCos; } -static int32_t applySonarMedianFilter(int32_t newSonarReading) +static int32_t applyMedianFilter(int32_t newReading) { #define DISTANCE_SAMPLES_MEDIAN 5 - static int32_t sonarFilterSamples[DISTANCE_SAMPLES_MEDIAN]; + static int32_t filterSamples[DISTANCE_SAMPLES_MEDIAN]; static int filterSampleIndex = 0; static bool medianFilterReady = false; - if (newSonarReading > SONAR_OUT_OF_RANGE) {// only accept samples that are in range - sonarFilterSamples[filterSampleIndex] = newSonarReading; + 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(sonarFilterSamples) : newSonarReading; + return medianFilterReady ? quickMedianFilter5(filterSamples) : newReading; } /* * This is called periodically by the scheduler */ -void sonarUpdate(void) +void rangefinderUpdate(void) { - if (sonarFunctionPointers.update) { - sonarFunctionPointers.update(); + if (rangefinderFunctionPointers.update) { + rangefinderFunctionPointers.update(); } } /** - * Get the last distance measured by the sonar in centimeters. When the ground is too far away, SONAR_OUT_OF_RANGE is returned. + * Get the last distance measured by the sonar in centimeters. When the ground is too far away, RANGEFINDER_OUT_OF_RANGE is returned. */ -int32_t sonarRead(void) +int32_t rangefinderRead(void) { - if (sonarFunctionPointers.read) { - const int32_t distance = sonarFunctionPointers.read(); - return applySonarMedianFilter(distance); + if (rangefinderFunctionPointers.read) { + const int32_t distance = rangefinderFunctionPointers.read(); + return applyMedianFilter(distance); } return 0; } @@ -218,25 +204,24 @@ int32_t sonarRead(void) * 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. + * When the ground is too far away or the tilt is too large, RANGEFINDER_OUT_OF_RANGE is returned. */ -int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle) +int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle) { // calculate sonar altitude only if the ground is in the sonar cone - if (cosTiltAngle < sonarMaxTiltCos) - calculatedAltitude = SONAR_OUT_OF_RANGE; - else if (sonarDistance == SONAR_OUT_OF_RANGE) - calculatedAltitude = SONAR_OUT_OF_RANGE; - else - calculatedAltitude = sonarDistance * cosTiltAngle; + if (cosTiltAngle < rangefinderMaxTiltCos || rangefinderDistance == RANGEFINDER_OUT_OF_RANGE) { + calculatedAltitude = RANGEFINDER_OUT_OF_RANGE; + } else { + calculatedAltitude = rangefinderDistance * cosTiltAngle; + } return calculatedAltitude; } /** - * Get the latest altitude that was computed by a call to sonarCalculateAltitude(), or SONAR_OUT_OF_RANGE if sonarCalculateAltitude + * Get the latest altitude that was computed by a call to rangefinderCalculateAltitude(), or RANGEFINDER_OUT_OF_RANGE if sonarCalculateAltitude * has never been called. */ -int32_t sonarGetLatestAltitude(void) +int32_t rangefinderGetLatestAltitude(void) { return calculatedAltitude; } diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h new file mode 100644 index 0000000000..b39b959658 --- /dev/null +++ b/src/main/sensors/rangefinder.h @@ -0,0 +1,45 @@ +/* + * 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 . + */ + +#pragma once + +#include + +typedef enum { + RANGEFINDER_NONE = 0, + RANGEFINDER_HCSR04, + RANGEFINDER_SRF10 +} rangefinderType_e; + +struct rangefinder_s; +typedef void (*rangefinderInitFunctionPtr)(struct rangefinder_s *rangefinderRange); +typedef void (*rangefinderUpdateFunctionPtr)(void); +typedef int32_t (*rangefinderReadFunctionPtr)(void); + +typedef struct rangefinderFunctionPointers_s { + rangefinderInitFunctionPtr init; + rangefinderUpdateFunctionPtr update; + rangefinderReadFunctionPtr read; +} rangefinderFunctionPointers_t; + +int32_t rangefinderCalculateAltitude(int32_t rangefinderDistance, float cosTiltAngle); +int32_t rangefinderGetLatestAltitude(void); +rangefinderType_e rangefinderDetect(void); +void rangefinderInit(rangefinderType_e rangefinderType); +void rangefinderUpdate(void); +int32_t rangefinderRead(void); + diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index 1741612f52..71bf6bff92 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -21,13 +21,11 @@ typedef enum { SENSOR_INDEX_GYRO = 0, SENSOR_INDEX_ACC, SENSOR_INDEX_BARO, - SENSOR_INDEX_MAG + SENSOR_INDEX_MAG, + SENSOR_INDEX_RANGEFINDER, + SENSOR_INDEX_COUNT } sensorIndex_e; -#define MAX_SENSORS_TO_DETECT (SENSOR_INDEX_MAG + 1) - -extern uint8_t detectedSensors[MAX_SENSORS_TO_DETECT]; - typedef struct int16_flightDynamicsTrims_s { int16_t roll; int16_t pitch; diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h deleted file mode 100644 index 20e312d9e8..0000000000 --- a/src/main/sensors/sonar.h +++ /dev/null @@ -1,47 +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 . - */ - -#pragma once - -#include "drivers/sonar_hcsr04.h" -#include "drivers/sonar.h" -#include "io/rc_controls.h" // for throttleStatus_e required by battery.h -#include "sensors/battery.h" - -typedef enum { - SONAR_NONE = 0, - SONAR_HCSR04, - SONAR_SRF10 -} sonarHardwareType_e; - -typedef void (*sonarInitFunctionPtr)(sonarRange_t *sonarRange); -typedef void (*sonarUpdateFunctionPtr)(void); -typedef int32_t (*sonarReadFunctionPtr)(void); - -typedef struct sonarFunctionPointers_s { - sonarInitFunctionPtr init; - sonarUpdateFunctionPtr update; - sonarReadFunctionPtr read; -} sonarFunctionPointers_t; - -const sonarHcsr04Hardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor); -int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle); -int32_t sonarGetLatestAltitude(void); -void sonarInit(void); -void sonarUpdate(void); -int32_t sonarRead(void); -