Paparazzi UAS  v5.15_devel-113-g1b57ff1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "subsystems/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;
74  agl_measurement_time = now;
75 
76  // update multirate exponentially weighted moving average filter
78  } else {
79  agl_dist_valid = false;
80  }
81 }
Event structure to store callbacks in a linked list.
Definition: abi_common.h:65
#define AGL_DIST_ID
default sonar
Definition: agl_dist.c:40
Main include for ABI (AirBorneInterface).
static float get_sys_time_float(void)
Get the time in seconds since startup.
Definition: sys_time.h:129
unsigned long uint32_t
Definition: types.h:18
void agl_dist_init(void)
Definition: agl_dist.c:56
Bind to agl ABI message and provide a filtered value to be used in flight plans.
float agl_dist_value
Definition: agl_dist.c:34
#define AGL_DIST_FILTER
Definition: agl_dist.c:49
unsigned char uint8_t
Definition: types.h:14
static void agl_cb(uint8_t sender_id, uint32_t stamp, float distance)
Definition: agl_dist.c:67
float agl_measurement_time
Definition: agl_dist.c:36
abi_event agl_ev
Definition: agl_dist.c:52
float agl_dist_valid
Definition: agl_dist.c:33
float agl_dist_value_filtered
Definition: agl_dist.c:35
#define AGL_DIST_MIN_RANGE
Definition: agl_dist.c:46