From 3ca385d92e9902ea17dfd8a9c9b5c0d47bb271b8 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Thu, 23 Jun 2016 11:04:38 +0100 Subject: [PATCH] Tidied up sonar targets --- src/main/drivers/pwm_mapping.c | 6 +-- src/main/drivers/pwm_mapping.h | 2 +- src/main/drivers/sonar_hcsr04.c | 9 ++-- src/main/drivers/sonar_hcsr04.h | 2 +- src/main/main.c | 22 ++++----- src/main/sensors/sonar.c | 47 +++++++++++--------- src/main/sensors/sonar.h | 1 + src/main/target/EUSTM32F103RC/sonar_config.c | 28 ------------ src/main/target/EUSTM32F103RC/target.h | 5 ++- src/main/target/NAZE/sonar_config.c | 28 ------------ src/main/target/NAZE/target.h | 6 ++- src/main/target/PORT103R/sonar_config.c | 28 ------------ src/main/target/PORT103R/target.h | 5 ++- 13 files changed, 57 insertions(+), 132 deletions(-) delete mode 100644 src/main/target/EUSTM32F103RC/sonar_config.c delete mode 100644 src/main/target/NAZE/sonar_config.c delete mode 100644 src/main/target/PORT103R/sonar_config.c diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 440068fe9..2b9fb09e2 100755 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -189,10 +189,10 @@ pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init) #endif #ifdef SONAR - if (init->sonarConfig && + if (init->useSonar && ( - timerHardwarePtr->tag == init->sonarConfig->triggerTag || - timerHardwarePtr->tag == init->sonarConfig->echoTag + timerHardwarePtr->tag == init->sonarConfig.triggerTag || + timerHardwarePtr->tag == init->sonarConfig.echoTag )) { continue; } diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 60b146288..756dac3fb 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -77,7 +77,7 @@ typedef struct drv_pwm_config_s { uint16_t motorPwmRate; uint16_t idlePulse; // PWM value to use when initializing the driver. set this to either PULSE_1MS (regular pwm), // some higher value (used by 3d mode), or 0, for brushed pwm drivers. - sonarIOConfig_t *sonarConfig; + sonarIOConfig_t sonarConfig; } drv_pwm_config_t; enum { diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index 04d1b2886..6374f81d3 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/src/main/drivers/sonar_hcsr04.c @@ -39,7 +39,7 @@ #if defined(SONAR) STATIC_UNIT_TESTED volatile int32_t measurement = -1; static uint32_t lastMeasurementAt; -static sonarHardware_t const *sonarHardware; +sonarHardware_t sonarHardwareHCSR04; extiCallbackRec_t hcsr04_extiCallbackRec; @@ -63,9 +63,8 @@ void hcsr04_extiHandler(extiCallbackRec_t* cb) } } -void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sonarRange) +void hcsr04_init(sonarRange_t *sonarRange) { - sonarHardware = initialSonarHardware; sonarRange->maxRangeCm = HCSR04_MAX_RANGE_CM; sonarRange->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES; sonarRange->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES; @@ -83,12 +82,12 @@ void hcsr04_init(const sonarHardware_t *initialSonarHardware, sonarRange_t *sona #endif // trigger pin - triggerIO = IOGetByTag(sonarHardware->triggerTag); + triggerIO = IOGetByTag(sonarHardwareHCSR04.triggerTag); IOInit(triggerIO, OWNER_SONAR, RESOURCE_INPUT); IOConfigGPIO(triggerIO, IOCFG_OUT_PP); // echo pin - echoIO = IOGetByTag(sonarHardware->echoTag); + echoIO = IOGetByTag(sonarHardwareHCSR04.echoTag); IOInit(echoIO, OWNER_SONAR, RESOURCE_INPUT); IOConfigGPIO(echoIO, IOCFG_IN_FLOATING); diff --git a/src/main/drivers/sonar_hcsr04.h b/src/main/drivers/sonar_hcsr04.h index d0e3ecaf9..d734f62f4 100644 --- a/src/main/drivers/sonar_hcsr04.h +++ b/src/main/drivers/sonar_hcsr04.h @@ -38,6 +38,6 @@ typedef struct sonarRange_s { #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 -void hcsr04_init(const sonarHardware_t *sonarHardware, sonarRange_t *sonarRange); +void hcsr04_init(sonarRange_t *sonarRange); void hcsr04_start_reading(void); int32_t hcsr04_get_distance(void); diff --git a/src/main/main.c b/src/main/main.c index fbfeb4999..b94bcab93 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -137,8 +137,7 @@ void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse); void spektrumBind(rxConfig_t *rxConfig); -const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig); -void sonarInit(const sonarHardware_t *sonarHardware); +const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor); void osdInit(void); typedef enum { @@ -264,15 +263,13 @@ void init(void) memset(&pwm_params, 0, sizeof(pwm_params)); #ifdef SONAR - const sonarHardware_t *sonarHardware = NULL; - if (feature(FEATURE_SONAR)) { - sonarHardware = sonarGetHardwareConfiguration(&masterConfig.batteryConfig); - sonarIOConfig_t sonarConfig = { - .triggerTag = sonarHardware->triggerTag, - .echoTag = sonarHardware->echoTag - }; - pwm_params.sonarConfig = &sonarConfig; + const sonarHardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType); + if (sonarHardware) { + pwm_params.useSonar = true; + pwm_params.sonarConfig.triggerTag = sonarHardware->triggerTag; + pwm_params.sonarConfig.echoTag = sonarHardware->echoTag; + } } #endif @@ -302,9 +299,6 @@ void init(void) pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP); pwm_params.usePPM = feature(FEATURE_RX_PPM); pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL); -#ifdef SONAR - pwm_params.useSonar = feature(FEATURE_SONAR); -#endif #ifdef USE_SERVOS pwm_params.useServos = isMixerUsingServos(); @@ -548,7 +542,7 @@ void init(void) #ifdef SONAR if (feature(FEATURE_SONAR)) { - sonarInit(sonarHardware); + sonarInit(); } #endif diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 206527f3f..89be0464e 100644 --- a/src/main/sensors/sonar.c +++ b/src/main/sensors/sonar.c @@ -22,6 +22,8 @@ #include "platform.h" #include "build_config.h" +#ifdef SONAR + #include "common/maths.h" #include "common/axis.h" @@ -36,10 +38,9 @@ // Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range. // Inclination is adjusted by imu - float baro_cf_vel; // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity) - float baro_cf_alt; // apply CF to use ACC for height estimation -#ifdef SONAR +extern sonarHardware_t sonarHardwareHCSR04; + int16_t sonarMaxRangeCm; int16_t sonarMaxAltWithTiltCm; int16_t sonarCfAltCm; // Complimentary Filter altitude @@ -48,34 +49,38 @@ float sonarMaxTiltCos; static int32_t calculatedAltitude; -#ifdef SONAR_CUSTOM_CONFIG -const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig); -#endif - -const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig) +const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor) { -#if defined(SONAR_TRIGGER_PIN) && defined(SONAR_ECHO_PIN) - UNUSED(batteryConfig); - static const sonarHardware_t const sonarHardware = { - .triggerTag = IO_TAG(SONAR_TRIGGER_PIN), - .echoTag = IO_TAG(SONAR_ECHO_PIN), - }; - return &sonarHardware; -#elif defined(SONAR_CUSTOM_CONFIG) - return sonarGetTargetHardwareConfiguration(batteryConfig); +#if defined(SONAR_TRIGGER_PIN_PWM) && defined(SONAR_ECHO_PIN_PWM) + // 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) && currentSensor == CURRENT_SENSOR_ADC)) { + sonarHardwareHCSR04.triggerTag = IO_TAG(SONAR_TRIGGER_PIN_PWM); + sonarHardwareHCSR04.echoTag = IO_TAG(SONAR_ECHO_PIN_PWM); + } else { + sonarHardwareHCSR04.triggerTag = IO_TAG(SONAR_TRIGGER_PIN); + sonarHardwareHCSR04.echoTag = IO_TAG(SONAR_ECHO_PIN); + } + return &sonarHardwareHCSR04; +#elif defined(SONAR_TRIGGER_PIN) && defined(SONAR_ECHO_PIN) + UNUSED(currentSensor); + sonarHardwareHCSR04.triggerTag = IO_TAG(SONAR_TRIGGER_PIN); + sonarHardwareHCSR04.echoTag = IO_TAG(SONAR_ECHO_PIN); + return &sonarHardwareHCSR04; #elif defined(UNIT_TEST) - UNUSED(batteryConfig); - return 0; + UNUSED(currentSensor); + return NULL; #else #error Sonar not defined for target #endif } -void sonarInit(const sonarHardware_t *sonarHardware) +void sonarInit(void) { sonarRange_t sonarRange; - hcsr04_init(sonarHardware, &sonarRange); + hcsr04_init(&sonarRange); sensorsSet(SENSOR_SONAR); sonarMaxRangeCm = sonarRange.maxRangeCm; sonarCfAltCm = sonarMaxRangeCm / 2; diff --git a/src/main/sensors/sonar.h b/src/main/sensors/sonar.h index 84f006bc9..be49afb7b 100644 --- a/src/main/sensors/sonar.h +++ b/src/main/sensors/sonar.h @@ -23,6 +23,7 @@ extern int16_t sonarMaxRangeCm; extern int16_t sonarCfAltCm; extern int16_t sonarMaxAltWithTiltCm; +void sonarInit(void); void sonarUpdate(void); int32_t sonarRead(void); int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle); diff --git a/src/main/target/EUSTM32F103RC/sonar_config.c b/src/main/target/EUSTM32F103RC/sonar_config.c deleted file mode 100644 index 1f9cf8104..000000000 --- a/src/main/target/EUSTM32F103RC/sonar_config.c +++ /dev/null @@ -1,28 +0,0 @@ - -#include "drivers/sonar_hcsr04.h" -#include "drivers/io.h" -#include "config/runtime_config.h" -#include "config/config.h" - -#include "sensors/battery.h" -#include "sensors/sonar.h" - -const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig) -{ - static const sonarHardware_t const sonarPWM56 = { - .triggerTag = IO_TAG(PB8), - .echoTag = IO_TAG(PB9), - }; - static const sonarHardware_t sonarRC78 = { - .triggerTag = IO_TAG(PB0), - .echoTag = IO_TAG(PB1), - }; - // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8 - if (feature(FEATURE_SOFTSERIAL) - || feature(FEATURE_RX_PARALLEL_PWM ) - || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) { - return &sonarPWM56; - } else { - return &sonarRC78; - } -} \ No newline at end of file diff --git a/src/main/target/EUSTM32F103RC/target.h b/src/main/target/EUSTM32F103RC/target.h index 2545ff49f..0ff1e2f0b 100644 --- a/src/main/target/EUSTM32F103RC/target.h +++ b/src/main/target/EUSTM32F103RC/target.h @@ -69,7 +69,10 @@ #define MAG_AK8975_ALIGN CW180_DEG_FLIP #define SONAR -#define SONAR_CUSTOM_CONFIG +#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN_PWM PB8 +#define SONAR_ECHO_PIN_PWM PB9 #define DISPLAY diff --git a/src/main/target/NAZE/sonar_config.c b/src/main/target/NAZE/sonar_config.c deleted file mode 100644 index f427de095..000000000 --- a/src/main/target/NAZE/sonar_config.c +++ /dev/null @@ -1,28 +0,0 @@ - -#include "drivers/sonar_hcsr04.h" -#include "drivers/io.h" -#include "config/runtime_config.h" -#include "config/config.h" - -#include "sensors/battery.h" -#include "sensors/sonar.h" - -const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig) -{ - static const sonarHardware_t const sonarPWM56 = { - .triggerTag = IO_TAG(PB8), - .echoTag = IO_TAG(PB9), - }; - static const sonarHardware_t sonarRC78 = { - .triggerTag = IO_TAG(PB0), - .echoTag = IO_TAG(PB1), - }; - // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8 - if (feature(FEATURE_SOFTSERIAL) - || feature(FEATURE_RX_PARALLEL_PWM ) - || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) { - return &sonarPWM56; - } else { - return &sonarRC78; - } -} diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index a99c0040e..fffbe42b4 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -111,7 +111,11 @@ #define MAG_HMC5883_ALIGN CW180_DEG #define SONAR -#define SONAR_CUSTOM_CONFIG +#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN_PWM PB8 +#define SONAR_ECHO_PIN_PWM PB9 + #define DISPLAY #define USE_USART1 diff --git a/src/main/target/PORT103R/sonar_config.c b/src/main/target/PORT103R/sonar_config.c deleted file mode 100644 index 1f9cf8104..000000000 --- a/src/main/target/PORT103R/sonar_config.c +++ /dev/null @@ -1,28 +0,0 @@ - -#include "drivers/sonar_hcsr04.h" -#include "drivers/io.h" -#include "config/runtime_config.h" -#include "config/config.h" - -#include "sensors/battery.h" -#include "sensors/sonar.h" - -const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig) -{ - static const sonarHardware_t const sonarPWM56 = { - .triggerTag = IO_TAG(PB8), - .echoTag = IO_TAG(PB9), - }; - static const sonarHardware_t sonarRC78 = { - .triggerTag = IO_TAG(PB0), - .echoTag = IO_TAG(PB1), - }; - // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8 - if (feature(FEATURE_SOFTSERIAL) - || feature(FEATURE_RX_PARALLEL_PWM ) - || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC)) { - return &sonarPWM56; - } else { - return &sonarRC78; - } -} \ No newline at end of file diff --git a/src/main/target/PORT103R/target.h b/src/main/target/PORT103R/target.h index 2e0292bc5..a4b4df7a5 100644 --- a/src/main/target/PORT103R/target.h +++ b/src/main/target/PORT103R/target.h @@ -87,7 +87,10 @@ #define USE_FLASH_M25P16 #define SONAR -#define SONAR_CUSTOM_CONFIG +#define SONAR_TRIGGER_PIN PB0 +#define SONAR_ECHO_PIN PB1 +#define SONAR_TRIGGER_PIN_PWM PB8 +#define SONAR_ECHO_PIN_PWM PB9 #define DISPLAY