]> git.mxchange.org Git - flightgear.git/blobdiff - src/Autopilot/newauto.cxx
Moved some of the low level scene graph construction code over to simgear.
[flightgear.git] / src / Autopilot / newauto.cxx
index f99c4412f23c8539dab88498ab32cafa6a131494..6762c0a30d8e5ef987cedd0d4be4f193d4211e07 100644 (file)
@@ -5,6 +5,7 @@
 // Contributions by Jeff Goeke-Smith <jgoeke@voyager.net>
 //                  Norman Vine <nhv@cape.com>
 //                  Curtis Olson <curt@flightgear.org>
+//                  Wendell Turner <wendell@adsi-m4.com>
 //
 // This program is free software; you can redistribute it and/or
 // modify it under the terms of the GNU General Public License as
@@ -37,7 +38,6 @@
 #include <simgear/math/sg_random.h>
 #include <simgear/route/route.hxx>
 
-#include <Cockpit/steam.hxx>
 #include <Cockpit/radiostack.hxx>
 #include <Controls/controls.hxx>
 #include <FDM/flight.hxx>
@@ -45,6 +45,7 @@
 #include <Scenery/scenery.hxx>
 
 #include "newauto.hxx"
+#include "auto_gui.hxx"
 
 
 /// These statics will eventually go into the class
@@ -213,6 +214,9 @@ void FGAutopilot::init ()
     altitude_agl_node = fgGetNode("/position/altitude-agl-ft", true);
     vertical_speed_node = fgGetNode("/velocities/vertical-speed-fps", true);
     heading_node = fgGetNode("/orientation/heading-deg", true);
+    dg_heading_node
+        = fgGetNode("/instrumentation/heading-indicator/indicated-heading-deg",
+                    true);
     roll_node = fgGetNode("/orientation/roll-deg", true);
     pitch_node = fgGetNode("/orientation/pitch-deg", true);
 
@@ -238,7 +242,7 @@ void FGAutopilot::init ()
     roll_out_node = fgGetNode("/autopilot/config/roll-out-deg", true);
     roll_out_smooth_node = fgGetNode("/autopilot/config/roll-out-smooth-deg", true);
 
-    current_throttle = fgGetNode("/controls/throttle");
+    current_throttle = fgGetNode("/controls/engines/engine/throttle");
 
     // initialize config properties with defaults (in case config isn't there)
     if ( TargetClimbRate->getFloatValue() < 1 )
@@ -341,10 +345,17 @@ FGAutopilot::bind ()
     fgTie("/autopilot/locks/heading", this,
          &FGAutopilot::getAPHeadingLock, &FGAutopilot::setAPHeadingLock);
     fgSetArchivable("/autopilot/locks/heading");
+
     fgTie("/autopilot/settings/heading-bug-deg", this,
          &FGAutopilot::getAPHeadingBug, &FGAutopilot::setAPHeadingBug);
     fgSetArchivable("/autopilot/settings/heading-bug-deg");
     fgSetDouble("/autopilot/settings/heading-bug-deg", 0.0f);
+
+    fgTie("/autopilot/settings/waypoint", this,
+         &FGAutopilot::getAPwaypoint, &FGAutopilot::setAPwaypoint);
+    fgSetArchivable("/autopilot/settings/waypoint");
+    fgSetString("/autopilot/settings/waypoint", "");
+
     fgTie("/autopilot/locks/wing-leveler", this,
          &FGAutopilot::getAPWingLeveler, &FGAutopilot::setAPWingLeveler);
     fgSetArchivable("/autopilot/locks/wing-leveler");
@@ -393,7 +404,7 @@ void FGAutopilot::reset() {
        
     update_old_control_values();
 
-    sprintf( NewTgtAirportId, "%s", fgGetString("/sim/startup/airport-id") );
+    sprintf( NewTgtAirportId, "%s", fgGetString("/sim/presets/airport-id") );
        
     MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
 }
