X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fnewauto.cxx;h=a54eaf8fe67ea72395271e73d06fee02a0ea90f6;hb=26c6f3f407a94e5faf414479e2f742a27e756ce5;hp=c5a83ec985e45d963aa22389b68495778db27a52;hpb=6bc7ed9ba2de059b276dc00e0e1fe8aa340b191c;p=flightgear.git diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx index c5a83ec98..a54eaf8fe 100644 --- a/src/Autopilot/newauto.cxx +++ b/src/Autopilot/newauto.cxx @@ -31,13 +31,14 @@ #include #include -#include +#include +#include +#include #include #include #include -#include
-#include
+#include
#include #include "newauto.hxx" @@ -49,7 +50,8 @@ 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; // fpm +// const double ideal_climb_rate = 500.0 * SG_FEET_TO_METER; // fpm -> mpm +// const double ideal_decent_rate = 1000.0 * SG_FEET_TO_METER; // fpm -> mpm /// These statics will eventually go into the class /// they are just here while I am experimenting -- NHV :-) @@ -66,9 +68,20 @@ extern char *coord_format_lat(float); extern char *coord_format_lon(float); +// constructor +FGAutopilot::FGAutopilot(): +TargetClimbRate(500 * SG_FEET_TO_METER), +TargetDecentRate(1000 * SG_FEET_TO_METER) +{ +} + +// destructor +FGAutopilot::~FGAutopilot() {} + + void FGAutopilot::MakeTargetLatLonStr( double lat, double lon ) { - sprintf( TargetLatitudeStr , "%s", coord_format_lat(TargetLatitude) ); - sprintf( TargetLongitudeStr, "%s", coord_format_lon(TargetLongitude) ); + sprintf( TargetLatitudeStr , "%s", coord_format_lat(get_TargetLatitude())); + sprintf( TargetLongitudeStr, "%s", coord_format_lon(get_TargetLongitude())); sprintf( TargetLatLonStr, "%s %s", TargetLatitudeStr, TargetLongitudeStr ); } @@ -98,64 +111,143 @@ static inline double get_speed( void ) { 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() - * current_options.get_speed_up();; - 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; } -void FGAutopilot::MakeTargetDistanceStr( double distance ) { - double eta = distance*METER_TO_NM / get_ground_speed(); - if ( eta >= 100.0 ) { eta = 99.999; } - int major, minor; - if ( eta < (1.0/6.0) ) { - // within 10 minutes, bump up to min/secs - eta *= 60.0; +void FGAutopilot::MakeTargetWPStr( double distance ) { + static time_t last_time = 0; + time_t current_time = time(NULL); + if ( last_time == current_time ) { + return; + } + + last_time = current_time; + + double accum = 0.0; + + int size = globals->get_route()->size(); + + // start by wiping the strings + TargetWP1Str[0] = 0; + TargetWP2Str[0] = 0; + TargetWP3Str[0] = 0; + + // current route + if ( size > 0 ) { + SGWayPoint wp1 = globals->get_route()->get_waypoint( 0 ); + accum += distance; + 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) ) { + // within 10 minutes, bump up to min/secs + eta *= 60.0; + } + major = (int)eta; + minor = (int)((eta - (int)eta) * 60.0); + sprintf( TargetWP1Str, "%s %.2f NM ETA %d:%02d", + wp1.get_id().c_str(), + accum*SG_METER_TO_NM, major, minor ); + // cout << "distance = " << distance*SG_METER_TO_NM + // << " gndsp = " << get_ground_speed() + // << " time = " << eta + // << " major = " << major + // << " minor = " << minor + // << endl; + } + + // next route + if ( size > 1 ) { + SGWayPoint wp2 = globals->get_route()->get_waypoint( 1 ); + accum += wp2.get_distance(); + + 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) ) { + // within 10 minutes, bump up to min/secs + eta *= 60.0; + } + major = (int)eta; + minor = (int)((eta - (int)eta) * 60.0); + sprintf( TargetWP2Str, "%s %.2f NM ETA %d:%02d", + wp2.get_id().c_str(), + accum*SG_METER_TO_NM, major, minor ); + } + + // next route + if ( size > 2 ) { + for ( int i = 2; i < size; ++i ) { + accum += globals->get_route()->get_waypoint( i ).get_distance(); + } + + SGWayPoint wpn = globals->get_route()->get_waypoint( size - 1 ); + + 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) ) { + // within 10 minutes, bump up to min/secs + eta *= 60.0; + } + major = (int)eta; + minor = (int)((eta - (int)eta) * 60.0); + sprintf( TargetWP3Str, "%s %.2f NM ETA %d:%02d", + wpn.get_id().c_str(), + accum*SG_METER_TO_NM, major, minor ); } - major = (int)eta; - minor = (int)((eta - (int)eta) * 60.0); - sprintf( TargetDistanceStr, "APDistance %.2f NM ETA %d:%02d", - distance*METER_TO_NM, major, minor ); - // cout << "distance = " << distance*METER_TO_NM - // << " gndsp = " << get_ground_speed() - // << " time = " << eta - // << " major = " << major - // << " minor = " << minor - // << endl; } void FGAutopilot::update_old_control_values() { - old_aileron = FGBFI::getAileron(); - old_elevator = FGBFI::getElevator(); - old_elevator_trim = FGBFI::getElevatorTrim(); - old_rudder = FGBFI::getRudder(); + 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" ); + SG_LOG( SG_AUTOPILOT, SG_INFO, "Init AutoPilot Subsystem" ); + + 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); + heading_node = fgGetNode("/orientation/heading-deg", true); + roll_node = fgGetNode("/orientation/roll-deg", true); 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; + + sg_srandom_time(); + DGTargetHeading = sg_random() * 360.0; // Initialize target location to startup location - old_lat = TargetLatitude = FGBFI::getLatitude(); - old_lon = TargetLongitude = FGBFI::getLongitude(); + old_lat = latitude_node->getDoubleValue(); + old_lon = longitude_node->getDoubleValue(); + // set_WayPoint( old_lon, old_lat, 0.0, "default" ); - MakeTargetLatLonStr( TargetLatitude, TargetLongitude); + 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. @@ -198,11 +290,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; @@ -210,11 +303,9 @@ void FGAutopilot::reset() { update_old_control_values(); - sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() ); + sprintf( NewTgtAirportId, "%s", fgGetString("/sim/startup/airport-id").c_str() ); - TargetLatitude = FGBFI::getLatitude(); - TargetLongitude = FGBFI::getLongitude(); - MakeTargetLatLonStr( TargetLatitude, TargetLongitude ); + MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() ); } @@ -238,7 +329,7 @@ static double LinearExtrapolate( double x, double x1, double y1, double x2, doub // Could be // static double y = 0.0; // double dx = x2 -x1; - // if( (dx < -FG_EPSILON ) || ( dx > FG_EPSILON ) ) + // if( (dx < -SG_EPSILON ) || ( dx > SG_EPSILON ) ) // { double m, b, y; // the constants to find in y=mx+b @@ -262,14 +353,14 @@ int FGAutopilot::run() { // passing in the data pointer // get control settings - // double aileron = FGBFI::getAileron(); - // double elevator = FGBFI::getElevator(); - // double elevator_trim = FGBFI::getElevatorTrim(); - // double rudder = FGBFI::getRudder(); - double lat = FGBFI::getLatitude(); - double lon = FGBFI::getLongitude(); + double lat = latitude_node->getDoubleValue(); + double lon = longitude_node->getDoubleValue(); + double alt = altitude_node->getDoubleValue() * SG_FEET_TO_METER; + SG_LOG( SG_ALL, SG_DEBUG, "FGAutopilot::run() lat = " << lat << + " lon = " << lon << " alt = " << alt ); + #ifdef FG_FORCE_AUTO_DISENGAGE // see if somebody else has changed them if( fabs(aileron - old_aileron) > disengage_threshold || @@ -295,40 +386,71 @@ int FGAutopilot::run() { } #endif - // heading hold enabled? + // 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; - if ( heading_mode == FG_HEADING_LOCK ) { - // leave target heading alone + TargetHeading = DGTargetHeading + FGSteam::get_DG_err(); + while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; } + while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; } + MakeTargetHeadingStr( TargetHeading ); + } 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 + while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; } + while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; } + MakeTargetHeadingStr( TargetHeading ); } else if ( heading_mode == FG_HEADING_NAV1 ) { - double tgt_radial; - double cur_radial; + // 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() ) { - tgt_radial = current_radiostack->get_nav1_radial() + 180.0; + // ILS localizers radials are already "true" in our + // database } else { - tgt_radial = current_radiostack->get_nav1_radial(); + cur_radial += 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; } } - cur_radial = current_radiostack->get_nav1_heading(); - cout << "target rad = " << tgt_radial - << " current rad = " << 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_dist() * METER_TO_NM); - if ( diff < -30.0 ) { diff = -30.0; } - if ( diff > 30.0 ) { diff = 30.0; } - TargetHeading = cur_radial - diff; + // determine the target radial in "true" heading + double tgt_radial = current_radiostack->get_nav1_radial(); + if ( current_radiostack->get_nav1_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(); + } + + // determine the heading adjustment needed. + double adjustment = + current_radiostack->get_nav1_heading_needle_deflection() + * (current_radiostack->get_nav1_loc_dist() * SG_METER_TO_NM); + if ( adjustment < -30.0 ) { adjustment = -30.0; } + if ( adjustment > 30.0 ) { adjustment = 30.0; } + + // determine the target heading to fly to intercept the + // tgt_radial + TargetHeading = tgt_radial + adjustment; while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; } while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; } - cout << "target course = " << TargetHeading << endl; + + MakeTargetHeadingStr( TargetHeading ); + // cout << "target course (true) = " << TargetHeading << endl; } else if ( heading_mode == FG_HEADING_WAYPOINT ) { // update target heading to waypoint - double wp_course, wp_reverse, wp_distance; + double wp_course, wp_distance; #ifdef DO_fgAP_CORRECTED_COURSE // compute course made good @@ -347,135 +469,174 @@ int FGAutopilot::run() { // compute course to way_point // need to test for iter - if( ! geo_inverse_wgs_84( 0, //fgAPget_altitude(), - lat, - lon, - TargetLatitude, - TargetLongitude, - &wp_course, - &wp_reverse, - &wp_distance ) ) { - + SGWayPoint wp = globals->get_route()->get_first(); + wp.CourseAndDistance( lon, lat, alt, + &wp_course, &wp_distance ); + #ifdef DO_fgAP_CORRECTED_COURSE - corrected_course = course - wp_course; - if( fabs(corrected_course) > 0.1 ) - printf("fgAP: course %f wp_course %f %f %f\n", - course, wp_course, fabs(corrected_course), - distance ); + corrected_course = course - wp_course; + if( fabs(corrected_course) > 0.1 ) + printf("fgAP: course %f wp_course %f %f %f\n", + course, wp_course, fabs(corrected_course), + distance ); #endif // DO_fgAP_CORRECTED_COURSE - if ( wp_distance > 100 ) { - // corrected_course = course - wp_course; - TargetHeading = NormalizeDegrees(wp_course); + if ( wp_distance > 100 ) { + // 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(); + } + + // see if there are more waypoints on the list + if ( globals->get_route()->size() ) { + // more waypoints + set_HeadingMode( FG_HEADING_WAYPOINT ); } else { - printf("distance(%f) to close\n", wp_distance); - // Real Close -- set heading hold to current heading - // and Ring the arival bell !! - heading_mode = FG_HEADING_LOCK; + // end of the line + heading_mode = FG_TRUE_HEADING_LOCK; // use current heading - TargetHeading = FGBFI::getHeading(); + TargetHeading = heading_node->getDoubleValue(); } - MakeTargetHeadingStr( TargetHeading ); - // Force this just in case - TargetDistance = wp_distance; - MakeTargetDistanceStr( wp_distance ); } + MakeTargetHeadingStr( TargetHeading ); + // Force this just in case + TargetDistance = wp_distance; + 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; } + globals->get_controls()->set_aileron( AileronSet ); + globals->get_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 + - 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 ); + // Now it is time to deterime how far we should be rolled. + SG_LOG( SG_AUTOPILOT, SG_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 ); + 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 - 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 ); + globals->get_controls()->set_aileron( AileronSet ); + globals->get_controls()->set_rudder( AileronSet / 4.0 ); + // controls.set_rudder( 0.0 ); + } } - // altitude hold? + // 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; if ( altitude_mode == FG_ALTITUDE_LOCK ) { - // normal altitude hold - // 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() * SG_FEET_TO_METER ) * 8.0; } else if ( altitude_mode == FG_ALTITUDE_GS1 ) { - double x = current_radiostack->get_nav1_dist(); - double y = (FGBFI::getAltitude() - - current_radiostack->get_nav1_elev()) * FEET_TO_METER; - double angle = atan2( y, x ) * RAD_TO_DEG; - double gs_diff = current_radiostack->get_nav1_target_gs() - angle; - climb_error_accum += gs_diff * 2.0; - TargetClimbRate = gs_diff * 200.0 + climb_error_accum; + double x = current_radiostack->get_nav1_gs_dist(); + double y = (altitude_node->getDoubleValue() + - current_radiostack->get_nav1_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 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() + * SG_FEET_TO_METER * 60.0; + // cout << "Horizontal vel = " << horiz_vel << endl; + 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 - TargetClimbRate = - ( TargetAGL - FGBFI::getAGL()*FEET_TO_METER ) * 16.0; + climb_rate = + ( TargetAGL - altitude_agl_node->getDoubleValue() + * SG_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(); @@ -484,30 +645,36 @@ int FGAutopilot::run() { 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_climb_rate ) { - TargetClimbRate = -ideal_climb_rate; + if ( climb_rate < -fabs(TargetDecentRate) ) { + climb_rate = -fabs(TargetDecentRate); } - error = FGBFI::getVerticalSpeed() * FEET_TO_METER - TargetClimbRate; - // cout << "climb rate = " << fgAPget_climb() - // << " error = " << error << endl; + // cout << "Target climb rate = " << TargetClimbRate << endl; + // cout << "given our speed, modified desired climb rate = " + // << climb_rate * SG_METER_TO_FEET + // << " fpm" << endl; + // cout << "Current climb rate = " + // << vertical_speed_node->getDoubleValue() * 60 << " fpm" << endl; + + error = vertical_speed_node->getDoubleValue() * 60 + - climb_rate * SG_METER_TO_FEET; // accumulate the error under the curve ... this really should // be *= delta t @@ -516,7 +683,7 @@ 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 / 8000.0; + int_adj = int_error / 20000.0; // caclulate proportional error prop_error = error; @@ -534,10 +701,10 @@ int FGAutopilot::run() { total_adj = -1.0; } - controls.set_elevator( total_adj ); + globals->get_controls()->set_elevator_trim( total_adj ); } - // auto throttle enabled? + // auto throttle if ( auto_throttle ) { double error; double prop_error, int_error; @@ -573,7 +740,8 @@ int FGAutopilot::run() { total_adj = 0.0; } - controls.set_throttle( FGControls::ALL_ENGINES, total_adj ); + globals->get_controls()->set_throttle( FGControls::ALL_ENGINES, + total_adj ); } #ifdef THIS_CODE_IS_NOT_USED @@ -605,16 +773,18 @@ 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 + // Ok, we are done + SG_LOG( SG_ALL, SG_DEBUG, "FGAutopilot::run( returns )" ); + return 0; } @@ -622,36 +792,52 @@ int FGAutopilot::run() { void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) { heading_mode = mode; - if ( heading_mode == FG_HEADING_LOCK ) { + 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(); + } 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 ) { - double course, reverse, distance; - // turn on location hold - // turn on heading hold - old_lat = FGBFI::getLatitude(); - old_lon = FGBFI::getLongitude(); - - // need to test for iter - if( !geo_inverse_wgs_84( FGBFI::getAltitude() * FEET_TO_METER, - FGBFI::getLatitude(), - FGBFI::getLongitude(), - TargetLatitude, - TargetLongitude, - &course, - &reverse, - &distance ) ) { + if ( globals->get_route()->size() ) { + double course, distance; + + old_lat = latitude_node->getDoubleValue(); + old_lon = longitude_node->getDoubleValue(); + + waypoint = globals->get_route()->get_first(); + waypoint.CourseAndDistance( longitude_node->getDoubleValue(), + latitude_node->getDoubleValue(), + altitude_node->getDoubleValue() + * SG_FEET_TO_METER, + &course, &distance ); TargetHeading = course; TargetDistance = distance; - MakeTargetDistanceStr( distance ); - } + MakeTargetLatLonStr( waypoint.get_target_lat(), + waypoint.get_target_lon() ); + MakeTargetWPStr( distance ); + + if ( waypoint.get_target_alt() > 0.0 ) { + TargetAltitude = waypoint.get_target_alt(); + altitude_mode = FG_ALTITUDE_LOCK; + set_AltitudeEnabled( true ); + MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET ); + } - FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetLocation: ( " - << TargetLatitude << " " - << TargetLongitude << " ) " - ); + 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; + } } - + MakeTargetHeadingStr( TargetHeading ); update_old_control_values(); } @@ -663,47 +849,48 @@ void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) { alt_error_accum = 0.0; if ( altitude_mode == FG_ALTITUDE_LOCK ) { - // lock at current altitude - TargetAltitude = FGBFI::getAltitude() * FEET_TO_METER; + if ( TargetAltitude < altitude_agl_node->getDoubleValue() + * SG_FEET_TO_METER ) { + } - if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) { - MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET ); + if ( 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 ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) { - MakeTargetAltitudeStr( TargetAGL * METER_TO_FEET ); + if ( 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() * RAD_TO_DEG ); + return( cur_fdm_state->get_Gamma_vert_rad() * SGD_RADIANS_TO_DEGREES ); } static inline double fgAPget_latitude( void ) { - return( cur_fdm_state->get_Latitude() * RAD_TO_DEG ); + return( cur_fdm_state->get_Latitude() * SGD_RADIANS_TO_DEGREES ); } static inline double fgAPget_longitude( void ) { - return( cur_fdm_state->get_Longitude() * RAD_TO_DEG ); + return( cur_fdm_state->get_Longitude() * SGD_RADIANS_TO_DEGREES ); } static inline double fgAPget_roll( void ) { - return( cur_fdm_state->get_Phi() * RAD_TO_DEG ); + return( cur_fdm_state->get_Phi() * SGD_RADIANS_TO_DEGREES ); } static inline double get_pitch( void ) { @@ -711,16 +898,16 @@ static inline double get_pitch( void ) { } double fgAPget_heading( void ) { - return( cur_fdm_state->get_Psi() * RAD_TO_DEG ); + 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 ); + return( cur_fdm_state->get_Altitude() * SG_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 ); + return( cur_fdm_state->get_Climb_Rate() * SG_FEET_TO_METER * 60 ); } static inline double get_sideslip( void ) { @@ -730,8 +917,8 @@ static inline double get_sideslip( void ) { static inline double fgAPget_agl( void ) { double agl; - agl = cur_fdm_state->get_Altitude() * FEET_TO_METER - - scenery.cur_elev; + agl = cur_fdm_state->get_Altitude() * SG_FEET_TO_METER + - scenery.get_cur_elev(); return( agl ); } @@ -743,12 +930,12 @@ void FGAutopilot::AltitudeSet( double new_altitude ) { // cout << "new altitude = " << new_altitude << endl; - if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) { - target_alt = new_altitude * FEET_TO_METER; + if ( 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 < scenery.get_cur_elev() ) { + target_alt = scenery.get_cur_elev(); } TargetAltitude = target_alt; @@ -756,8 +943,8 @@ void FGAutopilot::AltitudeSet( double new_altitude ) { // cout << "TargetAltitude = " << TargetAltitude << endl; - if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) { - target_alt *= METER_TO_FEET; + if ( fgGetString("/sim/startup/units") == "feet" ) { + target_alt *= SG_METER_TO_FEET; } // ApAltitudeDialogInput->setValue((float)target_alt); MakeTargetAltitudeStr( target_alt ); @@ -770,29 +957,42 @@ void FGAutopilot::AltitudeAdjust( double inc ) { double target_alt, target_agl; - if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) { - target_alt = TargetAltitude * METER_TO_FEET; - target_agl = TargetAGL * METER_TO_FEET; + if ( 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; } - target_alt = ( int ) ( target_alt / inc ) * inc + inc; - target_agl = ( int ) ( target_agl / inc ) * inc + inc; + // 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 ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) { - target_alt *= FEET_TO_METER; - target_agl *= FEET_TO_METER; + if ( fabs((int)(target_alt / inc) * inc - target_alt) < SG_EPSILON ) { + target_alt += inc; + } else { + target_alt = ( int ) ( target_alt / inc ) * inc + inc; + } + + if ( fabs((int)(target_agl / inc) * inc - target_agl) < SG_EPSILON ) { + target_agl += inc; + } else { + target_agl = ( int ) ( target_agl / inc ) * inc + inc; + } + + if ( fgGetString("/sim/startup/units") == "feet" ) { + target_alt *= SG_FEET_TO_METER; + target_agl *= SG_FEET_TO_METER; } TargetAltitude = target_alt; TargetAGL = target_agl; - if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) - target_alt *= METER_TO_FEET; - if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) - target_agl *= METER_TO_FEET; + if ( fgGetString("/sim/startup/units") == "feet" ) + target_alt *= SG_METER_TO_FEET; + if ( fgGetString("/sim/startup/units") == "feet" ) + target_agl *= SG_METER_TO_FEET; if ( altitude_mode == FG_ALTITUDE_LOCK ) { MakeTargetAltitudeStr( target_alt ); @@ -805,26 +1005,38 @@ void FGAutopilot::AltitudeAdjust( double inc ) void FGAutopilot::HeadingAdjust( double inc ) { - heading_mode = FG_HEADING_LOCK; - - double target = ( int ) ( TargetHeading / inc ) * inc + inc; + if ( heading_mode != FG_DG_HEADING_LOCK + && heading_mode != FG_TRUE_HEADING_LOCK ) + { + heading_mode = FG_DG_HEADING_LOCK; + } + + if ( heading_mode == FG_DG_HEADING_LOCK ) { + double target = ( int ) ( DGTargetHeading / inc ) * inc + inc; + DGTargetHeading = NormalizeDegrees( target ); + } else { + double target = ( int ) ( TargetHeading / inc ) * inc + inc; + TargetHeading = NormalizeDegrees( target ); + } - TargetHeading = NormalizeDegrees( target ); - // following cast needed ambiguous plib - // ApHeadingDialogInput -> setValue ((float)TargetHeading ); - MakeTargetHeadingStr( TargetHeading ); update_old_control_values(); } void FGAutopilot::HeadingSet( double new_heading ) { - heading_mode = FG_HEADING_LOCK; - - new_heading = NormalizeDegrees( new_heading ); - TargetHeading = new_heading; - // following cast needed ambiguous plib - // ApHeadingDialogInput -> setValue ((float)APData->TargetHeading ); - MakeTargetHeadingStr( TargetHeading ); + 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(); } @@ -838,12 +1050,12 @@ void FGAutopilot::AutoThrottleAdjust( double inc ) { void FGAutopilot::set_AutoThrottleEnabled( bool value ) { auto_throttle = value; - if ( auto_throttle = true ) { - TargetSpeed = FGBFI::getAirspeed(); + if ( auto_throttle == true ) { + TargetSpeed = fgGetDouble("/velocities/airspeed-kt"); speed_error_accum = 0.0; } update_old_control_values(); - FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: (" + SG_LOG( SG_COCKPIT, SG_INFO, " fgAPSetAutoThrottle: (" << auto_throttle << ") " << TargetSpeed ); }