X-Git-Url: https://git.mxchange.org/?a=blobdiff_plain;f=src%2FAutopilot%2Fnewauto.cxx;h=1341368d90bf2502ac8d3593a513456ec31f9d7a;hb=a4e81f4ff075e6a3c0c2ea1b5a29c0bcdfdbc671;hp=a142a4874c17c4d7bc568b2163a6ac6bda36d716;hpb=0cc3bed841c8b75d6fe48705a3589a8bb500c609;p=flightgear.git diff --git a/src/Autopilot/newauto.cxx b/src/Autopilot/newauto.cxx index a142a4874..1341368d9 100644 --- a/src/Autopilot/newauto.cxx +++ b/src/Autopilot/newauto.cxx @@ -28,6 +28,7 @@ #endif #include // sprintf() +#include // strcmp() #include #include @@ -215,6 +216,8 @@ void FGAutopilot::init () 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); @@ -230,6 +233,11 @@ void FGAutopilot::init () = 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); + current_throttle = fgGetNode("/controls/throttle"); // initialize config properties with defaults (in case config isn't there) @@ -249,12 +257,21 @@ void FGAutopilot::init () 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 ); /* 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"); @@ -281,9 +298,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; @@ -294,7 +308,7 @@ void FGAutopilot::init () #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 ) @@ -419,7 +433,7 @@ static double LinearExtrapolate( double x, double x1, double y1, double x2, doub void -FGAutopilot::update (int dt) +FGAutopilot::update (double dt) { // Remove the following lines when the calling funcitons start // passing in the data pointer @@ -430,6 +444,12 @@ FGAutopilot::update (int dt) 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 ); @@ -507,6 +527,13 @@ FGAutopilot::update (int dt) * (current_radiostack->get_nav1_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_nav1_loc_dist() > 5000) { + double clamp_angle = fabs(current_radiostack->get_nav1_heading_needle_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; @@ -595,8 +622,10 @@ FGAutopilot::update (int dt) // 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 ) { @@ -734,11 +763,17 @@ FGAutopilot::update (int dt) // 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; @@ -906,7 +941,7 @@ void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) { * SG_FEET_TO_METER ) { } - if ( fgGetString("/sim/startup/units") == "feet" ) { + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) { MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET ); } else { MakeTargetAltitudeStr( TargetAltitude * SG_METER_TO_FEET ); @@ -917,7 +952,7 @@ void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) { } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) { TargetAGL = altitude_agl_node->getDoubleValue() * SG_FEET_TO_METER; - if ( fgGetString("/sim/startup/units") == "feet" ) { + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) { MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET ); } else { MakeTargetAltitudeStr( TargetAGL * SG_METER_TO_FEET ); @@ -932,18 +967,18 @@ void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) { void FGAutopilot::AltitudeSet( double new_altitude ) { double target_alt = new_altitude; - if ( fgGetString("/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 ( fgGetString("/sim/startup/units") == "feet" ) { + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) { target_alt *= SG_METER_TO_FEET; } // ApAltitudeDialogInput->setValue((float)target_alt); @@ -957,7 +992,7 @@ void FGAutopilot::AltitudeAdjust( double inc ) { double target_alt, target_agl; - if ( fgGetString("/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 { @@ -977,7 +1012,7 @@ void FGAutopilot::AltitudeAdjust( double inc ) target_agl = ( int ) ( target_agl / inc ) * inc + inc; } - if ( fgGetString("/sim/startup/units") == "feet" ) { + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) { target_alt *= SG_FEET_TO_METER; target_agl *= SG_FEET_TO_METER; } @@ -985,9 +1020,9 @@ void FGAutopilot::AltitudeAdjust( double inc ) TargetAltitude = target_alt; TargetAGL = target_agl; - if ( fgGetString("/sim/startup/units") == "feet" ) + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) target_alt *= SG_METER_TO_FEET; - if ( fgGetString("/sim/startup/units") == "feet" ) + if ( !strcmp(fgGetString("/sim/startup/units"), "feet") ) target_agl *= SG_METER_TO_FEET; if ( altitude_mode == FG_ALTITUDE_LOCK ) { @@ -1363,3 +1398,4 @@ FGAutopilot::setAPThrottleControl (double value) } // end of newauto.cxx +