@@ -484,8 +495,10 @@ FGAutopilot::update (double dt)
     // heading hold
     if ( heading_hold == true ) {
        if ( heading_mode == FG_DG_HEADING_LOCK ) {
-           TargetHeading = DGTargetHeading +
-             globals->get_steam()->get_DG_err();
+            double dg_error = heading_node->getDoubleValue()
+                - dg_heading_node->getDoubleValue();
+           TargetHeading = DGTargetHeading + dg_error;
+            // cout << "dg_error = " << dg_error << endl;
            while ( TargetHeading <   0.0 ) { TargetHeading += 360.0; }
            while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
            MakeTargetHeadingStr( TargetHeading );
@@ -503,37 +516,37 @@ FGAutopilot::update (double dt)
 
            // determine our current radial position relative to the
            // navaid in "true" heading.
-           double cur_radial = current_radiostack->get_nav1_heading();
-           if ( current_radiostack->get_nav1_loc() ) {
+           double cur_radial = current_radiostack->get_navcom1()->get_nav_heading();
+           if ( current_radiostack->get_navcom1()->get_nav_loc() ) {
                // ILS localizers radials are already "true" in our
                // database
            } else {
-               cur_radial += current_radiostack->get_nav1_magvar();
+               cur_radial += current_radiostack->get_navcom1()->get_nav_magvar();
            }
-           if ( current_radiostack->get_nav1_from_flag() ) {
+           if ( current_radiostack->get_navcom1()->get_nav_from_flag() ) {
                cur_radial += 180.0;
                while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
            }
 
            // determine the target radial in "true" heading
-           double tgt_radial = current_radiostack->get_nav1_radial();
-           if ( current_radiostack->get_nav1_loc() ) {
+           double tgt_radial = current_radiostack->get_navcom1()->get_nav_radial();
+           if ( current_radiostack->get_navcom1()->get_nav_loc() ) {
                // ILS localizers radials are already "true" in our
                // database
            } else {
                // VOR radials need to have that vor's offset added in
-               tgt_radial += current_radiostack->get_nav1_magvar();
+               tgt_radial += current_radiostack->get_navcom1()->get_nav_magvar();
            }
 
            // determine the heading adjustment needed.
            double adjustment = 
-               current_radiostack->get_nav1_heading_needle_deflection()
-               * (current_radiostack->get_nav1_loc_dist() * SG_METER_TO_NM);
+               current_radiostack->get_navcom1()->get_nav_heading_needle_deflection()
+               * (current_radiostack->get_navcom1()->get_nav_loc_dist() * SG_METER_TO_NM);
            SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
 
             // clamp closer when inside cone when beyond 5km...
-            if (current_radiostack->get_nav1_loc_dist() > 5000) {
-              double clamp_angle = fabs(current_radiostack->get_nav1_heading_needle_deflection()) * 3;
+            if (current_radiostack->get_navcom1()->get_nav_loc_dist() > 5000) {
+              double clamp_angle = fabs(current_radiostack->get_navcom1()->get_nav_heading_needle_deflection()) * 3;
               if (clamp_angle < 30)
                 SG_CLAMP_RANGE( adjustment, -clamp_angle, clamp_angle);
             }
@@ -594,7 +607,7 @@ FGAutopilot::update (double dt)
                    set_HeadingMode( FG_HEADING_WAYPOINT );
                } else {
                    // end of the line
-                   heading_mode = FG_TRUE_HEADING_LOCK;
+                   heading_mode = DEFAULT_AP_HEADING_LOCK;
                    // use current heading
                    TargetHeading = heading_node->getDoubleValue();
                }
@@ -607,7 +620,8 @@ FGAutopilot::update (double dt)
 
        if ( heading_mode == FG_TC_HEADING_LOCK ) {
            // drive the turn coordinator to zero
-           double turn = globals->get_steam()->get_TC_std();
+           double turn =
+                fgGetDouble("/instrumentation/turn-indicator/indicated-turn-rate");
            double AileronSet = -turn / 2.0;
             SG_CLAMP_RANGE( AileronSet, -1.0, 1.0 );
            globals->get_controls()->set_aileron( AileronSet );
@@ -694,14 +708,16 @@ FGAutopilot::update (double dt)
        if ( altitude_mode == FG_ALTITUDE_LOCK ) {
            climb_rate =
                ( TargetAltitude -
-                 globals->get_steam()->get_ALT_ft() * SG_FEET_TO_METER ) * 8.0;
+                 fgGetDouble("/instrumentation/altimeter/indicated-altitude-ft") * SG_FEET_TO_METER ) * 8.0;
+        } else if ( altitude_mode == FG_TRUE_ALTITUDE_LOCK ) {
+            climb_rate = ( TargetAltitude - alt ) * 8.0;
        } else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
-           double x = current_radiostack->get_nav1_gs_dist();
+           double x = current_radiostack->get_navcom1()->get_nav_gs_dist();
            double y = (altitude_node->getDoubleValue()
-                       - current_radiostack->get_nav1_elev()) * SG_FEET_TO_METER;
+                       - current_radiostack->get_navcom1()->get_nav_elev()) * SG_FEET_TO_METER;
            double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
 
-           double target_angle = current_radiostack->get_nav1_target_gs();
+           double target_angle = current_radiostack->get_navcom1()->get_nav_target_gs();
 
            double gs_diff = target_angle - current_angle;
 
@@ -885,10 +901,7 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
     heading_mode = mode;
 
     if ( heading_mode == FG_DG_HEADING_LOCK ) {
-       // set heading hold to current heading (as read from DG)
-       // ... no, leave target heading along ... just use the current
-       // heading bug value
-        //  DGTargetHeading = FGSteam::get_DG_deg();
+       // use current heading bug value
     } else if ( heading_mode == FG_TC_HEADING_LOCK ) {
        // set autopilot to hold a zero turn (as reported by the TC)
     } else if ( heading_mode == FG_TRUE_HEADING_LOCK ) {
@@ -915,7 +928,7 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
 
            if ( waypoint.get_target_alt() > 0.0 ) {
                TargetAltitude = waypoint.get_target_alt();
-               altitude_mode = FG_ALTITUDE_LOCK;
+               altitude_mode = DEFAULT_AP_ALTITUDE_LOCK;
                set_AltitudeEnabled( true );
                MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
            }
@@ -941,7 +954,7 @@ void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
     alt_error_accum = 0.0;
 
 
-    if ( altitude_mode == FG_ALTITUDE_LOCK ) {
+    if ( altitude_mode == DEFAULT_AP_ALTITUDE_LOCK ) {
        if ( TargetAltitude < altitude_agl_node->getDoubleValue()
              * SG_FEET_TO_METER ) {
        }
@@ -971,6 +984,7 @@ void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
 
 void FGAutopilot::AltitudeSet( double new_altitude ) {
     double target_alt = new_altitude;
+    altitude_mode = DEFAULT_AP_ALTITUDE_LOCK;
 
     if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
        target_alt = new_altitude * SG_FEET_TO_METER;
@@ -981,7 +995,6 @@ void FGAutopilot::AltitudeSet( double new_altitude ) {
     }
 
     TargetAltitude = target_alt;
-    altitude_mode = FG_ALTITUDE_LOCK;
 
     if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
        target_alt *= SG_METER_TO_FEET;
@@ -1030,7 +1043,7 @@ void FGAutopilot::AltitudeAdjust( double inc )
     if ( !strcmp(fgGetString("/sim/startup/units"), "feet") )
        target_agl *= SG_METER_TO_FEET;
 
-    if ( altitude_mode == FG_ALTITUDE_LOCK ) {
+    if ( altitude_mode == DEFAULT_AP_ALTITUDE_LOCK ) {
        MakeTargetAltitudeStr( target_alt );
     } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
        MakeTargetAltitudeStr( target_agl );
@@ -1044,7 +1057,7 @@ void FGAutopilot::HeadingAdjust( double inc ) {
     if ( heading_mode != FG_DG_HEADING_LOCK
         && heading_mode != FG_TRUE_HEADING_LOCK )
     {
-       heading_mode = FG_DG_HEADING_LOCK;
+       heading_mode = DEFAULT_AP_HEADING_LOCK;
     }
 
     if ( heading_mode == FG_DG_HEADING_LOCK ) {
@@ -1060,6 +1073,7 @@ void FGAutopilot::HeadingAdjust( double inc ) {
 
 
 void FGAutopilot::HeadingSet( double new_heading ) {
+    heading_mode = DEFAULT_AP_HEADING_LOCK;
     if( heading_mode == FG_TRUE_HEADING_LOCK ) {
         new_heading = NormalizeDegrees( new_heading );
         TargetHeading = new_heading;
@@ -1114,7 +1128,7 @@ FGAutopilot::getAPAltitudeLock () const
 {
     return (get_AltitudeEnabled() &&
            get_AltitudeMode()
-           == FGAutopilot::FG_ALTITUDE_LOCK);
+           == DEFAULT_AP_ALTITUDE_LOCK);
 }
 
 
@@ -1125,8 +1139,8 @@ void
 FGAutopilot::setAPAltitudeLock (bool lock)
 {
   if (lock)
-    set_AltitudeMode(FGAutopilot::FG_ALTITUDE_LOCK);
-  if (get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_LOCK)
+    set_AltitudeMode(DEFAULT_AP_ALTITUDE_LOCK);
+  if (get_AltitudeMode() == DEFAULT_AP_ALTITUDE_LOCK)
     set_AltitudeEnabled(lock);
 }
 
@@ -1268,6 +1282,54 @@ FGAutopilot::setAPHeadingBug (double heading)
 }
 
 
+/**
+ * return blank-separated string of waypoints
+ */
+const char *
+FGAutopilot::getAPwaypoint () const
+{
+  static char wplist[500];
+  char *p = wplist;
+  int   WPListsize, i;
+
+  // FIXME: This can cause a possible buffer overflow, EMH
+  if ( globals->get_route()->size() > 0 ) { 
+      WPListsize = globals->get_route()->size(); 
+
+      for (i = 0; i < globals->get_route()->size(); i++ ) {
+         p += sprintf(p, "%5s ",
+               globals->get_route()->get_waypoint(i).get_id().c_str() );
+      }
+      return wplist;
+
+  } else {
+    return "none specified";
+  }    
+}
+
+
+/**
+ * set next waypoint (if str='0', then delete next(first) waypoint)
+ */
+void
+FGAutopilot::setAPwaypoint (const char * apt)
+{
+  if (strcmp(apt, "0")==0)
+  {
+    SG_LOG( SG_AUTOPILOT, SG_INFO, "delete of first wp" );
+    if ( globals->get_route()->size() )
+                   globals->get_route()->delete_first();
+    return;
+  }
+
+  if ( NewWaypoint( apt ) == 0)
+    SG_LOG( SG_AUTOPILOT, SG_INFO, "Waypoint " << apt <<  "not in d.b."  );
+  else
+  {
+    /* SG_LOG( SG_AUTOPILOT, SG_INFO, "GOOD!" ); */
+  }
+}
+
 /**
  * Get the autopilot wing leveler lock (true=on).
  */