X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fnewauto.cxx;h=b699c8c9d8e4bee1806a728192512f38381b6efb;hb=73e70fdb29c4c512779b6759d1e6aa487818e841;hp=8fd9fc0c5e6597d17b27c66c35fce758d1bc2e5a;hpb=17c96ae69ed7a3c33c816e821ae88c501ab7cf65;p=flightgear.git diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx index 8fd9fc0c5..b699c8c9d 100644 --- a/src/Autopilot/newauto.cxx +++ b/src/Autopilot/newauto.cxx @@ -5,6 +5,7 @@ // Contributions by Jeff Goeke-Smith // Norman Vine // Curtis Olson +// Wendell Turner // // This program is free software; you can redistribute it and/or // modify it under the terms of the GNU General Public License as @@ -28,32 +29,25 @@ #endif #include // sprintf() +#include // strcmp() #include +#include #include #include #include +#include -#include #include #include #include -#include
#include
#include #include "newauto.hxx" +#include "auto_gui.hxx" -FGAutopilot *current_autopilot; - - -// 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 - /// These statics will eventually go into the class /// they are just here while I am experimenting -- NHV :-) // AutoPilot Gain Adjuster members @@ -70,8 +64,7 @@ extern char *coord_format_lon(float); // constructor -FGAutopilot::FGAutopilot(): -TargetClimbRate(1000 * FEET_TO_METER) +FGAutopilot::FGAutopilot() { } @@ -96,7 +89,7 @@ void FGAutopilot::MakeTargetAltitudeStr( double altitude ) { void FGAutopilot::MakeTargetHeadingStr( double bearing ) { - if( bearing < 0. ) { + if ( bearing < 0. ) { bearing += 360.; } else if (bearing > 360. ) { bearing -= 360.; @@ -105,15 +98,13 @@ void FGAutopilot::MakeTargetHeadingStr( double bearing ) { } -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"); + double ft_s = cur_fdm_state->get_V_ground_speed() - * fgGetInt("/sim/speed-up"); // FIXME: inefficient - double kts = ft_s * FEET_TO_METER * 3600 * METER_TO_NM; + * speedup_node->getIntValue(); + double kts = ft_s * SG_FEET_TO_METER * 3600 * SG_METER_TO_NM; return kts; } @@ -141,7 +132,7 @@ void FGAutopilot::MakeTargetWPStr( double distance ) { if ( size > 0 ) { SGWayPoint wp1 = globals->get_route()->get_waypoint( 0 ); accum += distance; - double eta = accum * METER_TO_NM / get_ground_speed(); + double eta = accum * SG_METER_TO_NM / get_ground_speed(); if ( eta >= 100.0 ) { eta = 99.999; } int major, minor; if ( eta < (1.0/6.0) ) { @@ -152,13 +143,7 @@ void FGAutopilot::MakeTargetWPStr( double distance ) { minor = (int)((eta - (int)eta) * 60.0); sprintf( TargetWP1Str, "%s %.2f NM ETA %d:%02d", wp1.get_id().c_str(), - accum*METER_TO_NM, major, minor ); - // cout << "distance = " << distance*METER_TO_NM - // << " gndsp = " << get_ground_speed() - // << " time = " << eta - // << " major = " << major - // << " minor = " << minor - // << endl; + accum*SG_METER_TO_NM, major, minor ); } // next route @@ -166,7 +151,7 @@ void FGAutopilot::MakeTargetWPStr( double distance ) { SGWayPoint wp2 = globals->get_route()->get_waypoint( 1 ); accum += wp2.get_distance(); - double eta = accum * METER_TO_NM / get_ground_speed(); + double eta = accum * SG_METER_TO_NM / get_ground_speed(); if ( eta >= 100.0 ) { eta = 99.999; } int major, minor; if ( eta < (1.0/6.0) ) { @@ -177,7 +162,7 @@ void FGAutopilot::MakeTargetWPStr( double distance ) { minor = (int)((eta - (int)eta) * 60.0); sprintf( TargetWP2Str, "%s %.2f NM ETA %d:%02d", wp2.get_id().c_str(), - accum*METER_TO_NM, major, minor ); + accum*SG_METER_TO_NM, major, minor ); } // next route @@ -188,7 +173,7 @@ void FGAutopilot::MakeTargetWPStr( double distance ) { SGWayPoint wpn = globals->get_route()->get_waypoint( size - 1 ); - double eta = accum * METER_TO_NM / get_ground_speed(); + double eta = accum * SG_METER_TO_NM / get_ground_speed(); if ( eta >= 100.0 ) { eta = 99.999; } int major, minor; if ( eta < (1.0/6.0) ) { @@ -199,44 +184,138 @@ void FGAutopilot::MakeTargetWPStr( double distance ) { minor = (int)((eta - (int)eta) * 60.0); sprintf( TargetWP3Str, "%s %.2f NM ETA %d:%02d", wpn.get_id().c_str(), - accum*METER_TO_NM, major, minor ); + accum*SG_METER_TO_NM, major, minor ); } } void FGAutopilot::update_old_control_values() { - old_aileron = controls.get_aileron(); - old_elevator = controls.get_elevator(); - old_elevator_trim = controls.get_elevator_trim(); - old_rudder = controls.get_rudder(); + old_aileron = globals->get_controls()->get_aileron(); + old_elevator = globals->get_controls()->get_elevator(); + old_elevator_trim = globals->get_controls()->get_elevator_trim(); + old_rudder = globals->get_controls()->get_rudder(); } // Initialize autopilot subsystem -void FGAutopilot::init() { - FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" ); +void FGAutopilot::init () +{ + SG_LOG( SG_AUTOPILOT, SG_INFO, "Init AutoPilot Subsystem" ); + + // bind data input property nodes... + latitude_node = fgGetNode("/position/latitude-deg", true); + longitude_node = fgGetNode("/position/longitude-deg", true); + 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); + TargetDescentRate + = fgGetNode("/autopilot/config/target-descent-rate-fpm", true); + min_climb = fgGetNode("/autopilot/config/min-climb-speed-kt", true); + best_climb = fgGetNode("/autopilot/config/best-climb-speed-kt", true); + elevator_adj_factor + = fgGetNode("/autopilot/config/elevator-adj-factor", true); + integral_contrib + = fgGetNode("/autopilot/config/integral-contribution", true); + zero_pitch_throttle + = fgGetNode("/autopilot/config/zero-pitch-throttle", true); + zero_pitch_trim_full_throttle + = fgGetNode("/autopilot/config/zero-pitch-trim-full-throttle", true); + 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/target-climb-rate-fpm", 500); + if ( TargetDescentRate->getFloatValue() < 1 ) + fgSetFloat( "/autopilot/config/target-descent-rate-fpm", 1000 ); + if ( min_climb->getFloatValue() < 1) + fgSetFloat( "/autopilot/config/min-climb-speed-kt", 70 ); + if (best_climb->getFloatValue() < 1) + fgSetFloat( "/autopilot/config/best-climb-speed-kt", 120 ); + if (elevator_adj_factor->getFloatValue() < 1) + fgSetFloat( "/autopilot/config/elevator-adj-factor", 5000 ); + if ( integral_contrib->getFloatValue() < 0.0000001 ) + fgSetFloat( "/autopilot/config/integral-contribution", 0.01 ); + if ( zero_pitch_throttle->getFloatValue() < 0.0000001 ) + 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; - sg_srandom_time(); - DGTargetHeading = sg_random() * 360.0; + 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 = FGBFI::getLatitude(); - old_lon = FGBFI::getLongitude(); + old_lat = latitude_node->getDoubleValue(); + old_lon = longitude_node->getDoubleValue(); // set_WayPoint( old_lon, old_lat, 0.0, "default" ); MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() ); - 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 ); + MakeTargetHeadingStr( TargetHeading ); // These eventually need to be read from current_aircaft somehow. @@ -246,9 +325,6 @@ void FGAutopilot::init() { // 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; @@ -256,22 +332,98 @@ void FGAutopilot::init() { // 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 ) update_old_control_values(); - - // Initialize GUI components of autopilot - // NewTgtAirportInit(); - // fgAPAdjustInit() ; - // NewHeadingInit(); - // NewAltitudeInit(); }; +void +FGAutopilot::bind () +{ + // Autopilot control property get/set bindings + fgTie("/autopilot/locks/altitude", this, + &FGAutopilot::getAPAltitudeLock, &FGAutopilot::setAPAltitudeLock); + fgSetArchivable("/autopilot/locks/altitude"); + fgTie("/autopilot/settings/altitude-ft", this, + &FGAutopilot::getAPAltitude, &FGAutopilot::setAPAltitude); + fgSetArchivable("/autopilot/settings/altitude-ft"); + fgTie("/autopilot/locks/glide-slope", this, + &FGAutopilot::getAPGSLock, &FGAutopilot::setAPGSLock); + fgSetArchivable("/autopilot/locks/glide-slope"); + fgSetDouble("/autopilot/settings/altitude-ft", 3000.0f); + fgTie("/autopilot/locks/terrain", this, + &FGAutopilot::getAPTerrainLock, &FGAutopilot::setAPTerrainLock); + fgSetArchivable("/autopilot/locks/terrain"); + fgTie("/autopilot/settings/climb-rate-fpm", this, + &FGAutopilot::getAPClimb, &FGAutopilot::setAPClimb, false); + fgSetArchivable("/autopilot/settings/climb-rate-fpm"); + 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"); + fgTie("/autopilot/locks/nav[0]", this, + &FGAutopilot::getAPNAV1Lock, &FGAutopilot::setAPNAV1Lock); + fgSetArchivable("/autopilot/locks/nav[0]"); + fgTie("/autopilot/locks/auto-throttle", this, + &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); + fgSetArchivable("/autopilot/control-overrides/rudder"); + fgTie("/autopilot/control-overrides/elevator", this, + &FGAutopilot::getAPElevatorControl, + &FGAutopilot::setAPElevatorControl); + fgSetArchivable("/autopilot/control-overrides/elevator"); + fgTie("/autopilot/control-overrides/throttle", this, + &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 +FGAutopilot::unbind () +{ +} // Reset the autopilot system void FGAutopilot::reset() { @@ -279,11 +431,12 @@ void FGAutopilot::reset() { 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; - TargetHeading = 0.0; // default direction, due north + // TargetHeading = 0.0; // default direction, due north MakeTargetHeadingStr( TargetHeading ); - TargetAltitude = 3000; // default altitude in meters + // TargetAltitude = 3000; // default altitude in meters MakeTargetAltitudeStr( TargetAltitude ); alt_error_accum = 0.0; @@ -291,12 +444,8 @@ void FGAutopilot::reset() { update_old_control_values(); - sprintf( NewTgtAirportId, "%s", fgGetString("/sim/startup/airport-id").c_str() ); + sprintf( NewTgtAirportId, "%s", fgGetString("/sim/presets/airport-id") ); - // TargetLatitude = FGBFI::getLatitude(); - // TargetLongitude = FGBFI::getLongitude(); - // set_WayPoint( FGBFI::getLongitude(), FGBFI::getLatitude(), 0.0, "reset" ); - MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() ); } @@ -305,13 +454,10 @@ static double NormalizeDegrees( double Input ) { // normalize the input to the range (-180,180] // Input should not be greater than -360 to 360. // Current rules send the output to an undefined state. - if ( Input > 180 ) - while(Input > 180 ) - Input -= 360; - else if ( Input <= -180 ) - while ( Input <= -180 ) - Input += 360; - return ( Input ); + while ( Input > 180.0 ) { Input -= 360.0; } + while ( Input <= -180.0 ) { Input += 360.0; } + + return Input; }; static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 ) { @@ -340,20 +486,24 @@ static double LinearExtrapolate( double x, double x1, double y1, double x2, doub }; -int FGAutopilot::run() { - // Remove the following lines when the calling funcitons start - // passing in the data pointer +void +FGAutopilot::update (double dt) +{ // get control settings - // double aileron = FGBFI::getAileron(); - // double elevator = FGBFI::getElevator(); - // double elevator_trim = FGBFI::getElevatorTrim(); - // double rudder = FGBFI::getRudder(); + 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 ); - 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 if( fabs(aileron - old_aileron) > disengage_threshold || @@ -382,10 +532,10 @@ int FGAutopilot::run() { // heading hold if ( heading_hold == true ) { if ( heading_mode == FG_DG_HEADING_LOCK ) { - // cout << "DG heading = " << FGSteam::get_DG_deg() - // << " DG error = " << FGSteam::get_DG_err() << endl; - - 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 ); @@ -395,39 +545,49 @@ int FGAutopilot::run() { // coordinator zero'd } else if ( heading_mode == FG_TRUE_HEADING_LOCK ) { // leave "true" target heading as is + while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; } + while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; } + MakeTargetHeadingStr( TargetHeading ); } else if ( heading_mode == FG_HEADING_NAV1 ) { // track the NAV1 heading needle deflection // 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() * METER_TO_NM); - if ( adjustment < -30.0 ) { adjustment = -30.0; } - if ( adjustment > 30.0 ) { adjustment = 30.0; } + 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 @@ -436,7 +596,6 @@ int FGAutopilot::run() { while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; } MakeTargetHeadingStr( TargetHeading ); - // cout << "target course (true) = " << TargetHeading << endl; } else if ( heading_mode == FG_HEADING_WAYPOINT ) { // update target heading to waypoint @@ -475,9 +634,6 @@ int FGAutopilot::run() { // corrected_course = course - wp_course; TargetHeading = NormalizeDegrees(wp_course); } else { - 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(); @@ -489,9 +645,9 @@ int FGAutopilot::run() { 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 = FGBFI::getHeading(); + TargetHeading = heading_node->getDoubleValue(); } } MakeTargetHeadingStr( TargetHeading ); @@ -502,13 +658,12 @@ int FGAutopilot::run() { 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 turn = + fgGetDouble("/instrumentation/turn-indicator/indicated-turn-rate"); 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 ); + SG_CLAMP_RANGE( AileronSet, -1.0, 1.0 ); + globals->get_controls()->set_aileron( AileronSet ); + globals->get_controls()->set_rudder( AileronSet / 4.0 ); } else { // steer towards the target heading @@ -518,12 +673,15 @@ int FGAutopilot::run() { double AileronSet; RelHeading - = NormalizeDegrees( TargetHeading - FGBFI::getHeading() ); + = NormalizeDegrees( TargetHeading + - heading_node->getDoubleValue() ); // 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 ); - + 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 ) { @@ -551,9 +709,10 @@ int FGAutopilot::run() { // Compare Target roll to Current Roll, Generate Rel Roll - FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll ); + SG_LOG( SG_COCKPIT, SG_BULK, "TargetRoll: " << TargetRoll ); - RelRoll = NormalizeDegrees( TargetRoll - FGBFI::getRoll() ); + RelRoll = NormalizeDegrees( TargetRoll + - roll_node->getDoubleValue() ); // Check if we are further from heading than the roll out // smooth point @@ -571,8 +730,8 @@ int FGAutopilot::run() { MaxAileron ); } - controls.set_aileron( AileronSet ); - controls.set_rudder( AileronSet / 4.0 ); + globals->get_controls()->set_aileron( AileronSet ); + globals->get_controls()->set_rudder( AileronSet / 4.0 ); // controls.set_rudder( 0.0 ); } } @@ -585,85 +744,95 @@ int FGAutopilot::run() { double prop_adj, int_adj, total_adj; if ( altitude_mode == FG_ALTITUDE_LOCK ) { - // normal altitude hold - // cout << "TargetAltitude = " << TargetAltitude - // << "Altitude = " << FGBFI::getAltitude() * FEET_TO_METER - // << endl; climb_rate = - ( TargetAltitude - FGSteam::get_ALT_ft() * 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 y = (FGBFI::getAltitude() - - current_radiostack->get_nav1_elev()) * FEET_TO_METER; + double x = current_radiostack->get_navcom1()->get_nav_gs_dist(); + double y = (altitude_node->getDoubleValue() + - current_radiostack->get_navcom1()->get_nav_elev()) * SG_FEET_TO_METER; double current_angle = atan2( y, x ) * SGD_RADIANS_TO_DEGREES; - // cout << "current angle = " << current_angle << endl; - - double target_angle = current_radiostack->get_nav1_target_gs(); - // cout << "target angle = " << target_angle << endl; + double target_angle = current_radiostack->get_navcom1()->get_nav_target_gs(); double gs_diff = target_angle - current_angle; - // cout << "difference from desired = " << gs_diff << endl; // convert desired vertical path angle into a climb rate double des_angle = current_angle - 10 * gs_diff; - // cout << "desired angle = " << des_angle << endl; - // convert to meter/min - // cout << "raw ground speed = " << cur_fdm_state->get_V_ground_speed() << endl; - double horiz_vel = cur_fdm_state->get_V_ground_speed() - * FEET_TO_METER * 60.0; - // cout << "Horizontal vel = " << horiz_vel << endl; + // 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; - // cout << "climb_rate = " << climb_rate << endl; /* climb_error_accum += gs_diff * 2.0; */ /* climb_rate = gs_diff * 200.0 + climb_error_accum; */ } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) { // brain dead ground hugging with no look ahead climb_rate = - ( TargetAGL - FGBFI::getAGL()*FEET_TO_METER ) * 16.0; - // cout << "target agl = " << TargetAGL - // << " current agl = " << fgAPget_agl() - // << " target climb rate = " << climb_rate - // << endl; + ( TargetAGL - altitude_agl_node->getDoubleValue() + * 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 ) { + if ( speed < min_climb->getFloatValue() ) { max_climb = 0.0; - } else if ( speed < best_climb ) { - max_climb = ((best_climb - min_climb) - (best_climb - speed)) - * fabs(TargetClimbRate) - / (best_climb - min_climb); + } else if ( speed < best_climb->getFloatValue() ) { + max_climb = ((best_climb->getFloatValue() + - min_climb->getFloatValue()) + - (best_climb->getFloatValue() - speed)) + * fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER) + / (best_climb->getFloatValue() - min_climb->getFloatValue()); } else { - max_climb = ( speed - best_climb ) * 10.0 + fabs(TargetClimbRate); + max_climb = ( speed - best_climb->getFloatValue() ) * 10.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 > fabs(TargetClimbRate) ) { - climb_rate = fabs(TargetClimbRate); - } + if (altitude_mode != FG_VERT_SPEED) { - if ( climb_rate > max_climb ) { - climb_rate = max_climb; - } + // 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 + = fabs(TargetClimbRate->getFloatValue() * SG_FEET_TO_METER); + } - if ( climb_rate < -fabs(TargetClimbRate) ) { - climb_rate = -fabs(TargetClimbRate); - } - // cout << "Target climb rate = " << TargetClimbRate << endl; - // cout << "given our speed, modified desired climb rate = " - // << climb_rate * METER_TO_FEET - // << " fpm" << endl; + if ( climb_rate > max_climb ) { + climb_rate = max_climb; + } - error = FGBFI::getVerticalSpeed() * FEET_TO_METER - climb_rate; - // cout << "climb rate = " << FGBFI::getVerticalSpeed() - // << " vsi rate = " << FGSteam::get_VSI_fps() << endl; + if ( climb_rate < + -fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER) ) { + climb_rate + = -fabs(TargetDescentRate->getFloatValue() * SG_FEET_TO_METER); + } + + } + + error = vertical_speed_node->getDoubleValue() * 60 + - climb_rate * SG_METER_TO_FEET; // accumulate the error under the curve ... this really should // be *= delta t @@ -672,25 +841,34 @@ int FGAutopilot::run() { // 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 / 20000.0; + + // 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 / 2000.0; - - total_adj = 0.9 * prop_adj + 0.1 * int_adj; - // if ( total_adj > 0.6 ) { - // total_adj = 0.6; - // } else if ( total_adj < -0.2 ) { - // total_adj = -0.2; - // } - if ( total_adj > 1.0 ) { - total_adj = 1.0; - } else if ( total_adj < -1.0 ) { - total_adj = -1.0; - } + prop_adj = prop_error / elev_adj_factor; + + total_adj = ((double) 1.0 - (double) integral_contrib->getFloatValue()) * prop_adj + + (double) integral_contrib->getFloatValue() * int_adj; + + // stop on autopilot trim at 30% +/- +// if ( total_adj > 0.3 ) { +// total_adj = 0.3; +// } else if ( total_adj < -0.3 ) { +// total_adj = -0.3; +// } - controls.set_elevator( total_adj ); + // adjust for throttle pitch gain + total_adj += ((current_throttle->getFloatValue() - zero_pitch_throttle->getFloatValue()) + / (1 - zero_pitch_throttle->getFloatValue())) + * zero_pitch_trim_full_throttle->getFloatValue(); + + globals->get_controls()->set_elevator_trim( total_adj ); } // auto throttle @@ -698,8 +876,17 @@ int FGAutopilot::run() { double error; double prop_error, int_error; double prop_adj, int_adj, total_adj; + double lookahead; + + // estimate speed in 10 seconds + lookahead = airspeed_node->getFloatValue() + ( airspeed_node->getFloatValue() - previous_speed) * (10/(dt + 0.000001)); + previous_speed = airspeed_node->getFloatValue(); - error = TargetSpeed - get_speed(); + // 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 @@ -715,21 +902,26 @@ int FGAutopilot::run() { 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; } - controls.set_throttle( FGControls::ALL_ENGINES, total_adj ); + globals->get_controls()->set_throttle( FGControls::ALL_ENGINES, + current_ap_throttle ); } #ifdef THIS_CODE_IS_NOT_USED @@ -761,17 +953,17 @@ int FGAutopilot::run() { // stash this runs control settings // update_old_control_values(); - old_aileron = controls.get_aileron(); - old_elevator = controls.get_elevator(); - old_elevator_trim = controls.get_elevator_trim(); - old_rudder = controls.get_rudder(); + old_aileron = globals->get_controls()->get_aileron(); + old_elevator = globals->get_controls()->get_elevator(); + old_elevator_trim = globals->get_controls()->get_elevator_trim(); + old_rudder = globals->get_controls()->get_rudder(); // for cross track error old_lat = lat; old_lon = lon; - // Ok, we are done - return 0; + // Ok, we are done + SG_LOG( SG_ALL, SG_DEBUG, "FGAutopilot::run( returns )" ); } @@ -779,26 +971,24 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) { 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 ) { // set heading hold to current heading - TargetHeading = FGBFI::getHeading(); + TargetHeading = heading_node->getDoubleValue(); } else if ( heading_mode == FG_HEADING_WAYPOINT ) { if ( globals->get_route()->size() ) { double course, distance; - old_lat = FGBFI::getLatitude(); - old_lon = FGBFI::getLongitude(); + old_lat = latitude_node->getDoubleValue(); + old_lon = longitude_node->getDoubleValue(); waypoint = globals->get_route()->get_first(); - waypoint.CourseAndDistance( FGBFI::getLongitude(), - FGBFI::getLatitude(), - FGBFI::getLatitude() * FEET_TO_METER, + waypoint.CourseAndDistance( longitude_node->getDoubleValue(), + latitude_node->getDoubleValue(), + altitude_node->getDoubleValue() + * SG_FEET_TO_METER, &course, &distance ); TargetHeading = course; TargetDistance = distance; @@ -808,19 +998,18 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) { 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 * METER_TO_FEET ); + MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET ); } - FG_LOG( FG_COCKPIT, FG_INFO, " set_HeadingMode: ( " + SG_LOG( SG_COCKPIT, SG_INFO, " set_HeadingMode: ( " << get_TargetLatitude() << " " << get_TargetLongitude() << " ) " ); } else { // no more way points, default to heading lock. heading_mode = FG_TC_HEADING_LOCK; - // TargetHeading = FGBFI::getHeading(); } } @@ -832,105 +1021,56 @@ void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) { 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 ( TargetAltitude < FGBFI::getAGL() * FEET_TO_METER ) { - // TargetAltitude = FGBFI::getAltitude() * FEET_TO_METER; + if ( altitude_mode == DEFAULT_AP_ALTITUDE_LOCK ) { + if ( TargetAltitude < altitude_agl_node->getDoubleValue() + * SG_FEET_TO_METER ) { } - if ( fgGetString("/sim/startup/units") == "feet" ) { - MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET ); + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) { + MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET ); } else { - MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET ); + MakeTargetAltitudeStr( TargetAltitude * SG_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; + TargetAGL = altitude_agl_node->getDoubleValue() * SG_FEET_TO_METER; - if ( fgGetString("/sim/startup/units") == "feet" ) { - MakeTargetAltitudeStr( TargetAGL * METER_TO_FEET ); + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) { + MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET ); } else { - MakeTargetAltitudeStr( TargetAGL * METER_TO_FEET ); + MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET ); } } update_old_control_values(); - FG_LOG( FG_COCKPIT, FG_INFO, " set_AltitudeMode():" ); + SG_LOG( SG_COCKPIT, SG_INFO, " set_AltitudeMode():" ); } -#if 0 -static inline double get_aoa( void ) { - return( cur_fdm_state->get_Gamma_vert_rad() * SGD_RADIANS_TO_DEGREES ); -} - -static inline double fgAPget_latitude( void ) { - return( cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES ); -} - -static inline double fgAPget_longitude( void ) { - return( cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES ); -} - -static inline double fgAPget_roll( void ) { - return( cur_fdm_state->get_Phi() * SGD_RADIANS_TO_DEGREES ); -} - -static inline double get_pitch( void ) { - return( cur_fdm_state->get_Theta() ); -} - -double fgAPget_heading( void ) { - return( cur_fdm_state->get_Psi() * SGD_RADIANS_TO_DEGREES ); -} - -static inline double fgAPget_altitude( void ) { - return( cur_fdm_state->get_Altitude() * FEET_TO_METER ); -} - -static inline double fgAPget_climb( void ) { - // return in meters per minute - return( cur_fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 ); -} - -static inline double get_sideslip( void ) { - return( cur_fdm_state->get_Beta() ); -} - -static inline double fgAPget_agl( void ) { - double agl; - - agl = cur_fdm_state->get_Altitude() * FEET_TO_METER - - scenery.cur_elev; - - return( agl ); -} -#endif - - void FGAutopilot::AltitudeSet( double new_altitude ) { double target_alt = new_altitude; + altitude_mode = DEFAULT_AP_ALTITUDE_LOCK; - // cout << "new altitude = " << new_altitude << endl; - - if ( fgGetString("/sim/startup/units") == "feet" ) { - target_alt = new_altitude * FEET_TO_METER; + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) { + target_alt = new_altitude * SG_FEET_TO_METER; } - if( target_alt < scenery.cur_elev ) { - target_alt = scenery.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; - - // cout << "TargetAltitude = " << TargetAltitude << endl; - if ( fgGetString("/sim/startup/units") == "feet" ) { - target_alt *= METER_TO_FEET; + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) { + target_alt *= SG_METER_TO_FEET; } // ApAltitudeDialogInput->setValue((float)target_alt); MakeTargetAltitudeStr( target_alt ); @@ -943,18 +1083,14 @@ void FGAutopilot::AltitudeAdjust( double inc ) { double target_alt, target_agl; - if ( fgGetString("/sim/startup/units") == "feet" ) { - target_alt = TargetAltitude * METER_TO_FEET; - target_agl = TargetAGL * METER_TO_FEET; + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) { + target_alt = TargetAltitude * SG_METER_TO_FEET; + target_agl = TargetAGL * SG_METER_TO_FEET; } else { target_alt = TargetAltitude; target_agl = TargetAGL; } - // cout << "target_agl = " << target_agl << endl; - // cout << "target_agl / inc = " << target_agl / inc << endl; - // cout << "(int)(target_agl / inc) = " << (int)(target_agl / inc) << endl; - if ( fabs((int)(target_alt / inc) * inc - target_alt) < SG_EPSILON ) { target_alt += inc; } else { @@ -967,20 +1103,20 @@ void FGAutopilot::AltitudeAdjust( double inc ) target_agl = ( int ) ( target_agl / inc ) * inc + inc; } - if ( fgGetString("/sim/startup/units") == "feet" ) { - target_alt *= FEET_TO_METER; - target_agl *= FEET_TO_METER; + 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 ( fgGetString("/sim/startup/units") == "feet" ) - target_alt *= METER_TO_FEET; - if ( fgGetString("/sim/startup/units") == "feet" ) - target_agl *= METER_TO_FEET; + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) + target_alt *= SG_METER_TO_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 ); @@ -994,7 +1130,7 @@ void FGAutopilot::HeadingAdjust( double inc ) { 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 ) { @@ -1010,32 +1146,460 @@ void FGAutopilot::HeadingAdjust( double inc ) { void FGAutopilot::HeadingSet( double new_heading ) { - heading_mode = FG_DG_HEADING_LOCK; - - new_heading = NormalizeDegrees( new_heading ); - DGTargetHeading = new_heading; - // following cast needed ambiguous plib - // ApHeadingDialogInput -> setValue ((float)APData->TargetHeading ); - MakeTargetHeadingStr( DGTargetHeading ); + heading_mode = DEFAULT_AP_HEADING_LOCK; + if( heading_mode == FG_TRUE_HEADING_LOCK ) { + new_heading = NormalizeDegrees( new_heading ); + TargetHeading = new_heading; + MakeTargetHeadingStr( TargetHeading ); + } else { + heading_mode = FG_DG_HEADING_LOCK; + + new_heading = NormalizeDegrees( new_heading ); + DGTargetHeading = new_heading; + // following cast needed ambiguous plib + // ApHeadingDialogInput -> setValue ((float)APData->TargetHeading ); + MakeTargetHeadingStr( DGTargetHeading ); + } update_old_control_values(); } 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 = FGBFI::getAirspeed(); - 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(); - FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: (" + SG_LOG( SG_COCKPIT, SG_INFO, " fgAPSetAutoThrottle: (" << auto_throttle << ") " << TargetSpeed ); } + + + + +//////////////////////////////////////////////////////////////////////// +// Kludged methods for tying to properties. +// +// These should change eventually; they all used to be static +// functions. +//////////////////////////////////////////////////////////////////////// + +/** + * Get the autopilot altitude lock (true=on). + */ +bool +FGAutopilot::getAPAltitudeLock () const +{ + return (get_AltitudeEnabled() && + get_AltitudeMode() + == DEFAULT_AP_ALTITUDE_LOCK); +} + + +/** + * Set the autopilot altitude lock (true=on). + */ +void +FGAutopilot::setAPAltitudeLock (bool lock) +{ + if (lock) + set_AltitudeMode(DEFAULT_AP_ALTITUDE_LOCK); + if (get_AltitudeMode() == DEFAULT_AP_ALTITUDE_LOCK) + set_AltitudeEnabled(lock); +} + + +/** + * Get the autopilot target altitude in feet. + */ +double +FGAutopilot::getAPAltitude () const +{ + return get_TargetAltitude() * SG_METER_TO_FEET; +} + + +/** + * Set the autopilot target altitude in feet. + */ +void +FGAutopilot::setAPAltitude (double altitude) +{ + set_TargetAltitude( altitude * SG_FEET_TO_METER ); +} + +/** + * Get the autopilot altitude lock (true=on). + */ +bool +FGAutopilot::getAPGSLock () const +{ + return (get_AltitudeEnabled() && + (get_AltitudeMode() + == FGAutopilot::FG_ALTITUDE_GS1)); +} + + +/** + * Set the autopilot altitude lock (true=on). + */ +void +FGAutopilot::setAPGSLock (bool lock) +{ + if (lock) + set_AltitudeMode(FGAutopilot::FG_ALTITUDE_GS1); + if (get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_GS1) + set_AltitudeEnabled(lock); +} + + +/** + * Get the autopilot terrain lock (true=on). + */ +bool +FGAutopilot::getAPTerrainLock () const +{ + return (get_AltitudeEnabled() && + (get_AltitudeMode() + == FGAutopilot::FG_ALTITUDE_TERRAIN)); +} + + +/** + * Set the autopilot terrain lock (true=on). + */ +void +FGAutopilot::setAPTerrainLock (bool lock) +{ + if (lock) { + set_AltitudeMode(FGAutopilot::FG_ALTITUDE_TERRAIN); + set_TargetAGL(fgGetFloat("/position/altitude-agl-ft") * + SG_FEET_TO_METER); + } + if (get_AltitudeMode() == FGAutopilot::FG_ALTITUDE_TERRAIN) + 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. + */ +double +FGAutopilot::getAPClimb () const +{ + return get_TargetClimbRate() * SG_METER_TO_FEET; +} + + +/** + * Set the autopilot target altitude in feet. + */ +void +FGAutopilot::setAPClimb (double rate) +{ + set_TargetClimbRate( rate * SG_FEET_TO_METER ); +} + + +/** + * Get the autopilot heading lock (true=on). + */ +bool +FGAutopilot::getAPHeadingLock () const +{ + return + (get_HeadingEnabled() && + get_HeadingMode() == DEFAULT_AP_HEADING_LOCK); +} + + +/** + * Set the autopilot heading lock (true=on). + */ +void +FGAutopilot::setAPHeadingLock (bool lock) +{ + if (lock) + set_HeadingMode(DEFAULT_AP_HEADING_LOCK); + if (get_HeadingMode() == DEFAULT_AP_HEADING_LOCK) + set_HeadingEnabled(lock); +} + + +/** + * Get the autopilot heading bug in degrees. + */ +double +FGAutopilot::getAPHeadingBug () const +{ + return get_DGTargetHeading(); +} + + +/** + * Set the autopilot heading bug in degrees. + */ +void +FGAutopilot::setAPHeadingBug (double heading) +{ + set_DGTargetHeading( heading ); +} + + +/** + * 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). + */ +bool +FGAutopilot::getAPWingLeveler () const +{ + return + (get_HeadingEnabled() && + get_HeadingMode() == FGAutopilot::FG_TC_HEADING_LOCK); +} + + +/** + * Set the autopilot wing leveler lock (true=on). + */ +void +FGAutopilot::setAPWingLeveler (bool lock) +{ + if (lock) + set_HeadingMode(FGAutopilot::FG_TC_HEADING_LOCK); + if (get_HeadingMode() == FGAutopilot::FG_TC_HEADING_LOCK) + set_HeadingEnabled(lock); +} + +/** + * Return true if the autopilot is locked to NAV1. + */ +bool +FGAutopilot::getAPNAV1Lock () const +{ + return + (get_HeadingEnabled() && + get_HeadingMode() == FGAutopilot::FG_HEADING_NAV1); +} + + +/** + * Set the autopilot NAV1 lock. + */ +void +FGAutopilot::setAPNAV1Lock (bool lock) +{ + if (lock) + set_HeadingMode(FGAutopilot::FG_HEADING_NAV1); + if (get_HeadingMode() == FGAutopilot::FG_HEADING_NAV1) + set_HeadingEnabled(lock); +} + +/** + * Get the autopilot autothrottle lock. + */ +bool +FGAutopilot::getAPAutoThrottleLock () const +{ + return get_AutoThrottleEnabled(); +} + + +/** + * Set the autothrottle lock. + */ +void +FGAutopilot::setAPAutoThrottleLock (bool lock) +{ + set_AutoThrottleEnabled(lock); +} + + +// kludge +double +FGAutopilot::getAPRudderControl () const +{ + if (getAPHeadingLock()) + return get_TargetHeading(); + else + return globals->get_controls()->get_rudder(); +} + +// kludge +void +FGAutopilot::setAPRudderControl (double value) +{ + if (getAPHeadingLock()) { + SG_LOG(SG_GENERAL, SG_DEBUG, "setAPRudderControl " << value ); + value -= get_TargetHeading(); + HeadingAdjust(value < 0.0 ? -1.0 : 1.0); + } else { + globals->get_controls()->set_rudder(value); + } +} + +// kludge +double +FGAutopilot::getAPElevatorControl () const +{ + if (getAPAltitudeLock()) + return get_TargetAltitude(); + else + return globals->get_controls()->get_elevator(); +} + +// kludge +void +FGAutopilot::setAPElevatorControl (double value) +{ + if (value != 0 && getAPAltitudeLock()) { + SG_LOG(SG_GENERAL, SG_DEBUG, "setAPElevatorControl " << value ); + value -= get_TargetAltitude(); + AltitudeAdjust(value < 0.0 ? 100.0 : -100.0); + } else { + globals->get_controls()->set_elevator(value); + } +} + +// kludge +double +FGAutopilot::getAPThrottleControl () const +{ + if (getAPAutoThrottleLock()) + return 0.0; // always resets + else + return globals->get_controls()->get_throttle(0); +} + +// kludge +void +FGAutopilot::setAPThrottleControl (double value) +{ + if (getAPAutoThrottleLock()) + AutoThrottleAdjust(value < 0.0 ? -0.01 : 0.01); + else + 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 +