fly/flightgear
1
0
Fork 0
Code Activity

Further GPS and route manager behavioural fixes

Browse source 
* When the nav-radio is slaved, calculated radial/target-hdg-deg
 (needed by some autopilot logic)
* Handle editing (including deletion) of route waypoints correctly,
 including deleting the active waypoint
* Add a signal to the route manager when the last wpt is reached, and
 use it in the GPS to revert to OBS mode.
* Change the altitude handling to use the specified cruise altitude
* Fix a bug where autopilot/locks/altitude was treated as a boolean
This commit is contained in:
jmt 2009-10-15 21:30:10 +00:00 • committed by Tim Moore
parent a865555fed
commit afb1e7ffe9
6 changed files with 111 additions and 65 deletions

87
src/Autopilot/route_mgr.cxx
View file

@ -139,8 +139,11 @@ void FGRouteMgr::init() {
airborne = fgGetNode(RM "airborne", true);
airborne->setBoolValue(false);
currentWp = fgGetNode(RM "current-wp", true);
currentWp->setIntValue(_route->current_index());
_edited = fgGetNode(RM "signals/edited", true);
_finished = fgGetNode(RM "signals/finished", true);
rm->tie("current-wp", SGRawValueMethods<FGRouteMgr, int>
(*this, &FGRouteMgr::currentWaypoint, &FGRouteMgr::jumpToIndex));
// temporary distance / eta calculations, for backward-compatability
wp0 = fgGetNode(RM "wp", 0, true);
@ -278,21 +281,36 @@ void FGRouteMgr::setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDi
void FGRouteMgr::add_waypoint( const SGWayPoint& wp, int n ) {
_route->add_waypoint( wp, n );
if (_route->current_index() > n) {
_route->set_current(_route->current_index() + 1);
}
update_mirror();
_edited->fireValueChanged();
}
SGWayPoint FGRouteMgr::pop_waypoint( int n ) {
SGWayPoint wp;
if ( _route->size() > 0 ) {
if ( n < 0 )
n = _route->size() - 1;
wp = _route->get_waypoint(n);
_route->delete_waypoint(n);
if ( _route->size() <= 0 ) {
return SGWayPoint();
}
if ( n < 0 ) {
n = _route->size() - 1;
}
if (_route->current_index() > n) {
_route->set_current(_route->current_index() - 1);
}
SGWayPoint wp = _route->get_waypoint(n);
_route->delete_waypoint(n);
update_mirror();
_edited->fireValueChanged();
checkFinished();
return wp;
}
@ -310,10 +328,6 @@ void FGRouteMgr::new_waypoint( const string& target, int n ) {
add_waypoint( *wp, n );
delete wp;
if ( !near_ground() ) {
fgSetString( "/autopilot/locks/heading", "true-heading-hold" );
}
}
@ -420,20 +434,6 @@ void FGRouteMgr::update_mirror() {
mirror->setIntValue("num", _route->size());
}
bool FGRouteMgr::near_ground() {
SGPropertyNode *gear = fgGetNode( "/gear/gear/wow", false );
if ( !gear || gear->getType() == simgear::props::NONE )
return fgGetBool( "/sim/presets/onground", true );
if ( fgGetDouble("/position/altitude-agl-ft", 300.0)
< fgGetDouble("/autopilot/route-manager/min-lock-altitude-agl-ft") )
return true;
return gear->getBoolValue();
}
// command interface /autopilot/route-manager/input:
//
// @CLEAR ... clear route
@ -544,10 +544,7 @@ void FGRouteMgr::sequence()
return;
}
if (_route->current_index() == _route->size()) {
SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
// what now?
active->setBoolValue(false);
if (checkFinished()) {
return;
}
@ -555,6 +552,19 @@ void FGRouteMgr::sequence()
currentWaypointChanged();
}
bool FGRouteMgr::checkFinished()
{
int lastWayptIndex = _route->size() - 1;
if (_route->current_index() < lastWayptIndex) {
return false;
}
SG_LOG(SG_AUTOPILOT, SG_INFO, "reached end of active route");
_finished->fireValueChanged();
active->setBoolValue(false);
return true;
}
void FGRouteMgr::jumpToIndex(int index)
{
if (!active->getBoolValue()) {
@ -588,7 +598,6 @@ void FGRouteMgr::currentWaypointChanged()
wp1->getChild("id")->setStringValue("");
}
currentWp->setIntValue(_route->current_index());
SG_LOG(SG_AUTOPILOT, SG_INFO, "route manager, current-wp is now " << _route->current_index());
}
@ -706,7 +715,7 @@ void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP)
}
SGPropertyNode_ptr altProp = aWP->getChild("altitude-ft");
double alt = -9999.0;
double alt = cruise->getDoubleValue("altitude-ft") * SG_FEET_TO_METER;
if (altProp) {
alt = altProp->getDoubleValue();
}
@ -714,10 +723,6 @@ void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP)
string ident(aWP->getStringValue("ident"));
if (aWP->hasChild("longitude-deg")) {
// explicit longitude/latitude
if (alt < -9990.0) {
alt = 0.0; // don't export wyapoints with invalid altitude
}
SGWayPoint swp(aWP->getDoubleValue("longitude-deg"),
aWP->getDoubleValue("latitude-deg"), alt,
SGWayPoint::WGS84, ident, aWP->getStringValue("name"));
@ -740,10 +745,6 @@ void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP)
SGGeodesy::direct(p->geod(), radialDeg, offsetNm * SG_NM_TO_METER, pos, az2);
}
if (alt < -9990.0) {
alt = p->elevation();
}
SGWayPoint swp(pos.getLongitudeDeg(), pos.getLatitudeDeg(), alt,
SGWayPoint::WGS84, ident, "");
add_waypoint(swp);
@ -754,10 +755,6 @@ void FGRouteMgr::parseRouteWaypoint(SGPropertyNode* aWP)
throw sg_io_exception("bad route file, unknown waypoint:" + ident);
}
if (alt < -9990.0) {
alt = p->elevation();
}
SGWayPoint swp(p->longitude(), p->latitude(), alt,
SGWayPoint::WGS84, p->ident(), p->name());
add_waypoint(swp);

