conf/airframes/jsbsim.xml

@@ -240,7 +240,7 @@ sim.srcs = $(SRC_ARCH)/sim_jsbsim.c $(SRC_ARCH)/ivy_transport.c
 sim.srcs += latlong.c radio_control.c downlink.c commands.c gps.c inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c datalink.c
 sim.srcs += nav_line.c nav_survey_rectangle.c
 
-sim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_fw.c
+sim.srcs += $(SIMDIR)/sim_ac_jsbsim.c $(SIMDIR)/sim_ac_booz.c
 
   </makefile>
 </airframe>

sw/simulator/nps_fdm.h

@@ -12,11 +12,11 @@ struct NpsFdm {
   
   VEC* ecef_pos;
   VEC* ecef_vel;
-  VEC* ecef_accel;
+  VEC* body_accel;
 
   VEC* ltp_to_body_quat;
-  VEC* ltp_body_rate;
-  VEC* ltp_body_accel;
+  VEC* body_rate;
+  VEC* body_rate_dot;
 
 };
 

sw/simulator/nps_fdm_jsbsim.c

@@ -8,15 +8,21 @@ using namespace JSBSim;
 
 static void feed_jsbsim(double* commands);
 static void fetch_state(void);
+static void jsbsimvec_to_vec(VEC* vector, const FGColumnVector3* jsb_vector);
+static void jsbsimloc_to_vec(VEC* vector, FGLocation* location);
+static void jsbsimquat_to_vec(VEC* ltp_to_body_quat, FGQuaternion* quat);
 
+struct NpsFdm fdm;
 FGFDMExec* FDMExec;
 
 void nps_fdm_init(double dt) {
 
 #if 0
 
+  char rootdir[1024];
   JSBSim::FGState* State;
 
+  sprintf(rootdir,"%s/conf/simulator",getenv("PPRZ_HOME"));
   FDMExec = new FGFDMExec();
 
   State = FDMExec->GetState();
@@ -28,17 +34,17 @@ void nps_fdm_init(double dt) {
     FDMExec->DisableOutput();
     FDMExec->SetDebugLevel(0); // No DEBUG messages
     
-    if ( ! FDMExec->LoadModel( RootDir + "aircraft",
-                               RootDir + "engine",
-                               RootDir + "systems",
-                               AircraftName)){
+    if ( ! FDMExec->LoadModel( rootdir + "aircraft",
+                               rootdir + "engine",
+                               rootdir + "systems",
+                               AIRFRAME_NAME)){
       cerr << "  JSBSim could not be started" << endl << endl;
       delete FDMExec;
       exit(-1);
     }
     
     JSBSim::FGInitialCondition *IC = FDMExec->GetIC();
-    if ( ! IC->Load(ICName)) {
+    if ( ! IC->Load(NPS_INITIAL_CONDITITONS)) {
       delete FDMExec;
       cerr << "Initialization unsuccessful" << endl;
       exit(-1);
@@ -72,7 +78,7 @@ static void feed_jsbsim(double* commands) {
   char buf[64];
   
   for (int i=0; i<SERVOS_NB; i++) {
-    sprintf(buf,"fcs/%s",ACTUATOR_COMMAND_NAMES[i]);
+    sprintf(buf,"fcs/%s",NPS_ACTUATOR_NAMES[i]);
     FDMExec->GetPropertyManager()->SetDouble(buf,commands[i]);
   }
 #endif
@@ -81,23 +87,45 @@ static void feed_jsbsim(double* commands) {
 
 static void fetch_state(void) {
 
-#if 0
   FGGroundReactions* ground_reactions;
-  FGPropertyManager* cur_node;
-  double cur_value
-  
-  ground_reactions = FGGroundReactions(FDMExec);
+  FGPropagate* propagate;
+  FGPropagate::VehicleState* VState;
 
-  bool_t fdm.on_ground /* with */ bool ground_reactions->GetWOW();
-  VEC* ecef_pos /* with */ bool ground_reactions->GetWOW();
+  ground_reactions = FDMExec->GetGroundReactions();
+  propagate = FDMExec->GetPropagate();
+  VState = propagate->GetVState();
 
-  VEC* ecef_vel;
-  VEC* ecef_accel;
+  fdm.on_ground = ground_reactions->GetWOW();
 
-  VEC* ltp_to_body_quat;
-  VEC* ltp_body_rate;
-  VEC* ltp_body_accel;
-#endif
-  /* JSBSim to fdm */
+  jsbsimloc_to_vec(fdm.ecef_pos,&VState->vLocation);
+  jsbsimvec_to_vec(fdm.ecef_vel,&VState->vUVW);
+
+  jsbsimvec_to_vec(fdm.body_accel,&propagate->GetUVWdot());
+
+  jsbsimquat_to_vec(fdm.ltp_to_body_quat,&VState->vQtrn);
+  jsbsimvec_to_vec(fdm.body_rate,&VState->vPQR);
+  jsbsimvec_to_vec(fdm.body_rate_dot,&propagate->GetPQRdot());
 
 }
+
+static void jsbsimloc_to_vec(VEC* vector, FGLocation* location) {
+  int i;
+  for (i=0; i<3; i++) {
+    vector->ve[i] = location->Entry(i+1);
+  }
+}
+
+static void jsbsimquat_to_vec(VEC* vector, FGQuaternion* quat) {
+  int i;
+  for (i=0; i<3; i++) {
+    vector->ve[i] = quat->Entry(i+1);
+  }
+}
+
+static void jsbsimvec_to_vec(VEC* vector, const FGColumnVector3* jsb_vector) {
+  int i;
+  for (i=0; i<3; i++) {
+    vector->ve[i] = jsb_vector->Entry(i+1);
+  }  
+}
+