[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

@@ -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);
 

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);