Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-28 18:07:25 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

update agl dist filter

Browse Source
This commit is contained in:
kirkscheper
2017-10-16 16:40:00 +02:00
committed by Gautier Hattenberger
parent 68255aa454
commit 542014588f
2 changed files with 25 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/sonar/agl_dist.c
+24 -20
View File
@@ -22,7 +22,7 @@
/** /**
* @file modules/sonar/agl_dist.h * @file modules/sonar/agl_dist.h
* *
* Bind to sonar ABI message and provide a filtered value to be used in flight plans * Bind to agl ABI message and provide a filtered value to be used in flight plans
* *
*/ */
@@ -33,45 +33,49 @@
float agl_dist_valid; float agl_dist_valid;
float agl_dist_value; float agl_dist_value;
float agl_dist_value_filtered; float agl_dist_value_filtered;
float agl_measurement_time;
/** default sonar */ /** default sonar */
#ifndef AGL_DIST_SONAR_ID #ifndef AGL_DIST_ID
#define AGL_DIST_SONAR_ID ABI_BROADCAST #define AGL_DIST_ID ABI_BROADCAST
#endif #endif
#ifndef AGL_DIST_SONAR_MAX_RANGE #ifndef AGL_DIST_MAX_RANGE
#define AGL_DIST_SONAR_MAX_RANGE 5.0 #define AGL_DIST_MAX_RANGE 5.0f
#endif #endif
#ifndef AGL_DIST_SONAR_MIN_RANGE #ifndef AGL_DIST_MIN_RANGE
#define AGL_DIST_SONAR_MIN_RANGE 0.001 #define AGL_DIST_MIN_RANGE 0.001f
#endif #endif
#ifndef AGL_DIST_SONAR_FILTER #ifndef AGL_DIST_FILTER
#define AGL_DIST_SONAR_FILTER 5 #define AGL_DIST_FILTER 0.1f
#endif #endif
abi_event sonar_ev; abi_event agl_ev;
static void sonar_cb(uint8_t sender_id, float distance); static void agl_cb(uint8_t sender_id, float distance);
void agl_dist_init(void) void agl_dist_init(void)
{ {
agl_dist_valid = false; agl_dist_valid = false;
agl_dist_value = 0.; agl_dist_value = 0.f;
agl_dist_value_filtered = 0.; agl_dist_value_filtered = 0.f;
agl_measurement_time = get_sys_time_float();
// Bind to AGL message // Bind to AGL message
AbiBindMsgAGL(AGL_DIST_SONAR_ID, &sonar_ev, sonar_cb); AbiBindMsgAGL(AGL_DIST_ID, &agl_ev, agl_cb);
} }
static void agl_cb(uint8_t __attribute__((unused)) sender_id, float distance)
static void sonar_cb(uint8_t __attribute__((unused)) sender_id, float distance)
{ {
if (distance < AGL_DIST_SONAR_MAX_RANGE && distance > AGL_DIST_SONAR_MIN_RANGE) { if (distance < AGL_DIST_MAX_RANGE && distance > AGL_DIST_MIN_RANGE) {
agl_dist_value = distance; agl_dist_value = distance;
agl_dist_valid = true; agl_dist_valid = true;
agl_dist_value_filtered = (AGL_DIST_SONAR_FILTER * agl_dist_value_filtered + agl_dist_value) / float now = get_sys_time_float();
(AGL_DIST_SONAR_FILTER + 1); float dt = now - agl_measurement_time;
agl_measurement_time = now;
// update multirate exponentially weighted moving average filter
agl_dist_value_filtered += (agl_dist_value - agl_dist_value_filtered) * dt / (AGL_DIST_FILTER + dt);
} else { } else {
agl_dist_valid = false; agl_dist_valid = false;
} }
} }
sw/airborne/modules/sonar/agl_dist.h
+1 -1
View File
@@ -22,7 +22,7 @@
/** /**
* @file modules/sonar/agl_dist.h * @file modules/sonar/agl_dist.h
* *
* Bind to sonar ABI message and provide a filtered value to be used in flight plans * Bind to agl ABI message and provide a filtered value to be used in flight plans
* *
*/ */