src/main/sensors/sonar.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include "platform.h"
  22 #include "build_config.h"
  23
  24 #include "common/maths.h"
  25 #include "common/axis.h"
  26
  27 #include "drivers/sonar_hcsr04.h"
  28 #include "drivers/gpio.h"
  29 #include "config/runtime_config.h"
  30 #include "config/config.h"
  31
  32 #include "sensors/sensors.h"
  33 #include "sensors/battery.h"
  34 #include "sensors/sonar.h"
  35
  36 // Sonar measurements are in cm, a value of SONAR_OUT_OF_RANGE indicates sonar is not in range.
  37 // Inclination is adjusted by imu
  38     float baro_cf_vel;                      // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity)
  39     float baro_cf_alt;                      // apply CF to use ACC for height estimation
  40
  41 #ifdef SONAR
  42 int16_t sonarMaxRangeCm;
  43 int16_t sonarMaxAltWithTiltCm;
  44 STATIC_UNIT_TESTED int16_t sonarMaxTiltDeciDegrees;
  45 float sonarMaxTiltCos;
  46
  47 static int32_t calculatedAltitude;
  48
  49 const sonarHardware_t *sonarGetHardwareConfiguration(batteryConfig_t *batteryConfig)
  50 {
  51 #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
  52     static const sonarHardware_t sonarPWM56 = {
  53         .trigger_pin = Pin_8,   // PWM5 (PB8) - 5v tolerant
  54         .echo_pin = Pin_9,      // PWM6 (PB9) - 5v tolerant
  55         .exti_line = EXTI_Line9,
  56         .exti_pin_source = GPIO_PinSource9,
  57         .exti_irqn = EXTI9_5_IRQn
  58     };
  59     static const sonarHardware_t sonarRC78 = {
  60         .trigger_pin = Pin_0,   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
  61         .echo_pin = Pin_1,      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
  62         .exti_line = EXTI_Line1,
  63         .exti_pin_source = GPIO_PinSource1,
  64         .exti_irqn = EXTI1_IRQn
  65     };
  66     // If we are using parallel PWM for our receiver or ADC current sensor, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
  67     if (feature(FEATURE_RX_PARALLEL_PWM ) || (feature(FEATURE_CURRENT_METER) && batteryConfig->currentMeterType == CURRENT_SENSOR_ADC) ) {
  68         return &sonarPWM56;
  69     } else {
  70         return &sonarRC78;
  71     }
  72 #elif defined(OLIMEXINO)
  73     UNUSED(batteryConfig);
  74     static const sonarHardware_t const sonarHardware = {
  75         .trigger_pin = Pin_0,   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
  76         .echo_pin = Pin_1,      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
  77         .exti_line = EXTI_Line1,
  78         .exti_pin_source = GPIO_PinSource1,
  79         .exti_irqn = EXTI1_IRQn
  80     };
  81     return &sonarHardware;
  82 #elif defined(CC3D)
  83     UNUSED(batteryConfig);
  84     static const sonarHardware_t const sonarHardware = {
  85         .trigger_pin = Pin_5,   // (PB5)
  86         .echo_pin = Pin_0,      // (PB0) - only 3.3v ( add a 1K Ohms resistor )
  87         .exti_line = EXTI_Line0,
  88         .exti_pin_source = GPIO_PinSource0,
  89         .exti_irqn = EXTI0_IRQn
  90     };
  91     return &sonarHardware;
  92 #elif defined(SPRACINGF3)
  93     UNUSED(batteryConfig);
  94     static const sonarHardware_t const sonarHardware = {
  95         .trigger_pin = Pin_0,   // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
  96         .echo_pin = Pin_1,      // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
  97         .exti_line = EXTI_Line1,
  98         .exti_pin_source = EXTI_PinSource1,
  99         .exti_irqn = EXTI1_IRQn
 100     };
 101     return &sonarHardware;
 102 #elif defined(UNIT_TEST)
 103     UNUSED(batteryConfig);
 104     return 0;
 105 #else
 106 #error Sonar not defined for target
 107 #endif
 108 }
 109
 110 void sonarInit(const sonarHardware_t *sonarHardware)
 111 {
 112     sonarRange_t sonarRange;
 113
 114     hcsr04_init(sonarHardware, &sonarRange);
 115     sensorsSet(SENSOR_SONAR);
 116     sonarMaxRangeCm = sonarRange.maxRangeCm;
 117     sonarMaxTiltDeciDegrees =  sonarRange.detectionConeExtendedDeciDegrees / 2;
 118     sonarMaxTiltCos = cos_approx(sonarMaxTiltDeciDegrees / 10.0f * RAD);
 119     sonarMaxAltWithTiltCm = sonarMaxRangeCm * sonarMaxTiltCos;
 120     calculatedAltitude = SONAR_OUT_OF_RANGE;
 121 }
 122
 123 void sonarUpdate(void)
 124 {
 125     hcsr04_start_reading();
 126 }
 127
 128 #define SONAR_SAMPLES_MEDIAN 3
 129
 130 /**
 131  * Get the last distance measured by the sonar in centimeters. When the ground is too far away, SONAR_OUT_OF_RANGE is returned.
 132  */
 133 int32_t sonarRead(void)
 134 {
 135     int32_t distance = hcsr04_get_distance();
 136     if (distance > HCSR04_MAX_RANGE_CM)
 137         distance = SONAR_OUT_OF_RANGE;
 138     return distance;
 139 }
 140
 141 /**
 142  * Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
 143  * the altitude. Returns the computed altitude in centimeters.
 144  *
 145  * When the ground is too far away or the tilt is too large, SONAR_OUT_OF_RANGE is returned.
 146  */
 147 int32_t sonarCalculateAltitude(int32_t sonarDistance, float cosTiltAngle)
 148 {
 149     // calculate sonar altitude only if the ground is in the sonar cone
 150     if (cosTiltAngle <= sonarMaxTiltCos)
 151         calculatedAltitude = SONAR_OUT_OF_RANGE;
 152     else
 153         // altitude = distance * cos(tiltAngle), use approximation
 154         calculatedAltitude = sonarDistance * cosTiltAngle;
 155
 156     return calculatedAltitude;
 157 }
 158
 159 /**
 160  * Get the latest altitude that was computed by a call to sonarCalculateAltitude(), or SONAR_OUT_OF_RANGE if sonarCalculateAltitude
 161  * has never been called.
 162  */
 163 int32_t sonarGetLatestAltitude(void)
 164 {
 165     return calculatedAltitude;
 166 }
 167
 168 #endif