Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
sonar_pwm.c
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2020 OpenuAS
3 *
4 * Thanks to Jean-François Erdelyi & Gautier Hattenberger for ADC one
5 *
6 * This file is part of paparazzi
7 *
8 * paparazzi is free software; you can redistribute it and/or modify
9 * it under the terms of the GNU General Public License as published by
10 * the Free Software Foundation; either version 2, or (at your option)
11 * any later version.
12 *
13 * paparazzi is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU General Public License for more details.
17 *
18 * You should have received a copy of the GNU General Public License
19 * along with paparazzi; see the file COPYING. If not, see
20 * <http://www.gnu.org/licenses/>.
21 */
22 
38 #include "mcu_periph/pwm_input.h"
39 #include "subsystems/abi.h"
40 
41 #include "filters/median_filter.h"
42 
43 #if PERIODIC_TELEMETRY
45 #include "pprzlink/messages.h"
46 #endif
47 #ifdef SENSOR_SYNC_SEND_SONAR
49 #endif
50 #include "generated/airframe.h"
51 #ifdef SITL
52 #include "state.h"
53 #endif
54 
55 #ifdef SONAR_USE_PWM_FILTER
57 #endif
58 
59 /* Config parameters for sensor */
60 
62 #ifndef SONAR_PWM_CHANNEL
63 #error "No SONAR_PWM_CHANNEL defined to use sonar_pwm module, add a line like define=SONAR_PWM_CHANNEL value=PWM_INPUT1"
64 #endif
65 
67 #ifndef SONAR_OFFSET
68 #define SONAR_OFFSET 0
69 #endif
70 
72 #ifndef SONAR_PWM_PERIOD
73 #define SONAR_PWM_PERIOD 4096
74 #endif
75 
77 #ifndef SONAR_SCALE
78 #define SONAR_SCALE SONAR_PWM_PERIOD
79 #endif
80 
82 #ifndef SONAR_MEDIAN_SIZE
83 #define SONAR_MEDIAN_SIZE 7 //Good for a noisy sonar
84 #endif
85 
87 #ifndef SONAR_MIN_RANGE
88 #define SONAR_MIN_RANGE 0.15f //Default is a common value for regular sonars. Cannot measure closer than that
89 #endif
90 
92 #ifndef SONAR_MAX_RANGE
93 #define SONAR_MAX_RANGE 7.0f //Reasonable maximum value for regular sonars.
94 #endif
95 
97 #ifndef SONAR_PWM_OFFSET
98 #define SONAR_PWM_OFFSET 820 //822
99 #endif
100 
101 // Enable telemetry report
102 #ifndef SENSOR_SYNC_SEND_SONAR
103 #define SENSOR_SYNC_SEND_SONAR TRUE
104 #endif
105 
107 
108 /* Set the default values at initialization */
109 void sonar_pwm_init(void)
110 {
113  sonar_pwm.raw = 0.0f;
114  #ifdef SONAR_USE_PWM_FILTER
116  #endif
117 }
118 
119 /* Read the sensor current measured value */
120 void sonar_pwm_read(void) {
121 
122 #ifndef SITL
123  // raw duty cycle in usec
124  uint16_t sonar_duty_raw = get_pwm_input_duty_in_usec(SONAR_PWM_CHANNEL);
125 
126  // remove PWM offset where needed
127  sonar_pwm.raw = sonar_duty_raw - SONAR_PWM_OFFSET;
128 
129 #ifdef SONAR_USE_PWM_FILTER
131 #else
133 #endif
134 
135  Bound(sonar_pwm.distance, (float)SONAR_MIN_RANGE, (float)SONAR_MAX_RANGE);
136 
137 #if SONAR_COMPENSATE_ROTATION
138  float phi = stateGetNedToBodyEulers_f()->phi;
139  float theta = stateGetNedToBodyEulers_f()->theta;
140  float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
142 #endif
143 
144 #else // SITL
146 #endif // SITL
147 
148 #if USE_SONAR
149  uint32_t now_ts = get_sys_time_usec();
150  AbiSendMsgAGL(AGL_SONAR_PWM_ID, now_ts, sonar_pwm.distance);
151 #endif
152 
153 #ifdef SENSOR_SYNC_SEND_SONAR
154  DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_pwm.raw, &sonar_pwm.distance);
155 #endif
156 }
uint16_t
unsigned short uint16_t
Definition: types.h:16
sonar_pwm
struct SonarPwm sonar_pwm
Definition: sonar_pwm.c:106
SonarPwm::distance
float distance
Distance measured.
Definition: sonar_pwm.h:46
SonarPwm::scale
float scale
scale to convert raw to a real distance
Definition: sonar_pwm.h:45
SONAR_MEDIAN_SIZE
#define SONAR_MEDIAN_SIZE
The Median Filter strength.
Definition: sonar_pwm.c:83
abi.h
SonarPwm
Definition: sonar_pwm.h:42
stateGetPositionEnu_f
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition: state.h:719
stateGetNedToBodyEulers_f
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
uint32_t
unsigned long uint32_t
Definition: types.h:18
AGL_SONAR_PWM_ID
#define AGL_SONAR_PWM_ID
Definition: abi_sender_ids.h:177
update_median_filter_f
static float update_median_filter_f(struct MedianFilterFloat *filter, float new_data)
Definition: median_filter.h:175
EnuCoor_f::z
float z
in meters
Definition: pprz_geodetic_float.h:75
FloatEulers::theta
float theta
in radians
Definition: pprz_algebra_float.h:86
SONAR_SCALE
#define SONAR_SCALE
Ranger scale or sensitivity as you wish.
Definition: sonar_pwm.c:78
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
telemetry.h
SONAR_PWM_OFFSET
#define SONAR_PWM_OFFSET
Some sensor may need an initial PWM offset (823 usec in the case of an EZ1 sensor)
Definition: sonar_pwm.c:98
FloatEulers::phi
float phi
in radians
Definition: pprz_algebra_float.h:85
get_pwm_input_duty_in_usec
uint32_t get_pwm_input_duty_in_usec(uint32_t channel)
Definition: pwm_input.c:39
sonar_pwm_init
void sonar_pwm_init(void)
Definition: sonar_pwm.c:109
sonar_adc
struct SonarAdc sonar_adc
Definition: sonar_adc.c:50
sonar_pwm.h
median_filter.h
pwm_input.h
arch independent PWM input capture API
MedianFilterFloat
Definition: median_filter.h:142
SONAR_MAX_RANGE
#define SONAR_MAX_RANGE
The maximum range for the device to be able to measure.
Definition: sonar_pwm.c:93
SonarAdc::distance
float distance
Distance measured in meters.
Definition: sonar_adc.h:35
SONAR_MIN_RANGE
#define SONAR_MIN_RANGE
The minimum range for the device to be able to measure.
Definition: sonar_pwm.c:88
sonar_pwm_read
void sonar_pwm_read(void)
Definition: sonar_pwm.c:120
init_median_filter_f
static void init_median_filter_f(struct MedianFilterFloat *filter, uint8_t size)
Definition: median_filter.h:148
sonar_filt
struct MedianFilterFloat sonar_filt
Definition: sonar_bebop.c:47
SonarPwm::raw
uint16_t raw
raw PWM value
Definition: sonar_pwm.h:43
state.h
SONAR_OFFSET
#define SONAR_OFFSET
The input channel of the PWM based sensor.
Definition: sonar_pwm.c:68
SonarPwm::offset
float offset
offset
Definition: sonar_pwm.h:44