Vivan Meazza:

The maths, so far, is now correct. Roll and pitch are now both in the
correct sense. The aircraft velocity is added correctly to the
submodel velocity, and the submodel is now visible when instantiated.

However, the velocity is measured at the aircraft centre. To be totally
correct we ought to take into account the aircraft's rotational
velocity. We have pitch rate and roll rate available, but not yaw rate
(small anyway).

diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx
index 94760bb7799216358df88c21d0657b735711dc3e..4b2b51c7ac4f9966a1c748316be9d8d98d4b93ed 100644 (file)
--- a/src/AIModel/AIBallistic.cxx
+++ b/src/AIModel/AIBallistic.cxx
@@ -46,6 +46,7 @@ bool FGAIBallistic::init() {
    aero_stabilized = true;
    hdg = azimuth;
    pitch = elevation;
+   Transform();
    return true;
 }
 

diff --git a/src/Systems/submodel.cxx b/src/Systems/submodel.cxx
index 3b012e54c4b61152f45fc073d4064ddb0a6bd908..b2a2171a7067bfbeb55fe9d5d486f541d3db7818 100644 (file)
--- a/src/Systems/submodel.cxx
+++ b/src/Systems/submodel.cxx
@@ -1,3 +1,4 @@
+
 // submodel.cxx - models a releasable submodel.
 // Written by Dave Culp, started Aug 2004
 //
@@ -50,6 +51,10 @@ SubmodelSystem::init ()
     _user_wind_from_east_node  = fgGetNode("/environment/wind-from-east-fps",true);
     _user_wind_from_north_node = fgGetNode("/environment/wind-from-north-fps",true);
 
+    _user_speed_down_fps_node   = fgGetNode("/velocities/speed-down-fps",true);
+    _user_speed_east_fps_node   = fgGetNode("/velocities/speed-east-fps",true);
+       _user_speed_north_fps_node  = fgGetNode("/velocities/speed-north-fps",true);
+       
     ai = (FGAIManager*)globals->get_subsystem("ai_model");
 
 
@@ -195,14 +200,19 @@ SubmodelSystem::transform( submodel* sm)
   IC.lat =        _user_lat_node->getDoubleValue();    
   IC.lon =        _user_lon_node->getDoubleValue();       
   IC.alt =        _user_alt_node->getDoubleValue();    
-  IC.roll =     - _user_roll_node->getDoubleValue();    // rotation about x axis
-  IC.elevation =  _user_pitch_node->getDoubleValue();   // rotation about y axis
+  IC.roll =     - _user_roll_node->getDoubleValue();    // rotation about x axis (-ve)
+  IC.elevation = - _user_pitch_node->getDoubleValue();   // rotation about y axis (-ve)
   IC.azimuth =    _user_heading_node->getDoubleValue(); // rotation about z axis
   
   IC.speed =           _user_speed_node->getDoubleValue();
   IC.wind_from_east =  _user_wind_from_east_node->getDoubleValue();
   IC.wind_from_north = _user_wind_from_north_node->getDoubleValue();
   
+  IC.speed_down_fps =   _user_speed_down_fps_node->getDoubleValue();
+  IC.speed_east_fps =   _user_speed_east_fps_node->getDoubleValue();
+  IC.speed_north_fps =  _user_speed_north_fps_node ->getDoubleValue();
+
+  
   in[0] = sm->x_offset;
   in[1] = sm->y_offset;
   in[2] = sm->z_offset; 
@@ -255,11 +265,50 @@ SubmodelSystem::transform( submodel* sm)
    
    // Get submodel initial velocity vector angles in XZ and XY planes.
    // This needs to be fixed. This vector should be added to aircraft's vector. 
-   IC.elevation += (sm->pitch_offset * cosRx) + (sm->yaw_offset * sinRx);
+   IC.elevation += (sm->yaw_offset * sinRx) - (sm->pitch_offset * cosRx);
    IC.azimuth   += (sm->yaw_offset * cosRx) - (sm->pitch_offset * sinRx);
  
    // For now assume vector is close to airplane's vector.  This needs to be fixed.
