Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
agl_dist.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Gautier Hattenberger <gautier.hattenberger@enac.fr>
3  *
4  * This file is part of paparazzi
5 
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, see
18  * <http://www.gnu.org/licenses/>.
19  *
20  */
21 
29 #include "modules/sonar/agl_dist.h"
30 #include "modules/core/abi.h"
31 #include "generated/airframe.h"
32 
37 
39 #ifndef AGL_DIST_ID
40 #define AGL_DIST_ID ABI_BROADCAST
41 #endif
42 #ifndef AGL_DIST_MAX_RANGE
43 #define AGL_DIST_MAX_RANGE 5.0f
44 #endif
45 #ifndef AGL_DIST_MIN_RANGE
46 #define AGL_DIST_MIN_RANGE 0.001f
47 #endif
48 #ifndef AGL_DIST_FILTER
49 #define AGL_DIST_FILTER 0.1f
50 #endif
51 
53 
54 static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance);
55 
56 void agl_dist_init(void)
57 {
58  agl_dist_valid = false;
59  agl_dist_value = 0.f;
62 
63  // Bind to AGL message
64  AbiBindMsgAGL(AGL_DIST_ID, &agl_ev, agl_cb);
65 }
66 
67 static void agl_cb(uint8_t __attribute__((unused)) sender_id, uint32_t __attribute__((unused)) stamp, float distance)
68 {
69  if (distance < AGL_DIST_MAX_RANGE && distance > AGL_DIST_MIN_RANGE) {
70  agl_dist_value = distance;
71  agl_dist_valid = true;
72  float now = get_sys_time_float();
73  float dt = now - agl_measurement_time;
75 
76  // update multirate exponentially weighted moving average filter
78  } else {
79  agl_dist_valid = false;
80  }
81 }
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
agl_dist.h
abi.h
get_sys_time_float
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:138
AGL_DIST_ID
#define AGL_DIST_ID
default sonar
Definition: agl_dist.c:40
abi_struct
Event structure to store callbacks in a linked list.
Definition: abi_common.h:66
agl_dist_value_filtered
float agl_dist_value_filtered
Definition: agl_dist.c:35
agl_ev
abi_event agl_ev
Definition: agl_dist.c:52
agl_dist_valid
float agl_dist_valid
Definition: agl_dist.c:33
AGL_DIST_MIN_RANGE
#define AGL_DIST_MIN_RANGE
Definition: agl_dist.c:46
agl_dist_init
void agl_dist_init(void)
Definition: agl_dist.c:56
agl_cb
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Definition: agl_dist.c:67
AGL_DIST_FILTER
#define AGL_DIST_FILTER
Definition: agl_dist.c:49
agl_dist_value
float agl_dist_value
Definition: agl_dist.c:34
agl_measurement_time
float agl_measurement_time
Definition: agl_dist.c:36