From d56a4a5381b9fe811a3c40c9cbe25c8b56c50d8a Mon Sep 17 00:00:00 2001
From: Felix Ruess <felix.ruess@gmail.com>
Date: Tue, 10 Mar 2015 14:30:03 +0100
Subject: [PATCH] [nps] use angular velocity wrt inertial frame for gyro sim

---
 sw/simulator/nps/nps_fdm.h          | 12 ++++++++----
 sw/simulator/nps/nps_fdm_crrcsim.c  | 12 ++++++------
 sw/simulator/nps/nps_fdm_jsbsim.cpp |  3 +++
 sw/simulator/nps/nps_sensor_gyro.c  |  2 +-
 4 files changed, 18 insertions(+), 11 deletions(-)

diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h
index 387ebe668e..8364d78880 100644
--- a/sw/simulator/nps/nps_fdm.h
+++ b/sw/simulator/nps/nps_fdm.h
@@ -59,9 +59,6 @@ struct NpsFdm {
   struct LlaCoor_d lla_pos_geoc; //geocentric lla from jsbsim
   double agl; //AGL from jsbsim in m
 
-  /** acceleration in body frame, wrt ECI inertial frame */
-  struct DoubleVect3 body_inertial_accel;
-
   /** velocity in ECEF frame, wrt ECEF frame */
   struct EcefCoor_d  ecef_ecef_vel;
   /** acceleration in ECEF frame, wrt ECEF frame */
@@ -82,6 +79,9 @@ struct NpsFdm {
   /** accel in ltppprz frame, wrt ECEF frame */
   struct NedCoor_d ltpprz_ecef_accel;
 
+  /** acceleration in body frame, wrt ECI inertial frame */
+  struct DoubleVect3 body_inertial_accel;
+
   /** acceleration in body frame as measured by an accelerometer (incl. gravity) */
   struct DoubleVect3 body_accel;
 
@@ -92,10 +92,14 @@ struct NpsFdm {
   struct DoubleQuat   ltpprz_to_body_quat;
   struct DoubleEulers ltpprz_to_body_eulers;
 
-  /*  velocity and acceleration wrt ecef frame expressed in body frame     */
+  /*  angular velocity and acceleration in body frame, wrt ECEF frame */
   struct DoubleRates  body_ecef_rotvel;
   struct DoubleRates  body_ecef_rotaccel;
 
+  /*  angular velocity and acceleration in body frame, wrt inertial ECI frame */
+  struct DoubleRates  body_inertial_rotvel;
+  struct DoubleRates  body_inertial_rotaccel;
+
   struct DoubleVect3 ltp_g;
   struct DoubleVect3 ltp_h;
 
diff --git a/sw/simulator/nps/nps_fdm_crrcsim.c b/sw/simulator/nps/nps_fdm_crrcsim.c
index 4a7a0013bf..b6825dc8eb 100644
--- a/sw/simulator/nps/nps_fdm_crrcsim.c
+++ b/sw/simulator/nps/nps_fdm_crrcsim.c
@@ -436,9 +436,9 @@ void decode_imupacket(struct NpsFdm * fdm, byte* buffer)
 
 
   /* angular rate (0.9387340515702713e4 rad/s to rad/s) */
-  fdm->body_ecef_rotvel.p  = (double)ShortOfBuf(buffer,9)*1.06526e-04;
-  fdm->body_ecef_rotvel.q  = (double)ShortOfBuf(buffer,11)*1.06526e-04;
-  fdm->body_ecef_rotvel.r  = (double)ShortOfBuf(buffer,13)*1.06526e-04;
+  fdm->body_inertial_rotvel.p  = (double)ShortOfBuf(buffer,9)*1.06526e-04;
+  fdm->body_inertial_rotvel.q  = (double)ShortOfBuf(buffer,11)*1.06526e-04;
+  fdm->body_inertial_rotvel.r  = (double)ShortOfBuf(buffer,13)*1.06526e-04;
 
   /* magnetic field in Gauss */
   //fdm->mag.x = (double)ShortOfBuf(buffer,15)*6.10352e-05;
@@ -454,9 +454,9 @@ void decode_imupacket(struct NpsFdm * fdm, byte* buffer)
       fdm->body_ecef_accel.x,
       fdm->body_ecef_accel.y,
       fdm->body_ecef_accel.z,
-      fdm->body_ecef_rotvel.p,
-      fdm->body_ecef_rotvel.q,
-      fdm->body_ecef_rotvel.r);
+      fdm->body_inertial_rotvel.p,
+      fdm->body_inertial_rotvel.q,
+      fdm->body_inertial_rotvel.r);
 #endif
 }
 
diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp
index d808eb21bc..bd3af57d65 100644
--- a/sw/simulator/nps/nps_fdm_jsbsim.cpp
+++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp
@@ -352,6 +352,9 @@ static void fetch_state(void) {
   jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR());
   jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot());
 
+  jsbsimvec_to_rate(&fdm.body_inertial_rotvel, &propagate->GetPQRi());
+  jsbsimvec_to_rate(&fdm.body_inertial_rotaccel, &accelerations->GetPQRidot());
+
 
   /*
    * wind
diff --git a/sw/simulator/nps/nps_sensor_gyro.c b/sw/simulator/nps/nps_sensor_gyro.c
index f6248a9ddd..623234fbb0 100644
--- a/sw/simulator/nps/nps_sensor_gyro.c
+++ b/sw/simulator/nps/nps_sensor_gyro.c
@@ -33,7 +33,7 @@ void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct Do
     return;
 
   /* transform body rates to IMU frame */
-  struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_ecef_rotvel);
+  struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_inertial_rotvel);
   struct DoubleVect3 rate_imu;
   MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body );
   /* compute gyros readings */