X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fxmlauto.cxx;h=5d5556e7b3f976d6c63bc431e4e5372ce1c91013;hb=73e70fdb29c4c512779b6759d1e6aa487818e841;hp=88c0f99849d6bdf98dc1d500c71723c8970c9341;hpb=bffca78da400c9f78c9798d63861be8050c81aa5;p=flightgear.git diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx index 88c0f9984..5d5556e7b 100644 --- a/src/Autopilot/xmlauto.cxx +++ b/src/Autopilot/xmlauto.cxx @@ -26,6 +26,7 @@ #include
#include
+#include
#include "xmlauto.hxx" @@ -34,6 +35,8 @@ FGPIDController::FGPIDController( SGPropertyNode *node ): debug( false ), y_n( 0.0 ), r_n( 0.0 ), + y_scale( 1.0 ), + r_scale( 1.0 ), Kp( 0.0 ), alpha( 0.1 ), beta( 1.0 ), @@ -74,6 +77,10 @@ FGPIDController::FGPIDController( SGPropertyNode *node ): if ( prop != NULL ) { input_prop = fgGetNode( prop->getStringValue(), true ); } + prop = child->getChild( "scale" ); + if ( prop != NULL ) { + y_scale = prop->getDoubleValue(); + } } else if ( cname == "reference" ) { SGPropertyNode *prop = child->getChild( "prop" ); if ( prop != NULL ) { @@ -84,6 +91,10 @@ FGPIDController::FGPIDController( SGPropertyNode *node ): r_n = prop->getDoubleValue(); } } + prop = child->getChild( "scale" ); + if ( prop != NULL ) { + r_scale = prop->getDoubleValue(); + } } else if ( cname == "output" ) { int i = 0; SGPropertyNode *prop; @@ -196,14 +207,14 @@ FGPIDController::FGPIDController( SGPropertyNode *node ): */ void FGPIDController::update( double dt ) { - double ep_n; // proportional error with reference weighing - double e_n; // error - double ed_n; // derivative error - double edf_n; // derivative error filter - double Tf; // filter time - double delta_u_n; // incremental output - double u_n; // absolute output - double Ts = dt; // Sampling interval (sec) + double ep_n; // proportional error with reference weighing + double e_n; // error + double ed_n; // derivative error + double edf_n; // derivative error filter + double Tf; // filter time + double delta_u_n = 0.0; // incremental output + double u_n = 0.0; // absolute output + double Ts = dt; // Sampling interval (sec) if ( Ts <= 0.0 ) { // do nothing if time step is not positive (i.e. no time has @@ -212,22 +223,31 @@ void FGPIDController::update( double dt ) { } if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) { + if ( !enabled ) { + // first time being enabled, seed u_n with current + // property tree value + u_n = output_list[0]->getDoubleValue(); + // and clip + if ( u_n < u_min ) { u_n = u_min; } + if ( u_n > u_max ) { u_n = u_max; } + u_n_1 = u_n; + } enabled = true; } else { enabled = false; } - if ( enabled ) { + if ( enabled && Ts > 0.0) { if ( debug ) cout << "Updating " << name << endl; double y_n = 0.0; if ( input_prop != NULL ) { - y_n = input_prop->getDoubleValue(); + y_n = input_prop->getDoubleValue() * y_scale; } double r_n = 0.0; if ( r_n_prop != NULL ) { - r_n = r_n_prop->getDoubleValue(); + r_n = r_n_prop->getDoubleValue() * r_scale; } else { r_n = r_n_value; } @@ -247,26 +267,44 @@ void FGPIDController::update( double dt ) { ed_n = gamma * r_n - y_n; if ( debug ) cout << " ed_n = " << ed_n; - // Calculates filter time: - Tf = alpha * Td; - if ( debug ) cout << " Tf = " << Tf; + if ( Td > 0.0 ) { + // Calculates filter time: + Tf = alpha * Td; + if ( debug ) cout << " Tf = " << Tf; - // Filters the derivate error: - edf_n = edf_n_1 / (Ts/Tf + 1) - + ed_n * (Ts/Tf) / (Ts/Tf + 1); - if ( debug ) cout << " edf_n = " << edf_n; + // Filters the derivate error: + edf_n = edf_n_1 / (Ts/Tf + 1) + + ed_n * (Ts/Tf) / (Ts/Tf + 1); + if ( debug ) cout << " edf_n = " << edf_n; + } else { + edf_n = ed_n; + } // Calculates the incremental output: - delta_u_n = Kp * ( (ep_n - ep_n_1) - + ((Ts/Ti) * e_n) - + ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) ); - if ( debug ) cout << " delta_u_n = " << delta_u_n << endl; + if ( Ti > 0.0 ) { + delta_u_n = Kp * ( (ep_n - ep_n_1) + + ((Ts/Ti) * e_n) + + ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) ); + } else if ( Ti <= 0.0 ) { + delta_u_n = Kp * ( (ep_n - ep_n_1) + + ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) ); + } + + if ( debug ) { + cout << " delta_u_n = " << delta_u_n << endl; + cout << "P:" << Kp * (ep_n - ep_n_1) + << " I:" << Kp * ((Ts/Ti) * e_n) + << " D:" << Kp * ((Td/Ts) * (edf_n - 2*edf_n_1 + edf_n_2)) + << endl; + } // Integrator anti-windup logic: if ( delta_u_n > (u_max - u_n_1) ) { delta_u_n = 0; + if ( debug ) cout << " max saturation " << endl; } else if ( delta_u_n < (u_min - u_n_1) ) { delta_u_n = 0; + if ( debug ) cout << " min saturation " << endl; } // Calculates absolute output: @@ -284,7 +322,6 @@ void FGPIDController::update( double dt ) { output_list[i]->setDoubleValue( u_n ); } } else if ( !enabled ) { - u_n = 0.0; ep_n = 0.0; edf_n = 0.0; // Updates indexed values; @@ -308,6 +345,8 @@ FGPISimpleController::FGPISimpleController( SGPropertyNode *node ): debug( false ), y_n( 0.0 ), r_n( 0.0 ), + y_scale( 1.0 ), + r_scale ( 1.0 ), u_min( 0.0 ), u_max( 0.0 ) { @@ -338,6 +377,10 @@ FGPISimpleController::FGPISimpleController( SGPropertyNode *node ): if ( prop != NULL ) { input_prop = fgGetNode( prop->getStringValue(), true ); } + prop = child->getChild( "scale" ); + if ( prop != NULL ) { + y_scale = prop->getDoubleValue(); + } } else if ( cname == "reference" ) { SGPropertyNode *prop = child->getChild( "prop" ); if ( prop != NULL ) { @@ -348,6 +391,10 @@ FGPISimpleController::FGPISimpleController( SGPropertyNode *node ): r_n = prop->getDoubleValue(); } } + prop = child->getChild( "scale" ); + if ( prop != NULL ) { + r_scale = prop->getDoubleValue(); + } } else if ( cname == "output" ) { int i = 0; SGPropertyNode *prop; @@ -407,12 +454,12 @@ void FGPISimpleController::update( double dt ) { if ( debug ) cout << "Updating " << name << endl; double input = 0.0; if ( input_prop != NULL ) { - input = input_prop->getDoubleValue(); + input = input_prop->getDoubleValue() * y_scale; } double r_n = 0.0; if ( r_n_prop != NULL ) { - r_n = r_n_prop->getDoubleValue(); + r_n = r_n_prop->getDoubleValue() * r_scale; } else { r_n = r_n_value; } @@ -465,6 +512,86 @@ void FGPISimpleController::update( double dt ) { } +FGPredictor::FGPredictor ( SGPropertyNode *node ): + last_value ( 999999999.9 ), + average ( 0.0 ), + seconds( 0.0 ), + filter_gain( 0.0 ), + debug( false ), + ivalue( 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() { } @@ -536,6 +663,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 ); @@ -610,6 +740,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 ); @@ -623,19 +755,61 @@ 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 ); static SGPropertyNode *vs_fpm = fgGetNode( "/autopilot/internal/vert-speed-fpm", true ); - vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 ); + vs_fpm->setDoubleValue( vs_fps->getDoubleValue() * 60.0 ); + + + // Calculate static port pressure rate in [inhg/s]. + // Used to determine vertical speed. + static SGPropertyNode *static_pressure + = fgGetNode( "/systems/static[0]/pressure-inhg", true ); + static SGPropertyNode *pressure_rate + = fgGetNode( "/autopilot/internal/pressure-rate", true ); + static SGPropertyNode *filter_time + = fgGetNode( "/autopilot/internal/ft", true ); + + static double last_static_pressure = 0.0; + static double last_pressure_rate = 0.0; + + double Tf = filter_time->getDoubleValue(); + + if ( dt > 0.0 && Tf > 0.0) { + double current_static_pressure = static_pressure->getDoubleValue(); + + double current_pressure_rate = + ( current_static_pressure - last_static_pressure ) / dt; + + double W = dt/Tf; + + // Low Pass Filter + current_pressure_rate = + (1.0/(W + 1.0))*last_pressure_rate + + ((W)/(W + 1.0))*current_pressure_rate; + + pressure_rate->setDoubleValue(current_pressure_rate); + + last_static_pressure = current_static_pressure; + last_pressure_rate = current_pressure_rate; + } + }