-   IC.speed += sm->speed; 
+   //IC.speed += ; 
+
+   // calcuate the total speed north
+
+   IC.total_speed_north = sm->speed * cos(IC.elevation*SG_DEGREES_TO_RADIANS)*
+                    cos(IC.azimuth*SG_DEGREES_TO_RADIANS) + IC.speed_north_fps;
+
+   // calculate the total speed east
+
+   IC.total_speed_east = sm->speed * cos(IC.elevation*SG_DEGREES_TO_RADIANS)*
+                     sin(IC.azimuth*SG_DEGREES_TO_RADIANS) + IC.speed_east_fps;
+
+   // calculate the total speed down
+  
+   IC.total_speed_down = sm->speed * -sin(IC.elevation*SG_DEGREES_TO_RADIANS) +
+                        IC.speed_down_fps;
+
+  // re-calculate speed, elevation and azimuth
+
+   IC.speed = sqrt( IC.total_speed_north * IC.total_speed_north + 
+                      IC.total_speed_east * IC.total_speed_east +
+                                         IC.total_speed_down * IC.total_speed_down);
+                         
+   IC.azimuth = atan(IC.total_speed_east/IC.total_speed_north) * SG_RADIANS_TO_DEGREES;
+   
+   // rationalise the output
+   
+   if (IC.total_speed_north <= 0){
+    IC.azimuth = 180 + IC.azimuth;
+       }
+   else{
+     if(IC.total_speed_east <= 0){
+     IC.azimuth = 360 + IC.azimuth;
+        }
+  }
+        
+   
+   
+   IC.elevation = atan(IC.total_speed_down/sqrt(IC.total_speed_north * IC.total_speed_north + 
+                      IC.total_speed_east * IC.total_speed_east)) * SG_RADIANS_TO_DEGREES;
 
 }
 
@@ -273,3 +322,4 @@ SubmodelSystem::updatelat(double lat)
 
 // end of submodel.cxx
 
+

diff --git a/src/Systems/submodel.hxx b/src/Systems/submodel.hxx
index 499eb00494eae3c90b8d242abaa2c5b2746e6ae6..d5cb3ecf8404c8940c5afdd7021e06403667c657 100644 (file)
--- a/src/Systems/submodel.hxx
+++ b/src/Systems/submodel.hxx
@@ -59,6 +59,12 @@ public:
   double     speed;
   double     wind_from_east;
   double     wind_from_north;
+  double     speed_down_fps;
+  double     speed_east_fps;
+  double     speed_north_fps;
+  double     total_speed_down;
+  double     total_speed_east;
+  double     total_speed_north;
  } IC_struct;  
 
     SubmodelSystem ();
@@ -111,6 +117,10 @@ private:
     SGPropertyNode* _user_speed_node;
     SGPropertyNode* _user_wind_from_east_node;
     SGPropertyNode* _user_wind_from_north_node;
+       SGPropertyNode* _user_speed_down_fps_node;
+       SGPropertyNode* _user_speed_east_fps_node;
+       SGPropertyNode* _user_speed_north_fps_node;
+       
     FGAIManager* ai;
     IC_struct  IC;
 

diff --git a/src/Systems/system_mgr.cxx b/src/Systems/system_mgr.cxx
index 07639af1e27997e7194a52f36268832d563715f5..fbd6ee6a0c7ae5e64f3f9cc87cf5e94d087d861e 100644 (file)
--- a/src/Systems/system_mgr.cxx
+++ b/src/Systems/system_mgr.cxx
@@ -19,7 +19,7 @@ FGSystemMgr::FGSystemMgr ()
     set_subsystem( "static", new StaticSystem );
     set_subsystem( "vacuum-l", new VacuumSystem(0) );
     set_subsystem( "vacuum-r", new VacuumSystem(1) );
-    // set_subsystem( "submodel", new SubmodelSystem() );
+    set_subsystem( "submodel", new SubmodelSystem() );
 }
 
 FGSystemMgr::~FGSystemMgr ()