From 2845a757536e8332a193d5f74e929d7798b84f18 Mon Sep 17 00:00:00 2001 From: curt Date: Fri, 19 Mar 2004 03:23:28 +0000 Subject: [PATCH] Jim Wilson: Add FGPredictor class to xmlauto. Add support for horizontal navigation based on flight track as opposed to heading. Add crosstrack-error support to nav. Simplify error adjust calculation for horizontal nav (better interception). Fixed potential divide by zero that was producing nan issues in the xmlauto code. --- src/Autopilot/xmlauto.cxx | 92 +++++++++++++++++++++++++++++++++++++++ src/Autopilot/xmlauto.hxx | 29 ++++++++++++ src/Cockpit/navcom.cxx | 79 +++++++++++++++++++++++---------- src/Cockpit/navcom.hxx | 1 + 4 files changed, 177 insertions(+), 24 deletions(-) diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index 0f0996edf..191d745a3 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -491,6 +491,86 @@ void FGPISimpleController::update( double dt ) { } +FGPredictor::FGPredictor ( SGPropertyNode *node ): + debug( false ), + ivalue( 0.0 ), + last_value ( 999999999.9 ), + average ( 0.0 ), + seconds( 0.0 ), + filter_gain( 0.0 ) +{ + int i; + for ( i = 0; i < node->nChildren(); ++i ) { + SGPropertyNode *child = node->getChild(i); + string cname = child->getName(); + string cval = child->getStringValue(); + if ( cname == "name" ) { + name = cval; + } else if ( cname == "debug" ) { + debug = child->getBoolValue(); + } else if ( cname == "input" ) { + input_prop = fgGetNode( child->getStringValue(), true ); + } else if ( cname == "seconds" ) { + seconds = child->getDoubleValue(); + } else if ( cname == "filter-gain" ) { + filter_gain = child->getDoubleValue(); + } else if ( cname == "output" ) { + SGPropertyNode *tmp = fgGetNode( child->getStringValue(), true ); + output_list.push_back( tmp ); + } + } +} + +void FGPredictor::update( double dt ) { + /* + Simple moving average filter converts input value to predicted value "seconds". + + Smoothing as described by Curt Olson: + gain would be valid in the range of 0 - 1.0 + 1.0 would mean no filtering. + 0.0 would mean no input. + 0.5 would mean (1 part past value + 1 part current value) / 2 + 0.1 would mean (9 parts past value + 1 part current value) / 10 + 0.25 would mean (3 parts past value + 1 part current value) / 4 + + */ + + if ( input_prop != NULL ) { + ivalue = input_prop->getDoubleValue(); + // no sense if there isn't an input :-) + enabled = true; + } else { + enabled = false; + } + + if ( enabled ) { + + // first time initialize average + if (last_value >= 999999999.0) { + last_value = ivalue; + } + + if ( dt > 0.0 ) { + double current = (ivalue - last_value)/dt; // calculate current error change (per second) + if ( dt < 1.0 ) { + average = (1.0 - dt) * average + current * dt; + } else { + average = current; + } + + // calculate output with filter gain adjustment + double output = ivalue + (1.0 - filter_gain) * (average * seconds) + filter_gain * (current * seconds); + + unsigned int i; + for ( i = 0; i < output_list.size(); ++i ) { + output_list[i]->setDoubleValue( output ); + } + } + last_value = ivalue; + } +} + + FGXMLAutopilot::FGXMLAutopilot() { } @@ -562,6 +642,9 @@ bool FGXMLAutopilot::build() { } else if ( name == "pi-simple-controller" ) { FGXMLAutoComponent *c = new FGPISimpleController( node ); components.push_back( c ); + } else if ( name == "predict-simple" ) { + FGXMLAutoComponent *c = new FGPredictor( node ); + components.push_back( c ); } else { SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: " << name ); @@ -636,6 +719,8 @@ static void update_helper( double dt ) { = fgGetNode( "/autopilot/settings/true-heading-deg", true ); static SGPropertyNode *true_hdg = fgGetNode( "/orientation/heading-deg", true ); + static SGPropertyNode *true_track + = fgGetNode( "/instrumentation/gps/indicated-track-true-deg", true ); static SGPropertyNode *true_error = fgGetNode( "/autopilot/internal/true-heading-error-deg", true ); @@ -649,12 +734,19 @@ static void update_helper( double dt ) { = fgGetNode( "/radios/nav[0]/radials/target-auto-hdg-deg", true ); static SGPropertyNode *true_nav1 = fgGetNode( "/autopilot/internal/nav1-heading-error-deg", true ); + static SGPropertyNode *true_track_nav1 + = fgGetNode( "/autopilot/internal/nav1-track-error-deg", true ); diff = target_nav1->getDoubleValue() - true_hdg->getDoubleValue(); if ( diff < -180.0 ) { diff += 360.0; } if ( diff > 180.0 ) { diff -= 360.0; } true_nav1->setDoubleValue( diff ); + diff = target_nav1->getDoubleValue() - true_track->getDoubleValue(); + if ( diff < -180.0 ) { diff += 360.0; } + if ( diff > 180.0 ) { diff -= 360.0; } + true_track_nav1->setDoubleValue( diff ); + // Calculate vertical speed in fpm static SGPropertyNode *vs_fps = fgGetNode( "/velocities/vertical-speed-fps", true ); diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx index 237678eb4..03fd3f232 100644 --- a/src/Autopilot/xmlauto.hxx +++ b/src/Autopilot/xmlauto.hxx @@ -175,6 +175,35 @@ public: }; +/** + * Predictor - calculates value in x seconds future. + */ + +class FGPredictor : public FGXMLAutoComponent { + +private: + + // proportional component data + double last_value; + double average; + double seconds; + double filter_gain; + + // debug flag + bool debug; + + // Input values + double ivalue; // input value + +public: + + FGPredictor( SGPropertyNode *node ); + ~FGPredictor() {} + + void update( double dt ); +}; + + /** * Model an autopilot system. * diff --git a/src/Cockpit/navcom.cxx b/src/Cockpit/navcom.cxx index 35e86df9e..03fae8220 100644 --- a/src/Cockpit/navcom.cxx +++ b/src/Cockpit/navcom.cxx @@ -64,6 +64,7 @@ FGNavCom::FGNavCom() : nav_target_radial(0.0), nav_target_radial_true(0.0), nav_target_auto_hdg(0.0), + nav_gs_rate_of_climb(0.0), nav_vol_btn(0.0), nav_ident_btn(true), horiz_vel(0.0), @@ -211,6 +212,9 @@ FGNavCom::bind () snprintf(propname, 256, "/radios/nav[%d]/heading-needle-deflection", index); fgTie( propname, this, &FGNavCom::get_nav_cdi_deflection ); + snprintf(propname, 256, "/radios/nav[%d]/crosstrack-error-m", index); + fgTie( propname, this, &FGNavCom::get_nav_cdi_xtrack_error ); + snprintf(propname, 256, "/radios/nav[%d]/has-gs", index); fgTie( propname, this, &FGNavCom::get_nav_has_gs ); @@ -475,22 +479,19 @@ FGNavCom::update(double dt) } // determine the heading adjustment needed. - double adjustment = get_nav_cdi_deflection() - * (nav_loc_dist * SG_METER_TO_NM); - SG_CLAMP_RANGE( adjustment, -30.0, 30.0 ); - -#if 0 - // CLO - 01/24/2004 - This #ifdef'd out code makes no sense to - // me. Someone please justify it and explain why it should be - // here if they want this reenabled. - - // clamp closer when inside cone when beyond 5km... - if ( nav_loc_dist > 5000 ) { - double clamp_angle = fabs(get_nav_cdi_deflection()) * 3; - if (clamp_angle < 30) - SG_CLAMP_RANGE( adjustment, -clamp_angle, clamp_angle); + // over 8km scale by 3.0 + // (3 is chosen because max deflection is 10 + // and 30 is clamped angle to radial) + // under 8km scale by 10.0 + // because the overstated error helps drive it to the radial in a + // moderate cross wind. + double adjustment = 0.0; + if (nav_loc_dist > 8000) { + adjustment = get_nav_cdi_deflection() * 3.0; + } else { + adjustment = get_nav_cdi_deflection() * 10.0; } -#endif + SG_CLAMP_RANGE( adjustment, -30.0, 30.0 ); // determine the target heading to fly to intercept the // tgt_radial @@ -516,16 +517,19 @@ FGNavCom::update(double dt) // estimate horizontal speed towards ILS in meters per minute double dist = last_x - x; last_x = x; - double new_vel = ( dist / dt ); + if ( dt > 0.0 ) { + // avoid nan + double new_vel = ( dist / dt ); - horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel; - // double horiz_vel = cur_fdm_state->get_V_ground_speed() - // * SG_FEET_TO_METER * 60.0; - // double horiz_vel = airspeed_node->getFloatValue() - // * SG_FEET_TO_METER * 60.0; - - nav_gs_rate_of_climb = -sin( des_angle * SGD_DEGREES_TO_RADIANS ) - * horiz_vel * SG_METER_TO_FEET; + horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel; + // double horiz_vel = cur_fdm_state->get_V_ground_speed() + // * SG_FEET_TO_METER * 60.0; + // double horiz_vel = airspeed_node->getFloatValue() + // * SG_FEET_TO_METER * 60.0; + + nav_gs_rate_of_climb = -sin( des_angle * SGD_DEGREES_TO_RADIANS ) + * horiz_vel * SG_METER_TO_FEET; + } } else { nav_inrange = false; // cout << "not picking up vor. :-(" << endl; @@ -771,6 +775,33 @@ double FGNavCom::get_nav_cdi_deflection() const { return r; } +// return the amount of cross track distance error, returns a meters +double FGNavCom::get_nav_cdi_xtrack_error() const { + double r, m; + + if ( nav_inrange + && nav_serviceable->getBoolValue() && cdi_serviceable->getBoolValue() ) + { + r = nav_radial - nav_target_radial; + // cout << "Target radial = " << nav_target_radial + // << " Actual radial = " << nav_radial + // << " r = " << r << endl; + + while ( r > 180.0 ) { r -= 360.0;} + while ( r < -180.0 ) { r += 360.0;} + if ( fabs(r) > 90.0 ) + r = ( r<0.0 ? -r-180.0 : -r+180.0 ); + + r = -r; // reverse, since radial is outbound + + m = nav_loc_dist * sin(r * SGD_DEGREES_TO_RADIANS); + + } else { + m = 0.0; + } + + return m; +} // return the amount of glide slope needle deflection (.i.e. the // number of degrees we are off the glide slope * 5.0 diff --git a/src/Cockpit/navcom.hxx b/src/Cockpit/navcom.hxx index 8dfdb96e8..698a50391 100644 --- a/src/Cockpit/navcom.hxx +++ b/src/Cockpit/navcom.hxx @@ -237,6 +237,7 @@ public: inline double get_nav_target_gs() const { return nav_target_gs; } inline double get_nav_twist() const { return nav_twist; } double get_nav_cdi_deflection() const; + double get_nav_cdi_xtrack_error() const; double get_nav_gs_deflection() const; inline double get_nav_vol_btn() const { return nav_vol_btn; } inline bool get_nav_ident_btn() const { return nav_ident_btn; } -- 2.39.5