Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 24 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" #include "filters/median_filter.h"
struct MedianFilterInt baro_median; struct MedianFilterInt baro_median;
#if USE_SONAR
/* sonar */ /* sonar */
bool_t ins_update_on_agl; bool_t ins_update_on_agl;
int32_t ins_sonar_alt; int32_t ins_sonar_alt;
@@ -92,6 +93,7 @@ struct MedianFilterInt sonar_median;
#endif #endif
#define VFF_R_SONAR_0 0.1 #define VFF_R_SONAR_0 0.1
#define VFF_R_SONAR_OF_M 0.2 #define VFF_R_SONAR_OF_M 0.2
#endif // USE_SONAR
/* output */ /* output */
struct NedCoor_i ins_ltp_pos; struct NedCoor_i ins_ltp_pos;
@@ -122,9 +124,13 @@ void ins_init() {
ins_baro_initialised = FALSE; ins_baro_initialised = FALSE;
init_median_filter(&baro_median); init_median_filter(&baro_median);
#if USE_SONAR
ins_update_on_agl = FALSE; ins_update_on_agl = FALSE;
init_median_filter(&sonar_median); init_median_filter(&sonar_median);
ins_sonar_offset = INS_SONAR_OFFSET; ins_sonar_offset = INS_SONAR_OFFSET;
#endif
vff_init(0., 0., 0., 0.); vff_init(0., 0., 0., 0.);
ins.vf_realign = FALSE; ins.vf_realign = FALSE;
ins.hf_realign = FALSE; ins.hf_realign = FALSE;
@@ -203,6 +209,7 @@ void ins_update_baro() {
vff_update_baro(alt_float); vff_update_baro(alt_float);
} }
} }
INS_NED_TO_STATE();
} }
@@ -244,6 +251,7 @@ void ins_update_gps(void) {
#endif /* hff not used */ #endif /* hff not used */
} }
INS_NED_TO_STATE();
#endif /* USE_GPS */ #endif /* USE_GPS */
} }
@@ -261,6 +269,8 @@ float var_err[VAR_ERR_MAX];
uint8_t var_idx = 0; uint8_t var_idx = 0;
#endif #endif
#if USE_SONAR
void ins_update_sonar() { void ins_update_sonar() {
static float last_offset = 0.; static float last_offset = 0.;
// new value filtered with median_filter // new value filtered with median_filter
@@ -309,3 +319,4 @@ void ins_update_sonar() {
vff_update_offset(last_offset); vff_update_offset(last_offset);
} }
} }
#endif // USE_SONAR