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

[ins] prevent drifting of the ins_invariant while waiting first gps data

Browse Source
This commit is contained in:
Gautier Hattenberger
2014-06-19 10:50:15 +02:00
parent daae114124
commit e8ac068976
2 changed files with 9 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/estimation/ins_float_invariant.xml
+1
View File
@@ -3,6 +3,7 @@
<settings>
<dl_settings>
<dl_settings NAME="invariant">
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ins_impl.reset" shortname="reset"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.lv" shortname="lv" module="subsystems/ins/ins_float_invariant"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.lb" shortname="lb"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_impl.gains.mv" shortname="mv"/>
sw/airborne/subsystems/ins/ins_float_invariant.c
+8 -2
View File
@@ -194,6 +194,9 @@ PRINT_CONFIG_VAR(INS_BARO_ID)
abi_event baro_ev;
static void baro_cb(uint8_t sender_id, const float *pressure);
/* gps */
bool_t ins_gps_fix_once;
/* error computation */
static inline void error_output(struct InsFloatInv * _ins);
@@ -217,6 +220,7 @@ static inline void init_invariant_state(void) {
// init baro
ins_baro_initialized = FALSE;
ins_gps_fix_once = FALSE;
}
void ins_init() {
@@ -465,6 +469,7 @@ void ahrs_propagate(void) {
void ahrs_update_gps(void) {
if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING) {
ins_gps_fix_once = TRUE;
#if INS_UPDATE_FW_ESTIMATOR
if (state.utm_initialized_f) {
@@ -618,13 +623,14 @@ static inline void error_output(struct InsFloatInv * _ins) {
FLOAT_VECT3_DIFF(Eb, B, YBt);
// pos and speed error only if GPS data are valid
if (gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING
// or while waiting first GPS data to prevent diverging
if ((gps.fix == GPS_FIX_3D && ins.status == INS_RUNNING
#if INS_UPDATE_FW_ESTIMATOR
&& state.utm_initialized_f
#else
&& state.ned_initialized_f
#endif
) {
) || !ins_gps_fix_once) {
/* Ev = (V - YV) */
FLOAT_VECT3_DIFF(Ev, _ins->state.speed, _ins->meas.speed_gps);
/* Ex = (X - YX) */