Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 12:23:23 +08:00
Code Issues Actions 27 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
+50 -52
View File
@@ -302,74 +302,72 @@ static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
#if USE_GPS #if USE_GPS
void ins_int_update_gps(struct GpsState *gps_s) void ins_int_update_gps(struct GpsState *gps_s)
{ {
if (gps_s->fix == GPS_FIX_3D) { if (gps_s->fix != GPS_FIX_3D) {
if (!ins_int.ltp_initialized) { return;
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);
}
struct NedCoor_i gps_pos_cm_ned; if (!ins_int.ltp_initialized) {
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps_s->ecef_pos); ins_reset_local_origin();
}
/* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */ struct NedCoor_i gps_pos_cm_ned;
ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps_s->ecef_pos);
/* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */
#ifdef INS_BODY_TO_GPS_X #ifdef INS_BODY_TO_GPS_X
/* body2gps translation in body frame */ /* body2gps translation in body frame */
struct Int32Vect3 b2g_b = { struct Int32Vect3 b2g_b = {
.x = INS_BODY_TO_GPS_X, .x = INS_BODY_TO_GPS_X,
.y = INS_BODY_TO_GPS_Y, .y = INS_BODY_TO_GPS_Y,
.z = INS_BODY_TO_GPS_Z .z = INS_BODY_TO_GPS_Z
}; };
/* rotate offset given in body frame to navigation/ltp frame using current attitude */ /* rotate offset given in body frame to navigation/ltp frame using current attitude */
struct Int32Quat q_b2n; struct Int32Quat q_b2n;
memcpy(&q_b2n, stateGetNedToBodyQuat_i(), sizeof(struct Int32Quat)); memcpy(&q_b2n, stateGetNedToBodyQuat_i(), sizeof(struct Int32Quat));
QUAT_INVERT(q_b2n, q_b2n); QUAT_INVERT(q_b2n, q_b2n);
struct Int32Vect3 b2g_n; struct Int32Vect3 b2g_n;
int32_quat_vmult(&b2g_n, &q_b2n, &b2g_b); int32_quat_vmult(&b2g_n, &q_b2n, &b2g_b);
/* subtract body2gps translation in ltp from gps position */ /* subtract body2gps translation in ltp from gps position */
VECT3_SUB(gps_pos_cm_ned, b2g_n); VECT3_SUB(gps_pos_cm_ned, b2g_n);
#endif #endif
/// @todo maybe use gps_s->ned_vel directly?? /// @todo maybe use gps_s->ned_vel directly??
struct NedCoor_i gps_speed_cm_s_ned; struct NedCoor_i gps_speed_cm_s_ned;
ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps_s->ecef_vel); ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps_s->ecef_vel);
#if INS_USE_GPS_ALT #if INS_USE_GPS_ALT
vff_update_z_conf((float)gps_pos_cm_ned.z / 100.0, INS_VFF_R_GPS); vff_update_z_conf((float)gps_pos_cm_ned.z / 100.0, INS_VFF_R_GPS);
#endif #endif
#if USE_HFF #if USE_HFF
/* horizontal gps transformed to NED in meters as float */ /* horizontal gps transformed to NED in meters as float */
struct FloatVect2 gps_pos_m_ned; struct FloatVect2 gps_pos_m_ned;
VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y); VECT2_ASSIGN(gps_pos_m_ned, gps_pos_cm_ned.x, gps_pos_cm_ned.y);
VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f); VECT2_SDIV(gps_pos_m_ned, gps_pos_m_ned, 100.0f);
struct FloatVect2 gps_speed_m_s_ned; struct FloatVect2 gps_speed_m_s_ned;
VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y); VECT2_ASSIGN(gps_speed_m_s_ned, gps_speed_cm_s_ned.x, gps_speed_cm_s_ned.y);
VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.); VECT2_SDIV(gps_speed_m_s_ned, gps_speed_m_s_ned, 100.);
if (ins_int.hf_realign) { if (ins_int.hf_realign) {
ins_int.hf_realign = FALSE; ins_int.hf_realign = FALSE;
const struct FloatVect2 zero = {0.0f, 0.0f}; const struct FloatVect2 zero = {0.0f, 0.0f};
b2_hff_realign(gps_pos_m_ned, zero); b2_hff_realign(gps_pos_m_ned, zero);
} }
// run horizontal filter // run horizontal filter
b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned); b2_hff_update_gps(&gps_pos_m_ned, &gps_speed_m_s_ned);
// convert and copy result to ins_int // convert and copy result to ins_int
ins_update_from_hff(); ins_update_from_hff();
#else /* hff not used */ #else /* hff not used */
/* simply copy horizontal pos/speed from gps */ /* simply copy horizontal pos/speed from gps */
INT32_VECT2_SCALE_2(ins_int.ltp_pos, gps_pos_cm_ned, INT32_VECT2_SCALE_2(ins_int.ltp_pos, gps_pos_cm_ned,
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned, INT32_VECT2_SCALE_2(ins_int.ltp_speed, gps_speed_cm_s_ned,
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif /* USE_HFF */ #endif /* USE_HFF */
ins_ned_to_state(); ins_ned_to_state();
}
} }
#else #else
void ins_int_update_gps(struct GpsState *gps_s __attribute__((unused))) {} 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) void vff_realign(float z_meas)
{ {
vff.z = z_meas; vff_init(z_meas, 0., 0.);
vff.zdot = 0;
} }