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

[rotorcraft] ins_int_extended without sonar

Browse Source
This commit is contained in:
Felix Ruess
2013-08-02 01:03:54 +02:00
parent df2297f4d6
commit f59d32b799
1 changed files with 11 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ins/ins_int_extended.c
+11
View File
@@ -82,6 +82,7 @@ int32_t ins_baro_alt;
#include "filters/median_filter.h"
struct MedianFilterInt baro_median;
#if USE_SONAR
/* sonar */
bool_t ins_update_on_agl;
int32_t ins_sonar_alt;
@@ -92,6 +93,7 @@ struct MedianFilterInt sonar_median;
#endif
#define VFF_R_SONAR_0 0.1
#define VFF_R_SONAR_OF_M 0.2
#endif // USE_SONAR
/* output */
struct NedCoor_i ins_ltp_pos;
@@ -122,9 +124,13 @@ void ins_init() {
ins_baro_initialised = FALSE;
init_median_filter(&baro_median);
#if USE_SONAR
ins_update_on_agl = FALSE;
init_median_filter(&sonar_median);
ins_sonar_offset = INS_SONAR_OFFSET;
#endif
vff_init(0., 0., 0., 0.);
ins.vf_realign = FALSE;
ins.hf_realign = FALSE;
@@ -203,6 +209,7 @@ void ins_update_baro() {
vff_update_baro(alt_float);
}
}
INS_NED_TO_STATE();
}
@@ -244,6 +251,7 @@ void ins_update_gps(void) {
#endif /* hff not used */
}
INS_NED_TO_STATE();
#endif /* USE_GPS */
}
@@ -261,6 +269,8 @@ float var_err[VAR_ERR_MAX];
uint8_t var_idx = 0;
#endif
#if USE_SONAR
void ins_update_sonar() {
static float last_offset = 0.;
// new value filtered with median_filter
@@ -309,3 +319,4 @@ void ins_update_sonar() {
vff_update_offset(last_offset);
}
}
#endif // USE_SONAR