Merge pull request #573 from martinbudden/bf_sonar_tidy2

Tidied up sonar targets - for review

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.triggerTag ||
-                timerHardwarePtr->tag == init->sonarConfig->echoTag
+                timerHardwarePtr->tag == init->sonarConfig.echoTag
             )) {
             continue;
         }

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 {

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

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

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);
+const sonarHardware_t *sonarGetHardwareConfiguration(currentSensor_e currentSensor);
-void sonarInit(const sonarHardware_t *sonarHardware);
 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);
+        const sonarHardware_t *sonarHardware = sonarGetHardwareConfiguration(masterConfig.batteryConfig.currentMeterType);
-        sonarIOConfig_t sonarConfig = {
+        if (sonarHardware) {
-            .triggerTag = sonarHardware->triggerTag,
+            pwm_params.useSonar = true;
-            .echoTag = sonarHardware->echoTag
+            pwm_params.sonarConfig.triggerTag = sonarHardware->triggerTag;
-        };
+            pwm_params.sonarConfig.echoTag = sonarHardware->echoTag;
-        pwm_params.sonarConfig = &sonarConfig;
+        }
     }
 #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
 

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 *sonarGetHardwareConfiguration(currentSensor_e currentSensor)
-const sonarHardware_t *sonarGetTargetHardwareConfiguration(batteryConfig_t *batteryConfig);
-#endif
-
-const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig)
 {
-#if defined(SONAR_TRIGGER_PIN) && defined(SONAR_ECHO_PIN)
+#if defined(SONAR_TRIGGER_PIN_PWM) && defined(SONAR_ECHO_PIN_PWM)
-    UNUSED(batteryConfig);
+    // If we are using softserial, parallel PWM or ADC current sensor, then use motor pins for sonar, otherwise use RC pins
-    static const sonarHardware_t const sonarHardware = {
+    if (feature(FEATURE_SOFTSERIAL)
-		.triggerTag = IO_TAG(SONAR_TRIGGER_PIN),
+            || feature(FEATURE_RX_PARALLEL_PWM )
-		.echoTag = IO_TAG(SONAR_ECHO_PIN),
+            || (feature(FEATURE_CURRENT_METER) && currentSensor == CURRENT_SENSOR_ADC)) {
-    };
+        sonarHardwareHCSR04.triggerTag = IO_TAG(SONAR_TRIGGER_PIN_PWM);
-    return &sonarHardware;
+        sonarHardwareHCSR04.echoTag = IO_TAG(SONAR_ECHO_PIN_PWM);
-#elif defined(SONAR_CUSTOM_CONFIG)
+    } else {
-    return sonarGetTargetHardwareConfiguration(batteryConfig);
+        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);
+    UNUSED(currentSensor);
-    return 0;
+    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;

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

src/main/target/EUSTM32F103RC/sonar_config.c

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

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
 

src/main/target/NAZE/sonar_config.c

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

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

src/main/target/PORT103R/sonar_config.c

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

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