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

[ins] ins_int: properly reset vff state on realign and initial ltp fix

Browse Source
This commit is contained in:
Felix Ruess
2015-03-31 14:45:10 +02:00
parent 2502871d47
commit b7cb5fd39a
2 changed files with 51 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ins/ins_int.c
+5 -7
View File
@@ -302,13 +302,12 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
#if USE_GPS
void ins_int_update_gps(struct GpsState *gps_s)
{
if (gps_s->fix == GPS_FIX_3D) {
if (gps_s->fix != GPS_FIX_3D) {
return;
}
if (!ins_int.ltp_initialized) {
ltp_def_from_ecef_i(&ins_int.ltp_def, &gps_s->ecef_pos);
ins_int.ltp_def.lla.alt = gps_s->lla_pos.alt;
ins_int.ltp_def.hmsl = gps_s->hmsl;
ins_int.ltp_initialized = TRUE;
stateSetLocalOrigin_i(&ins_int.ltp_def);
ins_reset_local_origin();
}
struct NedCoor_i gps_pos_cm_ned;
@@ -369,7 +368,6 @@ void ins_int_update_gps(struct GpsState *gps_s)
#endif /* USE_HFF */
ins_ned_to_state();
}
}
#else
void ins_int_update_gps(struct GpsState *gps_s __attribute__((unused))) {}
sw/airborne/subsystems/ins/vf_float.c
+1 -2
View File
@@ -254,6 +254,5 @@ void vff_update_vz_conf(float vz_meas, float conf)
void vff_realign(float z_meas)
{
vff.z = z_meas;
vff.zdot = 0;
vff_init(z_meas, 0., 0.);
}