18
src/Autopilot/route_mgr.hxx
View file

@ -63,7 +63,6 @@ private:
SGPropertyNode_ptr active;
SGPropertyNode_ptr airborne;
SGPropertyNode_ptr currentWp;
SGPropertyNode_ptr wp0;
SGPropertyNode_ptr wp1;
@ -72,6 +71,16 @@ private:
SGPropertyNode_ptr _pathNode;
/**
* Signal property to notify people that the route was edited
*/
SGPropertyNode_ptr _edited;
/**
* Signal property to notify when the last waypoint is reached
*/
SGPropertyNode_ptr _finished;
void setETAPropertyFromDistance(SGPropertyNode_ptr aProp, double aDistance);
class InputListener : public SGPropertyChangeListener {
@ -99,7 +108,6 @@ private:
void update_mirror();
bool near_ground();
void currentWaypointChanged();
@ -107,6 +115,12 @@ private:
* Parse a route/wp node (from a saved, property-lsit formatted route)
*/
void parseRouteWaypoint(SGPropertyNode* aWP);
/**
* Check if we've reached the final waypoint.
* Returns true if we have.
*/
bool checkFinished();
public:
FGRouteMgr();

41
src/Instrumentation/gps.cxx
View file

@ -135,6 +135,10 @@ public:
_gps->routeActivated();
} else if (prop == _gps->_ref_navaid_id_node) {
_gps->referenceNavaidSet(prop->getStringValue(""));
} else if (prop == _gps->_routeEditedSignal) {
_gps->routeEdited();
} else if (prop == _gps->_routeFinishedSignal) {
_gps->routeFinished();
}
_guard = false;
@ -412,11 +416,8 @@ GPS::init ()
// should these move to the route manager?
_routeDistanceNm = node->getChild("route-distance-nm", 0, true);
_routeETE = node->getChild("ETE", 0, true);
// disable auto-sequencing in the route manager; we'll deal with it
// ourselves using turn anticipation
SGPropertyNode_ptr autoSeq = fgGetNode("/autopilot/route-manager/auto-sequence", true);
autoSeq->setBoolValue(false);
_routeEditedSignal = fgGetNode("/autopilot/route-manager/signals/edited", true);
_routeFinishedSignal = fgGetNode("/autopilot/route-manager/signals/finished", true);
// add listener to various things
_listener = new GPSListener(this);
@ -425,6 +426,8 @@ GPS::init ()
_route_active_node = fgGetNode("/autopilot/route-manager/active", true);
_route_active_node->addChangeListener(_listener);
_ref_navaid_id_node->addChangeListener(_listener);
_routeEditedSignal->addChangeListener(_listener);
_routeFinishedSignal->addChangeListener(_listener);
// navradio slaving properties
node->tie("cdi-deflection", SGRawValueMethods<GPS,double>
@ -433,6 +436,10 @@ GPS::init ()
SGPropertyNode* toFlag = node->getChild("to-flag", 0, true);
toFlag->alias(wp1_node->getChild("to-flag"));
SGPropertyNode* fromFlag = node->getChild("from-flag", 0, true);
fromFlag->alias(wp1_node->getChild("from-flag"));
// autopilot drive properties
_apTrueHeading = fgGetNode("/autopilot/settings/true-heading-deg",true);
_apTargetAltitudeFt = fgGetNode("/autopilot/settings/target-altitude-ft", true);
@ -747,6 +754,28 @@ void GPS::routeManagerSequenced()
wp1Changed();
}
void GPS::routeEdited()
{
if (_mode != "leg") {
return;
}
SG_LOG(SG_INSTR, SG_INFO, "GPS route edited while in LEG mode, updating waypoints");
routeManagerSequenced();
}
void GPS::routeFinished()
{
if (_mode != "leg") {
return;
}
SG_LOG(SG_INSTR, SG_INFO, "GPS route finished, reverting to OBS");
_mode = "obs";
_wp0_position = _indicated_pos;
wp1Changed();
}
void GPS::updateTurn()
{
bool printProgress = false;
@ -977,10 +1006,8 @@ void GPS::wp1Changed()
double altFt = _wp1_position.getElevationFt();
if (altFt < -9990.0) {
_apTargetAltitudeFt->setDoubleValue(0.0);
_apAltitudeLock->setBoolValue(false);
} else {
_apTargetAltitudeFt->setDoubleValue(altFt);
_apAltitudeLock->setBoolValue(true);
}
}

