[nps] fdm_jsbsim: use GetAccelBody for accelerometer simulation

paparazzi · Mar 10, 2015 · 8177db2 · 8177db2
1 parent 06b9456
commit 8177db2
Show file tree

Hide file tree

Showing 4 changed files with 38 additions and 14 deletions.
diff --git a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile
@@ -23,6 +23,8 @@ nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy $(shell pcre-co
 nps.CFLAGS  += -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I$(PAPARAZZI_SRC)/sw/simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps
 nps.LDFLAGS += $(shell sdl-config --libs)
 
+nps.CFLAGS  += -DNPS_ACCEL_FROM_UVWDOT
+
 #
 # add the simulator directory to the make searchpath
 #

diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h
@@ -59,22 +59,32 @@ struct NpsFdm {
   struct LlaCoor_d lla_pos_geoc; //geocentric lla from jsbsim
   double agl; //AGL from jsbsim in m
 
-  /*  velocity and acceleration wrt inertial frame expressed in ecef frame */
-  //  struct EcefCoor_d  ecef_inertial_vel;
-  //  struct EcefCoor_d  ecef_inertial_accel;
-  /*  velocity and acceleration wrt ecef frame expressed in ecef frame     */
+  /** 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 */
   struct EcefCoor_d  ecef_ecef_accel;
-  /*  velocity and acceleration wrt ecef frame expressed in body frame     */
+
+  /** velocity in body frame, wrt ECEF frame */
   struct DoubleVect3 body_ecef_vel;   /* aka UVW */
+  /** acceleration in body frame, wrt ECEF frame */
   struct DoubleVect3 body_ecef_accel;
-  /*  velocity and acceleration wrt ecef frame expressed in ltp frame     */
+
+  /** velocity in LTP frame, wrt ECEF frame */
   struct NedCoor_d ltp_ecef_vel;
+  /** acceleration in LTP frame, wrt ECEF frame */
   struct NedCoor_d ltp_ecef_accel;
-  /*  velocity and acceleration wrt ecef frame expressed in ltppprz frame */
+
+  /** velocity in ltppprz frame, wrt ECEF frame */
   struct NedCoor_d ltpprz_ecef_vel;
+  /** accel in ltppprz frame, wrt ECEF frame */
   struct NedCoor_d ltpprz_ecef_accel;
 
+  /** acceleration in body frame as measured by an accelerometer (incl. gravity) */
+  struct DoubleVect3 body_accel;
+
   /* attitude */
   struct DoubleQuat   ecef_to_body_quat;
   struct DoubleQuat   ltp_to_body_quat;

diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp
@@ -286,7 +286,13 @@ static void fetch_state(void) {
   const FGColumnVector3& fg_body_ecef_vel = propagate->GetUVW();
   jsbsimvec_to_vec(&fdm.body_ecef_vel, &fg_body_ecef_vel);
   const FGColumnVector3& fg_body_ecef_accel = accelerations->GetUVWdot();
-  jsbsimvec_to_vec(&fdm.body_ecef_accel,&fg_body_ecef_accel);
+  jsbsimvec_to_vec(&fdm.body_ecef_accel, &fg_body_ecef_accel);
+
+  const FGColumnVector3& fg_body_inertial_accel = accelerations->GetUVWidot();
+  jsbsimvec_to_vec(&fdm.body_inertial_accel, &fg_body_inertial_accel);
+
+  const FGColumnVector3& fg_body_accel = accelerations->GetBodyAccel();
+  jsbsimvec_to_vec(&fdm.body_accel, &fg_body_accel);
 
 #if DEBUG_NPS_JSBSIM
   printf("%f,%f,%f,%f,%f,%f,",(&fg_body_ecef_accel)->Entry(1),(&fg_body_ecef_accel)->Entry(2),(&fg_body_ecef_accel)->Entry(3),fdm.body_ecef_accel.x,fdm.body_ecef_accel.y,fdm.body_ecef_accel.z);
@@ -304,9 +310,10 @@ static void fetch_state(void) {
 #endif
 
   /* in ECEF frame */
-  const FGMatrix33& body_to_ecef = propagate->GetTb2ec();
-  const FGColumnVector3& fg_ecef_ecef_vel = body_to_ecef * fg_body_ecef_vel;
+  const FGColumnVector3& fg_ecef_ecef_vel = propagate->GetECEFVelocity();
   jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel);
+
+  const FGMatrix33& body_to_ecef = propagate->GetTb2ec();
   const FGColumnVector3& fg_ecef_ecef_accel = body_to_ecef * fg_body_ecef_accel;
   jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel);
 

diff --git a/sw/simulator/nps/nps_sensor_accel.c b/sw/simulator/nps/nps_sensor_accel.c
@@ -30,22 +30,27 @@ void   nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru
   if (time < accel->next_update)
     return;
 
+#if NPS_ACCEL_FROM_UVWDOT
   /* transform gravity to body frame */
   struct DoubleVect3 g_body;
   double_quat_vmult(&g_body, &fdm.ltp_to_body_quat, &fdm.ltp_g);
-  //  printf(" g_body %f %f %f\n", g_body.x, g_body.y, g_body.z);
+  //  printf(" g_body     % .3f % .3f % .3f\n", g_body.x, g_body.y, g_body.z);
 
-  //  printf(" accel_fdm %f %f %f\n", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z);
+  //  printf(" accel_fdm  % .3f % .3f % .3f\n", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z);
 
   /* substract gravity to acceleration in body frame */
   struct DoubleVect3 accelero_body;
   VECT3_DIFF(accelero_body, fdm.body_ecef_accel, g_body);
+#else
+  struct DoubleVect3 accelero_body;
+  VECT3_COPY(accelero_body, fdm.body_accel);
+#endif
 
-  //  printf(" accelero body %f %f %f\n", accelero_body.x, accelero_body.y, accelero_body.z);
+  //  printf(" accel body % .3f %.3f % .3f\n", accelero_body.x, accelero_body.y, accelero_body.z);
 
   /* transform to imu frame */
   struct DoubleVect3 accelero_imu;
-  MAT33_VECT3_MUL(accelero_imu, *body_to_imu, accelero_body );
+  MAT33_VECT3_MUL(accelero_imu, *body_to_imu, accelero_body);
 
   /* compute accelero readings */
   MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu);