From 68a53a754606508f80d60bbfb0c0614ace028f7f Mon Sep 17 00:00:00 2001 From: onox Date: Fri, 22 May 2015 05:09:02 +0200 Subject: [PATCH] submodel: Fix incorrect yaw and pitch offsets Signed-off-by: onox --- src/AIModel/AIBallistic.cxx | 2 +- src/AIModel/AIBallistic.hxx | 2 +- src/AIModel/submodel.cxx | 43 +++++++++++-------------------------- src/AIModel/submodel.hxx | 23 +++++--------------- 4 files changed, 20 insertions(+), 50 deletions(-) diff --git a/src/AIModel/AIBallistic.cxx b/src/AIModel/AIBallistic.cxx index 78ed4cdf0..a8e5a16b7 100644 --- a/src/AIModel/AIBallistic.cxx +++ b/src/AIModel/AIBallistic.cxx @@ -48,9 +48,9 @@ _ht_agl_ft(0.0), _azimuth(0.0), _elevation(0.0), _rotation(0.0), +hs(0), _az_random_error(0.0), _el_random_error(0.0), -hs(0), _elapsed_time(0), _aero_stabilised(false), _drag_area(0.007), diff --git a/src/AIModel/AIBallistic.hxx b/src/AIModel/AIBallistic.hxx index 5b18a88ea..eb7938b20 100644 --- a/src/AIModel/AIBallistic.hxx +++ b/src/AIModel/AIBallistic.hxx @@ -176,11 +176,11 @@ private: bool _aero_stabilised; // if true, object will align with trajectory double _drag_area; // equivalent drag area in ft2 double _buoyancy; // fps^2 - bool _wind; // if true, local wind will be applied to object double _cd; // current drag coefficient double _init_cd; // initial drag coefficient double _cd_randomness; // randomness of Cd. 1.0 means +- 100%, 0.0 means no randomness double _life_timer; // seconds + bool _wind; // if true, local wind will be applied to object double _mass; // slugs bool _random; // modifier for Cd, life, az double _life_randomness; // dimension for _random, only applies to life at present diff --git a/src/AIModel/submodel.cxx b/src/AIModel/submodel.cxx index cb07fd804..6638e3986 100644 --- a/src/AIModel/submodel.cxx +++ b/src/AIModel/submodel.cxx @@ -345,11 +345,6 @@ void FGSubmodelMgr::transform(submodel *sm) if (sm->speed_node != 0) sm->speed = sm->speed_node->getDoubleValue(); - double yaw_offset = sm->yaw_offset->get_value(); - double pitch_offset = sm->pitch_offset->get_value(); - - //cout << " name " << name << " id " << id << " sub id" << sub_id << endl; - // set the Initial Conditions for the types of submodel parent if (_impact || _hit || _expiry) { @@ -385,21 +380,10 @@ void FGSubmodelMgr::transform(submodel *sm) setParentNode(id); } - //cout << "Submodel: setting IC "<< name << endl; - //cout << "heading " << IC.azimuth << endl ; - //cout << "speed down " << IC.speed_down_fps << endl ; - //cout << "speed east " << IC.speed_east_fps << endl ; - //cout << "speed north " << IC.speed_north_fps << endl ; - //cout << "parent speed fps in " << IC.speed << "sm speed in " << sm->speed << endl ; - //cout << "lat " << IC.lat; - //cout << "alt " << IC.alt << endl ; - // Set the Initial Conditions that are common to all types of parent IC.wind_from_east = _user_wind_from_east_node->getDoubleValue(); IC.wind_from_north = _user_wind_from_north_node->getDoubleValue(); - //cout << "wind e " << IC.wind_from_east << " n " << IC.wind_from_north << endl; - userpos.setLatitudeDeg(IC.lat); userpos.setLongitudeDeg(IC.lon); userpos.setElevationFt(IC.alt); @@ -410,18 +394,15 @@ void FGSubmodelMgr::transform(submodel *sm) setOffsetPos(); - // Pre-process the trig functions - cosRx = cos(-IC.roll * SG_DEGREES_TO_RADIANS); - sinRx = sin(-IC.roll * SG_DEGREES_TO_RADIANS); - cosRy = cos(-IC.elevation * SG_DEGREES_TO_RADIANS); - sinRy = sin(-IC.elevation * SG_DEGREES_TO_RADIANS); - cosRz = cos(IC.azimuth * SG_DEGREES_TO_RADIANS); - sinRz = sin(IC.azimuth * SG_DEGREES_TO_RADIANS); + double yaw_offset = sm->yaw_offset->get_value(); + double pitch_offset = sm->pitch_offset->get_value(); + + // Compute azimuth and elevation given the yaw and pitch offsets + SGQuatd ic_quat = SGQuatd::fromYawPitchRollDeg(IC.azimuth, IC.elevation, IC.roll); + ic_quat *= SGQuatd::fromYawPitchRollDeg(yaw_offset, pitch_offset, 0.0); - // Get submodel initial velocity vector angles in XZ and XY planes. - // This vector should be added to aircraft's vector. - IC.elevation += (yaw_offset * sinRx) + (pitch_offset * cosRx); - IC.azimuth += (yaw_offset * cosRx) - (pitch_offset * sinRx); + double ic_roll; + ic_quat.getEulerDeg(IC.azimuth, IC.elevation, ic_roll); // Calculate the total speed north IC.total_speed_north = sm->speed * cos(IC.elevation * SG_DEGREES_TO_RADIANS) @@ -440,23 +421,25 @@ void FGSubmodelMgr::transform(submodel *sm) + IC.total_speed_east * IC.total_speed_east + IC.total_speed_down * IC.total_speed_down); + cout << "sm speed: " << sm->speed << " IC speed: " << IC.speed << endl; + cout << "az1: " << IC.azimuth << " el1: " << IC.elevation << endl; + // If speeds are low this calculation can become unreliable if (IC.speed > 1) { IC.azimuth = atan2(IC.total_speed_east, IC.total_speed_north) * SG_RADIANS_TO_DEGREES; - // cout << "azimuth1 " << IC.azimuth<= 360) IC.azimuth -= 360; - // cout << "azimuth2 " << IC.azimuth<