// Contributions by Jeff Goeke-Smith <jgoeke@voyager.net>
// Norman Vine <nhv@cape.com>
// Curtis Olson <curt@flightgear.org>
+// Wendell Turner <wendell@adsi-m4.com>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License as
#include <simgear/math/sg_random.h>
#include <simgear/route/route.hxx>
-#include <Cockpit/steam.hxx>
#include <Cockpit/radiostack.hxx>
#include <Controls/controls.hxx>
#include <FDM/flight.hxx>
#include <Scenery/scenery.hxx>
#include "newauto.hxx"
+#include "auto_gui.hxx"
/// These statics will eventually go into the class
}
-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
static const SGPropertyNode * speedup_node = fgGetNode("/sim/speed-up");
altitude_node = fgGetNode("/position/altitude-ft", true);
altitude_agl_node = fgGetNode("/position/altitude-agl-ft", true);
vertical_speed_node = fgGetNode("/velocities/vertical-speed-fps", true);
+ airspeed_node = fgGetNode("/velocities/airspeed-kt", true);
heading_node = fgGetNode("/orientation/heading-deg", true);
+
+ // support non-dg systems that indicate magnetic heading (e.g. 747-400)
+ if (fgGetBool("autopilot/config/indicated-heading-magnetic")) {
+ // use magnetic heading indicated
+ indicated_heading_node
+ = fgGetNode("/orientation/heading-magnetic-deg",
+ true);
+ } else {
+ // use dg heading indicated
+ indicated_heading_node
+ = fgGetNode("/instrumentation/heading-indicator/indicated-heading-deg",
+ true);
+ }
+
roll_node = fgGetNode("/orientation/roll-deg", true);
pitch_node = fgGetNode("/orientation/pitch-deg", true);
+
+
// bind config property nodes...
TargetClimbRate
= fgGetNode("/autopilot/config/target-climb-rate-fpm", true);
= fgGetNode("/autopilot/config/zero-pitch-throttle", true);
zero_pitch_trim_full_throttle
= fgGetNode("/autopilot/config/zero-pitch-trim-full-throttle", true);
- current_throttle = fgGetNode("/controls/throttle");
+ max_aileron_node = fgGetNode("/autopilot/config/max-aileron", true);
+ max_roll_node = fgGetNode("/autopilot/config/max-roll-deg", true);
+ roll_out_node = fgGetNode("/autopilot/config/roll-out-deg", true);
+ roll_out_smooth_node = fgGetNode("/autopilot/config/roll-out-smooth-deg", true);
+ throttle_adj_factor
+ = fgGetNode("/autopilot/config/throttle-adj-factor", true);
+ throttle_integral
+ = fgGetNode("/autopilot/config/throttle-integral", true);
+ speed_change_node
+ = fgGetNode("/autopilot/output/speed_change_anticipated_kt", true);
+
+ terrain_follow_factor = fgGetNode("/autopilot/config/terrain-follow-factor", true);
+
+ current_throttle = fgGetNode("/controls/engines/engine/throttle");
// initialize config properties with defaults (in case config isn't there)
if ( TargetClimbRate->getFloatValue() < 1 )
fgSetFloat( "/autopilot/config/zero-pitch-throttle", 0.60 );
if ( zero_pitch_trim_full_throttle->getFloatValue() < 0.0000001 )
fgSetFloat( "/autopilot/config/zero-pitch-trim-full-throttle", 0.15 );
+ if ( max_aileron_node->getFloatValue() < 0.0000001 )
+ fgSetFloat( "/autopilot/config/max-aileron", 0.2 );
+ if ( max_roll_node->getFloatValue() < 0.0000001 )
+ fgSetFloat( "/autopilot/config/max-roll-deg", 20 );
+ if ( roll_out_node->getFloatValue() < 0.0000001 )
+ fgSetFloat( "/autopilot/config/roll-out-deg", 20 );
+ if ( roll_out_smooth_node->getFloatValue() < 0.0000001 )
+ fgSetFloat( "/autopilot/config/roll-out-smooth-deg", 10 );
+ if (throttle_adj_factor->getFloatValue() < 1)
+ fgSetFloat( "/autopilot/config/throttle-adj-factor", 5000 );
+ if ( throttle_integral->getFloatValue() < 0.0000001 )
+ fgSetFloat( "/autopilot/config/throttle-integral", 0.001 );
+ if (terrain_follow_factor->getFloatValue() < 1)
+ fgSetFloat( "/autopilot/config/terrain-follow-factor", 16 );
/* set defaults */
heading_hold = false ; // turn the heading hold off
altitude_hold = false ; // turn the altitude hold off
auto_throttle = false ; // turn the auto throttle off
heading_mode = DEFAULT_AP_HEADING_LOCK;
+ altitude_mode = DEFAULT_AP_ALTITUDE_LOCK;
DGTargetHeading = fgGetDouble("/autopilot/settings/heading-bug-deg");
TargetHeading = fgGetDouble("/autopilot/settings/heading-bug-deg");
TargetAltitude = fgGetDouble("/autopilot/settings/altitude-ft") * SG_FEET_TO_METER;
+ TargetSpeed = fgGetDouble("/autopilot/settings/speed-kt");
// Initialize target location to startup location
old_lat = latitude_node->getDoubleValue();
// the deg from heading to start rolling out at, in Deg
RollOut = 20;
- // how far can I move the aleron from center.
- MaxAileron = .2;
-
// Smoothing distance for alerion control
RollOutSmooth = 10;
// 25% max control variablilty 0.5 / 2.0
disengage_threshold = 1.0;
+ // set default aileron max deflection
+ MaxAileron = 0.5;
+
+ // used to calculate acceleration
+ previous_speed = 0;
+
#if !defined( USING_SLIDER_CLASS )
MaxRollAdjust = 2 * MaxRoll;
RollOutAdjust = 2 * RollOut;
- MaxAileronAdjust = 2 * MaxAileron;
+ //MaxAileronAdjust = 2 * MaxAileron;
RollOutSmoothAdjust = 2 * RollOutSmooth;
#endif // !defined( USING_SLIDER_CLASS )
fgTie("/autopilot/locks/heading", this,
&FGAutopilot::getAPHeadingLock, &FGAutopilot::setAPHeadingLock);
fgSetArchivable("/autopilot/locks/heading");
+
+ fgTie("/autopilot/locks/vert-speed", this,
+ &FGAutopilot::getAPVertSpeedLock, &FGAutopilot::setAPVertSpeedLock);
+ fgSetArchivable("/autopilot/locks/vert-speed");
+
+
fgTie("/autopilot/settings/heading-bug-deg", this,
&FGAutopilot::getAPHeadingBug, &FGAutopilot::setAPHeadingBug);
fgSetArchivable("/autopilot/settings/heading-bug-deg");
fgSetDouble("/autopilot/settings/heading-bug-deg", 0.0f);
+
+ fgTie("/autopilot/settings/waypoint", this,
+ &FGAutopilot::getAPwaypoint, &FGAutopilot::setAPwaypoint);
+ fgSetArchivable("/autopilot/settings/waypoint");
+ fgSetString("/autopilot/settings/waypoint", "");
+
fgTie("/autopilot/locks/wing-leveler", this,
&FGAutopilot::getAPWingLeveler, &FGAutopilot::setAPWingLeveler);
fgSetArchivable("/autopilot/locks/wing-leveler");
&FGAutopilot::getAPAutoThrottleLock,
&FGAutopilot::setAPAutoThrottleLock);
fgSetArchivable("/autopilot/locks/auto-throttle");
+ fgTie("/autopilot/settings/speed-kt", this,
+ &FGAutopilot::getAPAutoThrottle, &FGAutopilot::setAPAutoThrottle);
+ fgSetArchivable("/autopilot/settings/altitude-ft");
fgTie("/autopilot/control-overrides/rudder", this,
&FGAutopilot::getAPRudderControl,
&FGAutopilot::setAPRudderControl);
&FGAutopilot::getAPThrottleControl,
&FGAutopilot::setAPThrottleControl);
fgSetArchivable("/autopilot/control-overrides/throttle");
+
+ fgTie("/autopilot/settings/vertical-speed-fpm", this,
+ &FGAutopilot::getAPVertSpeed, &FGAutopilot::setAPVertSpeed);
+ fgSetArchivable("/autopilot/settings/vertical-speed-fpm");
+ fgSetDouble("/autopilot/settings/vertical-speed-fpm", 0.0f);
+
}
void
update_old_control_values();
- sprintf( NewTgtAirportId, "%s", fgGetString("/sim/startup/airport-id") );
+ sprintf( NewTgtAirportId, "%s", fgGetString("/sim/presets/airport-id") );
MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
}
void
-FGAutopilot::update (int dt)
+FGAutopilot::update (double dt)
{
- // Remove the following lines when the calling funcitons start
- // passing in the data pointer
// get control settings
-
double lat = latitude_node->getDoubleValue();
double lon = longitude_node->getDoubleValue();
double alt = altitude_node->getDoubleValue() * SG_FEET_TO_METER;
+ // get config settings
+ MaxAileron = max_aileron_node->getDoubleValue();
+ MaxRoll = max_roll_node->getDoubleValue();
+ RollOut = roll_out_node->getDoubleValue();
+ RollOutSmooth = roll_out_smooth_node->getDoubleValue();
+
SG_LOG( SG_ALL, SG_DEBUG, "FGAutopilot::run() lat = " << lat <<
" lon = " << lon << " alt = " << alt );
// heading hold
if ( heading_hold == true ) {
if ( heading_mode == FG_DG_HEADING_LOCK ) {
- TargetHeading = DGTargetHeading + FGSteam::get_DG_err();
+ double dg_error = heading_node->getDoubleValue()
+ - indicated_heading_node->getDoubleValue();
+ TargetHeading = DGTargetHeading + dg_error;
+ // cout << "dg_error = " << dg_error << endl;
while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; }
while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
MakeTargetHeadingStr( TargetHeading );
// determine our current radial position relative to the
// navaid in "true" heading.
- double cur_radial = current_radiostack->get_nav1_heading();
- if ( current_radiostack->get_nav1_loc() ) {
+ double cur_radial = current_radiostack->get_navcom1()->get_nav_reciprocal_radial();
+ if ( current_radiostack->get_navcom1()->get_nav_loc() ) {
// ILS localizers radials are already "true" in our
// database
} else {
- cur_radial += current_radiostack->get_nav1_magvar();
+ cur_radial += current_radiostack->get_navcom1()->get_nav_twist();
}
- if ( current_radiostack->get_nav1_from_flag() ) {
+ if ( current_radiostack->get_navcom1()->get_nav_from_flag() ) {
cur_radial += 180.0;
while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
}
// determine the target radial in "true" heading
- double tgt_radial = current_radiostack->get_nav1_radial();
- if ( current_radiostack->get_nav1_loc() ) {
+ double tgt_radial
+ = current_radiostack->get_navcom1()->get_nav_target_radial();
+ if ( current_radiostack->get_navcom1()->get_nav_loc() ) {
// ILS localizers radials are already "true" in our
// database
} else {
// VOR radials need to have that vor's offset added in
- tgt_radial += current_radiostack->get_nav1_magvar();
+ tgt_radial += current_radiostack->get_navcom1()->get_nav_twist();
}
// determine the heading adjustment needed.
double adjustment =
- current_radiostack->get_nav1_heading_needle_deflection()
- * (current_radiostack->get_nav1_loc_dist() * SG_METER_TO_NM);
+ current_radiostack->get_navcom1()->get_nav_cdi_deflection()
+ * (current_radiostack->get_navcom1()->get_nav_loc_dist() * SG_METER_TO_NM);
SG_CLAMP_RANGE( adjustment, -30.0, 30.0 );
+ // clamp closer when inside cone when beyond 5km...
+ if (current_radiostack->get_navcom1()->get_nav_loc_dist() > 5000) {
+ double clamp_angle = fabs(current_radiostack->get_navcom1()->get_nav_cdi_deflection()) * 3;
+ if (clamp_angle < 30)
+ SG_CLAMP_RANGE( adjustment, -clamp_angle, clamp_angle);
+ }
+
// determine the target heading to fly to intercept the
// tgt_radial
TargetHeading = tgt_radial + adjustment;
set_HeadingMode( FG_HEADING_WAYPOINT );
} else {
// end of the line
- heading_mode = FG_TRUE_HEADING_LOCK;
+ heading_mode = DEFAULT_AP_HEADING_LOCK;
// use current heading
TargetHeading = heading_node->getDoubleValue();
}
if ( heading_mode == FG_TC_HEADING_LOCK ) {
// drive the turn coordinator to zero
- double turn = FGSteam::get_TC_std();
+ double turn =
+ fgGetDouble("/instrumentation/turn-indicator/indicated-turn-rate");
double AileronSet = -turn / 2.0;
SG_CLAMP_RANGE( AileronSet, -1.0, 1.0 );
globals->get_controls()->set_aileron( AileronSet );
// figure out how far off we are from desired heading
// Now it is time to deterime how far we should be rolled.
- SG_LOG( SG_AUTOPILOT, SG_DEBUG, "RelHeading: " << RelHeading );
-
+ SG_LOG( SG_AUTOPILOT, SG_DEBUG,
+ "Heading = " << heading_node->getDoubleValue() <<
+ " TargetHeading = " << TargetHeading <<
+ " RelHeading = " << RelHeading );
// Check if we are further from heading than the roll out point
if ( fabs( RelHeading ) > RollOut ) {
if ( altitude_mode == FG_ALTITUDE_LOCK ) {
climb_rate =
- ( TargetAltitude - FGSteam::get_ALT_ft() * SG_FEET_TO_METER ) * 8.0;
+ ( TargetAltitude -
+ fgGetDouble("/instrumentation/altimeter/indicated-altitude-ft") * SG_FEET_TO_METER ) * 8.0;
+ } else if ( altitude_mode == FG_TRUE_ALTITUDE_LOCK ) {
+ climb_rate = ( TargetAltitude - alt ) * 8.0;
} else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
- double x = current_radiostack->get_nav1_gs_dist();
+ double x = current_radiostack->get_navcom1()->get_nav_gs_dist();
double y = (altitude_node->getDoubleValue()
- - current_radiostack->get_nav1_elev()) * SG_FEET_TO_METER;
+ - current_radiostack->get_navcom1()->get_nav_elev()) * SG_FEET_TO_METER;
double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES;
- double target_angle = current_radiostack->get_nav1_target_gs();
-
+ double target_angle = current_radiostack->get_navcom1()->get_nav_target_gs();
double gs_diff = target_angle - current_angle;
// convert desired vertical path angle into a climb rate
double des_angle = current_angle - 10 * gs_diff;
- // convert to meter/min
- double horiz_vel = cur_fdm_state->get_V_ground_speed()
- * SG_FEET_TO_METER * 60.0;
+ // estimate horizontal speed towards ILS in meters per minute
+ static double horiz_vel = 0.0;
+ static double last_x = 0.0;
+ double dist = last_x - x;
+ last_x = x;
+ double new_vel = ( dist / dt ) * 60.0;
+ horiz_vel = 0.75 * horiz_vel + 0.25 * new_vel;
+ // double horiz_vel = cur_fdm_state->get_V_ground_speed()
+ // * SG_FEET_TO_METER * 60.0;
+ // double horiz_vel = airspeed_node->getFloatValue()
+ // * SG_FEET_TO_METER * 60.0;
+
climb_rate = -sin( des_angle * SGD_DEGREES_TO_RADIANS ) * horiz_vel;
/* climb_error_accum += gs_diff * 2.0; */
/* climb_rate = gs_diff * 200.0 + climb_error_accum; */
// brain dead ground hugging with no look ahead
climb_rate =
( TargetAGL - altitude_agl_node->getDoubleValue()
- * SG_FEET_TO_METER ) * 16.0;
+ * SG_FEET_TO_METER )
+ * terrain_follow_factor->getFloatValue();
+ } else if ( altitude_mode == FG_VERT_SPEED ) {
+ climb_rate = TargetVertSpeed * SG_FEET_TO_METER;
+ // switch to altitude hold, if set, within 500ft of target
+ if (fabs(TargetAltitude * SG_METER_TO_FEET - altitude_node->getDoubleValue()) < 500) {
+ set_AltitudeMode( FG_ALTITUDE_LOCK );
+ }
} else {
// just try to zero out rate of climb ...
climb_rate = 0.0;
}
- speed = get_speed();
+ speed = airspeed_node->getFloatValue();
if ( speed < min_climb->getFloatValue() ) {
max_climb = 0.0;
+ fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER);
}
- // this first one could be optional if we wanted to allow
- // better climb performance assuming we have the airspeed to
- // support it.
- if ( climb_rate >
+ if (altitude_mode != FG_VERT_SPEED) {
+
+ // this first one could be optional if we wanted to allow
+ // better climb performance assuming we have the airspeed to
+ // support it.
+ if ( climb_rate >
fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER) ) {
- climb_rate
+ climb_rate
= fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER);
- }
+ }
- if ( climb_rate > max_climb ) {
- climb_rate = max_climb;
- }
+ if ( climb_rate > max_climb ) {
+ climb_rate = max_climb;
+ }
- if ( climb_rate <
+ if ( climb_rate <
-fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER) ) {
- climb_rate
+ climb_rate
= -fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER);
- }
+ }
+
+ }
error = vertical_speed_node->getDoubleValue() * 60
- climb_rate * SG_METER_TO_FEET;
// calculate integral error, and adjustment amount
int_error = alt_error_accum;
// printf("error = %.2f int_error = %.2f\n", error, int_error);
- int_adj = int_error / elevator_adj_factor->getFloatValue();
+
+ // scale elev_adj_factor by speed of aircraft in relation to min climb
+ double elev_adj_factor = elevator_adj_factor->getFloatValue();
+ elev_adj_factor *=
+ pow(float(speed / min_climb->getFloatValue()), 3.0f);
+
+ int_adj = int_error / elev_adj_factor;
// caclulate proportional error
prop_error = error;
- prop_adj = prop_error / elevator_adj_factor->getDoubleValue();
+ prop_adj = prop_error / elev_adj_factor;
total_adj = ((double) 1.0 - (double) integral_contrib->getFloatValue()) * prop_adj
+ (double) integral_contrib->getFloatValue() * int_adj;
double error;
double prop_error, int_error;
double prop_adj, int_adj, total_adj;
+ double lookahead;
- error = TargetSpeed - get_speed();
+ // estimate speed in 10 seconds
+ lookahead = airspeed_node->getFloatValue() + ( airspeed_node->getFloatValue() - previous_speed) * (10/(dt + 0.000001));
+ previous_speed = airspeed_node->getFloatValue();
+
+ // compare targetspeed to anticipated airspeed
+ error = TargetSpeed - lookahead;
+
+ // output anticipated speed change...
+ speed_change_node->setDoubleValue(lookahead - airspeed_node->getFloatValue());
// accumulate the error under the curve ... this really should
// be *= delta t
int_error = speed_error_accum;
// printf("error = %.2f int_error = %.2f\n", error, int_error);
- int_adj = int_error / 200.0;
+ int_adj = int_error / throttle_adj_factor->getFloatValue();
// caclulate proportional error
prop_error = error;
- prop_adj = 0.5 + prop_error / 50.0;
+ prop_adj = prop_error / throttle_adj_factor->getFloatValue();
+
+ total_adj = (1.0 - throttle_integral->getFloatValue()) * prop_adj +
+ throttle_integral->getFloatValue() * int_adj;
- total_adj = 0.9 * prop_adj + 0.1 * int_adj;
- if ( total_adj > 1.0 ) {
- total_adj = 1.0;
+ current_ap_throttle = current_ap_throttle + total_adj;
+
+ if ( current_ap_throttle > 1.0 ) {
+ current_ap_throttle = 1.0;
}
- else if ( total_adj < 0.0 ) {
- total_adj = 0.0;
+ else if ( current_ap_throttle < 0.0 ) {
+ current_ap_throttle = 0.0;
}
globals->get_controls()->set_throttle( FGControls::ALL_ENGINES,
- total_adj );
+ current_ap_throttle );
}
#ifdef THIS_CODE_IS_NOT_USED
heading_mode = mode;
if ( heading_mode == FG_DG_HEADING_LOCK ) {
- // set heading hold to current heading (as read from DG)
- // ... no, leave target heading along ... just use the current
- // heading bug value
- // DGTargetHeading = FGSteam::get_DG_deg();
+ // use current heading bug value
} 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 ) {
if ( waypoint.get_target_alt() > 0.0 ) {
TargetAltitude = waypoint.get_target_alt();
- altitude_mode = FG_ALTITUDE_LOCK;
+ altitude_mode = DEFAULT_AP_ALTITUDE_LOCK;
set_AltitudeEnabled( true );
MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
}
void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
altitude_mode = mode;
- alt_error_accum = 0.0;
-
+ // only reset accum error if not in altitude mode for smooth transitions
+ // from one altitude mode to another until pitch control damping added.
+ if (!altitude_hold) {
+ alt_error_accum = 0.0;
+ }
- if ( altitude_mode == FG_ALTITUDE_LOCK ) {
+ if ( altitude_mode == DEFAULT_AP_ALTITUDE_LOCK ) {
if ( TargetAltitude < altitude_agl_node->getDoubleValue()
* SG_FEET_TO_METER ) {
}
- if ( !strcmp("/sim/startup/units", "feet") ) {
+ if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
} else {
MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET );
} else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
TargetAGL = altitude_agl_node->getDoubleValue() * SG_FEET_TO_METER;
- if ( !strcmp("/sim/startup/units", "feet") ) {
+ if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET );
} else {
MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET );
void FGAutopilot::AltitudeSet( double new_altitude ) {
double target_alt = new_altitude;
+ altitude_mode = DEFAULT_AP_ALTITUDE_LOCK;
- if ( !strcmp("/sim/startup/units", "feet") ) {
+ if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
target_alt = new_altitude * SG_FEET_TO_METER;
}
- if( target_alt < scenery.get_cur_elev() ) {
- target_alt = scenery.get_cur_elev();
+ if( target_alt < globals->get_scenery()->get_cur_elev() ) {
+ target_alt = globals->get_scenery()->get_cur_elev();
}
TargetAltitude = target_alt;
- altitude_mode = FG_ALTITUDE_LOCK;
- if ( !strcmp("/sim/startup/units", "feet") ) {
+ if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
target_alt *= SG_METER_TO_FEET;
}
// ApAltitudeDialogInput->setValue((float)target_alt);
{
double target_alt, target_agl;
- if ( !strcmp("/sim/startup/units", "feet") ) {
+ if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
target_alt = TargetAltitude * SG_METER_TO_FEET;
target_agl = TargetAGL * SG_METER_TO_FEET;
} else {
target_agl = ( int ) ( target_agl / inc ) * inc + inc;
}
- if ( !strcmp("/sim/startup/units", "feet") ) {
+ if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) {
target_alt *= SG_FEET_TO_METER;
target_agl *= SG_FEET_TO_METER;
}
TargetAltitude = target_alt;
TargetAGL = target_agl;
- if ( !strcmp("/sim/startup/units", "feet") )
+ if ( !strcmp(fgGetString("/sim/startup/units"), "feet") )
target_alt *= SG_METER_TO_FEET;
- if ( !strcmp("/sim/startup/units", "feet") )
+ if ( !strcmp(fgGetString("/sim/startup/units"), "feet") )
target_agl *= SG_METER_TO_FEET;
- if ( altitude_mode == FG_ALTITUDE_LOCK ) {
+ if ( altitude_mode == DEFAULT_AP_ALTITUDE_LOCK ) {
MakeTargetAltitudeStr( target_alt );
} else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
MakeTargetAltitudeStr( target_agl );
if ( heading_mode != FG_DG_HEADING_LOCK
&& heading_mode != FG_TRUE_HEADING_LOCK )
{
- heading_mode = FG_DG_HEADING_LOCK;
+ heading_mode = DEFAULT_AP_HEADING_LOCK;
}
if ( heading_mode == FG_DG_HEADING_LOCK ) {
void FGAutopilot::HeadingSet( double new_heading ) {
+ heading_mode = DEFAULT_AP_HEADING_LOCK;
if( heading_mode == FG_TRUE_HEADING_LOCK ) {
new_heading = NormalizeDegrees( new_heading );
TargetHeading = new_heading;
void FGAutopilot::AutoThrottleAdjust( double inc ) {
double target = ( int ) ( TargetSpeed / inc ) * inc + inc;
- TargetSpeed = target;
+ set_TargetSpeed( target );
+}
+
+/**
+ * Set the autothrottle speed
+ */
+void
+FGAutopilot::setAPAutoThrottle (double speed_kt)
+{
+ set_TargetSpeed( speed_kt );
}
+/**
+ * Get the autothrottle speed
+ */
+double
+FGAutopilot::getAPAutoThrottle () const
+{
+ return get_TargetSpeed();
+}
void FGAutopilot::set_AutoThrottleEnabled( bool value ) {
auto_throttle = value;
if ( auto_throttle == true ) {
- TargetSpeed = fgGetDouble("/velocities/airspeed-kt");
- speed_error_accum = 0.0;
+ if (TargetSpeed < 0.0001) {
+ TargetSpeed = fgGetDouble("/velocities/airspeed-kt");
+ }
+ speed_error_accum = 0.0;
+ // initialize autothrottle at current control setting;
+ current_ap_throttle = current_throttle->getFloatValue();
}
update_old_control_values();
{
return (get_AltitudeEnabled() &&
get_AltitudeMode()
- == FGAutopilot::FG_ALTITUDE_LOCK);
+ == DEFAULT_AP_ALTITUDE_LOCK);
}
FGAutopilot::setAPAltitudeLock (bool lock)
{
if (lock)
- set_AltitudeMode(FGAutopilot::FG_ALTITUDE_LOCK);
- if (get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_LOCK)
+ set_AltitudeMode(DEFAULT_AP_ALTITUDE_LOCK);
+ if (get_AltitudeMode() == DEFAULT_AP_ALTITUDE_LOCK)
set_AltitudeEnabled(lock);
}
set_AltitudeEnabled(lock);
}
+/**
+ * Get the autopilot vertical speed lock (true=on).
+ */
+bool
+FGAutopilot::getAPVertSpeedLock () const
+{
+ return (get_AltitudeEnabled() &&
+ (get_AltitudeMode()
+ == FGAutopilot::FG_VERT_SPEED));
+}
+
+
+/**
+ * Set the autopilot vert speed lock (true=on).
+ */
+void
+FGAutopilot::setAPVertSpeedLock (bool lock)
+{
+ if (lock)
+ set_AltitudeMode(FGAutopilot::FG_VERT_SPEED);
+ if (get_AltitudeMode() == FGAutopilot::FG_VERT_SPEED)
+ set_AltitudeEnabled(lock);
+}
+
/**
* Get the autopilot target altitude in feet.
}
+/**
+ * return blank-separated string of waypoints
+ */
+const char *
+FGAutopilot::getAPwaypoint () const
+{
+ static char wplist[500];
+ char *p = wplist;
+ int WPListsize, i;
+
+ // FIXME: This can cause a possible buffer overflow, EMH
+ if ( globals->get_route()->size() > 0 ) {
+ WPListsize = globals->get_route()->size();
+
+ for (i = 0; i < globals->get_route()->size(); i++ ) {
+ p += sprintf(p, "%5s ",
+ globals->get_route()->get_waypoint(i).get_id().c_str() );
+ }
+ return wplist;
+
+ } else {
+ return "none specified";
+ }
+}
+
+
+/**
+ * set next waypoint (if str='0', then delete next(first) waypoint)
+ */
+void
+FGAutopilot::setAPwaypoint (const char * apt)
+{
+ if (strcmp(apt, "0")==0)
+ {
+ SG_LOG( SG_AUTOPILOT, SG_INFO, "delete of first wp" );
+ if ( globals->get_route()->size() )
+ globals->get_route()->delete_first();
+ return;
+ }
+
+ if ( NewWaypoint( apt ) == 0)
+ SG_LOG( SG_AUTOPILOT, SG_INFO, "Waypoint " << apt << "not in d.b." );
+ else
+ {
+ /* SG_LOG( SG_AUTOPILOT, SG_INFO, "GOOD!" ); */
+ }
+}
+
/**
* Get the autopilot wing leveler lock (true=on).
*/
globals->get_controls()->set_throttle(FGControls::ALL_ENGINES, value);
}
+/**
+ * Get the vertical speed selected
+ */
+double
+FGAutopilot::getAPVertSpeed () const
+{
+ return TargetVertSpeed;
+}
+
+
+/**
+ * Set the selected vertical speed
+ */
+void
+FGAutopilot::setAPVertSpeed (double speed)
+{
+ TargetVertSpeed = speed;
+}
+
// end of newauto.cxx
+
projects / flightgear.git / blobdiff