From 921beb96d748313471d461b154a29b7759bf6064 Mon Sep 17 00:00:00 2001 From: curt Date: Sun, 30 Apr 2000 22:21:47 +0000 Subject: [PATCH] First stab at NAV1 and GS hold modes for the autopilot. --- src/Autopilot/newauto.cxx | 116 +++++++++++++++++++++---------------- src/Autopilot/newauto.hxx | 7 ++- src/Cockpit/panel.cxx | 24 ++++---- src/Cockpit/radiostack.cxx | 13 +++-- src/Cockpit/radiostack.hxx | 16 ++--- src/Cockpit/steam.cxx | 22 +------ src/Main/bfi.cxx | 20 ++----- src/Main/bfi.hxx | 6 +- src/Main/fg_init.cxx | 4 +- src/Main/keyboard.cxx | 14 +++++ 10 files changed, 120 insertions(+), 122 deletions(-) diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx index ef308fc48..64a243bd5 100644 --- a/src/Autopilot/newauto.cxx +++ b/src/Autopilot/newauto.cxx @@ -33,6 +33,7 @@ #include #include +#include #include #include #include
@@ -58,18 +59,6 @@ static double RollOutAdjust; // RollOutAdjust = 2 * APData->RollOut static double MaxAileronAdjust; // MaxAileronAdjust = 2 * APData->MaxAileron; static double RollOutSmoothAdjust; // RollOutSmoothAdjust = 2 * APData->RollOutSmooth; -#if 0 -static float MaxRollValue; // 0.1 -> 1.0 -static float RollOutValue; -static float MaxAileronValue; -static float RollOutSmoothValue; - -static float TmpMaxRollValue; // for cancel operation -static float TmpRollOutValue; -static float TmpMaxAileronValue; -static float TmpRollOutSmoothValue; -#endif - static char NewTgtAirportId[16]; // static char NewTgtAirportLabel[] = "Enter New TgtAirport ID"; @@ -103,6 +92,41 @@ void FGAutopilot::MakeTargetHeadingStr( double bearing ) { } +static inline double get_speed( void ) { + return( cur_fdm_state->get_V_equiv_kts() ); +} + +static inline double get_ground_speed() { + // starts in ft/s so we convert to kts + double ft_s = cur_fdm_state->get_V_ground_speed() + * current_options.get_speed_up();; + double kts = ft_s * FEET_TO_METER * 3600 * METER_TO_NM; + + return kts; +} + + +void FGAutopilot::MakeTargetDistanceStr( double distance ) { + double eta = distance*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); + sprintf( TargetDistanceStr, "APDistance %.2f NM ETA %d:%02d", + distance*METER_TO_NM, major, minor ); + // cout << "distance = " << distance*METER_TO_NM + // << " gndsp = " << get_ground_speed() + // << " time = " << eta + // << " major = " << major + // << " minor = " << minor + // << endl; +} + + void FGAutopilot::update_old_control_values() { old_aileron = FGBFI::getAileron(); old_elevator = FGBFI::getElevator(); @@ -128,6 +152,7 @@ void FGAutopilot::init() { TargetHeading = 0.0; // default direction, due north TargetAltitude = 3000; // default altitude in meters alt_error_accum = 0.0; + climb_error_accum = 0.0; MakeTargetAltitudeStr( 3000.0); MakeTargetHeadingStr( 0.0 ); @@ -181,6 +206,7 @@ void FGAutopilot::reset() { MakeTargetAltitudeStr( TargetAltitude ); alt_error_accum = 0.0; + climb_error_accum = 0.0; update_old_control_values(); @@ -192,41 +218,6 @@ void FGAutopilot::reset() { } -static inline double get_speed( void ) { - return( cur_fdm_state->get_V_equiv_kts() ); -} - -static inline double get_ground_speed() { - // starts in ft/s so we convert to kts - double ft_s = cur_fdm_state->get_V_ground_speed() - * current_options.get_speed_up();; - double kts = ft_s * FEET_TO_METER * 3600 * METER_TO_NM; - - return kts; -} - - -void FGAutopilot::MakeTargetDistanceStr( double distance ) { - double eta = distance*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); - sprintf( TargetDistanceStr, "APDistance %.2f NM ETA %d:%02d", - distance*METER_TO_NM, major, minor ); - // cout << "distance = " << distance*METER_TO_NM - // << " gndsp = " << get_ground_speed() - // << " time = " << eta - // << " major = " << major - // << " minor = " << minor - // << endl; -} - - static double NormalizeDegrees( double Input ) { // normalize the input to the range (-180,180] // Input should not be greater than -360 to 360. @@ -309,6 +300,22 @@ int FGAutopilot::run() { if ( heading_mode == FG_HEADING_LOCK ) { // leave target heading alone + } else if ( heading_mode == FG_HEADING_NAV1 ) { + double tgt_radial = current_radiostack->get_nav1_radial(); + double cur_radial = current_radiostack->get_nav1_heading() + 180.0; + // cout << "target rad = " << tgt_radial + // << " current rad = " << cur_radial + // << endl; + + double diff = (tgt_radial - cur_radial) + * (current_radiostack->get_nav1_dist() * METER_TO_NM); + if ( diff < -30.0 ) { diff = -30.0; } + if ( diff > 30.0 ) { diff = 30.0; } + + TargetHeading = cur_radial - diff; + while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; } + while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; } + // cout << "target course = " << TargetHeading << endl; } else if ( heading_mode == FG_HEADING_WAYPOINT ) { // update target heading to waypoint @@ -441,6 +448,14 @@ int FGAutopilot::run() { // << endl; TargetClimbRate = ( TargetAltitude - FGBFI::getAltitude() * FEET_TO_METER ) * 8.0; + } else if ( altitude_mode == FG_ALTITUDE_GS1 ) { + double x = current_radiostack->get_nav1_dist(); + double y = (FGBFI::getAltitude() + - current_radiostack->get_nav1_elev()) * FEET_TO_METER; + double angle = atan2( y, x ) * RAD_TO_DEG; + double gs_diff = current_radiostack->get_nav1_target_gs() - angle; + climb_error_accum += gs_diff * 2.0; + TargetClimbRate = gs_diff * 200.0 + climb_error_accum; } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) { // brain dead ground hugging with no look ahead TargetClimbRate = @@ -636,19 +651,22 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) { void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) { altitude_mode = mode; + alt_error_accum = 0.0; + if ( altitude_mode == FG_ALTITUDE_LOCK ) { // lock at current altitude TargetAltitude = FGBFI::getAltitude() * FEET_TO_METER; - alt_error_accum = 0.0; if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) { MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET ); } else { MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET ); } + } else if ( altitude_mode == FG_ALTITUDE_GS1 ) { + climb_error_accum = 0.0; + } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) { TargetAGL = FGBFI::getAGL() * FEET_TO_METER; - alt_error_accum = 0.0; if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) { MakeTargetAltitudeStr( TargetAGL * METER_TO_FEET ); diff --git a/src/Autopilot/newauto.hxx b/src/Autopilot/newauto.hxx index 9a2db9f41..334e2b26c 100644 --- a/src/Autopilot/newauto.hxx +++ b/src/Autopilot/newauto.hxx @@ -34,9 +34,9 @@ public: enum fgAutoHeadingMode { FG_HEADING_LOCK = 0, - FG_HEADING_WAYPOINT = 1, - FG_HEADING_NAV1 = 2, - FG_HEADING_NAV2 = 3 + FG_HEADING_NAV1 = 1, + FG_HEADING_NAV2 = 2, + FG_HEADING_WAYPOINT = 3 }; enum fgAutoAltitudeMode { @@ -64,6 +64,7 @@ private: double TargetClimbRate; // climb rate to shoot for double TargetSpeed; // speed to shoot for double alt_error_accum; // altitude error accumulator + double climb_error_accum; // climb error accumulator (for GS) double speed_error_accum; // speed error accumulator double TargetSlope; // the glide slope hold value diff --git a/src/Cockpit/panel.cxx b/src/Cockpit/panel.cxx index eb9b478f7..7e0ac7564 100644 --- a/src/Cockpit/panel.cxx +++ b/src/Cockpit/panel.cxx @@ -406,20 +406,20 @@ createNAV1 (int x, int y) // Action: increase selected radial inst->addAction(SIX_W/2 - SIX_W/5, -SIX_W/2, SIX_W/10, SIX_W/5, - new FGAdjustAction(FGBFI::getNAV1SelRadial, - FGBFI::setNAV1SelRadial, + new FGAdjustAction(FGBFI::getNAV1Radial, + FGBFI::setNAV1Radial, 1.0, 0.0, 360.0, true)); // Action: decrease selected radial inst->addAction(SIX_W/2 - SIX_W/10, -SIX_W/2, SIX_W/10, SIX_W/5, - new FGAdjustAction(FGBFI::getNAV1SelRadial, - FGBFI::setNAV1SelRadial, + new FGAdjustAction(FGBFI::getNAV1Radial, + FGBFI::setNAV1Radial, -1.0, 0.0, 360.0, true)); // Layer 0: background inst->addLayer(0, createTexture("Textures/Panel/gyro-bg.rgb")); inst->addTransformation(0, FGInstrumentLayer::ROTATION, - FGBFI::getNAV1SelRadial, + FGBFI::getNAV1Radial, -360.0, 360.0, -1.0, 0.0); // Layer 1: left-right needle. @@ -445,7 +445,7 @@ createNAV1 (int x, int y) inst->addTransformation(4, FGInstrumentLayer::XSHIFT, SIX_W/2 - 10); inst->addTransformation(4, FGInstrumentLayer::YSHIFT, -SIX_W/2 + 10); inst->addTransformation(4, FGInstrumentLayer::ROTATION, - FGBFI::getNAV1SelRadial, + FGBFI::getNAV1Radial, -360.0, 360.0, -1.0, 0.0); return inst; @@ -462,20 +462,20 @@ createNAV2 (int x, int y) // Action: increase selected radial inst->addAction(SIX_W/2 - SIX_W/5, -SIX_W/2, SIX_W/10, SIX_W/5, - new FGAdjustAction(FGBFI::getNAV2SelRadial, - FGBFI::setNAV2SelRadial, + new FGAdjustAction(FGBFI::getNAV2Radial, + FGBFI::setNAV2Radial, 1.0, 0.0, 360.0, true)); // Action: decrease selected radial inst->addAction(SIX_W/2 - SIX_W/10, -SIX_W/2, SIX_W/10, SIX_W/5, - new FGAdjustAction(FGBFI::getNAV2SelRadial, - FGBFI::setNAV2SelRadial, + new FGAdjustAction(FGBFI::getNAV2Radial, + FGBFI::setNAV2Radial, -1.0, 0.0, 360.0, true)); // Layer 0: background inst->addLayer(0, createTexture("Textures/Panel/gyro-bg.rgb")); inst->addTransformation(0, FGInstrumentLayer::ROTATION, - FGBFI::getNAV2SelRadial, + FGBFI::getNAV2Radial, -360.0, 360.0, -1.0, 0.0); // Layer 1: left-right needle. @@ -495,7 +495,7 @@ createNAV2 (int x, int y) inst->addTransformation(3, FGInstrumentLayer::XSHIFT, SIX_W/2 - 10); inst->addTransformation(3, FGInstrumentLayer::YSHIFT, -SIX_W/2 + 10); inst->addTransformation(3, FGInstrumentLayer::ROTATION, - FGBFI::getNAV2SelRadial, + FGBFI::getNAV2Radial, -360.0, 360.0, -1.0, 0.0); return inst; diff --git a/src/Cockpit/radiostack.cxx b/src/Cockpit/radiostack.cxx index f563b1fa1..298314a23 100644 --- a/src/Cockpit/radiostack.cxx +++ b/src/Cockpit/radiostack.cxx @@ -45,10 +45,6 @@ FGRadioStack::~FGRadioStack() { void FGRadioStack::update( double lon, double lat, double elev ) { need_update = false; - // Start with the selected radials. - nav1_radial = nav1_selected_radial; - nav2_radial = nav2_selected_radial; - // nav1 FGILS ils; if ( current_ilslist->query( lon, lat, elev, nav1_freq, @@ -59,7 +55,9 @@ void FGRadioStack::update( double lon, double lat, double elev ) { nav1_lat = ils.get_loclat(); nav1_elev = ils.get_gselev(); nav1_target_gs = ils.get_gsangle(); - nav1_radial = ils.get_locheading(); + nav1_radial = ils.get_locheading() + 180.0; + while ( nav1_radial < 0.0 ) { nav1_radial += 360.0; } + while ( nav1_radial > 360.0 ) { nav1_radial -= 360.0; } // cout << "Found a vor station in range" << endl; // cout << " id = " << ils.get_locident() << endl; // cout << " heading = " << nav1_heading @@ -69,7 +67,7 @@ void FGRadioStack::update( double lon, double lat, double elev ) { // cout << "not picking up vor. :-(" << endl; } - // nav1 + // nav2 FGNav nav; if ( current_navlist->query( lon, lat, elev, nav2_freq, &nav, &nav2_heading, &nav2_dist) ) { @@ -78,6 +76,9 @@ void FGRadioStack::update( double lon, double lat, double elev ) { nav2_lon = nav.get_lon(); nav2_lat = nav.get_lat(); nav2_elev = nav.get_elev(); + nav2_radial = nav2_heading + 180.0; + while ( nav2_radial < 0.0 ) { nav2_radial += 360.0; } + while ( nav2_radial > 360.0 ) { nav2_radial -= 360.0; } // cout << "Found a vor station in range" << endl; // cout << " id = " << nav.get_ident() << endl; // cout << " heading = " << nav2_heading diff --git a/src/Cockpit/radiostack.hxx b/src/Cockpit/radiostack.hxx index 23701c5db..1bcda8a09 100644 --- a/src/Cockpit/radiostack.hxx +++ b/src/Cockpit/radiostack.hxx @@ -36,7 +36,6 @@ class FGRadioStack { bool nav1_loc; double nav1_freq; double nav1_alt_freq; - double nav1_selected_radial; double nav1_radial; double nav1_lon; double nav1_lat; @@ -49,7 +48,6 @@ class FGRadioStack { bool nav2_loc; double nav2_freq; double nav2_alt_freq; - double nav2_selected_radial; double nav2_radial; double nav2_lon; double nav2_lat; @@ -80,8 +78,8 @@ public: nav1_freq = freq; need_update = true; } inline void set_nav1_alt_freq( double freq ) { nav1_alt_freq = freq; } - inline void set_nav1_sel_radial( double radial ) { - nav1_selected_radial = radial; need_update = true; + inline void set_nav1_radial( double radial ) { + nav1_radial = radial; need_update = true; } // NAV2 Setters @@ -89,8 +87,8 @@ public: nav2_freq = freq; need_update = true; } inline void set_nav2_alt_freq( double freq ) { nav2_alt_freq = freq; } - inline void set_nav2_sel_radial( double radial ) { - nav2_selected_radial = radial; need_update = true; + inline void set_nav2_radial( double radial ) { + nav2_radial = radial; need_update = true; } // ADF Setters @@ -104,12 +102,12 @@ public: // NAV1 Accessors inline double get_nav1_freq () { return nav1_freq; } inline double get_nav1_alt_freq () { return nav1_alt_freq; } - inline double get_nav1_sel_radial () { return nav1_selected_radial; } + inline double get_nav1_radial() const { return nav1_radial; } // NAV2 Accessors inline double get_nav2_freq () { return nav2_freq; } inline double get_nav2_alt_freq () { return nav2_alt_freq; } - inline double get_nav2_sel_radial () { return nav2_selected_radial; } + inline double get_nav2_radial() const { return nav2_radial; } // ADF Accessors inline double get_adf_freq () { return adf_freq; } @@ -119,7 +117,6 @@ public: // Calculated values. inline bool get_nav1_inrange() const { return nav1_inrange; } inline bool get_nav1_loc() const { return nav1_loc; } - inline double get_nav1_radial() const { return nav1_radial; } inline double get_nav1_lon() const { return nav1_lon; } inline double get_nav1_lat() const { return nav1_lat; } inline double get_nav1_elev() const { return nav1_elev; } @@ -129,7 +126,6 @@ public: inline bool get_nav2_inrange() const { return nav2_inrange; } inline bool get_nav2_loc() const { return nav2_loc; } - inline double get_nav2_radial() const { return nav2_radial; } inline double get_nav2_lon() const { return nav2_lon; } inline double get_nav2_lat() const { return nav2_lat; } inline double get_nav2_elev() const { return nav2_elev; } diff --git a/src/Cockpit/steam.cxx b/src/Cockpit/steam.cxx index e6b91ebe7..57267e996 100644 --- a/src/Cockpit/steam.cxx +++ b/src/Cockpit/steam.cxx @@ -214,24 +214,6 @@ void FGSteam::_CatchUp() // Everything below is a transient hack; expect it to disappear //////////////////////////////////////////////////////////////////////// -#if 0 -/* KMYF ILS */ -#define NAV1_LOC (1) -#define NAV1_Lat ( 32.0 + 48.94/60.0) -#define NAV1_Lon (-117.0 - 08.37/60.0) -#define NAV1_Rad 280.0 -#define NAV1_Alt 423 - -/* MZB stepdown radial */ -#define NAV2_Lat ( 32.0 + 46.93/60.0) -#define NAV2_Lon (-117.0 - 13.53/60.0) -#define NAV2_Rad 068.0 - -/* HAILE intersection */ -#define ADF_Lat ( 32.0 + 46.79/60.0) -#define ADF_Lon (-117.0 - 02.70/60.0) -#endif - double FGSteam::get_HackGS_deg () { @@ -272,7 +254,7 @@ double FGSteam::get_HackVOR1_deg () { if ( current_radiostack->get_nav1_inrange() ) { r = current_radiostack->get_nav1_radial() - - current_radiostack->get_nav1_heading() + 180.0; + current_radiostack->get_nav1_heading(); // cout << "Radial = " << current_radiostack->get_nav1_radial() // << " Bearing = " << current_radiostack->get_nav1_heading() // << endl; @@ -303,7 +285,7 @@ double FGSteam::get_HackVOR2_deg () { if ( current_radiostack->get_nav2_inrange() ) { r = current_radiostack->get_nav2_radial() - - current_radiostack->get_nav2_heading(); + current_radiostack->get_nav2_heading() + 180.0; // cout << "Radial = " << current_radiostack->get_nav1_radial() // << " Bearing = " << current_radiostack->get_nav1_heading() << endl; diff --git a/src/Main/bfi.cxx b/src/Main/bfi.cxx index 80dd3b95e..1834b2299 100644 --- a/src/Main/bfi.cxx +++ b/src/Main/bfi.cxx @@ -764,12 +764,6 @@ FGBFI::getNAV1AltFreq () return current_radiostack->get_nav1_alt_freq(); } -double -FGBFI::getNAV1SelRadial () -{ - return current_radiostack->get_nav1_sel_radial(); -} - double FGBFI::getNAV1Radial () { @@ -788,12 +782,6 @@ FGBFI::getNAV2AltFreq () return current_radiostack->get_nav2_alt_freq(); } -double -FGBFI::getNAV2SelRadial () -{ - return current_radiostack->get_nav2_sel_radial(); -} - double FGBFI::getNAV2Radial () { @@ -831,9 +819,9 @@ FGBFI::setNAV1AltFreq (double freq) } void -FGBFI::setNAV1SelRadial (double radial) +FGBFI::setNAV1Radial (double radial) { - current_radiostack->set_nav1_sel_radial(radial); + current_radiostack->set_nav1_radial(radial); } void @@ -849,9 +837,9 @@ FGBFI::setNAV2AltFreq (double freq) } void -FGBFI::setNAV2SelRadial (double radial) +FGBFI::setNAV2Radial (double radial) { - current_radiostack->set_nav2_sel_radial(radial); + current_radiostack->set_nav2_radial(radial); } void diff --git a/src/Main/bfi.hxx b/src/Main/bfi.hxx index 7fb7c6d27..dfb438dc7 100644 --- a/src/Main/bfi.hxx +++ b/src/Main/bfi.hxx @@ -127,12 +127,10 @@ public: // Radio Navigation static double getNAV1Freq (); static double getNAV1AltFreq (); - static double getNAV1SelRadial (); static double getNAV1Radial (); static double getNAV2Freq (); static double getNAV2AltFreq (); - static double getNAV2SelRadial (); static double getNAV2Radial (); static double getADFFreq (); @@ -141,11 +139,11 @@ public: static void setNAV1Freq (double freq); static void setNAV1AltFreq (double freq); - static void setNAV1SelRadial (double radial); + static void setNAV1Radial (double radial); static void setNAV2Freq (double freq); static void setNAV2AltFreq (double freq); - static void setNAV2SelRadial (double radial); + static void setNAV2Radial (double radial); static void setADFFreq (double freq); static void setADFAltFreq (double freq); diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index 9e34945f3..067b4aa8e 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -477,10 +477,10 @@ bool fgInitSubsystems( void ) { current_radiostack = new FGRadioStack; current_radiostack->set_nav1_freq( 110.30 ); - current_radiostack->set_nav1_sel_radial( 299.0 ); + current_radiostack->set_nav1_radial( 299.0 ); current_radiostack->set_nav2_freq( 115.70 ); - current_radiostack->set_nav2_sel_radial( 45.0 ); + current_radiostack->set_nav2_radial( 45.0 ); current_radiostack->set_adf_freq( 266.0 ); diff --git a/src/Main/keyboard.cxx b/src/Main/keyboard.cxx index 324a53e24..a7c84a3db 100644 --- a/src/Main/keyboard.cxx +++ b/src/Main/keyboard.cxx @@ -102,6 +102,13 @@ void GLUTkey(unsigned char k, int x, int y) { ! current_autopilot->get_AltitudeEnabled() ); return; + case 7: // Ctrl-G key + current_autopilot->set_AltitudeMode( + FGAutopilot::FG_ALTITUDE_GS1 ); + current_autopilot->set_AltitudeEnabled( + ! current_autopilot->get_AltitudeEnabled() + ); + return; case 8: // Ctrl-H key current_autopilot->set_HeadingMode( FGAutopilot::FG_HEADING_LOCK ); @@ -109,6 +116,13 @@ void GLUTkey(unsigned char k, int x, int y) { ! current_autopilot->get_HeadingEnabled() ); return; + case 14: // Ctrl-N key + current_autopilot->set_HeadingMode( + FGAutopilot::FG_HEADING_NAV1 ); + current_autopilot->set_HeadingEnabled( + ! current_autopilot->get_HeadingEnabled() + ); + return; case 18: // Ctrl-R key // temporary winding_ccw = !winding_ccw; -- 2.39.5