fly/flightgear
1
0
Fork 0
Code Activity

Merge commit 'refs/merge-requests/13' of git://gitorious.org/fg/flightgear into next

Browse source
This commit is contained in:
Csaba Halasz 2010-11-30 00:26:22 +01:00
commit 674a295896
5 changed files with 227 additions and 201 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

14
src/AIModel/AIBallistic.cxx
View file

@ -152,11 +152,11 @@ bool FGAIBallistic::init(bool search_in_AI_path) {
props->setStringValue("contents/path", _contents_path.c_str()); props->setStringValue("contents/path", _contents_path.c_str());
} }
if(_parent != ""){ //if(_parent != ""){
setParentNode(); // setParentNode();
} //}
setParentNodes(_selected_ac); //setParentNodes(_selected_ac);
//props->setStringValue("vector/path", _vector_path.c_str()); //props->setStringValue("vector/path", _vector_path.c_str());
@ -168,7 +168,11 @@ bool FGAIBallistic::init(bool search_in_AI_path) {
Transform(); Transform();
//cout << _name << " speed init: " << speed << endl; if(_parent != ""){
setParentNode();
}
setParentNodes(_selected_ac);
return true; return true;
} }

13
src/AIModel/AIWingman.cxx
View file

@ -215,13 +215,14 @@ bool FGAIWingman::init(bool search_in_AI_path) {
roll = _rotation; roll = _rotation;
_ht_agl_ft = 1e10; _ht_agl_ft = 1e10;
props->setStringValue("submodels/path", _path.c_str());
if(_parent != ""){ if(_parent != ""){
setParentNode(); setParentNode();
} }
setParentNodes(_selected_ac); setParentNodes(_selected_ac);
props->setStringValue("submodels/path", _path.c_str());
user_WoW_node = fgGetNode("gear/gear[1]/wow", true);
return true; return true;
} }
@ -277,7 +278,7 @@ double FGAIWingman::calcAngle(double range, SGGeod pos1, SGGeod pos2){
void FGAIWingman::formateToAC(double dt){ void FGAIWingman::formateToAC(double dt){
double p_hdg, p_pch, p_rll, p_agl, p_ht = 0; double p_hdg, p_pch, p_rll, p_agl, p_ht, p_wow = 0;
setTgtOffsets(dt, 25); setTgtOffsets(dt, 25);
@ -312,8 +313,9 @@ void FGAIWingman::formateToAC(double dt){
double h_feet = 3 * factor; double h_feet = 3 * factor;
p_agl = manager->get_user_agl(); p_agl = manager->get_user_agl();
p_wow = user_WoW_node->getDoubleValue();
if(p_agl <= 10) { if(p_agl <= 10 || p_wow == 1) {
_height = p_ht; _height = p_ht;
//cout << "ht case1 " ; //cout << "ht case1 " ;
} else if (p_agl > 10 && p_agl <= 150 ) { } else if (p_agl > 10 && p_agl <= 150 ) {
@ -332,7 +334,8 @@ void FGAIWingman::formateToAC(double dt){
pos.setLongitudeDeg(_offsetpos.getLongitudeDeg()); pos.setLongitudeDeg(_offsetpos.getLongitudeDeg());
// these calculations are unreliable at slow speeds // these calculations are unreliable at slow speeds
if(speed >= 10) { // and we don't want random movement on the ground
if(speed >= 10 && p_wow != 1) {
setHdg(p_hdg + h_angle, dt, 0.9); setHdg(p_hdg + h_angle, dt, 0.9);
setPch(p_pch + p_angle + _pitch_offset, dt, 0.9); setPch(p_pch + p_angle + _pitch_offset, dt, 0.9);

2
src/AIModel/AIWingman.hxx
View file

@ -67,7 +67,7 @@ private:
double _coeff_bnk; //dimensionless coefficient double _coeff_bnk; //dimensionless coefficient
double _coeff_spd; //dimensionless coefficient double _coeff_spd; //dimensionless coefficient
SGPropertyNode_ptr user_WoW_node;
inline void setFormate(bool f); inline void setFormate(bool f);
inline void setTgtHdg(double hdg); inline void setTgtHdg(double hdg);

396
src/Instrumentation/mrg.cxx
View file

@ -20,10 +20,11 @@
#include "mrg.hxx" #include "mrg.hxx"
const double MasterReferenceGyro::gravity = -32.1740485564;
MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) : MasterReferenceGyro::MasterReferenceGyro ( SGPropertyNode *node ) :
_name(node->getStringValue("name", "master-reference-gyro")), _name(node->getStringValue("name", "master-reference-gyro")),
_num(node->getIntValue("number", 0)) _num(node->getIntValue("number", 0))
{ {
} }
@ -34,237 +35,252 @@ MasterReferenceGyro::~MasterReferenceGyro ()
void void
MasterReferenceGyro::init () MasterReferenceGyro::init ()
{ {
_last_hdg = 0; _last_hdg = 0;
_last_roll = 0; _last_roll = 0;
_last_pitch = 0; _last_pitch = 0;
_indicated_hdg = 0; _indicated_hdg = 0;
_indicated_roll = 0; _indicated_roll = 0;
_indicated_pitch = 0; _indicated_pitch = 0;
_indicated_hdg_rate = 0; _indicated_hdg_rate = 0;
_indicated_roll_rate = 0; _indicated_roll_rate = 0;
_indicated_pitch_rate = 0; _indicated_pitch_rate = 0;
_erect_time = 0; _erect_time = 180;
_last_g = 1;
string branch; _g_error = 10;
branch = "/instrumentation/" + _name;
_pitch_in_node = fgGetNode("/orientation/pitch-deg", true); string branch;
_roll_in_node = fgGetNode("/orientation/roll-deg", true); branch = "/instrumentation/" + _name;
_hdg_in_node = fgGetNode("/orientation/heading-deg", true);
_hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
_pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true);
_roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
_yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
_g_in_node = fgGetNode("/accelerations/pilot-g", true);
_electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
_hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
_off_node = node->getChild("off-flag", 0, true);
_pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
_roll_out_node = node->getChild("indicated-roll-deg", 0, true);
_hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
_hdg_mag_out_node = node->getChild("indicated-mag-hdg-deg", 0, true);
_pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
_roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
_hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
_responsiveness_node = node->getChild("responsiveness", 0, true);
_error_out_node = node->getChild("heading-bug-error-deg", 0, true);
_hdg_input_source_node = node->getChild("heading-source", 0, true);
_fast_erect_node = node->getChild("fast-erect", 0, true);
_electrical_node->setDoubleValue(0); _pitch_in_node = fgGetNode("/orientation/pitch-deg", true);
_responsiveness_node->setDoubleValue(0.75); _roll_in_node = fgGetNode("/orientation/roll-deg", true);
_off_node->setBoolValue(false); _hdg_in_node = fgGetNode("/orientation/heading-deg", true);
_hdg_input_source_node->setBoolValue(false); _hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
_fast_erect_node->setBoolValue(false); _pitch_rate_node = fgGetNode("/orientation/pitch-rate-degps", true);
_roll_rate_node = fgGetNode("/orientation/roll-rate-degps", true);
_yaw_rate_node = fgGetNode("/orientation/yaw-rate-degps", true);
//_g_in_node = fgGetNode("/accelerations/pilot/z-accel-fps_sec", true);
_g_in_node = fgGetNode("/accelerations/pilot-g", true);
_electrical_node = fgGetNode("/systems/electrical/outputs/MRG", true);
_hdg_mag_in_node = fgGetNode("/orientation/heading-magnetic-deg", true);
SGPropertyNode *node = fgGetNode(branch.c_str(), _num, true );
_off_node = node->getChild("off-flag", 0, true);
_pitch_out_node = node->getChild("indicated-pitch-deg", 0, true);
_roll_out_node = node->getChild("indicated-roll-deg", 0, true);
_hdg_out_node = node->getChild("indicated-hdg-deg", 0, true);
_hdg_mag_out_node = node->getChild("indicated-mag-hdg-deg", 0, true);
_pitch_rate_out_node = node->getChild("indicated-pitch-rate-degps", 0, true);
_roll_rate_out_node = node->getChild("indicated-roll-rate-degps", 0, true);
_hdg_rate_out_node = node->getChild("indicated-hdg-rate-degps", 0, true);
_responsiveness_node = node->getChild("responsiveness", 0, true);
_error_out_node = node->getChild("heading-bug-error-deg", 0, true);
_hdg_input_source_node = node->getChild("heading-source", 0, true);
_fast_erect_node = node->getChild("fast-erect", 0, true);
_electrical_node->setDoubleValue(0);
_responsiveness_node->setDoubleValue(0.75);
_off_node->setBoolValue(false);
_hdg_input_source_node->setBoolValue(false);
_fast_erect_node->setBoolValue(false);
_g_in_node->setDoubleValue(1);
} }
void void
MasterReferenceGyro::bind () MasterReferenceGyro::bind ()
{ {
std::ostringstream temp; std::ostringstream temp;
string branch; string branch;
temp << _num; temp << _num;
branch = "/instrumentation/" + _name + "[" + temp.str() + "]"; branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
fgTie((branch + "/serviceable").c_str(), fgTie((branch + "/serviceable").c_str(),
&_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable); &_gyro, &Gyro::is_serviceable, &Gyro::set_serviceable);
fgTie((branch + "/spin").c_str(), fgTie((branch + "/spin").c_str(),
&_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm); &_gyro, &Gyro::get_spin_norm, &Gyro::set_spin_norm);
} }
void void
MasterReferenceGyro::unbind () MasterReferenceGyro::unbind ()
{ {
std::ostringstream temp; std::ostringstream temp;
string branch; string branch;
temp << _num; temp << _num;
branch = "/instrumentation/" + _name + "[" + temp.str() + "]"; branch = "/instrumentation/" + _name + "[" + temp.str() + "]";
fgUntie((branch + "/serviceable").c_str()); fgUntie((branch + "/serviceable").c_str());
fgUntie((branch + "/spin").c_str()); fgUntie((branch + "/spin").c_str());
} }
void void
MasterReferenceGyro::update (double dt) MasterReferenceGyro::update (double dt)
{ {
double indicated_roll = 0; //sanity check
double indicated_pitch = 0; if (!fgGetBool("/sim/fdm-initialized", false)) {
double indicated_hdg = 0;
double indicated_roll_rate = 0;
double indicated_pitch_rate = 0;
double indicated_hdg_rate = 0;
// Get the spin from the gyro
_gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
_gyro.update(dt);
double spin = _gyro.get_spin_norm();
// set the "off-flag"
if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) {
_off_node->setBoolValue(false);
} else {
_off_node->setBoolValue(true);
return; return;
} }
// Get the input values double indicated_roll = 0;
double hdg = _hdg_mag_in_node->getDoubleValue(); double indicated_pitch = 0;
double indicated_hdg = 0;
double indicated_roll_rate = 0;
double indicated_pitch_rate = 0;
double indicated_hdg_rate = 0;
double hdg = 0;
double erect_time_factor = 1;
if(_hdg_input_source_node->getBoolValue()) const double erect_time = 180;
hdg = _hdg_in_node->getDoubleValue(); const double max_g_error = 10.0;
double roll = _roll_in_node->getDoubleValue(); //Get the spin from the gyro
double pitch = _pitch_in_node->getDoubleValue(); _gyro.set_power_norm( _electrical_node->getDoubleValue()/24 );
_gyro.update(dt);
double roll_rate = _yaw_rate_node->getDoubleValue(); double spin = _gyro.get_spin_norm();
double pitch_rate = _yaw_rate_node->getDoubleValue();
double yaw_rate = _yaw_rate_node->getDoubleValue();
double g = _g_in_node->getDoubleValue();
//modulate the input by the spin rate // set the "off-flag"
double responsiveness = spin * spin * spin * spin * spin * spin; if ( _electrical_node->getDoubleValue() > 0 && spin >= 0.25) {
roll = fgGetLowPass( _last_roll, roll, responsiveness ); _off_node->setBoolValue(false);
pitch = fgGetLowPass( _last_pitch , pitch, responsiveness ); } else {
_off_node->setBoolValue(true);
return;
}
if ((hdg - _last_hdg) > 180) // Get the input values
_last_hdg += 360; if(_hdg_input_source_node->getBoolValue()){
if ((hdg - _last_hdg) < -180) hdg = _hdg_in_node->getDoubleValue();
_last_hdg -= 360; } else {
hdg = _hdg_mag_in_node->getDoubleValue();
}
hdg = fgGetLowPass( _last_hdg , hdg, responsiveness ); double roll = _roll_in_node->getDoubleValue();
double pitch = _pitch_in_node->getDoubleValue();
double g = _g_in_node->getDoubleValue()/* / gravity*/;
//but we need to filter the hdg and yaw_rate as well - yuk! double roll_rate = _yaw_rate_node->getDoubleValue();
responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin); double pitch_rate = _pitch_rate_node->getDoubleValue();
yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness ); double yaw_rate = _yaw_rate_node->getDoubleValue();
g = fgGetLowPass( _last_g , g, 0.015 );
//modulate the input by the spin rate
double responsiveness = spin * spin * spin * spin * spin * spin;
roll = fgGetLowPass( _last_roll, roll, responsiveness );
pitch = fgGetLowPass( _last_pitch , pitch, responsiveness );
if ((hdg - _last_hdg) > 180)
_last_hdg += 360;
if ((hdg - _last_hdg) < -180)
_last_hdg -= 360;
hdg = fgGetLowPass( _last_hdg , hdg, responsiveness );
//but we need to filter the hdg and yaw_rate as well - yuk!
responsiveness = 0.1 / (spin * spin * spin * spin * spin * spin);
yaw_rate = fgGetLowPass( _last_yaw_rate , yaw_rate, responsiveness );
g = fgGetLowPass( _last_g , g, 1.5);
// store the new values // store the new values
_last_roll = roll; _last_roll = roll;
_last_pitch = pitch; _last_pitch = pitch;
_last_hdg = hdg; _last_hdg = hdg;
_last_roll_rate = roll_rate; _last_roll_rate = roll_rate;
_last_pitch_rate = pitch_rate; _last_pitch_rate = pitch_rate;
_last_yaw_rate = yaw_rate; _last_yaw_rate = yaw_rate;
_last_g = g; _last_g = g;
if (_erect_time > 0){ //the gyro only erects inside limits
if ( fabs ( yaw_rate ) <= 5
&& g <= 1.5 && g >= -0.5){
if ( !_fast_erect_node->getBoolValue() ) if ( !_fast_erect_node->getBoolValue() ){
_erect_time -= dt; erect_time_factor = 1;
else } else {
_erect_time -= 2 * dt; erect_time_factor = 2;
}
_g_error -= (max_g_error/(erect_time * 0.33)) * dt * erect_time_factor;
} else {
_g_error += (max_g_error /(erect_time * 0.33)) * dt * 2;
//SG_LOG(SG_GENERAL, SG_ALERT,_num <<
// " g input " << _g_in_node->getDoubleValue() * gravity
// <<" _erect_time " << _erect_time
// << " yaw " << yaw_rate
// << " pitch " << _pitch_rate_node->getDoubleValue()
// << " roll " << _roll_rate_node->getDoubleValue());
} }
//SG_LOG(SG_GENERAL, SG_ALERT, //cout << "_g_error "<< _g_error << endl;
// "g input " << g <<" _erect_time " << _erect_time << " spin " << spin); _g_error = SGMiscd::clip(_g_error, 0, 10);
// cout << "_g_error clip "<< _g_error << endl;
indicated_roll = _last_roll + _g_error;
indicated_pitch = _last_pitch + _g_error;
indicated_hdg = _last_hdg + _g_error;
indicated_roll_rate = _last_roll_rate;
indicated_pitch_rate = _last_pitch_rate;
indicated_hdg_rate = _last_yaw_rate;
// calculate the difference between the indicated heading
// and the selected heading for use with an autopilot
static SGPropertyNode *bnode
= fgGetNode( "/autopilot/settings/heading-bug-deg", false );
//the gyro only erects inside limits if ( bnode ) {
if ( fabs ( yaw_rate ) <= 5 double diff = bnode->getDoubleValue() - indicated_hdg;
&& ( g <= 1.5 || g >= -0.5) if ( diff < -180.0 ) { diff += 360.0; }
&& _erect_time <=0 ) { if ( diff > 180.0 ) { diff -= 360.0; }
indicated_roll = _last_roll; _error_out_node->setDoubleValue( diff );
indicated_pitch = _last_pitch; //SG_LOG(SG_GENERAL, SG_ALERT,
indicated_hdg = _last_hdg; //"autopilot input " << bnode->getDoubleValue()
indicated_roll_rate = _last_roll_rate; //<< " output " << _error_out_node->getDoubleValue()<<);
indicated_pitch_rate = _last_pitch_rate; }
indicated_hdg_rate = _last_yaw_rate;
} else {
indicated_roll_rate = 0;
indicated_pitch_rate = 0;
indicated_hdg_rate = 0;
if (_erect_time <= 0 )
_erect_time = 34;
} //smooth the output
double factor = _responsiveness_node->getDoubleValue() * dt;
// calculate the difference between the indicated heading indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor );
// and the selected heading for use with an autopilot indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor );
static SGPropertyNode *bnode //indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor );
= fgGetNode( "/autopilot/settings/heading-bug-deg", false );
if ( bnode ) { indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor );
double diff = bnode->getDoubleValue() - indicated_hdg; indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor );
if ( diff < -180.0 ) { diff += 360.0; } indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor );
if ( diff > 180.0 ) { diff -= 360.0; }
_error_out_node->setDoubleValue( diff );
//SG_LOG(SG_GENERAL, SG_ALERT,
//"autopilot input " << bnode->getDoubleValue()
//<< " output " << _error_out_node->getDoubleValue()<<);
}
//smooth the output
double factor = _responsiveness_node->getDoubleValue() * dt;
indicated_roll = fgGetLowPass( _indicated_roll, indicated_roll, factor ); // store the new values
indicated_pitch = fgGetLowPass( _indicated_pitch , indicated_pitch, factor ); _indicated_roll = indicated_roll;
//indicated_hdg = fgGetLowPass( _indicated_hdg , indicated_hdg, factor ); _indicated_pitch = indicated_pitch;
_indicated_hdg = indicated_hdg;
indicated_roll_rate = fgGetLowPass( _indicated_roll_rate, indicated_roll_rate, factor ); _indicated_roll_rate = indicated_roll_rate;
indicated_pitch_rate = fgGetLowPass( _indicated_pitch_rate , indicated_pitch_rate, factor ); _indicated_pitch_rate = indicated_pitch_rate;
indicated_hdg_rate = fgGetLowPass( _indicated_hdg_rate , indicated_hdg_rate, factor ); _indicated_hdg_rate = indicated_hdg_rate;
// store the new values // add in a gyro underspin "error" if gyro is spinning too slowly
_indicated_roll = indicated_roll; const double spin_thresh = 0.8;
_indicated_pitch = indicated_pitch; const double max_roll_error = 40.0;
_indicated_hdg = indicated_hdg; const double max_pitch_error = 12.0;
const double max_hdg_error = 140.0;
double roll_error;
double pitch_error;
double hdg_error;
_indicated_roll_rate = indicated_roll_rate; if ( spin <= spin_thresh ) {
_indicated_pitch_rate = indicated_pitch_rate; double roll_error_factor = ( spin_thresh - spin ) / spin_thresh;
_indicated_hdg_rate = indicated_hdg_rate; double pitch_error_factor = ( spin_thresh - spin ) / spin_thresh;
double hdg_error_factor = ( spin_thresh - spin ) / spin_thresh;
roll_error = roll_error_factor * roll_error_factor * max_roll_error;
pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
hdg_error = hdg_error_factor * hdg_error_factor * max_hdg_error;
} else {
roll_error = 0.0;
pitch_error = 0.0;
hdg_error = 0.0;
}
// add in a gyro underspin "error" if gyro is spinning too slowly _roll_out_node->setDoubleValue( _indicated_roll + roll_error );
const double spin_thresh = 0.8; _pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error );
const double max_roll_error = 40.0; _hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error );
const double max_pitch_error = 12.0; _pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate );
const double max_hdg_error = 140.0; _roll_rate_out_node ->setDoubleValue( _indicated_roll_rate );
double roll_error; _hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate );
double pitch_error;
double hdg_error;
if ( spin <= spin_thresh ) {
double roll_error_factor = ( spin_thresh - spin ) / spin_thresh;
double pitch_error_factor = ( spin_thresh - spin ) / spin_thresh;
double hdg_error_factor = ( spin_thresh - spin ) / spin_thresh;
roll_error = roll_error_factor * roll_error_factor * max_roll_error;
pitch_error = pitch_error_factor * pitch_error_factor * max_pitch_error;
hdg_error = hdg_error_factor * hdg_error_factor * max_hdg_error;
} else {
roll_error = 0.0;
pitch_error = 0.0;
hdg_error = 0.0;
}
_roll_out_node->setDoubleValue( _indicated_roll + roll_error );
_pitch_out_node->setDoubleValue( _indicated_pitch + pitch_error );
_hdg_out_node->setDoubleValue( _indicated_hdg + hdg_error );
_pitch_rate_out_node ->setDoubleValue( _indicated_pitch_rate );
_roll_rate_out_node ->setDoubleValue( _indicated_roll_rate );
_hdg_rate_out_node ->setDoubleValue( _indicated_hdg_rate );
} }
// end of mrg.cxx // end of mrg.cxx

3
src/Instrumentation/mrg.hxx
View file

@ -49,6 +49,8 @@ public:
private: private:
static const double gravity; //conversion factor
string _name; string _name;
int _num; int _num;
@ -66,6 +68,7 @@ private:
double _last_yaw_rate; double _last_yaw_rate;
double _last_g; double _last_g;
double _erect_time; double _erect_time;
double _g_error;
Gyro _gyro; Gyro _gyro;