if( phase > SGLimitsd::min() ) {
while( value < min ) value += phase;
- while( value > max ) value -= phase;
+ while( value >= max ) value -= phase;
} else {
value = min; // phase is zero
}
filterType = gain;
} else if (val == "reciprocal") {
filterType = reciprocal;
+ } else if (val == "differential") {
+ filterType = differential;
+ // use a constant of two samples for current and previous input value
+ samplesInput.push_back( new FGXMLAutoInput(NULL, 2.0 ) );
}
} else if (aName == "filter-time" ) {
TfInput.push_back( new FGXMLAutoInput( aNode, 1.0 ) );
do_feedback();
}
- if ( enabled && dt > 0.0 ) {
- /*
- * Exponential filter
- *
- * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
- *
- */
- if( debug ) cout << "Updating " << get_name()
- << " dt " << dt << endl;
-
- if (filterType == exponential)
- {
- double alpha = 1 / ((TfInput.get_value()/dt) + 1);
- output.push_front(alpha * input[0] +
- (1 - alpha) * output[0]);
- }
- else if (filterType == doubleExponential)
+ if ( !enabled || dt < SGLimitsd::min() )
+ return;
+
+ /*
+ * Exponential filter
+ *
+ * Output[n] = alpha*Input[n] + (1-alpha)*Output[n-1]
+ *
+ */
+ if( debug ) cout << "Updating " << get_name()
+ << " dt " << dt << endl;
+
+ if (filterType == exponential)
+ {
+ double alpha = 1 / ((TfInput.get_value()/dt) + 1);
+ output.push_front(alpha * input[0] +
+ (1 - alpha) * output[0]);
+ }
+ else if (filterType == doubleExponential)
+ {
+ double alpha = 1 / ((TfInput.get_value()/dt) + 1);
+ output.push_front(alpha * alpha * input[0] +
+ 2 * (1 - alpha) * output[0] -
+ (1 - alpha) * (1 - alpha) * output[1]);
+ }
+ else if (filterType == movingAverage)
+ {
+ output.push_front(output[0] +
+ (input[0] - input.back()) / samplesInput.get_value());
+ }
+ else if (filterType == noiseSpike)
+ {
+ double maxChange = rateOfChangeInput.get_value() * dt;
+
+ if ((output[0] - input[0]) > maxChange)
{
- double alpha = 1 / ((TfInput.get_value()/dt) + 1);
- output.push_front(alpha * alpha * input[0] +
- 2 * (1 - alpha) * output[0] -
- (1 - alpha) * (1 - alpha) * output[1]);
+ output.push_front(output[0] - maxChange);
}
- else if (filterType == movingAverage)
+ else if ((output[0] - input[0]) < -maxChange)
{
- output.push_front(output[0] +
- (input[0] - input.back()) / samplesInput.get_value());
+ output.push_front(output[0] + maxChange);
}
- else if (filterType == noiseSpike)
+ else if (fabs(input[0] - output[0]) <= maxChange)
{
- double maxChange = rateOfChangeInput.get_value() * dt;
-
- if ((output[0] - input[0]) > maxChange)
- {
- output.push_front(output[0] - maxChange);
- }
- else if ((output[0] - input[0]) < -maxChange)
- {
- output.push_front(output[0] + maxChange);
- }
- else if (fabs(input[0] - output[0]) <= maxChange)
- {
- output.push_front(input[0]);
- }
+ output.push_front(input[0]);
}
- else if (filterType == gain)
- {
- output[0] = gainInput.get_value() * input[0];
+ }
+ else if (filterType == gain)
+ {
+ output[0] = gainInput.get_value() * input[0];
+ }
+ else if (filterType == reciprocal)
+ {
+ if (input[0] != 0.0) {
+ output[0] = gainInput.get_value() / input[0];
}
- else if (filterType == reciprocal)
- {
- if (input[0] != 0.0) {
- output[0] = gainInput.get_value() / input[0];
- }
+ }
+ else if (filterType == differential)
+ {
+ if( dt > SGLimitsd::min() ) {
+ output[0] = (input[0]-input[1]) * TfInput.get_value() / dt;
}
+ }
- output[0] = clamp(output[0]) ;
- set_output_value( output[0] );
+ output[0] = clamp(output[0]) ;
+ set_output_value( output[0] );
- output.resize(2);
+ output.resize(2);
- if (debug)
- {
- cout << "input:" << input[0]
- << "\toutput:" << output[0] << endl;
+ if (debug)
+ {
+ cout << "input:" << input[0]
+ << "\toutput:" << output[0] << endl;
+ }
+}
+
+FGXMLAutoLogic::FGXMLAutoLogic(SGPropertyNode * node ) :
+ FGXMLAutoComponent(),
+ inverted(false)
+{
+ parseNode(node);
+}
+
+bool FGXMLAutoLogic::parseNodeHook(const std::string& aName, SGPropertyNode* aNode)
+{
+ if (aName == "input") {
+ input = sgReadCondition( fgGetNode("/"), aNode );
+ } else if (aName == "inverted") {
+ inverted = aNode->getBoolValue();
+ } else {
+ return false;
+ }
+
+ return true;
+}
+
+void FGXMLAutoLogic::update(double dt)
+{
+ if ( isPropertyEnabled() ) {
+ if ( !enabled ) {
+ // we have just been enabled
}
+ enabled = true;
+ } else {
+ enabled = false;
+ do_feedback();
+ }
+
+ if ( !enabled || dt < SGLimitsd::min() )
+ return;
+
+ if( input == NULL ) {
+ if ( debug ) cout << "No input for " << get_name() << endl;
+ return;
}
+
+ bool i = input->test();
+
+ if ( debug ) cout << "Updating " << get_name() << ": " << (inverted ? !i : i) << endl;
+
+ set_output_value( i );
}
+
FGXMLAutopilotGroup::FGXMLAutopilotGroup() :
- SGSubsystemGroup(),
- average(0.0), // average/filtered prediction
+ SGSubsystemGroup()
+#ifdef XMLAUTO_USEHELPER
+ ,average(0.0), // average/filtered prediction
v_last(0.0), // last velocity
last_static_pressure(0.0),
vel(fgGetNode( "/velocities/airspeed-kt", true )),
vs_fpm(fgGetNode( "/autopilot/internal/vert-speed-fpm", true )),
static_pressure(fgGetNode( "/systems/static[0]/pressure-inhg", true )),
pressure_rate(fgGetNode( "/autopilot/internal/pressure-rate", true )),
- latNode(fgGetNode("/position/latitude-deg")),
- lonNode(fgGetNode("/position/longitude-deg"))
+ track(fgGetNode( "/orientation/track-deg", true ))
+#endif
{
}
{
// update all configured autopilots
SGSubsystemGroup::update( dt );
-
+#ifdef XMLAUTO_USEHELPER
// update helper values
double v = vel->getDoubleValue();
double a = 0.0;
true_nav1->setDoubleValue( diff );
// Calculate true groundtrack
- SGGeod currentPosition(SGGeod::fromDeg(
- lonNode->getDoubleValue(), latNode->getDoubleValue()));
- double true_track = SGGeodesy::courseDeg(lastPosition, currentPosition);
- lastPosition = currentPosition;
- diff = target_nav1->getDoubleValue() - true_track;
+ diff = target_nav1->getDoubleValue() - track->getDoubleValue();
SG_NORMALIZE_RANGE(diff, -180.0, 180.0);
true_track_nav1->setDoubleValue( diff );
pressure_rate->setDoubleValue(current_pressure_rate);
last_static_pressure = current_static_pressure;
}
+#endif
}
void FGXMLAutopilotGroup::reinit()
continue;
}
} catch (const sg_exception& e) {
- SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: "
+ SG_LOG( SG_AUTOPILOT, SG_ALERT, "Failed to load autopilot configuration: "
<< config.str() << ":" << e.getMessage() );
delete ap;
continue;
}
- SG_LOG( SG_ALL, SG_INFO, "adding autopilot subsystem " << apName );
+ SG_LOG( SG_AUTOPILOT, SG_INFO, "adding autopilot subsystem " << apName );
set_subsystem( apName, ap );
_autopilotNames.push_back( apName );
}
node = config_props->getChild(i);
string name = node->getName();
// cout << name << endl;
- SG_LOG( SG_ALL, SG_INFO, "adding autopilot component " << name );
+ SG_LOG( SG_AUTOPILOT, SG_BULK, "adding autopilot component " << name );
if ( name == "pid-controller" ) {
components.push_back( new FGPIDController( node ) );
} else if ( name == "pi-simple-controller" ) {
components.push_back( new FGPredictor( node ) );
} else if ( name == "filter" ) {
components.push_back( new FGDigitalFilter( node ) );
-// } else {
-// SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: "
-// << name );
+ } else if ( name == "logic" ) {
+ components.push_back( new FGXMLAutoLogic( node ) );
+ } else {
+ SG_LOG( SG_AUTOPILOT, SG_WARN, "Unknown top level autopilot section: " << name );
// return false;
}
}
projects / flightgear.git / blobdiff