// Climb speed constants
const double min_climb = 70.0; // kts
const double best_climb = 75.0; // kts
-const double ideal_climb_rate = 500.0 * FEET_TO_METER; // fpm -> mpm
-const double ideal_decent_rate = 1000.0 * FEET_TO_METER; // fpm -> mpm
+// const double ideal_climb_rate = 500.0 * FEET_TO_METER; // fpm -> mpm
+// const double ideal_decent_rate = 1000.0 * FEET_TO_METER; // fpm -> mpm
/// These statics will eventually go into the class
/// they are just here while I am experimenting -- NHV :-)
extern char *coord_format_lon(float);
+// constructor
+FGAutopilot::FGAutopilot():
+TargetClimbRate(1000 * FEET_TO_METER)
+{
+}
+
+// destructor
+FGAutopilot::~FGAutopilot() {}
+
+
void FGAutopilot::MakeTargetLatLonStr( double lat, double lon ) {
sprintf( TargetLatitudeStr , "%s", coord_format_lat(get_TargetLatitude()));
sprintf( TargetLongitudeStr, "%s", coord_format_lon(get_TargetLongitude()));
while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; }
while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
MakeTargetHeadingStr( TargetHeading );
- } else if ( heading_mode == FG_HEADING_LOCK ) {
- // leave target heading alone
+ } else if ( heading_mode == FG_TC_HEADING_LOCK ) {
+ // we don't set a specific target heading in
+ // TC_HEADING_LOCK mode, we instead try to keep the turn
+ // coordinator zero'd
+ } else if ( heading_mode == FG_TRUE_HEADING_LOCK ) {
+ // leave "true" target heading as is
} else if ( heading_mode == FG_HEADING_NAV1 ) {
// track the NAV1 heading needle deflection
while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; }
while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
-#if 0
- if ( current_radiostack->get_nav1_to_flag() ||
- current_radiostack->get_nav1_from_flag() ) {
- // We have an appropriate radial selected that the
- // autopilot can follow
- double tgt_radial;
- double cur_radial;
- if ( current_radiostack->get_nav1_loc() ) {
- // localizers radials are "true"
- tgt_radial = current_radiostack->get_nav1_radial();
- } else {
- tgt_radial = current_radiostack->get_nav1_radial() +
- current_radiostack->get_nav1_magvar();
- }
- cur_radial = current_radiostack->get_nav1_heading() +
- current_radiostack->get_nav1_magvar();
- if ( current_radiostack->get_nav1_from_flag() ) {
- cur_radial += 180.0;
- while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
- }
- // cout << "target rad (true) = " << tgt_radial
- // << " current rad (true) = " << cur_radial
- // << endl;
-
- double diff = (tgt_radial - cur_radial);
- while ( diff < -180.0 ) { diff += 360.0; }
- while ( diff > 180.0 ) { diff -= 360.0; }
-
- diff *= (current_radiostack->get_nav1_loc_dist() * METER_TO_NM);
- if ( diff < -30.0 ) { diff = -30.0; }
- if ( diff > 30.0 ) { diff = 30.0; }
-
- if ( current_radiostack->get_nav1_to_flag() ) {
- TargetHeading = cur_radial - diff;
- } else if ( current_radiostack->get_nav1_from_flag() ) {
- TargetHeading = cur_radial + diff;
- }
- while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; }
- while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
- } else {
- // neither TO, or FROM, maintain current heading.
- TargetHeading = FGBFI::getHeading();
- }
-#endif
MakeTargetHeadingStr( TargetHeading );
// cout << "target course (true) = " << TargetHeading << endl;
} else if ( heading_mode == FG_HEADING_WAYPOINT ) {
set_HeadingMode( FG_HEADING_WAYPOINT );
} else {
// end of the line
- heading_mode = FG_HEADING_LOCK;
+ heading_mode = FG_TRUE_HEADING_LOCK;
// use current heading
TargetHeading = FGBFI::getHeading();
}
MakeTargetWPStr( wp_distance );
}
- double RelHeading;
- double TargetRoll;
- double RelRoll;
- double AileronSet;
+ if ( heading_mode == FG_TC_HEADING_LOCK ) {
+ // drive the turn coordinator to zero
+ double turn = FGSteam::get_TC_std();
+ // cout << "turn rate = " << turn << endl;
+ double AileronSet = -turn / 2.0;
+ if ( AileronSet < -1.0 ) { AileronSet = -1.0; }
+ if ( AileronSet > 1.0 ) { AileronSet = 1.0; }
+ controls.set_aileron( AileronSet );
+ controls.set_rudder( AileronSet / 4.0 );
+ } else {
+ // steer towards the target heading
+
+ double RelHeading;
+ double TargetRoll;
+ double RelRoll;
+ double AileronSet;
- RelHeading = NormalizeDegrees( TargetHeading - FGBFI::getHeading() );
- // figure out how far off we are from desired heading
+ RelHeading
+ = NormalizeDegrees( TargetHeading - FGBFI::getHeading() );
+ // figure out how far off we are from desired heading
- // Now it is time to deterime how far we should be rolled.
- FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
+ // Now it is time to deterime how far we should be rolled.
+ FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
- // Check if we are further from heading than the roll out point
- if ( fabs( RelHeading ) > RollOut ) {
- // set Target Roll to Max in desired direction
- if ( RelHeading < 0 ) {
- TargetRoll = 0 - MaxRoll;
+ // Check if we are further from heading than the roll out point
+ if ( fabs( RelHeading ) > RollOut ) {
+ // set Target Roll to Max in desired direction
+ if ( RelHeading < 0 ) {
+ TargetRoll = 0 - MaxRoll;
+ } else {
+ TargetRoll = MaxRoll;
+ }
} else {
- TargetRoll = MaxRoll;
- }
- } else {
- // We have to calculate the Target roll
+ // We have to calculate the Target roll
- // This calculation engine thinks that the Target roll
- // should be a line from (RollOut,MaxRoll) to (-RollOut,
- // -MaxRoll) I hope this works well. If I get ambitious
- // some day this might become a fancier curve or
- // something.
+ // This calculation engine thinks that the Target roll
+ // should be a line from (RollOut,MaxRoll) to (-RollOut,
+ // -MaxRoll) I hope this works well. If I get ambitious
+ // some day this might become a fancier curve or
+ // something.
- TargetRoll = LinearExtrapolate( RelHeading, -RollOut,
- -MaxRoll, RollOut,
- MaxRoll );
- }
+ TargetRoll = LinearExtrapolate( RelHeading, -RollOut,
+ -MaxRoll, RollOut,
+ MaxRoll );
+ }
- // Target Roll has now been Found.
+ // Target Roll has now been Found.
- // Compare Target roll to Current Roll, Generate Rel Roll
+ // Compare Target roll to Current Roll, Generate Rel Roll
- FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
+ FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
- RelRoll = NormalizeDegrees( TargetRoll - FGBFI::getRoll() );
+ RelRoll = NormalizeDegrees( TargetRoll - FGBFI::getRoll() );
- // Check if we are further from heading than the roll out smooth point
- if ( fabs( RelRoll ) > RollOutSmooth ) {
- // set Target Roll to Max in desired direction
- if ( RelRoll < 0 ) {
+ // Check if we are further from heading than the roll out
+ // smooth point
+ if ( fabs( RelRoll ) > RollOutSmooth ) {
+ // set Target Roll to Max in desired direction
+ if ( RelRoll < 0 ) {
AileronSet = 0 - MaxAileron;
+ } else {
+ AileronSet = MaxAileron;
+ }
} else {
- AileronSet = MaxAileron;
+ AileronSet = LinearExtrapolate( RelRoll, -RollOutSmooth,
+ -MaxAileron,
+ RollOutSmooth,
+ MaxAileron );
}
- } else {
- AileronSet = LinearExtrapolate( RelRoll, -RollOutSmooth,
- -MaxAileron,
- RollOutSmooth,
- MaxAileron );
- }
- controls.set_aileron( AileronSet );
- controls.set_rudder( AileronSet / 4.0 );
- // controls.set_rudder( 0.0 );
+ controls.set_aileron( AileronSet );
+ controls.set_rudder( AileronSet / 4.0 );
+ // controls.set_rudder( 0.0 );
+ }
}
// altitude hold
if ( altitude_hold ) {
+ double climb_rate;
double speed, max_climb, error;
double prop_error, int_error;
double prop_adj, int_adj, total_adj;
// cout << "TargetAltitude = " << TargetAltitude
// << "Altitude = " << FGBFI::getAltitude() * FEET_TO_METER
// << endl;
- TargetClimbRate =
- ( TargetAltitude - FGBFI::getAltitude() * FEET_TO_METER ) * 8.0;
+ climb_rate =
+ ( TargetAltitude - FGSteam::get_ALT_ft() * FEET_TO_METER ) * 8.0;
} else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
double x = current_radiostack->get_nav1_gs_dist();
double y = (FGBFI::getAltitude()
double horiz_vel = cur_fdm_state->get_V_ground_speed()
* FEET_TO_METER * 60.0;
// cout << "Horizontal vel = " << horiz_vel << endl;
- TargetClimbRate = -sin( des_angle * DEG_TO_RAD ) * horiz_vel;
- // cout << "TargetClimbRate = " << TargetClimbRate << endl;
+ climb_rate = -sin( des_angle * DEG_TO_RAD ) * horiz_vel;
+ // cout << "climb_rate = " << climb_rate << endl;
/* climb_error_accum += gs_diff * 2.0; */
- /* TargetClimbRate = gs_diff * 200.0 + climb_error_accum; */
+ /* climb_rate = gs_diff * 200.0 + climb_error_accum; */
} else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
// brain dead ground hugging with no look ahead
- TargetClimbRate =
+ climb_rate =
( TargetAGL - FGBFI::getAGL()*FEET_TO_METER ) * 16.0;
// cout << "target agl = " << TargetAGL
// << " current agl = " << fgAPget_agl()
- // << " target climb rate = " << TargetClimbRate
+ // << " target climb rate = " << climb_rate
// << endl;
} else {
// just try to zero out rate of climb ...
- TargetClimbRate = 0.0;
+ climb_rate = 0.0;
}
speed = get_speed();
max_climb = 0.0;
} else if ( speed < best_climb ) {
max_climb = ((best_climb - min_climb) - (best_climb - speed))
- * ideal_climb_rate
+ * fabs(TargetClimbRate)
/ (best_climb - min_climb);
} else {
- max_climb = ( speed - best_climb ) * 10.0 + ideal_climb_rate;
+ max_climb = ( speed - best_climb ) * 10.0 + fabs(TargetClimbRate);
}
// this first one could be optional if we wanted to allow
// better climb performance assuming we have the airspeed to
// support it.
- if ( TargetClimbRate > ideal_climb_rate ) {
- TargetClimbRate = ideal_climb_rate;
+ if ( climb_rate > fabs(TargetClimbRate) ) {
+ climb_rate = fabs(TargetClimbRate);
}
- if ( TargetClimbRate > max_climb ) {
- TargetClimbRate = max_climb;
+ if ( climb_rate > max_climb ) {
+ climb_rate = max_climb;
}
- if ( TargetClimbRate < -ideal_decent_rate ) {
- TargetClimbRate = -ideal_decent_rate;
+ if ( climb_rate < -fabs(TargetClimbRate) ) {
+ climb_rate = -fabs(TargetClimbRate);
}
- // cout << "Target climb = " << TargetClimbRate * METER_TO_FEET
+ // cout << "Target climb rate = " << TargetClimbRate << endl;
+ // cout << "given our speed, modified desired climb rate = "
+ // << climb_rate * METER_TO_FEET
// << " fpm" << endl;
- error = FGBFI::getVerticalSpeed() * FEET_TO_METER - TargetClimbRate;
+ error = FGBFI::getVerticalSpeed() * FEET_TO_METER - climb_rate;
// cout << "climb rate = " << FGBFI::getVerticalSpeed()
// << " vsi rate = " << FGSteam::get_VSI_fps() << endl;
// ... no, leave target heading along ... just use the current
// heading bug value
// DGTargetHeading = FGSteam::get_DG_deg();
- } else if ( heading_mode == FG_HEADING_LOCK ) {
+ } else if ( heading_mode == FG_TC_HEADING_LOCK ) {
+ // set autopilot to hold a zero turn (as reported by the TC)
+ } else if ( heading_mode == FG_TRUE_HEADING_LOCK ) {
// set heading hold to current heading
TargetHeading = FGBFI::getHeading();
} else if ( heading_mode == FG_HEADING_WAYPOINT ) {
);
} else {
// no more way points, default to heading lock.
- heading_mode = FG_HEADING_LOCK;
- TargetHeading = FGBFI::getHeading();
+ heading_mode = FG_TC_HEADING_LOCK;
+ // TargetHeading = FGBFI::getHeading();
}
}
alt_error_accum = 0.0;
if ( altitude_mode == FG_ALTITUDE_LOCK ) {
- // lock at current altitude
- TargetAltitude = FGBFI::getAltitude() * FEET_TO_METER;
+ if ( TargetAltitude < FGBFI::getAGL() * FEET_TO_METER ) {
+ // TargetAltitude = FGBFI::getAltitude() * FEET_TO_METER;
+ }
if ( fgGetString("/sim/startup/units") == "feet" ) {
MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
void FGAutopilot::HeadingAdjust( double inc ) {
- if ( heading_mode != FG_DG_HEADING_LOCK && heading_mode != FG_HEADING_LOCK )
+ if ( heading_mode != FG_DG_HEADING_LOCK
+ && heading_mode != FG_TRUE_HEADING_LOCK )
{
heading_mode = FG_DG_HEADING_LOCK;
}
void FGAutopilot::HeadingSet( double new_heading ) {
- heading_mode = FG_HEADING_LOCK;
+ heading_mode = FG_DG_HEADING_LOCK;
new_heading = NormalizeDegrees( new_heading );
- TargetHeading = new_heading;
+ DGTargetHeading = new_heading;
// following cast needed ambiguous plib
// ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
- MakeTargetHeadingStr( TargetHeading );
+ MakeTargetHeadingStr( DGTargetHeading );
update_old_control_values();
}
// TODO: add more AP stuff
bool apHeadingLock = FGBFI::getAPHeadingLock();
- double apHeadingMag = FGBFI::getAPHeadingMag();
bool apAltitudeLock = FGBFI::getAPAltitudeLock();
double apAltitude = FGBFI::getAPAltitude();
bool gpsLock = FGBFI::getGPSLock();
// Restore all of the old states.
FGBFI::setAPHeadingLock(apHeadingLock);
- FGBFI::setAPHeadingMag(apHeadingMag);
FGBFI::setAPAltitudeLock(apAltitudeLock);
FGBFI::setAPAltitude(apAltitude);
FGBFI::setGPSLock(gpsLock);
// Autopilot
fgTie("/autopilot/locks/altitude", getAPAltitudeLock, setAPAltitudeLock);
fgTie("/autopilot/settings/altitude", getAPAltitude, setAPAltitude);
+ fgTie("/autopilot/settings/climb-rate", getAPClimb, setAPClimb, false);
fgTie("/autopilot/locks/heading", getAPHeadingLock, setAPHeadingLock);
- fgTie("/autopilot/settings/heading", getAPHeading, setAPHeading);
- fgTie("/autopilot/settings/heading-dg", getAPHeadingDG, setAPHeadingDG, false);
- fgTie("/autopilot/settings/heading-magnetic",
- getAPHeadingMag, setAPHeadingMag);
+ fgTie("/autopilot/settings/heading-bug", getAPHeadingBug, setAPHeadingBug,
+ false);
+ fgTie("/autopilot/locks/wing-leveler", getAPWingLeveler, setAPWingLeveler);
fgTie("/autopilot/locks/nav1", getAPNAV1Lock, setAPNAV1Lock);
// Weather
void
FGBFI::setAPAltitude (double altitude)
{
- current_autopilot->set_TargetAltitude( altitude );
+ current_autopilot->set_TargetAltitude( altitude * FEET_TO_METER );
}
/**
- * Get the autopilot heading lock (true=on).
+ * Get the autopilot target altitude in feet.
*/
-bool
-FGBFI::getAPHeadingLock ()
+double
+FGBFI::getAPClimb ()
{
- return
- (current_autopilot->get_HeadingEnabled() &&
- current_autopilot->get_HeadingMode() == FGAutopilot::FG_DG_HEADING_LOCK);
+ return current_autopilot->get_TargetClimbRate() * METER_TO_FEET;
}
/**
- * Set the autopilot heading lock (true=on).
+ * Set the autopilot target altitude in feet.
*/
void
-FGBFI::setAPHeadingLock (bool lock)
+FGBFI::setAPClimb (double rate)
{
- if (lock) {
- // We need to do this so that
- // it's possible to lock onto a
- // heading other than the current
- // heading.
- double heading = getAPHeadingMag();
- current_autopilot->set_HeadingMode(FGAutopilot::FG_DG_HEADING_LOCK);
- current_autopilot->set_HeadingEnabled(true);
- setAPHeadingMag(heading);
- } else if (current_autopilot->get_HeadingMode() ==
- FGAutopilot::FG_DG_HEADING_LOCK) {
- current_autopilot->set_HeadingEnabled(false);
- }
+ current_autopilot->set_TargetClimbRate( rate * FEET_TO_METER );
}
/**
- * Get the autopilot target heading in degrees.
+ * Get the autopilot heading lock (true=on).
*/
-double
-FGBFI::getAPHeading ()
+bool
+FGBFI::getAPHeadingLock ()
{
- return current_autopilot->get_TargetHeading();
+ return
+ (current_autopilot->get_HeadingEnabled() &&
+ current_autopilot->get_HeadingMode() == FGAutopilot::FG_DG_HEADING_LOCK);
}
/**
- * Set the autopilot target heading in degrees.
+ * Set the autopilot heading lock (true=on).
*/
void
-FGBFI::setAPHeading (double heading)
+FGBFI::setAPHeadingLock (bool lock)
{
- current_autopilot->set_TargetHeading( heading );
+ if (lock) {
+ current_autopilot->set_HeadingMode(FGAutopilot::FG_DG_HEADING_LOCK);
+ current_autopilot->set_HeadingEnabled(true);
+ } else {
+ current_autopilot->set_HeadingEnabled(false);
+ }
}
/**
- * Get the autopilot DG target heading in degrees.
+ * Get the autopilot heading bug in degrees.
*/
double
-FGBFI::getAPHeadingDG ()
+FGBFI::getAPHeadingBug ()
{
return current_autopilot->get_DGTargetHeading();
}
/**
- * Set the autopilot DG target heading in degrees.
+ * Set the autopilot heading bug in degrees.
*/
void
-FGBFI::setAPHeadingDG (double heading)
+FGBFI::setAPHeadingBug (double heading)
{
current_autopilot->set_DGTargetHeading( heading );
}
/**
- * Get the autopilot target heading in degrees.
+ * Get the autopilot wing leveler lock (true=on).
*/
-double
-FGBFI::getAPHeadingMag ()
+bool
+FGBFI::getAPWingLeveler ()
{
- return current_autopilot->get_TargetHeading() - getMagVar();
+ return
+ (current_autopilot->get_HeadingEnabled() &&
+ current_autopilot->get_HeadingMode() == FGAutopilot::FG_TC_HEADING_LOCK);
}
/**
- * Set the autopilot target heading in degrees.
+ * Set the autopilot wing leveler lock (true=on).
*/
void
-FGBFI::setAPHeadingMag (double heading)
+FGBFI::setAPWingLeveler (bool lock)
{
- current_autopilot->set_TargetHeading( heading + getMagVar() );
+ if (lock) {
+ current_autopilot->set_HeadingMode(FGAutopilot::FG_TC_HEADING_LOCK);
+ current_autopilot->set_HeadingEnabled(true);
+ } else {
+ current_autopilot->set_HeadingEnabled(false);
+ }
}
projects / flightgear.git / commitdiff