fly/flightgear
1
0
Fork 0
Code Activity

First stab at NAV1 and GS hold modes for the autopilot.

Browse source
This commit is contained in:
curt 2000-04-30 22:21:47 +00:00
parent b62900e44c
commit 921beb96d7
10 changed files with 120 additions and 122 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

116
src/Autopilot/newauto.cxx
View file

@ -33,6 +33,7 @@
#include <simgear/debug/logstream.hxx>
#include <simgear/math/fg_geodesy.hxx>
#include <Cockpit/radiostack.hxx>
#include <Controls/controls.hxx>
#include <FDM/flight.hxx>
#include <Main/bfi.hxx>
@ -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 );

7
src/Autopilot/newauto.hxx
View file

@ -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

24
src/Cockpit/panel.cxx
View file

@ -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;

13
src/Cockpit/radiostack.cxx
View file

@ -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

16
src/Cockpit/radiostack.hxx
View file

@ -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; }

22
src/Cockpit/steam.cxx
View file

@ -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;

20
src/Main/bfi.cxx
View file

@ -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

6
src/Main/bfi.hxx
View file

@ -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);

4
src/Main/fg_init.cxx
View file

@ -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 );

14
src/Main/keyboard.cxx
View file

@ -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;