}
+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() {
}
} 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 );
= 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 );
= 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 );
};
+/**
+ * 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.
*
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),
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 );
}
// 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
// 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;
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
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; }
projects / flightgear.git / commitdiff