diff --git a/src/Autopilot/Makefile.am b/src/Autopilot/Makefile.am index cda785cde..7ba3b5bb6 100644 --- a/src/Autopilot/Makefile.am +++ b/src/Autopilot/Makefile.am @@ -2,6 +2,7 @@ noinst_LIBRARIES = libAutopilot.a libAutopilot_a_SOURCES = \ route_mgr.cxx route_mgr.hxx \ - xmlauto.cxx xmlauto.hxx + xmlauto.cxx xmlauto.hxx \ + autobrake.cxx autobrake.hxx INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src diff --git a/src/Autopilot/autobrake.cxx b/src/Autopilot/autobrake.cxx new file mode 100644 index 000000000..a43841ee7 --- /dev/null +++ b/src/Autopilot/autobrake.cxx @@ -0,0 +1,295 @@ +// autobrake.cxx - generic, configurable autobrake system +// +// Written by James Turner, started September 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// $Id$ + +#include "autobrake.hxx" + +#include
+ +FGAutoBrake::FGAutoBrake() : + _lastGroundspeedKts(0.0), + _step(0), + _rto(false), + _armed(false), + _rtoArmed(false), + _engaged(false), + _targetDecel(0.0), + _fcsBrakeControl(0.0), + _leftBrakeOutput(0.0), + _rightBrakeOutput(0.0) +{ + // default config, close to Boeing 747-400 / 777 values + _configNumSteps = 4; + _configRTOStep = -1; + _configMaxDecel = 11; // ft-sec-sec + _configRTODecel = 11; // ft-sec-sec + _configRTOSpeed = 85; // kts +} + +FGAutoBrake::~FGAutoBrake() +{ +} + +void FGAutoBrake::init() +{ + _root = fgGetNode("/autopilot/autobrake", true); + + _root->tie("armed", SGRawValueMethods + (*this, &FGAutoBrake::getArmed, &FGAutoBrake::setArmed)); + + _root->tie("step", SGRawValueMethods + (*this, &FGAutoBrake::getStep, &FGAutoBrake::setStep)); + + _root->tie("rto", SGRawValueMethods + (*this, &FGAutoBrake::getRTO, &FGAutoBrake::setRTO)); + + _engineControlsNode = fgGetNode("/controls/engines"); + _groundspeedNode = fgGetNode("/velocities/groundspeed-kt"); + + // brake position nodes + _brakeInputs[0] = fgGetNode("/controls/gear/brake-left"); + _brakeInputs[1] = fgGetNode("/controls/gear/brake-right"); + _brakeInputs[2] = fgGetNode("/controls/gear/copilot-brake-left"); + _brakeInputs[3] = fgGetNode("/controls/gear/copilot-brake-right"); + + // config + SGPropertyNode_ptr config = _root->getChild("config", 0, true); + config->tie("num-steps", SGRawValuePointer(&_configNumSteps)); + config->tie("rto-step", SGRawValuePointer(&_configRTOStep)); + config->tie("rto-decel-fts-sec", SGRawValuePointer(&_configRTODecel)); + config->tie("max-decel-fts-sec", SGRawValuePointer(&_configMaxDecel)); + config->tie("rto-engage-kts", SGRawValuePointer(&_configRTOSpeed)); + + // outputs + _root->tie("target-decel-fts-sec", + SGRawValueMethods(*this, &FGAutoBrake::getTargetDecel)); + _root->tie("actual-decel-fts-sec", + SGRawValueMethods(*this, &FGAutoBrake::getActualDecel)); + _root->tie("fcs-brake", SGRawValuePointer(&_fcsBrakeControl)); + _root->tie("brake-left-output", SGRawValuePointer(&_leftBrakeOutput)); + _root->tie("brake-right-output", SGRawValuePointer(&_rightBrakeOutput)); + + _root->tie("engaged", SGRawValueMethods(*this, &FGAutoBrake::getEngaged)); +} + +void FGAutoBrake::postinit() +{ + _weightOnWheelsNode = fgGetNode("/gear/gear/wow"); + _lastWoW = _weightOnWheelsNode->getBoolValue(); +} + + +void FGAutoBrake::bind() +{ +} + +void FGAutoBrake::unbind() +{ +} + +void FGAutoBrake::update(double dt) +{ + if (dt <= 0.0) { + return; // paused + } + + double gs = _groundspeedNode->getDoubleValue(); + _actualDecel = (_lastGroundspeedKts - gs) / dt; + _lastGroundspeedKts = gs; + + _leftBrakeInput = SGMiscd::max(_brakeInputs[0]->getDoubleValue(), + _brakeInputs[2]->getDoubleValue()); + _rightBrakeInput = SGMiscd::max(_brakeInputs[1]->getDoubleValue(), + _brakeInputs[3]->getDoubleValue()); + + if (!_armed || (_step == 0)) { + // even if we're off or disarmed, the physical valve system must pass + // pilot control values through. + _leftBrakeOutput = _leftBrakeInput; + _rightBrakeOutput = _rightBrakeInput; + return; + } + + if (_engaged) { + updateEngaged(dt); + } else { + bool e = shouldEngage(); + if (e) { + engage(); + updateEngaged(dt); + } + } + + // sum pilot inpiuts with auto-brake FCS input + _leftBrakeOutput = SGMiscd::max(_leftBrakeInput, _fcsBrakeControl); + _rightBrakeOutput = SGMiscd::max(_rightBrakeInput, _fcsBrakeControl); + + _lastWoW = _weightOnWheelsNode->getBoolValue(); +} + +void FGAutoBrake::engage() +{ + SG_LOG(SG_AUTOPILOT, SG_INFO, "FGAutoBrake: engaging"); + _engaged = true; +} + +void FGAutoBrake::updateEngaged(double dt) +{ + // check for pilot braking input cancelling engage + const double BRAKE_INPUT_THRESHOLD = 0.02; // 2% application triggers + if ((_leftBrakeInput > BRAKE_INPUT_THRESHOLD) || (_rightBrakeInput > BRAKE_INPUT_THRESHOLD)) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "auto-brake, detected pilot brake input, disengaing"); + disengage(); + return; + } + + // update the target deceleration; note step can be changed while engaged + if (_rto) { + _targetDecel = _configRTODecel; + } else { + double f = (double) _step / _configNumSteps; + _targetDecel = _configMaxDecel * f; + } +} + +bool FGAutoBrake::shouldEngage() +{ + if (_rto) { + return shouldEngageRTO(); + } + + if (_lastWoW || !_weightOnWheelsNode->getBoolValue()) { + return false; + } + + SG_LOG(SG_AUTOPILOT, SG_INFO, "Autobrake, should enage"); + return true; +} + +bool FGAutoBrake::shouldEngageRTO() +{ + double speed = _groundspeedNode->getDoubleValue(); + + if (_rtoArmed) { + if (speed < _configRTOSpeed) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "FGAutoBrake: RTO armed, but speed is now below arming speed"); + _rtoArmed = false; + return false; + } + + if (!_weightOnWheelsNode->getBoolValue()) { + return false; + } + + return throttlesAtIdle(); + } else { + // RTO arming case + if (speed > _configRTOSpeed) { + SG_LOG(SG_AUTOPILOT, SG_INFO, "Autobrake RTO: passed " << _configRTOSpeed << " knots, arming RTO mode"); + _rtoArmed = true; + } + } + + return false; +} + +void FGAutoBrake::disengage() +{ + if (!_engaged) { + return; + } + + SG_LOG(SG_AUTOPILOT, SG_INFO, "FGAutoBrake: disengage"); + _engaged = false; + _armed = false; + _rtoArmed = false; + _targetDecel = 0.0; +} + +bool FGAutoBrake::throttlesAtIdle() +{ + SGPropertyNode_ptr e; + const double IDLE_THROTTLE = 0.05; // 5% margin for idle setting + + for (int index=0; (e = _engineControlsNode->getChild("engine", index)) != NULL; ++index) { + if ((e->getDoubleValue("throttle") > IDLE_THROTTLE) && !e->getBoolValue("reverser")) { + return false; + } + } // of engines iteration + + SG_LOG(SG_AUTOPILOT, SG_INFO, "FGAutoBrake: throttles at idle"); + return true; +} + +void FGAutoBrake::setArmed(bool aArmed) +{ + if (aArmed == _armed) { + return; + } + + disengage(); + _armed = aArmed; +} + +void FGAutoBrake::setRTO(bool aRTO) +{ + if (aRTO) { + // ensure we meet RTO selection criteria: + if (!_weightOnWheelsNode->getBoolValue()) { + SG_LOG(SG_AUTOPILOT, SG_WARN, "auto-brake: cannot select RTO mode, no weight on wheels"); + return; + } + + _rtoArmed = false; + _rto = true; + _step = _configRTOStep; + setArmed(true); + SG_LOG(SG_AUTOPILOT, SG_INFO, "RTO mode set"); + } else { + _rto = false; + _rtoArmed = false; + // and if we're enaged? + disengage(); + } +} + +void FGAutoBrake::setStep(int aStep) +{ + if (aStep == _step) { + return; + } + + SG_LOG(SG_AUTOPILOT, SG_INFO, "Autobrake step now=" << aStep); + _step = aStep; + bool validStep = false; + + if (aStep == _configRTOStep) { + setRTO(true); + validStep = true; + } else { + _rto = false; + validStep = (_step > 0) && (_step <= _configNumSteps); + } + + setArmed(validStep); // selecting a valid step arms the system +} + + diff --git a/src/Autopilot/autobrake.hxx b/src/Autopilot/autobrake.hxx new file mode 100644 index 000000000..0eb7497e7 --- /dev/null +++ b/src/Autopilot/autobrake.hxx @@ -0,0 +1,102 @@ +// autobrake.hxx - generic, configurable autobrake system +// +// Written by James Turner, started September 2009. +// +// Copyright (C) 2009 Curtis L. Olson +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License as +// published by the Free Software Foundation; either version 2 of the +// License, or (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, but +// WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +// General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +// +// $Id$ + +#ifndef FG_INSTR_AUTOBRAKE_HXX +#define FG_INSTR_AUTOBRAKE_HXX + +#include +#include + +// forward decls + +class FGAutoBrake : public SGSubsystem +{ +public: + FGAutoBrake(); + virtual ~FGAutoBrake(); + + virtual void init (); + virtual void postinit (); + virtual void bind (); + virtual void unbind (); + virtual void update (double dt); + +private: + + void engage(); + void disengage(); + + void updateEngaged(double dt); + + bool shouldEngage(); + bool shouldEngageRTO(); + + /** + * Helper to determine if all throttles are at idle + * (or have reverse thrust engaged) + */ + bool throttlesAtIdle(); + +// accessors, mostly for SGRawValueMethods use + void setArmed(bool aArmed); + bool getArmed() const { return _armed; } + + void setRTO(bool aRTO); + bool getRTO() const { return _rto; } + + void setStep(int aStep); + int getStep() const { return _step; } + + bool getEngaged() const { return _engaged;} + double getTargetDecel() const { return _targetDecel; } + double getActualDecel() const { return _actualDecel; } + +// members + double _lastGroundspeedKts; + int _step; + bool _rto; // true if in Rejected TakeOff mode + bool _armed; + bool _rtoArmed; ///< true if we have met arming criteria for RTO mode + bool _engaged; ///< true if auto-braking is currently active + double _targetDecel; // target deceleration ft-sec^2 + double _actualDecel; // measured current deceleration in ft-sec^2 + double _fcsBrakeControl; + bool _lastWoW; + double _leftBrakeInput; // summed pilot and co-pilot left brake input + double _rightBrakeInput; // summed pilot and co-pilot right brake input + double _leftBrakeOutput; + double _rightBrakeOutput; + + SGPropertyNode_ptr _root; + SGPropertyNode* _brakeInputs[4]; + SGPropertyNode_ptr _weightOnWheelsNode; + SGPropertyNode_ptr _engineControlsNode; + SGPropertyNode_ptr _groundspeedNode; + + int _configNumSteps; + int _configRTOStep; + double _configMaxDecel; ///< deceleration (in ft-sec^2) corresponding to step=numSteps + double _configRTODecel; ///< deceleration (in ft-sec^2) to use in RTO mode + double _configRTOSpeed; ///< speed (in kts) for RTO mode to arm +}; + +#endif // of FG_INSTR_AUTOBRAKE_HXX diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index 953e4a254..958a91ad0 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -73,6 +73,8 @@ #include #include #include +#include + #include #include #include @@ -1530,7 +1532,8 @@ bool fgInitSubsystems() { globals->add_subsystem( "xml-autopilot", new FGXMLAutopilot ); globals->add_subsystem( "route-manager", new FGRouteMgr ); - + globals->add_subsystem( "autobrake", new FGAutoBrake ); + //////////////////////////////////////////////////////////////////// // Initialize the view manager subsystem. ////////////////////////////////////////////////////////////////////