1 // newauto.cxx -- autopilot defines and prototypes (very alpha)
3 // Started April 1998 Copyright (C) 1998
5 // Contributions by Jeff Goeke-Smith <jgoeke@voyager.net>
6 // Norman Vine <nhv@cape.com>
7 // Curtis Olson <curt@flightgear.org>
9 // This program is free software; you can redistribute it and/or
10 // modify it under the terms of the GNU General Public License as
11 // published by the Free Software Foundation; either version 2 of the
12 // License, or (at your option) any later version.
14 // This program is distributed in the hope that it will be useful, but
15 // WITHOUT ANY WARRANTY; without even the implied warranty of
16 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
17 // General Public License for more details.
19 // You should have received a copy of the GNU General Public License
20 // along with this program; if not, write to the Free Software
21 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
30 #include <stdio.h> // sprintf()
32 #include <simgear/constants.h>
33 #include <simgear/debug/logstream.hxx>
34 #include <simgear/math/sg_geodesy.hxx>
36 #include <Cockpit/steam.hxx>
37 #include <Cockpit/radiostack.hxx>
38 #include <Controls/controls.hxx>
39 #include <FDM/flight.hxx>
40 #include <Main/bfi.hxx>
41 #include <Main/globals.hxx>
42 #include <Scenery/scenery.hxx>
44 #include "newauto.hxx"
47 FGAutopilot *current_autopilot;
50 // Climb speed constants
51 const double min_climb = 70.0; // kts
52 const double best_climb = 75.0; // kts
53 const double ideal_climb_rate = 500.0 * FEET_TO_METER; // fpm -> mpm
55 /// These statics will eventually go into the class
56 /// they are just here while I am experimenting -- NHV :-)
57 // AutoPilot Gain Adjuster members
58 static double MaxRollAdjust; // MaxRollAdjust = 2 * APData->MaxRoll;
59 static double RollOutAdjust; // RollOutAdjust = 2 * APData->RollOut;
60 static double MaxAileronAdjust; // MaxAileronAdjust = 2 * APData->MaxAileron;
61 static double RollOutSmoothAdjust; // RollOutSmoothAdjust = 2 * APData->RollOutSmooth;
63 static char NewTgtAirportId[16];
64 // static char NewTgtAirportLabel[] = "Enter New TgtAirport ID";
66 extern char *coord_format_lat(float);
67 extern char *coord_format_lon(float);
70 void FGAutopilot::MakeTargetLatLonStr( double lat, double lon ) {
71 sprintf( TargetLatitudeStr , "%s", coord_format_lat(get_TargetLatitude()));
72 sprintf( TargetLongitudeStr, "%s", coord_format_lon(get_TargetLongitude()));
73 sprintf( TargetLatLonStr, "%s %s", TargetLatitudeStr, TargetLongitudeStr );
77 void FGAutopilot::MakeTargetAltitudeStr( double altitude ) {
78 if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
79 sprintf( TargetAltitudeStr, "APAltitude %6.0f+", altitude );
81 sprintf( TargetAltitudeStr, "APAltitude %6.0f", altitude );
86 void FGAutopilot::MakeTargetHeadingStr( double bearing ) {
89 } else if (bearing > 360. ) {
92 sprintf( TargetHeadingStr, "APHeading %6.1f", bearing );
96 static inline double get_speed( void ) {
97 return( cur_fdm_state->get_V_equiv_kts() );
100 static inline double get_ground_speed() {
101 // starts in ft/s so we convert to kts
102 double ft_s = cur_fdm_state->get_V_ground_speed()
103 * fgGetInt("/sim/speed-up"); // FIXME: inefficient
104 double kts = ft_s * FEET_TO_METER * 3600 * METER_TO_NM;
110 void FGAutopilot::MakeTargetWPStr( double distance ) {
111 static time_t last_time = 0;
112 time_t current_time = time(NULL);
113 if ( last_time == current_time ) {
117 last_time = current_time;
121 int size = globals->get_route()->size();
123 // start by wiping the strings
130 SGWayPoint wp1 = globals->get_route()->get_waypoint( 0 );
132 double eta = accum * METER_TO_NM / get_ground_speed();
133 if ( eta >= 100.0 ) { eta = 99.999; }
135 if ( eta < (1.0/6.0) ) {
136 // within 10 minutes, bump up to min/secs
140 minor = (int)((eta - (int)eta) * 60.0);
141 sprintf( TargetWP1Str, "%s %.2f NM ETA %d:%02d",
142 wp1.get_id().c_str(),
143 accum*METER_TO_NM, major, minor );
144 // cout << "distance = " << distance*METER_TO_NM
145 // << " gndsp = " << get_ground_speed()
146 // << " time = " << eta
147 // << " major = " << major
148 // << " minor = " << minor
154 SGWayPoint wp2 = globals->get_route()->get_waypoint( 1 );
155 accum += wp2.get_distance();
157 double eta = accum * METER_TO_NM / get_ground_speed();
158 if ( eta >= 100.0 ) { eta = 99.999; }
160 if ( eta < (1.0/6.0) ) {
161 // within 10 minutes, bump up to min/secs
165 minor = (int)((eta - (int)eta) * 60.0);
166 sprintf( TargetWP2Str, "%s %.2f NM ETA %d:%02d",
167 wp2.get_id().c_str(),
168 accum*METER_TO_NM, major, minor );
173 for ( int i = 2; i < size; ++i ) {
174 accum += globals->get_route()->get_waypoint( i ).get_distance();
177 SGWayPoint wpn = globals->get_route()->get_waypoint( size - 1 );
179 double eta = accum * METER_TO_NM / get_ground_speed();
180 if ( eta >= 100.0 ) { eta = 99.999; }
182 if ( eta < (1.0/6.0) ) {
183 // within 10 minutes, bump up to min/secs
187 minor = (int)((eta - (int)eta) * 60.0);
188 sprintf( TargetWP3Str, "%s %.2f NM ETA %d:%02d",
189 wpn.get_id().c_str(),
190 accum*METER_TO_NM, major, minor );
195 void FGAutopilot::update_old_control_values() {
196 old_aileron = controls.get_aileron();
197 old_elevator = controls.get_elevator();
198 old_elevator_trim = controls.get_elevator_trim();
199 old_rudder = controls.get_rudder();
203 // Initialize autopilot subsystem
204 void FGAutopilot::init() {
205 FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
207 heading_hold = false ; // turn the heading hold off
208 altitude_hold = false ; // turn the altitude hold off
209 auto_throttle = false ; // turn the auto throttle off
211 // Initialize target location to startup location
212 old_lat = FGBFI::getLatitude();
213 old_lon = FGBFI::getLongitude();
214 // set_WayPoint( old_lon, old_lat, 0.0, "default" );
216 MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
218 TargetHeading = 0.0; // default direction, due north
219 TargetAltitude = 3000; // default altitude in meters
220 alt_error_accum = 0.0;
221 climb_error_accum = 0.0;
223 MakeTargetAltitudeStr( 3000.0);
224 MakeTargetHeadingStr( 0.0 );
226 // These eventually need to be read from current_aircaft somehow.
228 // the maximum roll, in Deg
231 // the deg from heading to start rolling out at, in Deg
234 // how far can I move the aleron from center.
237 // Smoothing distance for alerion control
240 // Hardwired for now should be in options
241 // 25% max control variablilty 0.5 / 2.0
242 disengage_threshold = 1.0;
244 #if !defined( USING_SLIDER_CLASS )
245 MaxRollAdjust = 2 * MaxRoll;
246 RollOutAdjust = 2 * RollOut;
247 MaxAileronAdjust = 2 * MaxAileron;
248 RollOutSmoothAdjust = 2 * RollOutSmooth;
249 #endif // !defined( USING_SLIDER_CLASS )
251 update_old_control_values();
253 // Initialize GUI components of autopilot
254 // NewTgtAirportInit();
255 // fgAPAdjustInit() ;
257 // NewAltitudeInit();
261 // Reset the autopilot system
262 void FGAutopilot::reset() {
264 heading_hold = false ; // turn the heading hold off
265 altitude_hold = false ; // turn the altitude hold off
266 auto_throttle = false ; // turn the auto throttle off
268 TargetHeading = 0.0; // default direction, due north
269 MakeTargetHeadingStr( TargetHeading );
271 TargetAltitude = 3000; // default altitude in meters
272 MakeTargetAltitudeStr( TargetAltitude );
274 alt_error_accum = 0.0;
275 climb_error_accum = 0.0;
277 update_old_control_values();
279 sprintf( NewTgtAirportId, "%s", fgGetString("/sim/startup/airport-id").c_str() );
281 // TargetLatitude = FGBFI::getLatitude();
282 // TargetLongitude = FGBFI::getLongitude();
283 // set_WayPoint( FGBFI::getLongitude(), FGBFI::getLatitude(), 0.0, "reset" );
285 MakeTargetLatLonStr( get_TargetLatitude(), get_TargetLongitude() );
289 static double NormalizeDegrees( double Input ) {
290 // normalize the input to the range (-180,180]
291 // Input should not be greater than -360 to 360.
292 // Current rules send the output to an undefined state.
296 else if ( Input <= -180 )
297 while ( Input <= -180 )
302 static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 ) {
303 // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
304 //assert(x1 != x2); // Divide by zero error. Cold abort for now
307 // static double y = 0.0;
308 // double dx = x2 -x1;
309 // if( (dx < -FG_EPSILON ) || ( dx > FG_EPSILON ) )
312 double m, b, y; // the constants to find in y=mx+b
315 m = ( y2 - y1 ) / ( x2 - x1 ); // calculate the m
317 b = y1 - m * x1; // calculate the b
319 y = m * x + b; // the final calculation
328 int FGAutopilot::run() {
329 // Remove the following lines when the calling funcitons start
330 // passing in the data pointer
332 // get control settings
333 // double aileron = FGBFI::getAileron();
334 // double elevator = FGBFI::getElevator();
335 // double elevator_trim = FGBFI::getElevatorTrim();
336 // double rudder = FGBFI::getRudder();
338 double lat = FGBFI::getLatitude();
339 double lon = FGBFI::getLongitude();
340 double alt = FGBFI::getAltitude() * FEET_TO_METER;
342 #ifdef FG_FORCE_AUTO_DISENGAGE
343 // see if somebody else has changed them
344 if( fabs(aileron - old_aileron) > disengage_threshold ||
345 fabs(elevator - old_elevator) > disengage_threshold ||
346 fabs(elevator_trim - old_elevator_trim) >
347 disengage_threshold ||
348 fabs(rudder - old_rudder) > disengage_threshold )
350 // if controls changed externally turn autopilot off
351 waypoint_hold = false ; // turn the target hold off
352 heading_hold = false ; // turn the heading hold off
353 altitude_hold = false ; // turn the altitude hold off
354 terrain_follow = false; // turn the terrain_follow hold off
355 // auto_throttle = false; // turn the auto_throttle off
357 // stash this runs control settings
358 old_aileron = aileron;
359 old_elevator = elevator;
360 old_elevator_trim = elevator_trim;
368 if ( heading_hold == true ) {
369 if ( heading_mode == FG_DG_HEADING_LOCK ) {
370 // cout << "DG heading = " << FGSteam::get_DG_deg()
371 // << " DG error = " << FGSteam::get_DG_err() << endl;
373 TargetHeading = DGTargetHeading + FGSteam::get_DG_err();
374 while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; }
375 while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
376 MakeTargetHeadingStr( TargetHeading );
377 } else if ( heading_mode == FG_HEADING_LOCK ) {
378 // leave target heading alone
379 } else if ( heading_mode == FG_HEADING_NAV1 ) {
380 // track the NAV1 heading needle deflection
381 double cur_radial = current_radiostack->get_nav1_heading() +
382 current_radiostack->get_nav1_magvar();
383 if ( current_radiostack->get_nav1_from_flag() ) {
385 while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
388 current_radiostack->get_nav1_heading_needle_deflection()
389 * (current_radiostack->get_nav1_loc_dist() * METER_TO_NM);
390 if ( adjustment < -30.0 ) { adjustment = -30.0; }
391 if ( adjustment > 30.0 ) { adjustment = 30.0; }
392 TargetHeading = cur_radial + adjustment;
393 while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; }
394 while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
396 if ( current_radiostack->get_nav1_to_flag() ||
397 current_radiostack->get_nav1_from_flag() ) {
398 // We have an appropriate radial selected that the
399 // autopilot can follow
402 if ( current_radiostack->get_nav1_loc() ) {
403 // localizers radials are "true"
404 tgt_radial = current_radiostack->get_nav1_radial();
406 tgt_radial = current_radiostack->get_nav1_radial() +
407 current_radiostack->get_nav1_magvar();
409 cur_radial = current_radiostack->get_nav1_heading() +
410 current_radiostack->get_nav1_magvar();
411 if ( current_radiostack->get_nav1_from_flag() ) {
413 while ( cur_radial >= 360.0 ) { cur_radial -= 360.0; }
415 // cout << "target rad (true) = " << tgt_radial
416 // << " current rad (true) = " << cur_radial
419 double diff = (tgt_radial - cur_radial);
420 while ( diff < -180.0 ) { diff += 360.0; }
421 while ( diff > 180.0 ) { diff -= 360.0; }
423 diff *= (current_radiostack->get_nav1_loc_dist() * METER_TO_NM);
424 if ( diff < -30.0 ) { diff = -30.0; }
425 if ( diff > 30.0 ) { diff = 30.0; }
427 if ( current_radiostack->get_nav1_to_flag() ) {
428 TargetHeading = cur_radial - diff;
429 } else if ( current_radiostack->get_nav1_from_flag() ) {
430 TargetHeading = cur_radial + diff;
432 while ( TargetHeading < 0.0 ) { TargetHeading += 360.0; }
433 while ( TargetHeading > 360.0 ) { TargetHeading -= 360.0; }
435 // neither TO, or FROM, maintain current heading.
436 TargetHeading = FGBFI::getHeading();
439 MakeTargetHeadingStr( TargetHeading );
440 // cout << "target course (true) = " << TargetHeading << endl;
441 } else if ( heading_mode == FG_HEADING_WAYPOINT ) {
442 // update target heading to waypoint
444 double wp_course, wp_distance;
446 #ifdef DO_fgAP_CORRECTED_COURSE
447 // compute course made good
448 // this needs lots of special casing before use
449 double course, reverse, distance, corrected_course;
450 // need to test for iter
451 geo_inverse_wgs_84( 0, //fgAPget_altitude(),
459 #endif // DO_fgAP_CORRECTED_COURSE
461 // compute course to way_point
462 // need to test for iter
463 SGWayPoint wp = globals->get_route()->get_first();
464 wp.CourseAndDistance( lon, lat, alt,
465 &wp_course, &wp_distance );
467 #ifdef DO_fgAP_CORRECTED_COURSE
468 corrected_course = course - wp_course;
469 if( fabs(corrected_course) > 0.1 )
470 printf("fgAP: course %f wp_course %f %f %f\n",
471 course, wp_course, fabs(corrected_course),
473 #endif // DO_fgAP_CORRECTED_COURSE
475 if ( wp_distance > 100 ) {
476 // corrected_course = course - wp_course;
477 TargetHeading = NormalizeDegrees(wp_course);
479 cout << "Reached waypoint within " << wp_distance << "meters"
482 // pop off this waypoint from the list
483 if ( globals->get_route()->size() ) {
484 globals->get_route()->delete_first();
487 // see if there are more waypoints on the list
488 if ( globals->get_route()->size() ) {
490 set_HeadingMode( FG_HEADING_WAYPOINT );
493 heading_mode = FG_HEADING_LOCK;
494 // use current heading
495 TargetHeading = FGBFI::getHeading();
498 MakeTargetHeadingStr( TargetHeading );
499 // Force this just in case
500 TargetDistance = wp_distance;
501 MakeTargetWPStr( wp_distance );
509 RelHeading = NormalizeDegrees( TargetHeading - FGBFI::getHeading() );
510 // figure out how far off we are from desired heading
512 // Now it is time to deterime how far we should be rolled.
513 FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
516 // Check if we are further from heading than the roll out point
517 if ( fabs( RelHeading ) > RollOut ) {
518 // set Target Roll to Max in desired direction
519 if ( RelHeading < 0 ) {
520 TargetRoll = 0 - MaxRoll;
522 TargetRoll = MaxRoll;
525 // We have to calculate the Target roll
527 // This calculation engine thinks that the Target roll
528 // should be a line from (RollOut,MaxRoll) to (-RollOut,
529 // -MaxRoll) I hope this works well. If I get ambitious
530 // some day this might become a fancier curve or
533 TargetRoll = LinearExtrapolate( RelHeading, -RollOut,
538 // Target Roll has now been Found.
540 // Compare Target roll to Current Roll, Generate Rel Roll
542 FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
544 RelRoll = NormalizeDegrees( TargetRoll - FGBFI::getRoll() );
546 // Check if we are further from heading than the roll out smooth point
547 if ( fabs( RelRoll ) > RollOutSmooth ) {
548 // set Target Roll to Max in desired direction
550 AileronSet = 0 - MaxAileron;
552 AileronSet = MaxAileron;
555 AileronSet = LinearExtrapolate( RelRoll, -RollOutSmooth,
561 controls.set_aileron( AileronSet );
562 controls.set_rudder( AileronSet / 4.0 );
563 // controls.set_rudder( 0.0 );
567 if ( altitude_hold ) {
568 double speed, max_climb, error;
569 double prop_error, int_error;
570 double prop_adj, int_adj, total_adj;
572 if ( altitude_mode == FG_ALTITUDE_LOCK ) {
573 // normal altitude hold
574 // cout << "TargetAltitude = " << TargetAltitude
575 // << "Altitude = " << FGBFI::getAltitude() * FEET_TO_METER
578 ( TargetAltitude - FGBFI::getAltitude() * FEET_TO_METER ) * 8.0;
579 } else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
580 double x = current_radiostack->get_nav1_gs_dist();
581 double y = (FGBFI::getAltitude()
582 - current_radiostack->get_nav1_elev()) * FEET_TO_METER;
583 double current_angle = atan2( y, x ) * RAD_TO_DEG;
584 // cout << "current angle = " << current_angle << endl;
586 double target_angle = current_radiostack->get_nav1_target_gs();
587 // cout << "target angle = " << target_angle << endl;
589 double gs_diff = target_angle - current_angle;
590 // cout << "difference from desired = " << gs_diff << endl;
592 // convert desired vertical path angle into a climb rate
593 double des_angle = current_angle - 10 * gs_diff;
594 // cout << "desired angle = " << des_angle << endl;
596 // convert to meter/min
597 // cout << "raw ground speed = " << cur_fdm_state->get_V_ground_speed() << endl;
598 double horiz_vel = cur_fdm_state->get_V_ground_speed()
599 * FEET_TO_METER * 60.0;
600 // cout << "Horizontal vel = " << horiz_vel << endl;
601 TargetClimbRate = -sin( des_angle * DEG_TO_RAD ) * horiz_vel;
602 // cout << "TargetClimbRate = " << TargetClimbRate << endl;
603 /* climb_error_accum += gs_diff * 2.0; */
604 /* TargetClimbRate = gs_diff * 200.0 + climb_error_accum; */
605 } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
606 // brain dead ground hugging with no look ahead
608 ( TargetAGL - FGBFI::getAGL()*FEET_TO_METER ) * 16.0;
609 // cout << "target agl = " << TargetAGL
610 // << " current agl = " << fgAPget_agl()
611 // << " target climb rate = " << TargetClimbRate
614 // just try to zero out rate of climb ...
615 TargetClimbRate = 0.0;
620 if ( speed < min_climb ) {
622 } else if ( speed < best_climb ) {
623 max_climb = ((best_climb - min_climb) - (best_climb - speed))
625 / (best_climb - min_climb);
627 max_climb = ( speed - best_climb ) * 10.0 + ideal_climb_rate;
630 // this first one could be optional if we wanted to allow
631 // better climb performance assuming we have the airspeed to
633 if ( TargetClimbRate > ideal_climb_rate ) {
634 TargetClimbRate = ideal_climb_rate;
637 if ( TargetClimbRate > max_climb ) {
638 TargetClimbRate = max_climb;
641 if ( TargetClimbRate < -ideal_climb_rate ) {
642 TargetClimbRate = -ideal_climb_rate;
645 error = FGBFI::getVerticalSpeed() * FEET_TO_METER - TargetClimbRate;
646 // cout << "climb rate = " << fgAPget_climb()
647 // << " error = " << error << endl;
649 // accumulate the error under the curve ... this really should
651 alt_error_accum += error;
653 // calculate integral error, and adjustment amount
654 int_error = alt_error_accum;
655 // printf("error = %.2f int_error = %.2f\n", error, int_error);
656 int_adj = int_error / 20000.0;
658 // caclulate proportional error
660 prop_adj = prop_error / 2000.0;
662 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
663 // if ( total_adj > 0.6 ) {
665 // } else if ( total_adj < -0.2 ) {
668 if ( total_adj > 1.0 ) {
670 } else if ( total_adj < -1.0 ) {
674 controls.set_elevator( total_adj );
678 if ( auto_throttle ) {
680 double prop_error, int_error;
681 double prop_adj, int_adj, total_adj;
683 error = TargetSpeed - get_speed();
685 // accumulate the error under the curve ... this really should
687 speed_error_accum += error;
688 if ( speed_error_accum > 2000.0 ) {
689 speed_error_accum = 2000.0;
691 else if ( speed_error_accum < -2000.0 ) {
692 speed_error_accum = -2000.0;
695 // calculate integral error, and adjustment amount
696 int_error = speed_error_accum;
698 // printf("error = %.2f int_error = %.2f\n", error, int_error);
699 int_adj = int_error / 200.0;
701 // caclulate proportional error
703 prop_adj = 0.5 + prop_error / 50.0;
705 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
706 if ( total_adj > 1.0 ) {
709 else if ( total_adj < 0.0 ) {
713 controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
716 #ifdef THIS_CODE_IS_NOT_USED
717 if (Mode == 2) // Glide slope hold
722 // First, calculate Relative slope and normalize it
723 RelSlope = NormalizeDegrees( TargetSlope - get_pitch());
725 // Now calculate the elevator offset from current angle
726 if ( abs(RelSlope) > SlopeSmooth )
728 if ( RelSlope < 0 ) // set RelElevator to max in the correct direction
729 RelElevator = -MaxElevator;
731 RelElevator = MaxElevator;
735 RelElevator = LinearExtrapolate(RelSlope,-SlopeSmooth,-MaxElevator,SlopeSmooth,MaxElevator);
738 fgElevMove(RelElevator);
741 #endif // THIS_CODE_IS_NOT_USED
743 // stash this runs control settings
744 // update_old_control_values();
745 old_aileron = controls.get_aileron();
746 old_elevator = controls.get_elevator();
747 old_elevator_trim = controls.get_elevator_trim();
748 old_rudder = controls.get_rudder();
750 // for cross track error
759 void FGAutopilot::set_HeadingMode( fgAutoHeadingMode mode ) {
762 if ( heading_mode == FG_DG_HEADING_LOCK ) {
763 // set heading hold to current heading (as read from DG)
764 DGTargetHeading = FGSteam::get_DG_deg();
765 } else if ( heading_mode == FG_HEADING_LOCK ) {
766 // set heading hold to current heading
767 TargetHeading = FGBFI::getHeading();
768 } else if ( heading_mode == FG_HEADING_WAYPOINT ) {
769 if ( globals->get_route()->size() ) {
770 double course, distance;
772 old_lat = FGBFI::getLatitude();
773 old_lon = FGBFI::getLongitude();
775 waypoint = globals->get_route()->get_first();
776 waypoint.CourseAndDistance( FGBFI::getLongitude(),
777 FGBFI::getLatitude(),
778 FGBFI::getLatitude() * FEET_TO_METER,
779 &course, &distance );
780 TargetHeading = course;
781 TargetDistance = distance;
782 MakeTargetLatLonStr( waypoint.get_target_lat(),
783 waypoint.get_target_lon() );
784 MakeTargetWPStr( distance );
786 if ( waypoint.get_target_alt() > 0.0 ) {
787 TargetAltitude = waypoint.get_target_alt();
788 altitude_mode = FG_ALTITUDE_LOCK;
789 set_AltitudeEnabled( true );
790 MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
793 FG_LOG( FG_COCKPIT, FG_INFO, " set_HeadingMode: ( "
794 << get_TargetLatitude() << " "
795 << get_TargetLongitude() << " ) "
798 // no more way points, default to heading lock.
799 heading_mode = FG_HEADING_LOCK;
800 TargetHeading = FGBFI::getHeading();
804 MakeTargetHeadingStr( TargetHeading );
805 update_old_control_values();
809 void FGAutopilot::set_AltitudeMode( fgAutoAltitudeMode mode ) {
810 altitude_mode = mode;
812 alt_error_accum = 0.0;
814 if ( altitude_mode == FG_ALTITUDE_LOCK ) {
815 // lock at current altitude
816 TargetAltitude = FGBFI::getAltitude() * FEET_TO_METER;
818 if ( fgGetString("/sim/startup/units") == "feet" ) {
819 MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
821 MakeTargetAltitudeStr( TargetAltitude * METER_TO_FEET );
823 } else if ( altitude_mode == FG_ALTITUDE_GS1 ) {
824 climb_error_accum = 0.0;
826 } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
827 TargetAGL = FGBFI::getAGL() * FEET_TO_METER;
829 if ( fgGetString("/sim/startup/units") == "feet" ) {
830 MakeTargetAltitudeStr( TargetAGL * METER_TO_FEET );
832 MakeTargetAltitudeStr( TargetAGL * METER_TO_FEET );
836 update_old_control_values();
837 FG_LOG( FG_COCKPIT, FG_INFO, " set_AltitudeMode():" );
842 static inline double get_aoa( void ) {
843 return( cur_fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
846 static inline double fgAPget_latitude( void ) {
847 return( cur_fdm_state->get_Latitude() * RAD_TO_DEG );
850 static inline double fgAPget_longitude( void ) {
851 return( cur_fdm_state->get_Longitude() * RAD_TO_DEG );
854 static inline double fgAPget_roll( void ) {
855 return( cur_fdm_state->get_Phi() * RAD_TO_DEG );
858 static inline double get_pitch( void ) {
859 return( cur_fdm_state->get_Theta() );
862 double fgAPget_heading( void ) {
863 return( cur_fdm_state->get_Psi() * RAD_TO_DEG );
866 static inline double fgAPget_altitude( void ) {
867 return( cur_fdm_state->get_Altitude() * FEET_TO_METER );
870 static inline double fgAPget_climb( void ) {
871 // return in meters per minute
872 return( cur_fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
875 static inline double get_sideslip( void ) {
876 return( cur_fdm_state->get_Beta() );
879 static inline double fgAPget_agl( void ) {
882 agl = cur_fdm_state->get_Altitude() * FEET_TO_METER
890 void FGAutopilot::AltitudeSet( double new_altitude ) {
891 double target_alt = new_altitude;
893 // cout << "new altitude = " << new_altitude << endl;
895 if ( fgGetString("/sim/startup/units") == "feet" ) {
896 target_alt = new_altitude * FEET_TO_METER;
899 if( target_alt < scenery.cur_elev ) {
900 target_alt = scenery.cur_elev;
903 TargetAltitude = target_alt;
904 altitude_mode = FG_ALTITUDE_LOCK;
906 // cout << "TargetAltitude = " << TargetAltitude << endl;
908 if ( fgGetString("/sim/startup/units") == "feet" ) {
909 target_alt *= METER_TO_FEET;
911 // ApAltitudeDialogInput->setValue((float)target_alt);
912 MakeTargetAltitudeStr( target_alt );
914 update_old_control_values();
918 void FGAutopilot::AltitudeAdjust( double inc )
920 double target_alt, target_agl;
922 if ( fgGetString("/sim/startup/units") == "feet" ) {
923 target_alt = TargetAltitude * METER_TO_FEET;
924 target_agl = TargetAGL * METER_TO_FEET;
926 target_alt = TargetAltitude;
927 target_agl = TargetAGL;
930 // cout << "target_agl = " << target_agl << endl;
931 // cout << "target_agl / inc = " << target_agl / inc << endl;
932 // cout << "(int)(target_agl / inc) = " << (int)(target_agl / inc) << endl;
934 if ( fabs((int)(target_alt / inc) * inc - target_alt) < FG_EPSILON ) {
937 target_alt = ( int ) ( target_alt / inc ) * inc + inc;
940 if ( fabs((int)(target_agl / inc) * inc - target_agl) < FG_EPSILON ) {
943 target_agl = ( int ) ( target_agl / inc ) * inc + inc;
946 if ( fgGetString("/sim/startup/units") == "feet" ) {
947 target_alt *= FEET_TO_METER;
948 target_agl *= FEET_TO_METER;
951 TargetAltitude = target_alt;
952 TargetAGL = target_agl;
954 if ( fgGetString("/sim/startup/units") == "feet" )
955 target_alt *= METER_TO_FEET;
956 if ( fgGetString("/sim/startup/units") == "feet" )
957 target_agl *= METER_TO_FEET;
959 if ( altitude_mode == FG_ALTITUDE_LOCK ) {
960 MakeTargetAltitudeStr( target_alt );
961 } else if ( altitude_mode == FG_ALTITUDE_TERRAIN ) {
962 MakeTargetAltitudeStr( target_agl );
965 update_old_control_values();
969 void FGAutopilot::HeadingAdjust( double inc ) {
970 if ( heading_mode != FG_DG_HEADING_LOCK && heading_mode != FG_HEADING_LOCK )
972 heading_mode = FG_DG_HEADING_LOCK;
975 if ( heading_mode == FG_DG_HEADING_LOCK ) {
976 double target = ( int ) ( DGTargetHeading / inc ) * inc + inc;
977 DGTargetHeading = NormalizeDegrees( target );
979 double target = ( int ) ( TargetHeading / inc ) * inc + inc;
980 TargetHeading = NormalizeDegrees( target );
983 update_old_control_values();
987 void FGAutopilot::HeadingSet( double new_heading ) {
988 heading_mode = FG_HEADING_LOCK;
990 new_heading = NormalizeDegrees( new_heading );
991 TargetHeading = new_heading;
992 // following cast needed ambiguous plib
993 // ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
994 MakeTargetHeadingStr( TargetHeading );
995 update_old_control_values();
998 void FGAutopilot::AutoThrottleAdjust( double inc ) {
999 double target = ( int ) ( TargetSpeed / inc ) * inc + inc;
1001 TargetSpeed = target;
1005 void FGAutopilot::set_AutoThrottleEnabled( bool value ) {
1006 auto_throttle = value;
1008 if ( auto_throttle == true ) {
1009 TargetSpeed = FGBFI::getAirspeed();
1010 speed_error_accum = 0.0;
1013 update_old_control_values();
1014 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
1015 << auto_throttle << ") " << TargetSpeed );