fly/flightgear
1
0
Fork 0
Code Activity

submodel: Fix incorrect yaw and pitch offsets

Browse source 
Signed-off-by: onox <denkpadje@gmail.com>
This commit is contained in:
onox 2015-05-22 05:09:02 +02:00 • committed by Erik Hofman
parent 0ece93074f
commit 68a53a7546
4 changed files with 20 additions and 50 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
src/AIModel/AIBallistic.cxx
View file

@ -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),

2
src/AIModel/AIBallistic.hxx
View file

@ -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

43
src/AIModel/submodel.cxx
View file

@ -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();
// 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);
// 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);
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<<endl;
// Rationalize the output
if (IC.azimuth < 0)
IC.azimuth += 360;
else if (IC.azimuth >= 360)
IC.azimuth -= 360;
// cout << "azimuth2 " << IC.azimuth<<endl;
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;
}
//cout << "IC.speed " << IC.speed / SG_KT_TO_FPS << endl;
cout << "az2: " << IC.azimuth << " el2: " << IC.elevation << endl;
}
void FGSubmodelMgr::updatelat(double lat)

23
src/AIModel/submodel.hxx
View file

@ -120,14 +120,6 @@ private:
submodel_vector_type subsubmodels;
submodel_vector_iterator submodel_iterator, subsubmodel_iterator;
//double Rx, Ry, Rz;
//double Sx, Sy, Sz;
//double Tx, Ty, Tz;
float cosRx, sinRx;
float cosRy, sinRy;
float cosRz, sinRz;
int index;
double ft_per_deg_longitude;
@ -143,14 +135,13 @@ private:
double _parent_pitch;
double _parent_roll;
double _parent_speed;
//double _parent_ID;
double _x_offset;
double _y_offset;
double _z_offset;
static const double lbs_to_slugs; //conversion factor
// Conversion factor
static const double lbs_to_slugs;
double contrail_altitude;
@ -184,22 +175,18 @@ private:
SGPropertyNode_ptr _selected_ac;
IC_struct IC;
/**
* Helper to retrieve the AI manager, if it currently exists
*/
// Helper to retrieve the AI manager, if it currently exists
FGAIManager* aiManager();
void loadAI();
void loadSubmodels();
void setData(int id, const std::string& path, bool serviceable, const std::string& property_path, submodel_vector_type& models);
void valueChanged (SGPropertyNode *);
void transform(submodel *);
void setParentNode(int parent_id);
bool release(submodel *, double dt);
int _count;
SGGeod userpos;