From ed5223e588276ac2de127afcba2f078dbdc9c376 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 29 Jan 2015 22:51:10 +0100 Subject: [PATCH] [ins] start considering BODY_TO_GPS translation If INS_BODY_TO_GPS_X|Y|Z is defined (in cm!) use it to correct for this offset in the INS gps position update. Todo: - also consider this offset when (re)setting local origin - consider this offset/lever for velocity updates (when angular rates != zero) --- sw/airborne/math/pprz_algebra_int.h | 1 + sw/airborne/subsystems/ins/ins_int.c | 19 +++++++++++++++++++ 2 files changed, 20 insertions(+) diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h index a75dd1cfe30..100dfc1053e 100644 --- a/sw/airborne/math/pprz_algebra_int.h +++ b/sw/airborne/math/pprz_algebra_int.h @@ -500,6 +500,7 @@ extern void int32_quat_integrate_fi(struct Int32Quat *q, struct Int64Quat *hr, s /** rotate 3D vector by quaternion. * vb = q_a2b * va * q_a2b^-1 + * Doesn't support inplace rotation, meaning v_out mustn't be a pointer to same struct as v_in. */ extern void int32_quat_vmult(struct Int32Vect3 *v_out, struct Int32Quat *q, struct Int32Vect3 *v_in); diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c index ee73131fba0..c8fe048e934 100644 --- a/sw/airborne/subsystems/ins/ins_int.c +++ b/sw/airborne/subsystems/ins/ins_int.c @@ -311,6 +311,25 @@ void ins_update_gps(void) struct NedCoor_i gps_pos_cm_ned; ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_impl.ltp_def, &gps.ecef_pos); + + /* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */ +#ifdef INS_BODY_TO_GPS_X + /* body2gps translation in body frame */ + struct Int32Vect3 b2g_b = { + .x = INS_BODY_TO_GPS_X, + .y = INS_BODY_TO_GPS_Y, + .z = INS_BODY_TO_GPS_Z + }; + /* rotate offset given in body frame to navigation/ltp frame using current attitude */ + struct Int32Quat q_b2n; + memcpy(&q_b2n, stateGetNedToBodyQuat_i(), sizeof(struct Int32Quat)); + QUAT_INVERT(q_b2n, q_b2n); + struct Int32Vect3 b2g_n; + int32_quat_vmult(&b2g_n, &q_b2n, &b2g_b); + /* subtract body2gps translation in ltp from gps position */ + VECT3_SUB(gps_pos_cm_ned, b2g_n); +#endif + /// @todo maybe use gps.ned_vel directly?? struct NedCoor_i gps_speed_cm_s_ned; ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_impl.ltp_def, &gps.ecef_vel);