4
src/Instrumentation/gps.hxx
View file

@ -206,6 +206,8 @@ private:
void routeActivated();
void routeManagerSequenced();
void routeEdited();
void routeFinished();
void updateTurn();
void updateOverflight();
@ -329,6 +331,8 @@ private:
SGPropertyNode_ptr _route_current_wp_node;
SGPropertyNode_ptr _routeDistanceNm;
SGPropertyNode_ptr _routeETE;
SGPropertyNode_ptr _routeEditedSignal;
SGPropertyNode_ptr _routeFinishedSignal;
double _selectedCourse;

5
src/Instrumentation/navradio.cxx
View file

@ -247,6 +247,7 @@ FGNavRadio::init ()
gps_has_gs_node = fgGetNode("/instrumentation/gps/has-gs", true);
gps_course_node = fgGetNode("/instrumentation/gps/selected-course-deg", true);
gps_xtrack_error_nm_node = fgGetNode("/instrumentation/gps/wp/wp[1]/course-error-nm", true);
_magvarNode = fgGetNode("/environment/magnetic-variation-deg", true);
std::ostringstream temp;
temp << _name << "nav-ident" << _num;
@ -621,7 +622,9 @@ void FGNavRadio::updateGPSSlaved()
_cdiCrossTrackErrorM = gps_xtrack_error_nm_node->getDoubleValue() * SG_NM_TO_METER;
_gsNeedleDeflection = 0.0; // FIXME, supply this
//sel_radial_node->setDoubleValue(gps_course_node->getDoubleValue());
double trtrue = gps_course_node->getDoubleValue() + _magvarNode->getDoubleValue();
SG_NORMALIZE_RANGE(trtrue, 0.0, 360.0);
target_radial_true_node->setDoubleValue( trtrue );
}
void FGNavRadio::updateCDI(double dt)

1
src/Instrumentation/navradio.hxx
View file

@ -115,6 +115,7 @@ class FGNavRadio : public SGSubsystem
SGPropertyNode_ptr gps_has_gs_node;
SGPropertyNode_ptr gps_course_node;
SGPropertyNode_ptr gps_xtrack_error_nm_node;
SGPropertyNode_ptr _magvarNode;
// internal (private) values