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
  23 #include "common/maths.h"
  24 #include "common/axis.h"
  25
  26 #include "drivers/sonar_hcsr04.h"
  27 #include "drivers/gpio.h"
  28 #include "config/runtime_config.h"
  29 #include "config/config.h"
  30
  31 #include "sensors/sensors.h"
  32 #include "sensors/sonar.h"
  33
  34 // in cm , -1 indicate sonar is not in range - inclination adjusted by imu
  35
  36 #ifdef SONAR
  37
  38 static int32_t calculatedAltitude;
  39
  40 void sonarInit(void)
  41 {
  42 #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
  43     static const sonarHardware_t const sonarPWM56 = {
  44         .trigger_pin = Pin_8,   // PWM5 (PB8) - 5v tolerant
  45         .echo_pin = Pin_9,      // PWM6 (PB9) - 5v tolerant
  46         .exti_line = EXTI_Line9,
  47         .exti_pin_source = GPIO_PinSource9,
  48         .exti_irqn = EXTI9_5_IRQn
  49     };
  50     static const sonarHardware_t const sonarRC78 = {
  51         .trigger_pin = Pin_0,   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
  52         .echo_pin = Pin_1,      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
  53         .exti_line = EXTI_Line1,
  54         .exti_pin_source = GPIO_PinSource1,
  55         .exti_irqn = EXTI1_IRQn
  56     };
  57     // If we are using parallel PWM for our receiver, then use motor pins 5 and 6 for sonar, otherwise use rc pins 7 and 8
  58     if (feature(FEATURE_RX_PARALLEL_PWM)) {
  59         hcsr04_init(&sonarPWM56);
  60     } else {
  61         hcsr04_init(&sonarRC78);
  62     }
  63 #elif defined(OLIMEXINO)
  64     static const sonarHardware_t const sonarHardware = {
  65         .trigger_pin = Pin_0,   // RX7 (PB0) - only 3.3v ( add a 1K Ohms resistor )
  66         .echo_pin = Pin_1,      // RX8 (PB1) - only 3.3v ( add a 1K Ohms resistor )
  67         .exti_line = EXTI_Line1,
  68         .exti_pin_source = GPIO_PinSource1,
  69         .exti_irqn = EXTI1_IRQn
  70     };
  71     hcsr04_init(&sonarHardware);
  72 #else
  73 #error Sonar not defined for target
  74 #endif
  75
  76     sensorsSet(SENSOR_SONAR);
  77     calculatedAltitude = -1;
  78 }
  79
  80 void sonarUpdate(void)
  81 {
  82     hcsr04_start_reading();
  83 }
  84
  85 int32_t sonarRead(void)
  86 {
  87     return hcsr04_get_distance();
  88 }
  89
  90 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
  91 {
  92     // calculate sonar altitude only if the sonar is facing downwards(<25deg)
  93     if (tiltAngle > 250)
  94         calculatedAltitude = -1;
  95     else
  96         calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f;
  97
  98     return calculatedAltitude;
  99 }
 100
 101 int32_t sonarGetLatestAltitude(void)
 102 {
 103     return calculatedAltitude;
 104 }
 105
 106 #endif