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 ()