#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>
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";
}
+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();
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 );
MakeTargetAltitudeStr( TargetAltitude );
alt_error_accum = 0.0;
+ climb_error_accum = 0.0;
update_old_control_values();
}
-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.
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
// << 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 =
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 );
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 {
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
// 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.
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;
// 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.
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;
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,
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
// cout << "not picking up vor. :-(" << endl;
}
- // nav1
+ // nav2
FGNav nav;
if ( current_navlist->query( lon, lat, elev, nav2_freq,
&nav, &nav2_heading, &nav2_dist) ) {
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
bool nav1_loc;
double nav1_freq;
double nav1_alt_freq;
- double nav1_selected_radial;
double nav1_radial;
double nav1_lon;
double nav1_lat;
bool nav2_loc;
double nav2_freq;
double nav2_alt_freq;
- double nav2_selected_radial;
double nav2_radial;
double nav2_lon;
double nav2_lat;
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
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
// 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; }
// 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; }
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; }
// 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 () {
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;
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;
return current_radiostack->get_nav1_alt_freq();
}
-double
-FGBFI::getNAV1SelRadial ()
-{
- return current_radiostack->get_nav1_sel_radial();
-}
-
double
FGBFI::getNAV1Radial ()
{
return current_radiostack->get_nav2_alt_freq();
}
-double
-FGBFI::getNAV2SelRadial ()
-{
- return current_radiostack->get_nav2_sel_radial();
-}
-
double
FGBFI::getNAV2Radial ()
{
}
void
-FGBFI::setNAV1SelRadial (double radial)
+FGBFI::setNAV1Radial (double radial)
{
- current_radiostack->set_nav1_sel_radial(radial);
+ current_radiostack->set_nav1_radial(radial);
}
void
}
void
-FGBFI::setNAV2SelRadial (double radial)
+FGBFI::setNAV2Radial (double radial)
{
- current_radiostack->set_nav2_sel_radial(radial);
+ current_radiostack->set_nav2_radial(radial);
}
void
// 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 ();
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);
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 );
! 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 );
! 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;