From: curt Date: Sat, 31 Jan 2004 19:47:45 +0000 (+0000) Subject: Curt Olson: X-Git-Url: https://git.mxchange.org/?a=commitdiff_plain;h=da5ea10d5db9675e6a5d75db5b33edfc350566b1;p=flightgear.git Curt Olson: Autopilot overhaul. --- diff --git a/src/Aircraft/aircraft.cxx b/src/Aircraft/aircraft.cxx index 1fe90e128..a3fcd9e47 100644 --- a/src/Aircraft/aircraft.cxx +++ b/src/Aircraft/aircraft.cxx @@ -40,7 +40,6 @@ #include #include #include -#include #include "aircraft.hxx" @@ -228,9 +227,9 @@ fgLoadAircraft (const SGPropertyNode * arg) // globals->get_viewmgr()->reinit(); globals->get_controls()->reset_all(); - globals->get_autopilot()->reset(); globals->get_aircraft_model()->reinit(); globals->get_subsystem("fx")->reinit(); + globals->get_subsystem("xml-autopilot")->reinit(); fgReInitSubsystems(); diff --git a/src/Autopilot/Makefile.am b/src/Autopilot/Makefile.am index 01ee5cedb..0a52f290c 100644 --- a/src/Autopilot/Makefile.am +++ b/src/Autopilot/Makefile.am @@ -2,6 +2,7 @@ noinst_LIBRARIES = libAutopilot.a libAutopilot_a_SOURCES = \ auto_gui.cxx auto_gui.hxx \ - newauto.cxx newauto.hxx + route_mgr.cxx route_mgr.hxx \ + xmlauto.cxx xmlauto.hxx INCLUDES = -I$(top_srcdir) -I$(top_srcdir)/src diff --git a/src/Autopilot/auto_gui.cxx b/src/Autopilot/auto_gui.cxx index 51363288c..4e3c19a84 100644 --- a/src/Autopilot/auto_gui.cxx +++ b/src/Autopilot/auto_gui.cxx @@ -28,8 +28,6 @@ #include -#include - #include #include #include @@ -55,7 +53,8 @@ #include #include "auto_gui.hxx" -#include "newauto.hxx" +#include "route_mgr.hxx" +#include "xmlauto.hxx" SG_USING_STD(string); @@ -206,27 +205,25 @@ void ApHeadingDialog_OK (puObject *me) if( strlen(c) ) { double NewHeading; - if( scan_number( c, &NewHeading ) ) - { - if ( !globals->get_autopilot()->get_HeadingEnabled() ) { - globals->get_autopilot()->set_HeadingEnabled( true ); - } - globals->get_autopilot()->HeadingSet( NewHeading ); - } else { - error = 1; - s = c; - s += " is not a valid number."; - } + if( scan_number( c, &NewHeading ) ) { + fgSetString( "/autopilot/locks/heading", "dg-heading-hold" ); + fgSetDouble( "/autopilot/settings/heading-bug-deg", + NewHeading ); + } else { + error = 1; + s = c; + s += " is not a valid number."; + } } ApHeadingDialog_Cancel(me); - if( error ) mkDialog(s.c_str()); + if ( error ) mkDialog(s.c_str()); } void NewHeading(puObject *cb) { // string ApHeadingLabel( "Enter New Heading" ); // ApHeadingDialogMessage -> setLabel(ApHeadingLabel.c_str()); - float heading = globals->get_autopilot()->get_DGTargetHeading(); + float heading = fgGetDouble( "/autopilot/settings/heading-bug-deg" ); while ( heading < 0.0 ) { heading += 360.0; } ApHeadingDialogInput -> setValue ( heading ); ApHeadingDialogInput -> acceptInput(); @@ -281,19 +278,16 @@ void ApAltitudeDialog_OK (puObject *me) char *c; ApAltitudeDialogInput->getValue( &c ); - if( strlen( c ) ) { + if ( strlen( c ) ) { double NewAltitude; - if( scan_number( c, &NewAltitude) ) - { - if ( !globals->get_autopilot()->get_AltitudeEnabled() ) { - globals->get_autopilot()->set_AltitudeEnabled( true ); - } - globals->get_autopilot()->AltitudeSet( NewAltitude ); - } else { - error = 1; - s = c; - s += " is not a valid number."; - } + if ( scan_number( c, &NewAltitude) ) { + fgSetString( "/autopilot/locks/altitude", "altitude-hold" ); + fgSetDouble( "/autopilot/settings/altitude-ft", NewAltitude ); + } else { + error = 1; + s = c; + s += " is not a valid number."; + } } ApAltitudeDialog_Cancel(me); if( error ) mkDialog(s.c_str()); @@ -301,7 +295,8 @@ void ApAltitudeDialog_OK (puObject *me) void NewAltitude(puObject *cb) { - float altitude = globals->get_autopilot()->get_TargetAltitude() * SG_METER_TO_FEET; + float altitude = fgGetDouble("/autopilot/settings/altitude-ft") + * SG_METER_TO_FEET; ApAltitudeDialogInput -> setValue( altitude ); ApAltitudeDialogInput -> acceptInput(); FG_PUSH_PUI_DIALOG( ApAltitudeDialog ); @@ -361,8 +356,9 @@ static void maxroll_adj( puObject *hs ) { hs-> getValue ( &val ) ; SG_CLAMP_RANGE ( val, 0.1f, 1.0f ) ; // printf ( "maxroll_adj( %p ) %f %f\n", hs, val, MaxRollAdjust * val ) ; - globals->get_autopilot()->set_MaxRoll( MaxRollAdjust * val ); - sprintf( SliderText[ 0 ], "%05.2f", globals->get_autopilot()->get_MaxRoll() ); + fgSetDouble( "/autopilot/config/max-roll-deg", MaxRollAdjust * val ); + sprintf( SliderText[ 0 ], "%05.2f", + fgGetDouble("/autopilot/config/max-roll-deg") ); APAdjustMaxRollText -> setLabel ( SliderText[ 0 ] ) ; } @@ -372,8 +368,9 @@ static void rollout_adj( puObject *hs ) { hs-> getValue ( &val ) ; SG_CLAMP_RANGE ( val, 0.1f, 1.0f ) ; // printf ( "rollout_adj( %p ) %f %f\n", hs, val, RollOutAdjust * val ) ; - globals->get_autopilot()->set_RollOut( RollOutAdjust * val ); - sprintf( SliderText[ 1 ], "%05.2f", globals->get_autopilot()->get_RollOut() ); + fgSetDouble( "/autopilot/config/roll-out-deg", RollOutAdjust * val ); + sprintf( SliderText[ 1 ], "%05.2f", + fgGetDouble("/autopilot/config/roll-out-deg") ); APAdjustRollOutText -> setLabel ( SliderText[ 1 ] ); } @@ -383,8 +380,9 @@ static void maxaileron_adj( puObject *hs ) { hs-> getValue ( &val ) ; SG_CLAMP_RANGE ( val, 0.1f, 1.0f ) ; // printf ( "maxaileron_adj( %p ) %f %f\n", hs, val, MaxAileronAdjust * val ) ; - globals->get_autopilot()->set_MaxAileron( MaxAileronAdjust * val ); - sprintf( SliderText[ 3 ], "%05.2f", globals->get_autopilot()->get_MaxAileron() ); + fgSetDouble( "/autopilot/config/max-aileron", MaxAileronAdjust * val ); + sprintf( SliderText[ 3 ], "%05.2f", + fgGetDouble("/autopilot/config/max-aileron") ); APAdjustMaxAileronText -> setLabel ( SliderText[ 3 ] ); } @@ -394,8 +392,10 @@ static void rolloutsmooth_adj( puObject *hs ) { hs -> getValue ( &val ) ; SG_CLAMP_RANGE ( val, 0.1f, 1.0f ) ; // printf ( "rolloutsmooth_adj( %p ) %f %f\n", hs, val, RollOutSmoothAdjust * val ) ; - globals->get_autopilot()->set_RollOutSmooth( RollOutSmoothAdjust * val ); - sprintf( SliderText[ 2 ], "%5.2f", globals->get_autopilot()->get_RollOutSmooth() ); + fgSetDouble( "/autopilot/config/roll-out-smooth-deg", + RollOutSmoothAdjust * val ); + sprintf( SliderText[ 2 ], "%5.2f", + fgGetDouble("/autopilot/config/roll-out-smooth-deg") ); APAdjustRollOutSmoothText-> setLabel ( SliderText[ 2 ] ); } @@ -406,19 +406,21 @@ static void goAwayAPAdjust (puObject *) } void cancelAPAdjust( puObject *self ) { - globals->get_autopilot()->set_MaxRoll( TmpMaxRollValue ); - globals->get_autopilot()->set_RollOut( TmpRollOutValue ); - globals->get_autopilot()->set_MaxAileron( TmpMaxAileronValue ); - globals->get_autopilot()->set_RollOutSmooth( TmpRollOutSmoothValue ); + fgSetDouble( "/autopilot/config/max-roll-deg", TmpMaxRollValue ); + fgSetDouble( "/autopilot/config/roll-out-deg", TmpRollOutValue ); + fgSetDouble( "/autopilot/config/max-aileron", TmpMaxAileronValue ); + fgSetDouble( "/autopilot/config/roll-out-smooth-deg", + TmpRollOutSmoothValue ); goAwayAPAdjust(self); } void resetAPAdjust( puObject *self ) { - globals->get_autopilot()->set_MaxRoll( MaxRollAdjust / 2 ); - globals->get_autopilot()->set_RollOut( RollOutAdjust / 2 ); - globals->get_autopilot()->set_MaxAileron( MaxAileronAdjust / 2 ); - globals->get_autopilot()->set_RollOutSmooth( RollOutSmoothAdjust / 2 ); + fgSetDouble( "/autopilot/config/max-roll-deg", MaxRollAdjust / 2 ); + fgSetDouble( "/autopilot/config/roll-out-deg", RollOutAdjust / 2 ); + fgSetDouble( "/autopilot/config/max-aileron", MaxAileronAdjust / 2 ); + fgSetDouble( "/autopilot/config/roll-out-smooth-deg", + RollOutSmoothAdjust / 2 ); FG_POP_PUI_DIALOG( APAdjustDialog ); @@ -426,15 +428,18 @@ void resetAPAdjust( puObject *self ) { } void fgAPAdjust( puObject *self ) { - TmpMaxRollValue = globals->get_autopilot()->get_MaxRoll(); - TmpRollOutValue = globals->get_autopilot()->get_RollOut(); - TmpMaxAileronValue = globals->get_autopilot()->get_MaxAileron(); - TmpRollOutSmoothValue = globals->get_autopilot()->get_RollOutSmooth(); - - MaxRollValue = globals->get_autopilot()->get_MaxRoll() / MaxRollAdjust; - RollOutValue = globals->get_autopilot()->get_RollOut() / RollOutAdjust; - MaxAileronValue = globals->get_autopilot()->get_MaxAileron() / MaxAileronAdjust; - RollOutSmoothValue = globals->get_autopilot()->get_RollOutSmooth() + TmpMaxRollValue = fgGetDouble("/autopilot/config/max-roll-deg"); + TmpRollOutValue = fgGetDouble("/autopilot/config/roll-out-deg"); + TmpMaxAileronValue = fgGetDouble("/autopilot/config/max-aileron"); + TmpRollOutSmoothValue = fgGetDouble("/autopilot/config/roll-out-smooth-deg"); + + MaxRollValue = fgGetDouble("/autopilot/config/max-roll-deg") + / MaxRollAdjust; + RollOutValue = fgGetDouble("/autopilot/config/roll-out-deg") + / RollOutAdjust; + MaxAileronValue = fgGetDouble("/autopilot/config/max-aileron") + / MaxAileronAdjust; + RollOutSmoothValue = fgGetDouble("/autopilot/config/roll-out-smooth-deg") / RollOutSmoothAdjust; APAdjustHS0-> setValue ( MaxRollValue ) ; @@ -470,20 +475,22 @@ void fgAPAdjustInit() { int slider_value_x = 160; float slider_delta = 0.1f; - TmpMaxRollValue = globals->get_autopilot()->get_MaxRoll(); - TmpRollOutValue = globals->get_autopilot()->get_RollOut(); - TmpMaxAileronValue = globals->get_autopilot()->get_MaxAileron(); - TmpRollOutSmoothValue = globals->get_autopilot()->get_RollOutSmooth(); - - MaxRollAdjust = 2 * globals->get_autopilot()->get_MaxRoll(); - RollOutAdjust = 2 * globals->get_autopilot()->get_RollOut(); - MaxAileronAdjust = 2 * globals->get_autopilot()->get_MaxAileron(); - RollOutSmoothAdjust = 2 * globals->get_autopilot()->get_RollOutSmooth(); - - MaxRollValue = globals->get_autopilot()->get_MaxRoll() / MaxRollAdjust; - RollOutValue = globals->get_autopilot()->get_RollOut() / RollOutAdjust; - MaxAileronValue = globals->get_autopilot()->get_MaxAileron() / MaxAileronAdjust; - RollOutSmoothValue = globals->get_autopilot()->get_RollOutSmooth() + TmpMaxRollValue = fgGetDouble("/autopilot/config/max-roll-deg"); + TmpRollOutValue = fgGetDouble("/autopilot/config/roll-out-deg"); + TmpMaxAileronValue = fgGetDouble("/autopilot/config/max-aileron"); + TmpRollOutSmoothValue = fgGetDouble("/autopilot/config/roll-out-smooth-deg"); + MaxRollAdjust = 2 * fgGetDouble("/autopilot/config/max-roll-deg"); + RollOutAdjust = 2 * fgGetDouble("/autopilot/config/roll-out-deg"); + MaxAileronAdjust = 2 * fgGetDouble("/autopilot/config/max-aileron"); + RollOutSmoothAdjust = 2 * fgGetDouble("/autopilot/config/roll-out-smooth-deg"); + + MaxRollValue = fgGetDouble("/autopilot/config/max-roll-deg") + / MaxRollAdjust; + RollOutValue = fgGetDouble("/autopilot/config/roll-out-deg") + / RollOutAdjust; + MaxAileronValue = fgGetDouble("/autopilot/config/max-aileron") + / MaxAileronAdjust; + RollOutSmoothValue = fgGetDouble("/autopilot/config/roll-out-smooth-deg") / RollOutSmoothAdjust; puGetDefaultFonts ( &APAdjustLegendFont, &APAdjustLabelFont ); @@ -510,7 +517,8 @@ void fgAPAdjustInit() { APAdjustHS0-> setCBMode ( PUSLIDER_DELTA ) ; APAdjustHS0-> setCallback ( maxroll_adj ) ; - sprintf( SliderText[ 0 ], "%05.2f", globals->get_autopilot()->get_MaxRoll() ); + sprintf( SliderText[ 0 ], "%05.2f", + fgGetDouble("/autopilot/config/max-roll-deg") ); APAdjustMaxRollTitle = new puText ( slider_title_x, slider_y ) ; APAdjustMaxRollTitle-> setDefaultValue ( "MaxRoll" ) ; APAdjustMaxRollTitle-> getDefaultValue ( &s ) ; @@ -527,7 +535,8 @@ void fgAPAdjustInit() { APAdjustHS1-> setCBMode ( PUSLIDER_DELTA ) ; APAdjustHS1-> setCallback ( rollout_adj ) ; - sprintf( SliderText[ 1 ], "%05.2f", globals->get_autopilot()->get_RollOut() ); + sprintf( SliderText[ 1 ], "%05.2f", + fgGetDouble("/autopilot/config/roll-out-deg") ); APAdjustRollOutTitle = new puText ( slider_title_x, slider_y ) ; APAdjustRollOutTitle-> setDefaultValue ( "AdjustRollOut" ) ; APAdjustRollOutTitle-> getDefaultValue ( &s ) ; @@ -545,7 +554,7 @@ void fgAPAdjustInit() { APAdjustHS2-> setCallback ( rolloutsmooth_adj ) ; sprintf( SliderText[ 2 ], "%5.2f", - globals->get_autopilot()->get_RollOutSmooth() ); + fgGetDouble("/autopilot/config/roll-out-smooth-deg") ); APAdjustRollOutSmoothTitle = new puText ( slider_title_x, slider_y ) ; APAdjustRollOutSmoothTitle-> setDefaultValue ( "RollOutSmooth" ) ; APAdjustRollOutSmoothTitle-> getDefaultValue ( &s ) ; @@ -563,7 +572,7 @@ void fgAPAdjustInit() { APAdjustHS3-> setCallback ( maxaileron_adj ) ; sprintf( SliderText[ 3 ], "%05.2f", - globals->get_autopilot()->get_MaxAileron() ); + fgGetDouble("/autopilot/config/max-aileron") ); APAdjustMaxAileronTitle = new puText ( slider_title_x, slider_y ) ; APAdjustMaxAileronTitle-> setDefaultValue ( "MaxAileron" ) ; APAdjustMaxAileronTitle-> getDefaultValue ( &s ) ; @@ -648,42 +657,40 @@ int NewWaypoint( string Tgt_Alt ) TgtAptId = Tgt_Alt; } - if ( fgFindAirportID( TgtAptId, &a ) ) { + FGRouteMgr *rm = (FGRouteMgr *)globals->get_subsystem("route-manager"); - SG_LOG( SG_GENERAL, SG_INFO, - "Adding waypoint (airport) = " << TgtAptId ); + if ( fgFindAirportID( TgtAptId, &a ) ) { - sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() ); + SG_LOG( SG_GENERAL, SG_INFO, + "Adding waypoint (airport) = " << TgtAptId ); - SGWayPoint wp( a.longitude, a.latitude, alt, - SGWayPoint::WGS84, TgtAptId ); - globals->get_route()->add_waypoint( wp ); + sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() ); - /* and turn on the autopilot */ - globals->get_autopilot()->set_HeadingEnabled( true ); - globals->get_autopilot()->set_HeadingMode( FGAutopilot::FG_HEADING_WAYPOINT -); + SGWayPoint wp( a.longitude, a.latitude, alt, + SGWayPoint::WGS84, TgtAptId ); + rm->add_waypoint( wp ); - return 1; + /* and turn on the autopilot */ + fgSetString( "/autopilot/locks/heading", "true-heading-hold" ); - } else if ( current_fixlist->query( TgtAptId, &f ) ) - { - SG_LOG( SG_GENERAL, SG_INFO, - "Adding waypoint (fix) = " << TgtAptId ); + return 1; - sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() ); + } else if ( current_fixlist->query( TgtAptId, &f ) ) { + SG_LOG( SG_GENERAL, SG_INFO, + "Adding waypoint (fix) = " << TgtAptId ); - SGWayPoint wp( f.get_lon(), f.get_lat(), alt, - SGWayPoint::WGS84, TgtAptId ); - globals->get_route()->add_waypoint( wp ); + sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() ); - /* and turn on the autopilot */ - globals->get_autopilot()->set_HeadingEnabled( true ); - globals->get_autopilot()->set_HeadingMode( FGAutopilot::FG_HEADING_WAYPOINT ); - return 2; + SGWayPoint wp( f.get_lon(), f.get_lat(), alt, + SGWayPoint::WGS84, TgtAptId ); + rm->add_waypoint( wp ); + /* and turn on the autopilot */ + fgSetString( "/autopilot/locks/heading", "true-heading-hold" ); + return 2; + } else { + return 0; } - else return 0; } @@ -743,11 +750,16 @@ void AddWayPoint(puObject *cb) } delete [] WPList[i]; } - if ( globals->get_route()->size() > 0 ) { - WPListsize = globals->get_route()->size(); + FGRouteMgr *rm = (FGRouteMgr *)globals->get_subsystem("route-manager"); + WPListsize = rm->size(); + if ( WPListsize > 0 ) { WPList = new char* [ WPListsize + 1 ]; - for (i = 0; i < globals->get_route()->size(); i++ ) { - sprintf(WPString, "%5s %3.2flon %3.2flat", globals->get_route()->get_waypoint(i).get_id().c_str(), globals->get_route()->get_waypoint(i).get_target_lon(), globals->get_route()->get_waypoint(i).get_target_lat()); + for (i = 0; i < WPListsize; i++ ) { + SGWayPoint wp = rm->get_waypoint(i); + sprintf( WPString, "%5s %3.2flon %3.2flat", + wp.get_id().c_str(), + wp.get_target_lon(), + wp.get_target_lat() ); WPList [i] = new char[ strlen(WPString)+1 ]; strcpy ( WPList [i], WPString ); } @@ -777,25 +789,14 @@ void AddWayPoint(puObject *cb) void PopWayPoint(puObject *cb) { - globals->get_route()->delete_first(); - - // see if there are more waypoints on the list - if ( globals->get_route()->size() ) { - // more waypoints - globals->get_autopilot()->set_HeadingMode( FGAutopilot::FG_HEADING_WAYPOINT ); - } else { - // end of the line - globals->get_autopilot()->set_HeadingMode( FGAutopilot::FG_TC_HEADING_LOCK ); - - // use current heading - globals->get_autopilot() - ->set_TargetHeading(fgGetDouble("/orientation/heading-deg")); - } + FGRouteMgr *rm = (FGRouteMgr *)globals->get_subsystem("route-manager"); + rm->pop_waypoint(); } void ClearRoute(puObject *cb) { - globals->get_route()->clear(); + FGRouteMgr *rm = (FGRouteMgr *)globals->get_subsystem("route-manager"); + rm->init(); } void NewTgtAirportInit() diff --git a/src/Autopilot/auto_gui.hxx b/src/Autopilot/auto_gui.hxx index 44db9965c..40a892a10 100644 --- a/src/Autopilot/auto_gui.hxx +++ b/src/Autopilot/auto_gui.hxx @@ -25,6 +25,12 @@ #ifndef _AUTO_GUI_HXX #define _AUTO_GUI_HXX +#include + +#include STL_STRING + +SG_USING_STD( string ); + // Defines #define AP_CURRENT_HEADING -1 diff --git a/src/Autopilot/newauto.hxx b/src/Autopilot/newauto.hxx index 467847f18..3daa9b0b1 100644 --- a/src/Autopilot/newauto.hxx +++ b/src/Autopilot/newauto.hxx @@ -22,6 +22,8 @@ // // $Id$ + +#error choke me #ifndef _NEWAUTO_HXX #define _NEWAUTO_HXX diff --git a/src/Autopilot/route_mgr.cxx b/src/Autopilot/route_mgr.cxx new file mode 100644 index 000000000..c9fc58765 --- /dev/null +++ b/src/Autopilot/route_mgr.cxx @@ -0,0 +1,220 @@ +// route_mgr.cxx - manage a route (i.e. a collection of waypoints) +// +// Written by Curtis Olson, started January 2004. +// +// Copyright (C) 2004 Curtis L. Olson - curt@flightgear.org +// +// 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., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + + +#include +#include
+ +#include "route_mgr.hxx" + + +FGRouteMgr::FGRouteMgr() : + route( new SGRoute ), + lon( NULL ), + lat( NULL ), + alt( NULL ), + true_hdg_deg( NULL ), + wp0_id( NULL ), + wp0_dist( NULL ), + wp0_eta( NULL ), + wp1_id( NULL ), + wp1_dist( NULL ), + wp1_eta( NULL ), + wpn_id( NULL ), + wpn_dist( NULL ), + wpn_eta( NULL ) +{ + cout << "route = " << route << endl; +} + + +FGRouteMgr::~FGRouteMgr() { + delete route; +} + + +void FGRouteMgr::init() { + lon = fgGetNode( "/position/longitude-deg", true ); + lat = fgGetNode( "/position/latitude-deg", true ); + alt = fgGetNode( "/position/altitude-ft", true ); + + true_hdg_deg = fgGetNode( "/autopilot/settings/true-heading-deg", true ); + + wp0_id = fgGetNode( "/autopilot/route-manager/wp[0]/id", true ); + wp0_dist = fgGetNode( "/autopilot/route-manager/wp[0]/dist", true ); + wp0_eta = fgGetNode( "/autopilot/route-manager/wp[0]/eta", true ); + + wp1_id = fgGetNode( "/autopilot/route-manager/wp[1]/id", true ); + wp1_dist = fgGetNode( "/autopilot/route-manager/wp[1]/dist", true ); + wp1_eta = fgGetNode( "/autopilot/route-manager/wp[1]/eta", true ); + + wpn_id = fgGetNode( "/autopilot/route-manager/wp-last/id", true ); + wpn_dist = fgGetNode( "/autopilot/route-manager/wp-last/dist", true ); + wpn_eta = fgGetNode( "/autopilot/route-manager/wp-last/eta", true ); + + route->clear(); +} + + +void FGRouteMgr::bind() { } +void FGRouteMgr::unbind() { } + + +static double get_ground_speed() { + // starts in ft/s so we convert to kts + static const SGPropertyNode * speedup_node = fgGetNode("/sim/speed-up"); + + double ft_s = cur_fdm_state->get_V_ground_speed() + * speedup_node->getIntValue(); + double kts = ft_s * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM; + + return kts; +} + + +void FGRouteMgr::update( double dt ) { + double accum = 0.0; + double wp_course, wp_distance; + char eta_str[128]; + + // first way point + if ( route->size() > 0 ) { + SGWayPoint wp = route->get_waypoint( 0 ); + wp.CourseAndDistance( lon->getDoubleValue(), lat->getDoubleValue(), + alt->getDoubleValue(), &wp_course, &wp_distance ); + + true_hdg_deg->setDoubleValue( wp_course ); + + if ( wp_distance < 200.0 ) { + pop_waypoint(); + } + } + + // next way point + if ( route->size() > 0 ) { + SGWayPoint wp = route->get_waypoint( 0 ); + // update the property tree info + + wp0_id->setStringValue( wp.get_id().c_str() ); + + accum += wp_distance; + wp0_dist->setDoubleValue( accum * SG_METER_TO_NM ); + + double eta = accum * SG_METER_TO_NM / get_ground_speed(); + if ( eta >= 100.0 ) { eta = 99.999; } + int major, minor; + if ( eta < (1.0/6.0) ) { + // within 10 minutes, bump up to min/secs + eta *= 60.0; + } + major = (int)eta; + minor = (int)((eta - (int)eta) * 60.0); + snprintf( eta_str, 128, "%d:%02d", major, minor ); + wp0_eta->setStringValue( eta_str ); + } + + // next way point + if ( route->size() > 1 ) { + SGWayPoint wp = route->get_waypoint( 1 ); + + // update the property tree info + + wp1_id->setStringValue( wp.get_id().c_str() ); + + accum += wp.get_distance(); + wp1_dist->setDoubleValue( accum * SG_METER_TO_NM ); + + double eta = accum * SG_METER_TO_NM / get_ground_speed(); + if ( eta >= 100.0 ) { eta = 99.999; } + int major, minor; + if ( eta < (1.0/6.0) ) { + // within 10 minutes, bump up to min/secs + eta *= 60.0; + } + major = (int)eta; + minor = (int)((eta - (int)eta) * 60.0); + snprintf( eta_str, 128, "%d:%02d", major, minor ); + wp1_eta->setStringValue( eta_str ); + } + + // summarize remaining way points + if ( route->size() > 2 ) { + SGWayPoint wp; + for ( int i = 2; i < route->size(); ++i ) { + wp = route->get_waypoint( i ); + accum += wp.get_distance(); + } + + // update the property tree info + + wpn_id->setStringValue( wp.get_id().c_str() ); + + wpn_dist->setDoubleValue( accum * SG_METER_TO_NM ); + + double eta = accum * SG_METER_TO_NM / get_ground_speed(); + if ( eta >= 100.0 ) { eta = 99.999; } + int major, minor; + if ( eta < (1.0/6.0) ) { + // within 10 minutes, bump up to min/secs + eta *= 60.0; + } + major = (int)eta; + minor = (int)((eta - (int)eta) * 60.0); + snprintf( eta_str, 128, "%d:%02d", major, minor ); + wpn_eta->setStringValue( eta_str ); + } +} + + +SGWayPoint FGRouteMgr::pop_waypoint() { + SGWayPoint wp; + + if ( route->size() > 0 ) { + wp = route->get_first(); + route->delete_first(); + } + + if ( route->size() <= 2 ) { + wpn_id->setStringValue( "" ); + wpn_dist->setDoubleValue( 0.0 ); + wpn_eta->setStringValue( "" ); + } + + if ( route->size() <= 1 ) { + wp1_id->setStringValue( "" ); + wp1_dist->setDoubleValue( 0.0 ); + wp1_eta->setStringValue( "" ); + } + + if ( route->size() <= 0 ) { + wp0_id->setStringValue( "" ); + wp0_dist->setDoubleValue( 0.0 ); + wp0_eta->setStringValue( "" ); + } + + return wp; +} + + +bool FGRouteMgr::build() { + return true; +} diff --git a/src/Autopilot/route_mgr.hxx b/src/Autopilot/route_mgr.hxx new file mode 100644 index 000000000..914bfd92b --- /dev/null +++ b/src/Autopilot/route_mgr.hxx @@ -0,0 +1,109 @@ +// route_mgr.hxx - manage a route (i.e. a collection of waypoints) +// +// Written by Curtis Olson, started January 2004. +// +// Copyright (C) 2004 Curtis L. Olson - curt@flightgear.org +// +// 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., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + + +#ifndef _ROUTE_MGR_HXX +#define _ROUTE_MGR_HXX 1 + +#ifndef __cplusplus +# error This library requires C++ +#endif + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include + +#include STL_STRING +#include + +SG_USING_STD(string); +SG_USING_STD(vector); + +#include +#include +#include + + +/** + * Top level route manager class + * + */ + +class FGRouteMgr : public SGSubsystem +{ + +private: + + SGRoute *route; + + // automatic inputs + SGPropertyNode *lon; + SGPropertyNode *lat; + SGPropertyNode *alt; + + // automatic outputs + SGPropertyNode *true_hdg_deg; + + SGPropertyNode *wp0_id; + SGPropertyNode *wp0_dist; + SGPropertyNode *wp0_eta; + + SGPropertyNode *wp1_id; + SGPropertyNode *wp1_dist; + SGPropertyNode *wp1_eta; + + SGPropertyNode *wpn_id; + SGPropertyNode *wpn_dist; + SGPropertyNode *wpn_eta; + + +public: + + FGRouteMgr(); + ~FGRouteMgr(); + + void init (); + void bind (); + void unbind (); + void update (double dt); + + bool build (); + + void add_waypoint( SGWayPoint wp ) { + route->add_waypoint( wp ); + } + + SGWayPoint get_waypoint( int i ) const { + return route->get_waypoint(i); + } + + SGWayPoint pop_waypoint(); + + int size() const { + return route->size(); + } +}; + + +#endif // _ROUTE_MGR_HXX diff --git a/src/Autopilot/xmlauto.cxx b/src/Autopilot/xmlauto.cxx new file mode 100644 index 000000000..2002ceb76 --- /dev/null +++ b/src/Autopilot/xmlauto.cxx @@ -0,0 +1,677 @@ +// xmlauto.cxx - a more flexible, generic way to build autopilots +// +// Written by Curtis Olson, started January 2004. +// +// Copyright (C) 2004 Curtis L. Olson - curt@flightgear.org +// +// 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., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + + +#include +#include + +#include
+#include
+ +#include "xmlauto.hxx" + + +FGPIDController::FGPIDController( SGPropertyNode *node, bool old ): + proportional( false ), + factor( 0.0 ), + offset_prop( NULL ), + offset_value( 0.0 ), + integral( false ), + gain( 0.0 ), + int_sum( 0.0 ), + one_eighty( false ), + clamp( false ), + debug( false ), + y_n( 0.0 ), + r_n( 0.0 ), + Kp( 0.0 ), + alpha( 0.1 ), + beta( 1.0 ), + gamma( 0.0 ), + Ti( 0.0 ), + Td( 0.0 ), + u_min( 0.0 ), + u_max( 0.0 ), + ep_n_1( 0.0 ), + edf_n_1( 0.0 ), + edf_n_2( 0.0 ), + u_n_1( 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 == "enable" ) { + cout << "parsing enable" << endl; + SGPropertyNode *prop = child->getChild( "prop" ); + if ( prop != NULL ) { + cout << "prop = " << prop->getStringValue() << endl; + enable_prop = fgGetNode( prop->getStringValue(), true ); + } else { + cout << "no prop child" << endl; + } + SGPropertyNode *val = child->getChild( "value" ); + if ( val != NULL ) { + enable_value = val->getStringValue(); + } + } else if ( cname == "debug" ) { + debug = child->getBoolValue(); + } else if ( cname == "input" ) { + SGPropertyNode *prop = child->getChild( "prop" ); + if ( prop != NULL ) { + input_prop = fgGetNode( prop->getStringValue(), true ); + } + } else if ( cname == "reference" ) { + SGPropertyNode *prop = child->getChild( "prop" ); + if ( prop != NULL ) { + r_n_prop = fgGetNode( prop->getStringValue(), true ); + } else { + prop = child->getChild( "value" ); + if ( prop != NULL ) { + r_n_value = prop->getDoubleValue(); + } + } + } else if ( cname == "output" ) { + int i = 0; + SGPropertyNode *prop; + while ( (prop = child->getChild("prop", i)) != NULL ) { + SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true ); + output_list.push_back( tmp ); + i++; + } + prop = child->getChild( "clamp" ); + if ( prop != NULL ) { + clamp = true; + + SGPropertyNode *tmp; + + tmp = prop->getChild( "min" ); + if ( tmp != NULL ) { + u_min = tmp->getDoubleValue(); + cout << "min = " << u_min << endl; + } + + tmp = prop->getChild( "max" ); + if ( tmp != NULL ) { + u_max = tmp->getDoubleValue(); + cout << "max = " << u_max << endl; + } + } + } else if ( cname == "proportional" ) { + proportional = true; + + SGPropertyNode *prop; + + prop = child->getChild( "pre" ); + if ( prop != NULL ) { + prop = prop->getChild( "one-eighty" ); + if ( prop != NULL && prop->getBoolValue() ) { + one_eighty = true; + } + } + + prop = child->getChild( "factor" ); + if ( prop != NULL ) { + factor = prop->getDoubleValue(); + } + + prop = child->getChild( "offset" ); + if ( prop != NULL ) { + SGPropertyNode *sub = prop->getChild( "prop" ); + if ( sub != NULL ) { + offset_prop = fgGetNode( sub->getStringValue(), true ); + cout << "offset prop = " << sub->getStringValue() << endl; + } else { + sub = prop->getChild( "value" ); + if ( sub != NULL ) { + offset_value = sub->getDoubleValue(); + cout << "offset value = " << offset_value << endl; + } + } + } + } else if ( cname == "integral" ) { + integral = true; + + SGPropertyNode *prop; + prop = child->getChild( "gain" ); + if ( prop != NULL ) { + gain = prop->getDoubleValue(); + } + } else { + SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" ); + } + } +} + + +FGPIDController::FGPIDController( SGPropertyNode *node ): + proportional( false ), + factor( 0.0 ), + offset_prop( NULL ), + offset_value( 0.0 ), + integral( false ), + gain( 0.0 ), + int_sum( 0.0 ), + one_eighty( false ), + clamp( false ), + debug( false ), + y_n( 0.0 ), + r_n( 0.0 ), + Kp( 0.0 ), + alpha( 0.1 ), + beta( 1.0 ), + gamma( 0.0 ), + Ti( 0.0 ), + Td( 0.0 ), + u_min( 0.0 ), + u_max( 0.0 ), + ep_n_1( 0.0 ), + edf_n_1( 0.0 ), + edf_n_2( 0.0 ), + u_n_1( 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 == "enable" ) { + cout << "parsing enable" << endl; + SGPropertyNode *prop = child->getChild( "prop" ); + if ( prop != NULL ) { + cout << "prop = " << prop->getStringValue() << endl; + enable_prop = fgGetNode( prop->getStringValue(), true ); + } else { + cout << "no prop child" << endl; + } + SGPropertyNode *val = child->getChild( "value" ); + if ( val != NULL ) { + enable_value = val->getStringValue(); + } + } else if ( cname == "input" ) { + SGPropertyNode *prop = child->getChild( "prop" ); + if ( prop != NULL ) { + input_prop = fgGetNode( prop->getStringValue(), true ); + } + } else if ( cname == "reference" ) { + SGPropertyNode *prop = child->getChild( "prop" ); + if ( prop != NULL ) { + r_n_prop = fgGetNode( prop->getStringValue(), true ); + } else { + prop = child->getChild( "value" ); + if ( prop != NULL ) { + r_n = prop->getDoubleValue(); + } + } + } else if ( cname == "output" ) { + int i = 0; + SGPropertyNode *prop; + while ( (prop = child->getChild("prop", i)) != NULL ) { + SGPropertyNode *tmp = fgGetNode( prop->getStringValue(), true ); + output_list.push_back( tmp ); + i++; + } + } else if ( cname == "config" ) { + SGPropertyNode *prop; + + prop = child->getChild( "Kp" ); + if ( prop != NULL ) { + Kp = prop->getDoubleValue(); + } + + prop = child->getChild( "beta" ); + if ( prop != NULL ) { + beta = prop->getDoubleValue(); + } + + prop = child->getChild( "alpha" ); + if ( prop != NULL ) { + alpha = prop->getDoubleValue(); + } + + prop = child->getChild( "gamma" ); + if ( prop != NULL ) { + gamma = prop->getDoubleValue(); + } + + prop = child->getChild( "Ti" ); + if ( prop != NULL ) { + Ti = prop->getDoubleValue(); + } + + prop = child->getChild( "Td" ); + if ( prop != NULL ) { + Td = prop->getDoubleValue(); + } + + prop = child->getChild( "u_min" ); + if ( prop != NULL ) { + u_min = prop->getDoubleValue(); + } + + prop = child->getChild( "u_max" ); + if ( prop != NULL ) { + u_max = prop->getDoubleValue(); + } + } else { + SG_LOG( SG_AUTOPILOT, SG_WARN, "Error in autopilot config logic" ); + } + } +} + + +void FGPIDController::update_old( double dt ) { + if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) { + if ( !enabled ) { + // we have just been enabled, zero out int_sum + int_sum = 0.0; + } + enabled = true; + } else { + enabled = false; + } + + if ( enabled ) { + if ( debug ) cout << "Updating " << name << endl; + double input = 0.0; + if ( input_prop != NULL ) { + input = input_prop->getDoubleValue(); + } + + double r_n = 0.0; + if ( r_n_prop != NULL ) { + r_n = r_n_prop->getDoubleValue(); + } else { + r_n = r_n_value; + } + + double error = r_n - input; + if ( one_eighty ) { + while ( error < -180.0 ) { error += 360.0; } + while ( error > 180.0 ) { error -= 360.0; } + } + if ( debug ) cout << "input = " << input + << " reference = " << r_n + << " error = " << error + << endl; + + double prop_comp = 0.0; + double offset = 0.0; + if ( offset_prop != NULL ) { + offset = offset_prop->getDoubleValue(); + if ( debug ) cout << "offset = " << offset << endl; + } else { + offset = offset_value; + } + + if ( proportional ) { + prop_comp = error * factor + offset; + } + + if ( integral ) { + int_sum += error * gain * dt; + } else { + int_sum = 0.0; + } + + if ( debug ) cout << "prop_comp = " << prop_comp + << " int_sum = " << int_sum << endl; + + double output = prop_comp + int_sum; + + if ( clamp ) { + if ( output < u_min ) { + output = u_min; + } + if ( output > u_max ) { + output = u_max; + } + } + if ( debug ) cout << "output = " << output << endl; + + unsigned int i; + for ( i = 0; i < output_list.size(); ++i ) { + output_list[i]->setDoubleValue( output ); + } + } +} + + +/* + * Roy Vegard Ovesen: + * + * Ok! Here is the PID controller algorithm that I would like to see + * implemented: + * + * delta_u_n = Kp * [ (ep_n - ep_n-1) + ((Ts/Ti)*e_n) + * + (Td/Ts)*(edf_n - 2*edf_n-1 + edf_n-2) ] + * + * u_n = u_n-1 + delta_u_n + * + * where: + * + * delta_u : The incremental output + * Kp : Proportional gain + * ep : Proportional error with reference weighing + * ep = beta * r - y + * where: + * beta : Weighing factor + * r : Reference (setpoint) + * y : Process value, measured + * e : Error + * e = r - y + * Ts : Sampling interval + * Ti : Integrator time + * Td : Derivator time + * edf : Derivate error with reference weighing and filtering + * edf_n = edf_n-1 / ((Ts/Tf) + 1) + ed_n * (Ts/Tf) / ((Ts/Tf) + 1) + * where: + * Tf : Filter time + * Tf = alpha * Td , where alpha usually is set to 0.1 + * ed : Unfiltered derivate error with reference weighing + * ed = gamma * r - y + * where: + * gamma : Weighing factor + * + * u : absolute output + * + * Index n means the n'th value. + * + * + * Inputs: + * enabled , + * y_n , r_n , beta=1 , gamma=0 , alpha=0.1 , + * Kp , Ti , Td , Ts (is the sampling time available?) + * u_min , u_max + * + * Output: + * u_n + */ + +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) + + if ( Ts <= 0.0 ) { + // do nothing if time step is not positive (i.e. no time has + // elapsed) + return; + } + + if (enable_prop != NULL && enable_prop->getStringValue() == enable_value) { + enabled = true; + } else { + enabled = false; + } + + if ( enabled ) { + if ( debug ) cout << "Updating " << name << endl; + + double y_n = 0.0; + if ( input_prop != NULL ) { + y_n = input_prop->getDoubleValue(); + } + + double r_n = 0.0; + if ( r_n_prop != NULL ) { + r_n = r_n_prop->getDoubleValue(); + } else { + r_n = r_n_value; + } + + if ( debug ) cout << " input = " << y_n << " ref = " << r_n << endl; + + // Calculates proportional error: + ep_n = beta * r_n - y_n; + if ( debug ) cout << " ep_n = " << ep_n; + if ( debug ) cout << " ep_n_1 = " << ep_n_1; + + // Calculates error: + e_n = r_n - y_n; + if ( debug ) cout << " e_n = " << e_n; + + // Calculates derivate error: + 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; + + // 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; + + // 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; + + // Integrator anti-windup logic: + if ( delta_u_n > (u_max - u_n_1) ) { + delta_u_n = 0; + } else if ( delta_u_n < (u_min - u_n_1) ) { + delta_u_n = 0; + } + + // Calculates absolute output: + u_n = u_n_1 + delta_u_n; + if ( debug ) cout << " output = " << u_n << endl; + + // Updates indexed values; + u_n_1 = u_n; + ep_n_1 = ep_n; + edf_n_2 = edf_n_1; + edf_n_1 = edf_n; + + unsigned int i; + for ( i = 0; i < output_list.size(); ++i ) { + output_list[i]->setDoubleValue( u_n ); + } + } else if ( !enabled ) { + u_n = 0.0; + ep_n = 0.0; + edf_n = 0.0; + // Updates indexed values; + u_n_1 = u_n; + ep_n_1 = ep_n; + edf_n_2 = edf_n_1; + edf_n_1 = edf_n; + } +} + + +FGXMLAutopilot::FGXMLAutopilot() { +} + + +FGXMLAutopilot::~FGXMLAutopilot() { +} + + +void FGXMLAutopilot::init() { + config_props = fgGetNode( "/autopilot/new-config", true ); + + SGPropertyNode *path_n = fgGetNode("/sim/systems/autopilot/path"); + + if ( path_n ) { + SGPath config( globals->get_fg_root() ); + config.append( path_n->getStringValue() ); + + SG_LOG( SG_ALL, SG_INFO, "Reading autopilot configuration from " + << config.str() ); + try { + readProperties( config.str(), config_props ); + + if ( ! build() ) { + SG_LOG( SG_ALL, SG_ALERT, + "Detected an internal inconsistancy in the autopilot"); + SG_LOG( SG_ALL, SG_ALERT, + " configuration. See earlier errors for" ); + SG_LOG( SG_ALL, SG_ALERT, + " details."); + exit(-1); + } + } catch (const sg_exception& exc) { + SG_LOG( SG_ALL, SG_ALERT, "Failed to load autopilot configuration: " + << config.str() ); + } + + } else { + SG_LOG( SG_ALL, SG_WARN, + "No autopilot configuration specified for this model!"); + } +} + + +void FGXMLAutopilot::reinit() { + components.clear(); + build(); +} + + +void FGXMLAutopilot::bind() { +} + +void FGXMLAutopilot::unbind() { +} + +bool FGXMLAutopilot::build() { + SGPropertyNode *node; + int i; + + int count = config_props->nChildren(); + for ( i = 0; i < count; ++i ) { + node = config_props->getChild(i); + string name = node->getName(); + // cout << name << endl; + if ( name == "pid-controller" ) { + FGXMLAutoComponent *c = new FGPIDController( node ); + components.push_back( c ); + } else { + SG_LOG( SG_ALL, SG_ALERT, "Unknown top level section: " + << name ); + return false; + } + } + + return true; +} + + +/* + * Update helper values + */ +static void update_helper( double dt ) { + // Estimate speed in 5,10 seconds + static SGPropertyNode *vel = fgGetNode( "/velocities/airspeed-kt", true ); + static SGPropertyNode *lookahead5 + = fgGetNode( "/autopilot/internal/lookahead-5-sec-airspeed-kt", true ); + static SGPropertyNode *lookahead10 + = fgGetNode( "/autopilot/internal/lookahead-10-sec-airspeed-kt", true ); + + static double average = 0.0; // average/filtered prediction + static double v_last = 0.0; // last velocity + + double v = vel->getDoubleValue(); + double a = 0.0; + if ( dt > 0.0 ) { + a = (v - v_last) / dt; + + if ( dt < 1.0 ) { + average = (1.0 - dt) * average + dt * a; + } else { + average = a; + } + + lookahead5->setDoubleValue( v + average * 5.0 ); + lookahead10->setDoubleValue( v + average * 10.0 ); + v_last = v; + } + + // Calculate heading bug error normalized to +/- 180.0 + static SGPropertyNode *bug + = fgGetNode( "/autopilot/settings/heading-bug-deg", true ); + static SGPropertyNode *ind_hdg + = fgGetNode( "/instrumentation/heading-indicator/indicated-heading-deg", + true ); + static SGPropertyNode *bug_error + = fgGetNode( "/autopilot/internal/heading-bug-error-deg", true ); + + double diff = bug->getDoubleValue() - ind_hdg->getDoubleValue(); + if ( diff < -180.0 ) { diff += 360.0; } + if ( diff > 180.0 ) { diff -= 360.0; } + bug_error->setDoubleValue( diff ); + + // Calculate true heading error normalized to +/- 180.0 + static SGPropertyNode *target_true + = fgGetNode( "/autopilot/settings/true-heading-deg", true ); + static SGPropertyNode *true_hdg + = fgGetNode( "/orientation/heading-deg", true ); + static SGPropertyNode *true_error + = fgGetNode( "/autopilot/internal/true-heading-error-deg", true ); + + diff = target_true->getDoubleValue() - true_hdg->getDoubleValue(); + if ( diff < -180.0 ) { diff += 360.0; } + if ( diff > 180.0 ) { diff -= 360.0; } + true_error->setDoubleValue( diff ); + + // Calculate nav1 target heading error normalized to +/- 180.0 + static SGPropertyNode *target_nav1 + = fgGetNode( "/radios/nav[0]/radials/target-auto-hdg-deg", true ); + static SGPropertyNode *true_nav1 + = fgGetNode( "/autopilot/internal/nav1-heading-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 ); +} + + +/* + * Update the list of autopilot components + */ + +void FGXMLAutopilot::update( double dt ) { + update_helper( dt ); + + unsigned int i; + for ( i = 0; i < components.size(); ++i ) { + components[i]->update( dt ); + } +} diff --git a/src/Autopilot/xmlauto.hxx b/src/Autopilot/xmlauto.hxx new file mode 100644 index 000000000..1e840d824 --- /dev/null +++ b/src/Autopilot/xmlauto.hxx @@ -0,0 +1,186 @@ +// xmlauto.hxx - a more flexible, generic way to build autopilots +// +// Written by Curtis Olson, started January 2004. +// +// Copyright (C) 2004 Curtis L. Olson - curt@flightgear.org +// +// 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., 675 Mass Ave, Cambridge, MA 02139, USA. +// +// $Id$ + + +#ifndef _XMLAUTO_HXX +#define _XMLAUTO_HXX 1 + +#ifndef __cplusplus +# error This library requires C++ +#endif + +#ifdef HAVE_CONFIG_H +# include +#endif + +#include + +#include STL_STRING +#include + +SG_USING_STD(string); +SG_USING_STD(vector); + +#include +#include + + +/** + * Base class for other autopilot components + */ + +class FGXMLAutoComponent { + +protected: + + string name; + + SGPropertyNode *enable_prop; + string enable_value; + bool enabled; + + SGPropertyNode *input_prop; + SGPropertyNode *r_n_prop; + double r_n_value; + vector output_list; + +public: + + FGXMLAutoComponent() : + enable_prop( NULL ), + enable_value( "" ), + enabled( false ), + input_prop( NULL ), + r_n_prop( NULL ), + r_n_value( 0.0 ) + { } + + virtual ~FGXMLAutoComponent() {} + + virtual void update (double dt) {} + + inline string get_name() { return name; } +}; + + +/** + * A simple proportional controler + */ + +class FGPIDController : public FGXMLAutoComponent { + +private: + + // proportional component data + bool proportional; + double factor; + SGPropertyNode *offset_prop; + double offset_value; + + // integral component data + bool integral; + double gain; + double int_sum; + + // prep functions for error term + bool one_eighty; + + // post functions for output + bool clamp; + + // debug flag + bool debug; + + // Input values + double y_n; // measured process value + double r_n; // reference (set point) value + + // Configuration values + double Kp; // proportional gain + + double alpha; // low pass filter weighing factor (usually 0.1) + double beta; // process value weighing factor for + // calculating proportional error + // (usually 1.0) + double gamma; // process value weighing factor for + // calculating derivative error + // (usually 0.0) + + double Ti; // Integrator time (sec) + double Td; // Derivator time (sec) + + double u_min; // Minimum output clamp + double u_max; // Maximum output clamp + + // Previous state tracking values + double ep_n_1; // ep[n-1] (prop error) + double edf_n_1; // edf[n-1] (derivative error) + double edf_n_2; // edf[n-2] (derivative error) + double u_n_1; // u[n-1] (output) + + + +public: + + FGPIDController( SGPropertyNode *node ); + FGPIDController( SGPropertyNode *node, bool old ); + ~FGPIDController() {} + + void update_old( double dt ); + void update( double dt ); +}; + + +/** + * Model an autopilot system. + * + */ + +class FGXMLAutopilot : public SGSubsystem +{ + +public: + + FGXMLAutopilot(); + ~FGXMLAutopilot(); + + void init(); + void reinit(); + void bind(); + void unbind(); + void update( double dt ); + + bool build(); + +protected: + + typedef vector comp_list; + +private: + + bool serviceable; + SGPropertyNode *config_props; + comp_list components; +}; + + +#endif // _XMLAUTO_HXX diff --git a/src/Cockpit/hud.cxx b/src/Cockpit/hud.cxx index 18e0ef636..f2b2bcc58 100644 --- a/src/Cockpit/hud.cxx +++ b/src/Cockpit/hud.cxx @@ -50,7 +50,7 @@ #include #include -#include +#include #include #include
#include
@@ -1095,8 +1095,17 @@ void drawHUD() glDisable(GL_DEPTH_TEST); glDisable(GL_LIGHTING); - static const SGPropertyNode * antialiased_node - = fgGetNode("/sim/hud/antialiased"); + static const SGPropertyNode *antialiased_node + = fgGetNode("/sim/hud/antialiased", true); + static const SGPropertyNode *heading_enabled + = fgGetNode("/autopilot/locks/heading", true); + static const SGPropertyNode *altitude_enabled + = fgGetNode("/autopilot/locks/altitude", true); + + static char hud_hdg_text[256]; + static char hud_wp0_text[256]; + static char hud_wp1_text[256]; + static char hud_wp2_text[256]; if( antialiased_node->getBoolValue() ) { glEnable(GL_LINE_SMOOTH); @@ -1164,35 +1173,50 @@ void drawHUD() int apY = 480 - 80; - // char scratch[128]; - // HUD_TextList.add( fgText( "AUTOPILOT", 20, apY) ); - // apY -= 15; - FGAutopilot *AP = globals->get_autopilot(); - if( AP->get_HeadingEnabled() ) { - HUD_TextList.add( fgText( 40, apY, AP->get_TargetHeadingStr()) ); + + if (strcmp( heading_enabled->getStringValue(), "dg-heading-hold") == 0 ) { + snprintf( hud_hdg_text, 256, "hdg = %.1f\n", + fgGetDouble("/autopilot/settings/heading-bug-deg") ); + HUD_TextList.add( fgText( 40, apY, hud_hdg_text ) ); apY -= 15; - } - if( AP->get_AltitudeEnabled() ) { - HUD_TextList.add( fgText( 40, apY, AP->get_TargetAltitudeStr()) ); + } else if ( strcmp(heading_enabled->getStringValue(), "true-heading-hold") == 0 ) { + snprintf( hud_hdg_text, 256, "hdg = %.1f\n", + fgGetDouble("/autopilot/settings/true-heading-deg") ); + HUD_TextList.add( fgText( 40, apY, hud_hdg_text ) ); apY -= 15; - } - if( AP->get_HeadingMode() == FGAutopilot::FG_HEADING_WAYPOINT ) - { - if ( strlen( AP->get_TargetWP1Str() ) ) { - HUD_TextList.add( fgText( 40, apY, AP->get_TargetWP1Str() ) ); + + string wp0_id = fgGetString( "/autopilot/route-manager/wp[0]/id" ); + if ( wp0_id.length() > 0 ) { + snprintf( hud_wp0_text, 256, "%5s %6.1fnm %s", wp0_id.c_str(), + fgGetDouble( "/autopilot/route-manager/wp[0]/dist" ), + fgGetString( "/autopilot/route-manager/wp[0]/eta" ) ); + HUD_TextList.add( fgText( 40, apY, hud_wp0_text ) ); apY -= 15; } - if ( strlen( AP->get_TargetWP2Str() ) ) { - HUD_TextList.add( fgText( 40, apY, AP->get_TargetWP2Str() ) ); + string wp1_id = fgGetString( "/autopilot/route-manager/wp[1]/id" ); + if ( wp1_id.length() > 0 ) { + snprintf( hud_wp1_text, 256, "%5s %6.1fnm %s", wp1_id.c_str(), + fgGetDouble( "/autopilot/route-manager/wp[1]/dist" ), + fgGetString( "/autopilot/route-manager/wp[1]/eta" ) ); + HUD_TextList.add( fgText( 40, apY, hud_wp1_text ) ); apY -= 15; } - if ( strlen( AP->get_TargetWP3Str() ) ) { - HUD_TextList.add( fgText( 40, apY, AP->get_TargetWP3Str() ) ); - apY -= 15; + string wp2_id = fgGetString( "/autopilot/route-manager/wp-last/id" ); + if ( wp2_id.length() > 0 ) { + snprintf( hud_wp2_text, 256, "%5s %6.1fnm %s", wp2_id.c_str(), + fgGetDouble( "/autopilot/route-manager/wp-last/dist" ), + fgGetString( "/autopilot/route-manager/wp-last/eta" ) ); + HUD_TextList.add( fgText( 40, apY, hud_wp2_text ) ); + apY -= 15; } } + if ( strcmp( altitude_enabled->getStringValue(), "altitude-hold" ) == 0 ) { + HUD_TextList.add( fgText( 40, apY, (char *)fgGetString("/autopilot/settings/altitude-ft") ) ); + apY -= 15; + } + HUD_TextList.draw(); HUD_LineList.draw(); diff --git a/src/Cockpit/navcom.cxx b/src/Cockpit/navcom.cxx index a67f67336..4c45eff8f 100644 --- a/src/Cockpit/navcom.cxx +++ b/src/Cockpit/navcom.cxx @@ -28,6 +28,7 @@ #include // snprintf #include +#include #include #include @@ -61,8 +62,12 @@ FGNavCom::FGNavCom() : nav_heading(0.0), nav_radial(0.0), nav_target_radial(0.0), + nav_target_radial_true(0.0), + nav_target_auto_hdg(0.0), nav_vol_btn(0.0), - nav_ident_btn(true) + nav_ident_btn(true), + horiz_vel(0.0), + last_x(0.0) { SGPath path( globals->get_fg_root() ); SGPath term = path; @@ -187,6 +192,13 @@ FGNavCom::bind () snprintf(propname, 256, "/radios/nav[%d]/radials/actual-deg", index); fgTie( propname, this, &FGNavCom::get_nav_radial ); + snprintf(propname, 256, "/radios/nav[%d]/radials/target-radial-deg", index); + fgTie( propname, this, &FGNavCom::get_nav_target_radial_true ); + + snprintf(propname, 256, "/radios/nav[%d]/radials/target-auto-hdg-deg", + index); + fgTie( propname, this, &FGNavCom::get_nav_target_auto_hdg ); + snprintf(propname, 256, "/radios/nav[%d]/to-flag", index); fgTie( propname, this, &FGNavCom::get_nav_to_flag ); @@ -205,6 +217,9 @@ FGNavCom::bind () snprintf(propname, 256, "/radios/nav[%d]/nav-loc", index); fgTie( propname, this, &FGNavCom::get_nav_loc ); + snprintf(propname, 256, "/radios/nav[%d]/gs-rate-of-climb", index); + fgTie( propname, this, &FGNavCom::get_nav_gs_rate_of_climb ); + snprintf(propname, 256, "/radios/nav[%d]/gs-needle-deflection", index); fgTie( propname, this, &FGNavCom::get_nav_gs_deflection ); @@ -425,6 +440,92 @@ FGNavCom::update(double dt) if ( !nav_loc ) { nav_target_radial = nav_sel_radial; } + + // Calculate some values for the nav/ils hold autopilot + + double cur_radial = get_nav_reciprocal_radial(); + if ( nav_loc ) { + // ILS localizers radials are already "true" in our + // database + } else { + cur_radial += nav_twist; + } + if ( get_nav_from_flag() ) { + cur_radial += 180.0; + while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; } + } + + // AUTOPILOT HELPERS + + // determine the target radial in "true" heading + nav_target_radial_true = nav_target_radial; + if ( nav_loc ) { + // ILS localizers radials are already "true" in our + // database + } else { + // VOR radials need to have that vor's offset added in + nav_target_radial_true += nav_twist; + } + + while ( nav_target_radial_true < 0.0 ) { + nav_target_radial_true += 360.0; + } + while ( nav_target_radial_true > 360.0 ) { + nav_target_radial_true -= 360.0; + } + + // 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); + } +#endif + + // determine the target heading to fly to intercept the + // tgt_radial + nav_target_auto_hdg = nav_target_radial_true + adjustment; + while ( nav_target_auto_hdg < 0.0 ) { nav_target_auto_hdg += 360.0; } + while ( nav_target_auto_hdg > 360.0 ) { nav_target_auto_hdg -= 360.0; } + + // cross track error + // ???? + + // Calculate desired rate of climb for intercepting the GS + double x = nav_gs_dist; + double y = (alt_node->getDoubleValue() - nav_elev) + * SG_FEET_TO_METER; + double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES; + + double target_angle = nav_target_gs; + double gs_diff = target_angle - current_angle; + + // convert desired vertical path angle into a climb rate + double des_angle = current_angle - 10 * gs_diff; + + // estimate horizontal speed towards ILS in meters per minute + double dist = last_x - x; + last_x = x; + 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; } else { nav_inrange = false; // cout << "not picking up vor. :-(" << endl; diff --git a/src/Cockpit/navcom.hxx b/src/Cockpit/navcom.hxx index 5d18a118e..8dfdb96e8 100644 --- a/src/Cockpit/navcom.hxx +++ b/src/Cockpit/navcom.hxx @@ -96,6 +96,8 @@ class FGNavCom : public SGSubsystem // exactly.) double nav_sel_radial; double nav_target_radial; + double nav_target_radial_true; + double nav_target_auto_hdg; double nav_loclon; double nav_loclat; double nav_x; @@ -110,6 +112,7 @@ class FGNavCom : public SGSubsystem sgdVec3 gs_base_vec; double nav_gs_dist; double nav_gs_dist_signed; + double nav_gs_rate_of_climb; SGTimeStamp prev_time; SGTimeStamp curr_time; double nav_elev; @@ -119,6 +122,8 @@ class FGNavCom : public SGSubsystem double nav_twist; double nav_vol_btn; bool nav_ident_btn; + double horiz_vel; + double last_x; // model standard VOR/DME/TACAN service volumes as per AIM 1-1-8 double adjustNavRange( double stationElev, double aircraftElev, @@ -194,6 +199,12 @@ public: inline double get_nav_alt_freq () const { return nav_alt_freq; } inline double get_nav_sel_radial() const { return nav_sel_radial; } inline double get_nav_target_radial() const { return nav_target_radial; } + inline double get_nav_target_radial_true() const { + return nav_target_radial_true; + } + inline double get_nav_target_auto_hdg() const { + return nav_target_auto_hdg; + } // Calculated values. inline bool get_comm_inrange() const { return comm_inrange; } @@ -216,6 +227,9 @@ public: inline double get_nav_gslat() const { return nav_gslat; } inline double get_nav_gs_dist() const { return nav_gs_dist; } inline double get_nav_gs_dist_signed() const { return nav_gs_dist_signed; } + inline double get_nav_gs_rate_of_climb() const { + return nav_gs_rate_of_climb; + } inline double get_nav_elev() const { return nav_elev; } double get_nav_heading() const; double get_nav_radial() const; diff --git a/src/GUI/gui_funcs.cxx b/src/GUI/gui_funcs.cxx index 46a4cdf0e..9882e84c3 100644 --- a/src/GUI/gui_funcs.cxx +++ b/src/GUI/gui_funcs.cxx @@ -69,7 +69,6 @@ #include #include #include -#include #include #include #include diff --git a/src/GUI/menubar.cxx b/src/GUI/menubar.cxx index 82724b2bc..8be70c1a2 100644 --- a/src/GUI/menubar.cxx +++ b/src/GUI/menubar.cxx @@ -3,11 +3,11 @@ #include #include +#include +#include #include
#include
-#include - #include "new_gui.hxx" #include "menubar.hxx" diff --git a/src/GUI/mouse.cxx b/src/GUI/mouse.cxx index b4b612ac9..27899a0d9 100644 --- a/src/GUI/mouse.cxx +++ b/src/GUI/mouse.cxx @@ -55,11 +55,9 @@ #include #include -//#include #include #include -//#include -#include +#include #include #include #include @@ -231,11 +229,15 @@ static inline float get_elevator() { } static inline bool AP_HeadingEnabled() { - return globals->get_autopilot()->get_HeadingEnabled(); + static const SGPropertyNode *heading_enabled + = fgGetNode("/autopilot/locks/heading"); + return ( strcmp( heading_enabled->getStringValue(), "" ) != 0 ); } static inline bool AP_AltitudeEnabled() { - return globals->get_autopilot()->get_AltitudeEnabled(); + static const SGPropertyNode *altitude_enabled + = fgGetNode("/autopilot/locks/altitude"); + return ( strcmp( altitude_enabled->getStringValue(), "" ) != 0 ); } void TurnCursorOn( void ) diff --git a/src/Input/input.cxx b/src/Input/input.cxx index 1527373e5..9c52389d9 100644 --- a/src/Input/input.cxx +++ b/src/Input/input.cxx @@ -48,8 +48,8 @@ #include #include -#include -#include +// #include +#include #include #include #include @@ -223,6 +223,9 @@ FGInput::makeDefault (bool status) void FGInput::doKey (int k, int modifiers, int x, int y) { + static SGPropertyNode *heading_enabled + = fgGetNode("/autopilot/locks/heading", true); + // Sanity check. if (k < 0 || k >= MAX_KEYS) { SG_LOG(SG_INPUT, SG_WARN, "Key value " << k << " out of range"); @@ -297,14 +300,11 @@ FGInput::doKey (int k, int modifiers, int x, int y) // START SPECIALS case 256+GLUT_KEY_F6: // F6 toggles Autopilot target location - if ( globals->get_autopilot()->get_HeadingMode() != - FGAutopilot::FG_HEADING_WAYPOINT ) { - globals->get_autopilot()->set_HeadingMode( - FGAutopilot::FG_HEADING_WAYPOINT ); - globals->get_autopilot()->set_HeadingEnabled( true ); + if ( strcmp( heading_enabled->getStringValue(), + "true-heading-hold" ) != 0 ) { + heading_enabled->setStringValue( "true-heading-hold" ); } else { - globals->get_autopilot()->set_HeadingMode( - FGAutopilot::FG_TC_HEADING_LOCK ); + heading_enabled->setStringValue( "" ); } return; } diff --git a/src/Main/Makefile.am b/src/Main/Makefile.am index c55d49418..c244aaf0f 100644 --- a/src/Main/Makefile.am +++ b/src/Main/Makefile.am @@ -54,7 +54,6 @@ fgfs_LDADD = \ $(top_builddir)/src/Main/libMain.a \ $(top_builddir)/src/Aircraft/libAircraft.a \ $(top_builddir)/src/ATC/libATC.a \ - $(top_builddir)/src/Autopilot/libAutopilot.a \ $(top_builddir)/src/Cockpit/libCockpit.a \ $(top_builddir)/src/Cockpit/built_in/libBuilt_in.a \ $(top_builddir)/src/Controls/libControls.a \ @@ -68,6 +67,7 @@ fgfs_LDADD = \ $(top_builddir)/src/FDM/LaRCsim/libLaRCsim.a \ $(top_builddir)/src/FDM/UIUCModel/libUIUCModel.a \ $(top_builddir)/src/GUI/libGUI.a \ + $(top_builddir)/src/Autopilot/libAutopilot.a \ $(top_builddir)/src/Input/libInput.a \ $(top_builddir)/src/Instrumentation/libInstrumentation.a \ $(top_builddir)/src/Model/libModel.a \ diff --git a/src/Main/fg_commands.cxx b/src/Main/fg_commands.cxx index 69edb7d0c..7685a369b 100644 --- a/src/Main/fg_commands.cxx +++ b/src/Main/fg_commands.cxx @@ -479,6 +479,43 @@ do_tile_cache_reload (const SGPropertyNode * arg) } +/** + * Set the sea level outside air temperature and assigning that to all + * boundary and aloft environment layers. + */ +static bool +do_set_sea_level_degc (const SGPropertyNode * arg) +{ + double temp_sea_level_degc = arg->getDoubleValue("temp-degc", 15.0); + + SGPropertyNode *node, *child; + + // boundary layers + node = fgGetNode( "/environment/config/boundary" ); + if ( node != NULL ) { + int i = 0; + while ( ( child = node->getNode( "entry", i ) ) != NULL ) { + child->setDoubleValue( "temperature-sea-level-degc", + temp_sea_level_degc ); + ++i; + } + } + + // aloft layers + node = fgGetNode( "/environment/config/aloft" ); + if ( node != NULL ) { + int i = 0; + while ( ( child = node->getNode( "entry", i ) ) != NULL ) { + child->setDoubleValue( "temperature-sea-level-degc", + temp_sea_level_degc ); + ++i; + } + } + + return true; +} + + /** * Set the outside air temperature at the "current" altitude by first * calculating the corresponding sea level temp, and assigning that to @@ -1079,6 +1116,7 @@ static struct { { "view-cycle", do_view_cycle }, { "screen-capture", do_screen_capture }, { "tile-cache-reload", do_tile_cache_reload }, + { "set-sea-level-air-temp-degc", do_set_sea_level_degc }, { "set-outside-air-temp-degc", do_set_oat_degc }, { "timeofday", do_timeofday }, { "property-toggle", do_property_toggle }, diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index 6952bb933..cff3d0147 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -76,7 +76,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -1504,13 +1505,19 @@ bool fgInitSubsystems() { fgAircraftInit(); // In the future this might not be the case. + //////////////////////////////////////////////////////////////////// + // Initialize the XML Autopilot subsystem. + //////////////////////////////////////////////////////////////////// + + globals->add_subsystem( "xml-autopilot", new FGXMLAutopilot ); + globals->add_subsystem( "route-manager", new FGRouteMgr ); + //////////////////////////////////////////////////////////////////// // Initialize the view manager subsystem. //////////////////////////////////////////////////////////////////// fgInitView(); - //////////////////////////////////////////////////////////////////// // Create and register the logger. //////////////////////////////////////////////////////////////////// @@ -1703,13 +1710,9 @@ bool fgInitSubsystems() { // Initialize the autopilot subsystem. //////////////////////////////////////////////////////////////////// - globals->set_autopilot(new FGAutopilot); - globals->get_autopilot()->init(); - globals->get_autopilot()->bind(); - // FIXME: these should go in the // GUI initialization code, not here. - fgAPAdjustInit(); + // fgAPAdjustInit(); NewTgtAirportInit(); NewHeadingInit(); NewAltitudeInit(); @@ -1842,7 +1845,6 @@ void fgReInitSubsystems() fgInitView(); globals->get_controls()->reset_all(); - globals->get_autopilot()->reset(); fgUpdateLocalTime(); diff --git a/src/Main/globals.cxx b/src/Main/globals.cxx index 204c29890..73d06b23e 100644 --- a/src/Main/globals.cxx +++ b/src/Main/globals.cxx @@ -54,8 +54,7 @@ FGGlobals::FGGlobals() : time_params( NULL ), ephem( NULL ), mag( NULL ), - autopilot( NULL ), - route( NULL ), + route_mgr( NULL ), current_panel( NULL ), soundmgr( NULL ), airports( NULL ), diff --git a/src/Main/globals.hxx b/src/Main/globals.hxx index 9e91b443b..a650f83b5 100644 --- a/src/Main/globals.hxx +++ b/src/Main/globals.hxx @@ -59,7 +59,6 @@ class SGMagVar; class SGMaterialLib; class SGModelLib; class SGPropertyNode; -class SGRoute; class SGTime; class SGSoundMgr; @@ -69,11 +68,11 @@ class FGAIMgr; class FGATCMgr; class FGATCDisplay; class FGAircraftModel; -class FGAutopilot; class FGControls; class FGIO; class FGLight; class FGModelMgr; +class FGRouteMgr; class FGScenery; #ifdef FG_MPLAYER_AS class FGMultiplayRxMgr; @@ -131,11 +130,8 @@ private: // Material properties library SGMaterialLib *matlib; - // Current autopilot - FGAutopilot *autopilot; - // Global autopilot "route" - SGRoute *route; + FGRouteMgr *route_mgr; // 2D panel FGPanel *current_panel; @@ -252,12 +248,6 @@ public: inline SGMaterialLib *get_matlib() const { return matlib; } inline void set_matlib( SGMaterialLib *m ) { matlib = m; } - inline FGAutopilot *get_autopilot() const { return autopilot; } - inline void set_autopilot( FGAutopilot *ap) { autopilot = ap; } - - inline SGRoute *get_route() const { return route; } - inline void set_route( SGRoute *r ) { route = r; } - inline FGAirportList *get_airports() const { return airports; } inline void set_airports( FGAirportList *a ) {airports = a; } diff --git a/src/Main/main.cxx b/src/Main/main.cxx index 5c7d7c94c..502207bf2 100644 --- a/src/Main/main.cxx +++ b/src/Main/main.cxx @@ -54,7 +54,6 @@ #include #include #include -#include #include #ifdef FG_USE_CLOUDS_3D @@ -82,7 +81,6 @@ #include #include #include -#include #include #include