From 2f581faf3e03536201e1aad34417b7cd7bea26a9 Mon Sep 17 00:00:00 2001
From: ehofman <ehofman>
Date: Tue, 14 Sep 2004 08:27:55 +0000
Subject: [PATCH] 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).
---
 src/AIModel/AIBallistic.cxx |  1 +
 src/Systems/submodel.cxx    | 58 ++++++++++++++++++++++++++++++++++---
 src/Systems/submodel.hxx    | 10 +++++++
 src/Systems/system_mgr.cxx  |  2 +-
 4 files changed, 66 insertions(+), 5 deletions(-)

diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx
index 94760bb77..4b2b51c7a 100644
--- 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 3b012e54c..b2a2171a7 100644
--- 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 499eb0049..d5cb3ecf8 100644
--- 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 07639af1e..fbd6ee6a0 100644
--- 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 ()