fly/flightgear
1
0
Fork 0
Code Activity

Lot's of massaging to get the WAYPOINT hold mechanism to work with the

Browse source 
new SGRoute manager.  Changed logic and gui a bit to match.
This commit is contained in:
curt 2000-10-12 01:06:07 +00:00
parent 2d206ebdcf
commit f5cc3b9b9c
4 changed files with 120 additions and 132 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

115
src/Autopilot/auto_gui.cxx
View file

@ -40,7 +40,9 @@
#include <GUI/gui.h>
#include <Main/bfi.hxx>
#include <Main/fg_init.hxx>
#include <Main/globals.hxx>
#include <Main/options.hxx>
#include <Navaids/fixlist.hxx>
#include "auto_gui.hxx"
#include "newauto.hxx"
@ -585,72 +587,51 @@ void TgtAptDialog_OK (puObject *)
char *s;
TgtAptDialogInput->getValue(&s);
TgtAptId = s;
string tmp = s;
double alt = 0.0;
int pos = tmp.find( "," );
if ( pos != string::npos ) {
TgtAptId = tmp.substr( 0, pos );
string alt_str = tmp.substr( pos + 1 );
alt = atof( alt_str.c_str() );
if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
alt *= FEET_TO_METER;
}
} else {
TgtAptId = tmp;
}
TgtAptDialog_Cancel( NULL );
if ( TgtAptId.length() ) {
// set initial position from TgtAirport id
FGAirport a;
FGFix f;
double t1, t2;
if ( fgFindAirportID( TgtAptId, &a ) ) {
FG_LOG( FG_GENERAL, FG_INFO,
"Adding waypoint (airport) = " << TgtAptId );
FGPath path( current_options.get_fg_root() );
path.append( "Airports" );
path.append( "simple.mk4" );
FGAirports airports( path.c_str() );
FGAirport a;
sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() );
SGWayPoint wp( a.longitude, a.latitude, alt,
SGWayPoint::WGS84, TgtAptId );
globals->get_route()->add_waypoint( wp );
} else if ( current_fixlist->query( TgtAptId, 0.0, 0.0, 0.0,
&f, &t1, &t2 ) )
{
FG_LOG( FG_GENERAL, FG_INFO,
"Adding waypoint (fix) = " << TgtAptId );
FG_LOG( FG_GENERAL, FG_INFO,
"Attempting to set starting position from airport code "
<< s );
if ( airports.search( TgtAptId, &a ) )
{
double course, reverse, distance;
// fgAPset_tgt_airport_id( TgtAptId.c_str() );
current_options.set_airport_id( TgtAptId.c_str() );
sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() );
current_autopilot->set_WayPoint( a.longitude, a.latitude,
TgtAptId );
// current_autopilot->set_TargetLatitude( a.latitude );
// current_autopilot->set_TargetLongitude( a.longitude );
current_autopilot->MakeTargetLatLonStr(
current_autopilot->get_TargetLatitude(),
current_autopilot->get_TargetLongitude() );
current_autopilot->set_old_lat( FGBFI::getLatitude() );
current_autopilot->set_old_lon( FGBFI::getLongitude() );
// need to test for iter
if( ! geo_inverse_wgs_84( FGBFI::getAltitude() * FEET_TO_METER,
FGBFI::getLatitude(),
FGBFI::getLongitude(),
current_autopilot->get_TargetLatitude(),
current_autopilot->get_TargetLongitude(),
&course,
&reverse,
&distance ) ) {
current_autopilot->set_TargetHeading( course );
current_autopilot->MakeTargetHeadingStr(
current_autopilot->get_TargetHeading() );
current_autopilot->set_TargetDistance( distance );
current_autopilot->MakeTargetDistanceStr( distance );
// This changes the AutoPilot Heading
// following cast needed
ApHeadingDialogInput->
setValue((float)current_autopilot->get_TargetHeading() );
// Force this !
current_autopilot->set_HeadingEnabled( true );
current_autopilot->set_HeadingMode(
FGAutopilot::FG_HEADING_WAYPOINT );
}
} else {
TgtAptId += " not in database.";
mkDialog(TgtAptId.c_str());
}
sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() );
SGWayPoint wp( f.get_lon(), f.get_lat(), alt,
SGWayPoint::WGS84, TgtAptId );
globals->get_route()->add_waypoint( wp );
} else {
TgtAptId += " not in database.";
mkDialog(TgtAptId.c_str());
}
// get_control_values();
// if( PauseMode != t->getPause() )
// t->togglePauseMode();
}
void TgtAptDialog_Reset(puObject *)
@ -661,7 +642,7 @@ void TgtAptDialog_Reset(puObject *)
TgtAptDialogInput->setCursor( 0 ) ;
}
void NewTgtAirport(puObject *cb)
void AddWayPoint(puObject *cb)
{
// strncpy( NewAirportId, current_options.get_airport_id().c_str(), 16 );
sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
@ -670,6 +651,16 @@ void NewTgtAirport(puObject *cb)
FG_PUSH_PUI_DIALOG( TgtAptDialog );
}
void PopWayPoint(puObject *cb)
{
globals->get_route()->delete_first();
}
void ClearRoute(puObject *cb)
{
globals->get_route()->clear();
}
void NewTgtAirportInit(void)
{
FG_LOG( FG_AUTOPILOT, FG_INFO, " enter NewTgtAirportInit()" );

40
src/Autopilot/auto_gui.hxx
View file

@ -41,49 +41,15 @@ FG_USING_STD(string);
// Defines
#define AP_CURRENT_HEADING -1
// prototypes
// void fgAPToggleWayPoint( void );
// void fgAPToggleHeading( void );
// void fgAPToggleAltitude( void );
// void fgAPToggleTerrainFollow( void );
// void fgAPToggleAutoThrottle( void );
// bool fgAPTerrainFollowEnabled( void );
// bool fgAPAltitudeEnabled( void );
// bool fgAPHeadingEnabled( void );
// bool fgAPWayPointEnabled( void );
// bool fgAPAutoThrottleEnabled( void );
// void fgAPAltitudeAdjust( double inc );
// void fgAPHeadingAdjust( double inc );
// void fgAPAutoThrottleAdjust( double inc );
// void fgAPHeadingSet( double value );
// double fgAPget_TargetLatitude( void );
// double fgAPget_TargetLongitude( void );
// // double fgAPget_TargetHeading( void );
// double fgAPget_TargetDistance( void );
// double fgAPget_TargetAltitude( void );
// char *fgAPget_TargetLatitudeStr( void );
// char *fgAPget_TargetLongitudeStr( void );
// char *fgAPget_TargetDistanceStr( void );
// char *fgAPget_TargetHeadingStr( void );
// char *fgAPget_TargetAltitudeStr( void );
// char *fgAPget_TargetLatLonStr( void );
//void fgAPset_tgt_airport_id( const string );
//string fgAPget_tgt_airport_id( void );
// void fgAPReset(void);
class puObject;
void fgAPAdjust( puObject * );
void NewHeading(puObject *cb);
void NewAltitude(puObject *cb);
void NewTgtAirport(puObject *cb);
void AddWayPoint(puObject *cb);
void PopWayPoint(puObject *cb);
void ClearRoute(puObject *cb);
void NewTgtAirportInit();
void fgAPAdjustInit() ;

87
src/Autopilot/newauto.cxx
View file

@ -37,6 +37,7 @@
#include <Controls/controls.hxx>
#include <FDM/flight.hxx>
#include <Main/bfi.hxx>
#include <Main/globals.hxx>
#include <Main/options.hxx>
#include <Scenery/scenery.hxx>
@ -116,7 +117,8 @@ void FGAutopilot::MakeTargetDistanceStr( double distance ) {
}
major = (int)eta;
minor = (int)((eta - (int)eta) * 60.0);
sprintf( TargetDistanceStr, "APDistance %.2f NM ETA %d:%02d",
sprintf( TargetDistanceStr, "%s %.2f NM ETA %d:%02d",
waypoint.get_id().c_str(),
distance*METER_TO_NM, major, minor );
// cout << "distance = " << distance*METER_TO_NM
// << " gndsp = " << get_ground_speed()
@ -146,7 +148,7 @@ void FGAutopilot::init() {
// Initialize target location to startup location
old_lat = FGBFI::getLatitude();
old_lon = FGBFI::getLongitude();
set_WayPoint( old_lon, old_lat, "default" );
// set_WayPoint( old_lon, old_lat, 0.0, "default" );
MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
@ -215,7 +217,7 @@ void FGAutopilot::reset() {
// TargetLatitude = FGBFI::getLatitude();
// TargetLongitude = FGBFI::getLongitude();
set_WayPoint( FGBFI::getLongitude(), FGBFI::getLatitude(), "reset" );
// s<et_WayPoint( FGBFI::getLongitude(), FGBFI::getLatitude(), 0.0, "reset" );
MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
}
@ -272,6 +274,7 @@ int FGAutopilot::run() {
double lat = FGBFI::getLatitude();
double lon = FGBFI::getLongitude();
double alt = FGBFI::getAltitude() * FEET_TO_METER;
#ifdef FG_FORCE_AUTO_DISENGAGE
// see if somebody else has changed them
@ -333,7 +336,7 @@ int FGAutopilot::run() {
} else if ( heading_mode == FG_HEADING_WAYPOINT ) {
// update target heading to waypoint
double wp_course, /*wp_reverse,*/ wp_distance;
double wp_course, wp_distance;
#ifdef DO_fgAP_CORRECTED_COURSE
// compute course made good
@ -352,7 +355,8 @@ int FGAutopilot::run() {
// compute course to way_point
// need to test for iter
waypoint.CourseAndDistance( lon, lat, &wp_course, &wp_distance );
waypoint.CourseAndDistance( lon, lat, alt,
&wp_course, &wp_distance );
#ifdef DO_fgAP_CORRECTED_COURSE
corrected_course = course - wp_course;
@ -366,12 +370,24 @@ int FGAutopilot::run() {
// corrected_course = course - wp_course;
TargetHeading = NormalizeDegrees(wp_course);
} else {
printf("distance(%f) to close\n", wp_distance);
// Real Close -- set heading hold to current heading
// and Ring the arival bell !!
heading_mode = FG_HEADING_LOCK;
// use current heading
TargetHeading = FGBFI::getHeading();
cout << "Reached waypoint within " << wp_distance << "meters"
<< endl;
// pop off this waypoint from the list
if ( globals->get_route()->size() ) {
globals->get_route()->delete_first();
}
// see if there are more waypoints on the list
if ( globals->get_route()->size() ) {
// more waypoints
set_HeadingMode( FG_HEADING_WAYPOINT );
} else {
// end of the line
heading_mode = FG_HEADING_LOCK;
// use current heading
TargetHeading = FGBFI::getHeading();
}
}
MakeTargetHeadingStr( TargetHeading );
// Force this just in case
@ -623,24 +639,41 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
// set heading hold to current heading
TargetHeading = FGBFI::getHeading();
} else if ( heading_mode == FG_HEADING_WAYPOINT ) {
double course, /*reverse,*/ distance;
// turn on location hold
// turn on heading hold
old_lat = FGBFI::getLatitude();
old_lon = FGBFI::getLongitude();
waypoint.CourseAndDistance( FGBFI::getLongitude(), FGBFI::getLatitude(),
&course, &distance );
TargetHeading = course;
TargetDistance = distance;
MakeTargetDistanceStr( distance );
if ( globals->get_route()->size() ) {
double course, distance;
FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetLocation: ( "
<< get_TargetLatitude() << " "
<< get_TargetLongitude() << " ) "
);
old_lat = FGBFI::getLatitude();
old_lon = FGBFI::getLongitude();
waypoint = globals->get_route()->get_first();
waypoint.CourseAndDistance( FGBFI::getLongitude(),
FGBFI::getLatitude(),
FGBFI::getLatitude() * FEET_TO_METER,
&course, &distance );
TargetHeading = course;
TargetDistance = distance;
MakeTargetLatLonStr( waypoint.get_target_lat(),
waypoint.get_target_lon() );
MakeTargetDistanceStr( distance );
if ( waypoint.get_target_alt() > 0.0 ) {
TargetAltitude = waypoint.get_target_alt();
altitude_mode = FG_ALTITUDE_LOCK;
set_AltitudeEnabled( true );
MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
}
FG_LOG( FG_COCKPIT, FG_INFO, " set_HeadingMode: ( "
<< get_TargetLatitude() << " "
<< get_TargetLongitude() << " ) "
);
} else {
// no more way points, default to heading lock.
heading_mode = FG_HEADING_LOCK;
TargetHeading = FGBFI::getHeading();
}
}
MakeTargetHeadingStr( TargetHeading );
update_old_control_values();
}

10
src/Autopilot/newauto.hxx
View file

@ -135,18 +135,16 @@ public:
inline bool get_AutoThrottleEnabled() const { return auto_throttle; }
void set_AutoThrottleEnabled( bool value );
inline void set_WayPoint( const double lon, const double lat,
const string s ) {
waypoint = SGWayPoint( lon, lat, SGWayPoint::WGS84, "Current WP" );
}
/* inline void set_WayPoint( const double lon, const double lat,
const double alt, const string s ) {
waypoint = SGWayPoint( lon, lat, alt, SGWayPoint::WGS84, "Current WP" );
} */
inline double get_TargetLatitude() const {
return waypoint.get_target_lat();
}
inline double get_TargetLongitude() const {
return waypoint.get_target_lon();
}
// inline void set_TargetLatitude( double val ) { TargetLatitude = val; }
// inline void set_TargetLongitude( double val ) { TargetLongitude = val; }
inline void set_old_lat( double val ) { old_lat = val; }
inline void set_old_lon( double val ) { old_lon = val; }
inline double get_TargetHeading() const { return TargetHeading; }