1 // autopilot.cxx -- autopilot subsystem
3 // Started by Jeff Goeke-Smith, started April 1998.
5 // Copyright (C) 1998 Jeff Goeke-Smith, jgoeke@voyager.net
7 // Heavy modifications and additions by Norman Vine and few by Curtis
10 // This program is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU General Public License as
12 // published by the Free Software Foundation; either version 2 of the
13 // License, or (at your option) any later version.
15 // This program is distributed in the hope that it will be useful, but
16 // WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // General Public License for more details.
20 // You should have received a copy of the GNU General Public License
21 // along with this program; if not, write to the Free Software
22 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
34 #include <Scenery/scenery.hxx>
36 #include <simgear/constants.h>
37 #include <simgear/debug/logstream.hxx>
39 #include <Airports/simple.hxx>
41 #include <Main/fg_init.hxx>
42 #include <Main/options.hxx>
43 #include <Time/fg_time.hxx>
45 #include "autopilot.hxx"
48 #define mySlider puSlider
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; // fpm
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 float MaxRollValue; // 0.1 -> 1.0
64 static float RollOutValue;
65 static float MaxAileronValue;
66 static float RollOutSmoothValue;
68 static float TmpMaxRollValue; // for cancel operation
69 static float TmpRollOutValue;
70 static float TmpMaxAileronValue;
71 static float TmpRollOutSmoothValue;
73 static puDialogBox *APAdjustDialog;
74 static puFrame *APAdjustFrame;
75 static puText *APAdjustDialogMessage;
76 static puFont APAdjustLegendFont;
77 static puFont APAdjustLabelFont;
79 static puOneShot *APAdjustOkButton;
80 static puOneShot *APAdjustResetButton;
81 static puOneShot *APAdjustCancelButton;
83 //static puButton *APAdjustDragButton;
85 static puText *APAdjustMaxRollTitle;
86 static puText *APAdjustRollOutTitle;
87 static puText *APAdjustMaxAileronTitle;
88 static puText *APAdjustRollOutSmoothTitle;
90 static puText *APAdjustMaxAileronText;
91 static puText *APAdjustMaxRollText;
92 static puText *APAdjustRollOutText;
93 static puText *APAdjustRollOutSmoothText;
95 static mySlider *APAdjustHS0;
96 static mySlider *APAdjustHS1;
97 static mySlider *APAdjustHS2;
98 static mySlider *APAdjustHS3;
100 static char SliderText[ 4 ][ 8 ];
102 ///////// AutoPilot New Heading Dialog
104 static puDialogBox *ApHeadingDialog;
105 static puFrame *ApHeadingDialogFrame;
106 static puText *ApHeadingDialogMessage;
107 static puInput *ApHeadingDialogInput;
108 static puOneShot *ApHeadingDialogOkButton;
109 static puOneShot *ApHeadingDialogCancelButton;
112 ///////// AutoPilot New Altitude Dialog
114 static puDialogBox *ApAltitudeDialog = 0;
115 static puFrame *ApAltitudeDialogFrame = 0;
116 static puText *ApAltitudeDialogMessage = 0;
117 static puInput *ApAltitudeDialogInput = 0;
119 static puOneShot *ApAltitudeDialogOkButton = 0;
120 static puOneShot *ApAltitudeDialogCancelButton = 0;
123 /// The beginnings of Lock AutoPilot to target location :-)
124 // Needs cleaning up but works
125 // These statics should disapear when this is a class
126 static puDialogBox *TgtAptDialog = 0;
127 static puFrame *TgtAptDialogFrame = 0;
128 static puText *TgtAptDialogMessage = 0;
129 static puInput *TgtAptDialogInput = 0;
131 static char NewTgtAirportId[16];
132 static char NewTgtAirportLabel[] = "Enter New TgtAirport ID";
134 static puOneShot *TgtAptDialogOkButton = 0;
135 static puOneShot *TgtAptDialogCancelButton = 0;
136 static puOneShot *TgtAptDialogResetButton = 0;
139 // global variable holding the AP info
140 // I want this gone. Data should be in aircraft structure
141 fgAPDataPtr APDataGlobal;
143 // Local Prototype section
144 static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 );
145 static double NormalizeDegrees( double Input );
146 // End Local ProtoTypes
148 extern char *coord_format_lat(float);
149 extern char *coord_format_lon(float);
152 static inline double get_ground_speed( void ) {
153 // starts in ft/s so we convert to kts
154 double ft_s = cur_fdm_state->get_V_ground_speed()
155 * current_options.get_speed_up();;
156 double kts = ft_s * FEET_TO_METER * 3600 * METER_TO_NM;
160 // The below routines were copied right from hud.c ( I hate reinventing
161 // the wheel more than necessary)
163 // The following routines obtain information concerntin the aircraft's
164 // current state and return it to calling instrument display routines.
165 // They should eventually be member functions of the aircraft.
167 static void get_control_values( void ) {
169 APData = APDataGlobal;
170 APData->old_aileron = controls.get_aileron();
171 APData->old_elevator = controls.get_elevator();
172 APData->old_elevator_trim = controls.get_elevator_trim();
173 APData->old_rudder = controls.get_rudder();
176 static void MakeTargetHeadingStr( fgAPDataPtr APData, double bearing ) {
179 else if(bearing > 360. )
181 sprintf(APData->TargetHeadingStr, "APHeading %6.1f", bearing);
184 static inline void MakeTargetDistanceStr( fgAPDataPtr APData, double distance ) {
185 double eta = distance*METER_TO_NM / get_ground_speed();
186 if ( eta >= 100.0 ) { eta = 99.999; }
188 if ( eta < (1.0/6.0) ) {
189 // within 10 minutes, bump up to min/secs
193 minor = (int)((eta - (int)eta) * 60.0);
194 sprintf(APData->TargetDistanceStr, "APDistance %.2f NM ETA %d:%02d",
195 distance*METER_TO_NM, major, minor);
196 // cout << "distance = " << distance*METER_TO_NM
197 // << " gndsp = " << get_ground_speed()
198 // << " time = " << eta
199 // << " major = " << major
200 // << " minor = " << minor
204 static inline void MakeTargetAltitudeStr( fgAPDataPtr APData, double altitude ) {
205 sprintf(APData->TargetAltitudeStr, "APAltitude %6.0f", altitude);
208 static inline void MakeTargetLatLonStr( fgAPDataPtr APData, double lat, double lon ) {
210 tmp = APData->TargetLatitude;
211 sprintf( APData->TargetLatitudeStr , "%s", coord_format_lat(tmp) );
212 tmp = APData->TargetLongitude;
213 sprintf( APData->TargetLongitudeStr, "%s", coord_format_lon(tmp) );
215 sprintf(APData->TargetLatLonStr, "%s %s",
216 APData->TargetLatitudeStr,
217 APData->TargetLongitudeStr );
220 static inline double get_speed( void ) {
221 return( cur_fdm_state->get_V_equiv_kts() );
224 static inline double get_aoa( void ) {
225 return( cur_fdm_state->get_Gamma_vert_rad() * RAD_TO_DEG );
228 static inline double fgAPget_latitude( void ) {
229 return( cur_fdm_state->get_Latitude() * RAD_TO_DEG );
232 static inline double fgAPget_longitude( void ) {
233 return( cur_fdm_state->get_Longitude() * RAD_TO_DEG );
236 static inline double fgAPget_roll( void ) {
237 return( cur_fdm_state->get_Phi() * RAD_TO_DEG );
240 static inline double get_pitch( void ) {
241 return( cur_fdm_state->get_Theta() );
244 double fgAPget_heading( void ) {
245 return( cur_fdm_state->get_Psi() * RAD_TO_DEG );
248 static inline double fgAPget_altitude( void ) {
249 return( cur_fdm_state->get_Altitude() * FEET_TO_METER );
252 static inline double fgAPget_climb( void ) {
253 // return in meters per minute
254 return( cur_fdm_state->get_Climb_Rate() * FEET_TO_METER * 60 );
257 static inline double get_sideslip( void ) {
258 return( cur_fdm_state->get_Beta() );
261 static inline double fgAPget_agl( void ) {
264 agl = cur_fdm_state->get_Altitude() * FEET_TO_METER
270 // End of copied section. ( thanks for the wheel :-)
271 double fgAPget_TargetLatitude( void ) {
272 fgAPDataPtr APData = APDataGlobal;
273 return APData->TargetLatitude;
276 double fgAPget_TargetLongitude( void ) {
277 fgAPDataPtr APData = APDataGlobal;
278 return APData->TargetLongitude;
281 double fgAPget_TargetHeading( void ) {
282 fgAPDataPtr APData = APDataGlobal;
283 return APData->TargetHeading;
286 double fgAPget_TargetDistance( void ) {
287 fgAPDataPtr APData = APDataGlobal;
288 return APData->TargetDistance;
291 double fgAPget_TargetAltitude( void ) {
292 fgAPDataPtr APData = APDataGlobal;
293 return APData->TargetAltitude;
296 char *fgAPget_TargetLatitudeStr( void ) {
297 fgAPDataPtr APData = APDataGlobal;
298 return APData->TargetLatitudeStr;
301 char *fgAPget_TargetLongitudeStr( void ) {
302 fgAPDataPtr APData = APDataGlobal;
303 return APData->TargetLongitudeStr;
306 char *fgAPget_TargetDistanceStr( void ) {
307 fgAPDataPtr APData = APDataGlobal;
308 return APData->TargetDistanceStr;
311 char *fgAPget_TargetHeadingStr( void ) {
312 fgAPDataPtr APData = APDataGlobal;
313 return APData->TargetHeadingStr;
316 char *fgAPget_TargetAltitudeStr( void ) {
317 fgAPDataPtr APData = APDataGlobal;
318 return APData->TargetAltitudeStr;
321 char *fgAPget_TargetLatLonStr( void ) {
322 fgAPDataPtr APData = APDataGlobal;
323 return APData->TargetLatLonStr;
326 bool fgAPWayPointEnabled( void )
330 APData = APDataGlobal;
332 // heading hold enabled?
333 return APData->waypoint_hold;
337 bool fgAPHeadingEnabled( void )
341 APData = APDataGlobal;
343 // heading hold enabled?
344 return APData->heading_hold;
347 bool fgAPAltitudeEnabled( void )
351 APData = APDataGlobal;
353 // altitude hold or terrain follow enabled?
354 return APData->altitude_hold;
357 bool fgAPTerrainFollowEnabled( void )
361 APData = APDataGlobal;
363 // altitude hold or terrain follow enabled?
364 return APData->terrain_follow ;
367 bool fgAPAutoThrottleEnabled( void )
371 APData = APDataGlobal;
373 // autothrottle enabled?
374 return APData->auto_throttle;
377 void fgAPAltitudeAdjust( double inc )
379 // Remove at a later date
380 fgAPDataPtr APData = APDataGlobal;
383 double target_alt, target_agl;
385 if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
386 target_alt = APData->TargetAltitude * METER_TO_FEET;
387 target_agl = APData->TargetAGL * METER_TO_FEET;
389 target_alt = APData->TargetAltitude;
390 target_agl = APData->TargetAGL;
393 target_alt = ( int ) ( target_alt / inc ) * inc + inc;
394 target_agl = ( int ) ( target_agl / inc ) * inc + inc;
396 if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET ) {
397 target_alt *= FEET_TO_METER;
398 target_agl *= FEET_TO_METER;
401 APData->TargetAltitude = target_alt;
402 APData->TargetAGL = target_agl;
404 if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
405 target_alt *= METER_TO_FEET;
406 ApAltitudeDialogInput->setValue((float)target_alt);
407 MakeTargetAltitudeStr( APData, target_alt);
409 get_control_values();
412 void fgAPAltitudeSet( double new_altitude ) {
413 // Remove at a later date
414 fgAPDataPtr APData = APDataGlobal;
416 double target_alt = new_altitude;
418 if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
419 target_alt = new_altitude * FEET_TO_METER;
421 if( target_alt < scenery.cur_elev )
422 target_alt = scenery.cur_elev;
424 APData->TargetAltitude = target_alt;
426 if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
427 target_alt *= METER_TO_FEET;
428 ApAltitudeDialogInput->setValue((float)target_alt);
429 MakeTargetAltitudeStr( APData, target_alt);
431 get_control_values();
434 void fgAPHeadingAdjust( double inc ) {
435 fgAPDataPtr APData = APDataGlobal;
437 if ( APData->waypoint_hold )
438 APData->waypoint_hold = false;
440 double target = ( int ) ( APData->TargetHeading / inc ) * inc + inc;
442 APData->TargetHeading = NormalizeDegrees( target );
443 // following cast needed ambiguous plib
444 ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
445 MakeTargetHeadingStr( APData, APData->TargetHeading );
446 get_control_values();
449 void fgAPHeadingSet( double new_heading ) {
450 fgAPDataPtr APData = APDataGlobal;
452 if ( APData->waypoint_hold )
453 APData->waypoint_hold = false;
455 new_heading = NormalizeDegrees( new_heading );
456 APData->TargetHeading = new_heading;
457 // following cast needed ambiguous plib
458 ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
459 MakeTargetHeadingStr( APData, APData->TargetHeading );
460 get_control_values();
463 void fgAPAutoThrottleAdjust( double inc ) {
464 fgAPDataPtr APData = APDataGlobal;
466 double target = ( int ) ( APData->TargetSpeed / inc ) * inc + inc;
468 APData->TargetSpeed = target;
471 // THIS NEEDS IMPROVEMENT !!!!!!!!!!!!!
472 static int scan_number(char *s, double *new_value)
477 char *WordBufPtr = WordBuf;
482 *WordBufPtr++ = *cptr++;
484 while (isdigit(*cptr) ) {
485 *WordBufPtr++ = *cptr++;
489 *WordBufPtr++ = *cptr++; // put the '.' into the string
490 while (isdigit(*cptr)) {
491 *WordBufPtr++ = *cptr++;
496 sscanf(WordBuf, "%lf", new_value);
503 void ApHeadingDialog_Cancel(puObject *)
505 ApHeadingDialogInput->rejectInput();
506 FG_POP_PUI_DIALOG( ApHeadingDialog );
509 void ApHeadingDialog_OK (puObject *me)
514 ApHeadingDialogInput -> getValue( &c );
518 if( scan_number( c, &NewHeading ) )
520 if(!fgAPHeadingEnabled())
522 fgAPHeadingSet( NewHeading );
526 s += " is not a valid number.";
529 ApHeadingDialog_Cancel(me);
530 if( error ) mkDialog(s.c_str());
533 void NewHeading(puObject *cb)
535 // string ApHeadingLabel( "Enter New Heading" );
536 // ApHeadingDialogMessage -> setLabel(ApHeadingLabel.c_str());
537 ApHeadingDialogInput -> acceptInput();
538 FG_PUSH_PUI_DIALOG( ApHeadingDialog );
541 void NewHeadingInit(void)
543 // printf("NewHeadingInit\n");
544 char NewHeadingLabel[] = "Enter New Heading";
547 float heading = fgAPget_heading();
549 (puGetStringWidth( puGetDefaultLabelFont(), NewHeadingLabel ) /2 );
551 ApHeadingDialog = new puDialogBox (150, 50);
553 ApHeadingDialogFrame = new puFrame (0, 0, 260, 150);
555 ApHeadingDialogMessage = new puText (len, 110);
556 ApHeadingDialogMessage -> setDefaultValue (NewHeadingLabel);
557 ApHeadingDialogMessage -> getDefaultValue (&s);
558 ApHeadingDialogMessage -> setLabel (s);
560 ApHeadingDialogInput = new puInput ( 50, 70, 210, 100 );
561 ApHeadingDialogInput -> setValue ( heading );
563 ApHeadingDialogOkButton = new puOneShot (50, 10, 110, 50);
564 ApHeadingDialogOkButton -> setLegend (gui_msg_OK);
565 ApHeadingDialogOkButton -> makeReturnDefault (TRUE);
566 ApHeadingDialogOkButton -> setCallback (ApHeadingDialog_OK);
568 ApHeadingDialogCancelButton = new puOneShot (140, 10, 210, 50);
569 ApHeadingDialogCancelButton -> setLegend (gui_msg_CANCEL);
570 ApHeadingDialogCancelButton -> setCallback (ApHeadingDialog_Cancel);
573 FG_FINALIZE_PUI_DIALOG( ApHeadingDialog );
576 void ApAltitudeDialog_Cancel(puObject *)
578 ApAltitudeDialogInput -> rejectInput();
579 FG_POP_PUI_DIALOG( ApAltitudeDialog );
582 void ApAltitudeDialog_OK (puObject *me)
587 ApAltitudeDialogInput->getValue( &c );
591 if( scan_number( c, &NewAltitude) )
593 if(!(fgAPAltitudeEnabled()))
594 fgAPToggleAltitude();
595 fgAPAltitudeSet( NewAltitude );
599 s += " is not a valid number.";
602 ApAltitudeDialog_Cancel(me);
603 if( error ) mkDialog(s.c_str());
604 get_control_values();
607 void NewAltitude(puObject *cb)
609 ApAltitudeDialogInput -> acceptInput();
610 FG_PUSH_PUI_DIALOG( ApAltitudeDialog );
613 void NewAltitudeInit(void)
615 // printf("NewAltitudeInit\n");
616 char NewAltitudeLabel[] = "Enter New Altitude";
619 float alt = cur_fdm_state->get_Altitude();
621 if ( current_options.get_units() == fgOPTIONS::FG_UNITS_METERS) {
622 alt *= FEET_TO_METER;
626 (puGetStringWidth( puGetDefaultLabelFont(), NewAltitudeLabel )/2);
628 // ApAltitudeDialog = new puDialogBox (150, 50);
629 ApAltitudeDialog = new puDialogBox (150, 200);
631 ApAltitudeDialogFrame = new puFrame (0, 0, 260, 150);
632 ApAltitudeDialogMessage = new puText (len, 110);
633 ApAltitudeDialogMessage -> setDefaultValue (NewAltitudeLabel);
634 ApAltitudeDialogMessage -> getDefaultValue (&s);
635 ApAltitudeDialogMessage -> setLabel (s);
637 ApAltitudeDialogInput = new puInput ( 50, 70, 210, 100 );
638 ApAltitudeDialogInput -> setValue ( alt );
639 // Uncomment the next line to have input active on startup
640 // ApAltitudeDialogInput -> acceptInput ( );
641 // cursor at begining or end of line ?
644 // ApAltitudeDialogInput -> setCursor ( len );
645 // ApAltitudeDialogInput -> setSelectRegion ( 5, 9 );
647 ApAltitudeDialogOkButton = new puOneShot (50, 10, 110, 50);
648 ApAltitudeDialogOkButton -> setLegend (gui_msg_OK);
649 ApAltitudeDialogOkButton -> makeReturnDefault (TRUE);
650 ApAltitudeDialogOkButton -> setCallback (ApAltitudeDialog_OK);
652 ApAltitudeDialogCancelButton = new puOneShot (140, 10, 210, 50);
653 ApAltitudeDialogCancelButton -> setLegend (gui_msg_CANCEL);
654 ApAltitudeDialogCancelButton -> setCallback (ApAltitudeDialog_Cancel);
657 FG_FINALIZE_PUI_DIALOG( ApAltitudeDialog );
660 /////// simple AutoPilot GAIN / LIMITS ADJUSTER
662 #define fgAP_CLAMP(val,min,max) \
663 ( (val) = (val) > (max) ? (max) : (val) < (min) ? (min) : (val) )
665 static void maxroll_adj( puObject *hs ) {
667 fgAPDataPtr APData = APDataGlobal;
669 hs-> getValue ( &val ) ;
670 fgAP_CLAMP ( val, 0.1, 1.0 ) ;
671 // printf ( "maxroll_adj( %p ) %f %f\n", hs, val, MaxRollAdjust * val ) ;
672 APData-> MaxRoll = MaxRollAdjust * val;
673 sprintf( SliderText[ 0 ], "%05.2f", APData->MaxRoll );
674 APAdjustMaxRollText -> setLabel ( SliderText[ 0 ] ) ;
677 static void rollout_adj( puObject *hs ) {
679 fgAPDataPtr APData = APDataGlobal;
681 hs-> getValue ( &val ) ;
682 fgAP_CLAMP ( val, 0.1, 1.0 ) ;
683 // printf ( "rollout_adj( %p ) %f %f\n", hs, val, RollOutAdjust * val ) ;
684 APData-> RollOut = RollOutAdjust * val;
685 sprintf( SliderText[ 1 ], "%05.2f", APData->RollOut );
686 APAdjustRollOutText -> setLabel ( SliderText[ 1 ] );
689 static void maxaileron_adj( puObject *hs ) {
692 APData = APDataGlobal;
694 hs-> getValue ( &val ) ;
695 fgAP_CLAMP ( val, 0.1, 1.0 ) ;
696 // printf ( "maxaileron_adj( %p ) %f %f\n", hs, val, MaxAileronAdjust * val ) ;
697 APData-> MaxAileron = MaxAileronAdjust * val;
698 sprintf( SliderText[ 3 ], "%05.2f", APData->MaxAileron );
699 APAdjustMaxAileronText -> setLabel ( SliderText[ 3 ] );
702 static void rolloutsmooth_adj( puObject *hs ) {
704 fgAPDataPtr APData = APDataGlobal;
706 hs -> getValue ( &val ) ;
707 fgAP_CLAMP ( val, 0.1, 1.0 ) ;
708 // printf ( "rolloutsmooth_adj( %p ) %f %f\n", hs, val, RollOutSmoothAdjust * val ) ;
709 APData->RollOutSmooth = RollOutSmoothAdjust * val;
710 sprintf( SliderText[ 2 ], "%5.2f", APData-> RollOutSmooth );
711 APAdjustRollOutSmoothText-> setLabel ( SliderText[ 2 ] );
715 static void goAwayAPAdjust (puObject *)
717 FG_POP_PUI_DIALOG( APAdjustDialog );
720 void cancelAPAdjust( puObject *self ) {
721 fgAPDataPtr APData = APDataGlobal;
723 APData-> MaxRoll = TmpMaxRollValue;
724 APData-> RollOut = TmpRollOutValue;
725 APData-> MaxAileron = TmpMaxAileronValue;
726 APData-> RollOutSmooth = TmpRollOutSmoothValue;
728 goAwayAPAdjust(self);
731 void resetAPAdjust( puObject *self ) {
732 fgAPDataPtr APData = APDataGlobal;
734 APData-> MaxRoll = MaxRollAdjust / 2;
735 APData-> RollOut = RollOutAdjust / 2;
736 APData-> MaxAileron = MaxAileronAdjust / 2;
737 APData-> RollOutSmooth = RollOutSmoothAdjust / 2;
739 FG_POP_PUI_DIALOG( APAdjustDialog );
744 void fgAPAdjust( puObject * ) {
745 fgAPDataPtr APData = APDataGlobal;
747 TmpMaxRollValue = APData-> MaxRoll;
748 TmpRollOutValue = APData-> RollOut;
749 TmpMaxAileronValue = APData-> MaxAileron;
750 TmpRollOutSmoothValue = APData-> RollOutSmooth;
752 MaxRollValue = APData-> MaxRoll / MaxRollAdjust;
753 RollOutValue = APData-> RollOut / RollOutAdjust;
754 MaxAileronValue = APData-> MaxAileron / MaxAileronAdjust;
755 RollOutSmoothValue = APData-> RollOutSmooth / RollOutSmoothAdjust;
757 APAdjustHS0-> setValue ( MaxRollValue ) ;
758 APAdjustHS1-> setValue ( RollOutValue ) ;
759 APAdjustHS2-> setValue ( RollOutSmoothValue ) ;
760 APAdjustHS3-> setValue ( MaxAileronValue ) ;
762 FG_PUSH_PUI_DIALOG( APAdjustDialog );
765 // Done once at system initialization
766 void fgAPAdjustInit( void ) {
768 // printf("fgAPAdjustInit\n");
769 #define HORIZONTAL FALSE
773 int DialogWidth = 230;
775 char Label[] = "AutoPilot Adjust";
778 fgAPDataPtr APData = APDataGlobal;
780 int labelX = (DialogWidth / 2) -
781 (puGetStringWidth( puGetDefaultLabelFont(), Label ) / 2);
782 labelX -= 30; // KLUDGEY
787 int slider_width = 210;
788 int slider_title_x = 15;
789 int slider_value_x = 160;
790 float slider_delta = 0.1f;
792 TmpMaxRollValue = APData-> MaxRoll;
793 TmpRollOutValue = APData-> RollOut;
794 TmpMaxAileronValue = APData-> MaxAileron;
795 TmpRollOutSmoothValue = APData-> RollOutSmooth;
797 MaxRollValue = APData-> MaxRoll / MaxRollAdjust;
798 RollOutValue = APData-> RollOut / RollOutAdjust;
799 MaxAileronValue = APData-> MaxAileron / MaxAileronAdjust;
800 RollOutSmoothValue = APData-> RollOutSmooth / RollOutSmoothAdjust;
802 puGetDefaultFonts ( &APAdjustLegendFont, &APAdjustLabelFont );
803 APAdjustDialog = new puDialogBox ( DialogX, DialogY ); {
804 int horiz_slider_height = puGetStringHeight (APAdjustLabelFont) +
805 puGetStringDescender (APAdjustLabelFont) +
806 PUSTR_TGAP + PUSTR_BGAP + 5;
808 APAdjustFrame = new puFrame ( 0, 0,
809 DialogWidth, 85 + nSliders * horiz_slider_height );
811 APAdjustDialogMessage = new puText ( labelX, 52 + nSliders * horiz_slider_height );
812 APAdjustDialogMessage -> setDefaultValue ( Label );
813 APAdjustDialogMessage -> getDefaultValue ( &s );
814 APAdjustDialogMessage -> setLabel ( s );
816 APAdjustHS0 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
817 APAdjustHS0-> setDelta ( slider_delta ) ;
818 APAdjustHS0-> setValue ( MaxRollValue ) ;
819 APAdjustHS0-> setCBMode ( PUSLIDER_DELTA ) ;
820 APAdjustHS0-> setCallback ( maxroll_adj ) ;
822 sprintf( SliderText[ 0 ], "%05.2f", APData->MaxRoll );
823 APAdjustMaxRollTitle = new puText ( slider_title_x, slider_y ) ;
824 APAdjustMaxRollTitle-> setDefaultValue ( "MaxRoll" ) ;
825 APAdjustMaxRollTitle-> getDefaultValue ( &s ) ;
826 APAdjustMaxRollTitle-> setLabel ( s ) ;
827 APAdjustMaxRollText = new puText ( slider_value_x, slider_y ) ;
828 APAdjustMaxRollText-> setLabel ( SliderText[ 0 ] ) ;
830 slider_y += horiz_slider_height;
832 APAdjustHS1 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
833 APAdjustHS1-> setDelta ( slider_delta ) ;
834 APAdjustHS1-> setValue ( RollOutValue ) ;
835 APAdjustHS1-> setCBMode ( PUSLIDER_DELTA ) ;
836 APAdjustHS1-> setCallback ( rollout_adj ) ;
838 sprintf( SliderText[ 1 ], "%05.2f", APData->RollOut );
839 APAdjustRollOutTitle = new puText ( slider_title_x, slider_y ) ;
840 APAdjustRollOutTitle-> setDefaultValue ( "AdjustRollOut" ) ;
841 APAdjustRollOutTitle-> getDefaultValue ( &s ) ;
842 APAdjustRollOutTitle-> setLabel ( s ) ;
843 APAdjustRollOutText = new puText ( slider_value_x, slider_y ) ;
844 APAdjustRollOutText-> setLabel ( SliderText[ 1 ] );
846 slider_y += horiz_slider_height;
848 APAdjustHS2 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
849 APAdjustHS2-> setDelta ( slider_delta ) ;
850 APAdjustHS2-> setValue ( RollOutSmoothValue ) ;
851 APAdjustHS2-> setCBMode ( PUSLIDER_DELTA ) ;
852 APAdjustHS2-> setCallback ( rolloutsmooth_adj ) ;
854 sprintf( SliderText[ 2 ], "%5.2f", APData->RollOutSmooth );
855 APAdjustRollOutSmoothTitle = new puText ( slider_title_x, slider_y ) ;
856 APAdjustRollOutSmoothTitle-> setDefaultValue ( "RollOutSmooth" ) ;
857 APAdjustRollOutSmoothTitle-> getDefaultValue ( &s ) ;
858 APAdjustRollOutSmoothTitle-> setLabel ( s ) ;
859 APAdjustRollOutSmoothText = new puText ( slider_value_x, slider_y ) ;
860 APAdjustRollOutSmoothText-> setLabel ( SliderText[ 2 ] );
862 slider_y += horiz_slider_height;
864 APAdjustHS3 = new mySlider ( slider_x, slider_y, slider_width, HORIZONTAL ) ;
865 APAdjustHS3-> setDelta ( slider_delta ) ;
866 APAdjustHS3-> setValue ( MaxAileronValue ) ;
867 APAdjustHS3-> setCBMode ( PUSLIDER_DELTA ) ;
868 APAdjustHS3-> setCallback ( maxaileron_adj ) ;
870 sprintf( SliderText[ 3 ], "%05.2f", APData->MaxAileron );
871 APAdjustMaxAileronTitle = new puText ( slider_title_x, slider_y ) ;
872 APAdjustMaxAileronTitle-> setDefaultValue ( "MaxAileron" ) ;
873 APAdjustMaxAileronTitle-> getDefaultValue ( &s ) ;
874 APAdjustMaxAileronTitle-> setLabel ( s ) ;
875 APAdjustMaxAileronText = new puText ( slider_value_x, slider_y ) ;
876 APAdjustMaxAileronText-> setLabel ( SliderText[ 3 ] );
878 APAdjustOkButton = new puOneShot ( 10, 10, 60, 50 );
879 APAdjustOkButton-> setLegend ( gui_msg_OK );
880 APAdjustOkButton-> makeReturnDefault ( TRUE );
881 APAdjustOkButton-> setCallback ( goAwayAPAdjust );
883 APAdjustCancelButton = new puOneShot ( 70, 10, 150, 50 );
884 APAdjustCancelButton-> setLegend ( gui_msg_CANCEL );
885 APAdjustCancelButton-> setCallback ( cancelAPAdjust );
887 APAdjustResetButton = new puOneShot ( 160, 10, 220, 50 );
888 APAdjustResetButton-> setLegend ( gui_msg_RESET );
889 APAdjustResetButton-> setCallback ( resetAPAdjust );
891 FG_FINALIZE_PUI_DIALOG( APAdjustDialog );
896 // Simple Dialog to input Target Airport
897 void TgtAptDialog_Cancel(puObject *)
899 FG_POP_PUI_DIALOG( TgtAptDialog );
902 void TgtAptDialog_OK (puObject *)
905 APData = APDataGlobal;
908 // FGTime *t = FGTime::cur_time_params;
909 // int PauseMode = t->getPause();
911 // t->togglePauseMode();
914 TgtAptDialogInput->getValue(&s);
917 TgtAptDialog_Cancel( NULL );
919 if ( TgtAptId.length() ) {
920 // set initial position from TgtAirport id
925 FG_LOG( FG_GENERAL, FG_INFO,
926 "Attempting to set starting position from airport code "
929 airports.load("apt_simple");
930 if ( airports.search( TgtAptId, &a ) )
932 double course, reverse, distance;
933 // fgAPset_tgt_airport_id( TgtAptId.c_str() );
934 current_options.set_airport_id( TgtAptId.c_str() );
935 sprintf( NewTgtAirportId, "%s", TgtAptId.c_str() );
937 APData->TargetLatitude = a.latitude; // * DEG_TO_RAD;
938 APData->TargetLongitude = a.longitude; // * DEG_TO_RAD;
939 MakeTargetLatLonStr( APData,
940 APData->TargetLatitude,
941 APData->TargetLongitude);
943 APData->old_lat = fgAPget_latitude();
944 APData->old_lon = fgAPget_longitude();
946 // need to test for iter
947 if( ! geo_inverse_wgs_84( fgAPget_altitude(),
950 APData->TargetLatitude,
951 APData->TargetLongitude,
955 APData->TargetHeading = course;
956 MakeTargetHeadingStr( APData, APData->TargetHeading );
957 APData->TargetDistance = distance;
958 MakeTargetDistanceStr( APData, distance );
959 // This changes the AutoPilot Heading
960 // following cast needed
961 ApHeadingDialogInput->
962 setValue((float)APData->TargetHeading);
964 APData->waypoint_hold = true ;
965 APData->heading_hold = true;
968 TgtAptId += " not in database.";
969 mkDialog(TgtAptId.c_str());
972 get_control_values();
973 // if( PauseMode != t->getPause() )
974 // t->togglePauseMode();
977 void TgtAptDialog_Reset(puObject *)
979 // strncpy( NewAirportId, current_options.get_airport_id().c_str(), 16 );
980 sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
981 TgtAptDialogInput->setValue ( NewTgtAirportId );
982 TgtAptDialogInput->setCursor( 0 ) ;
985 void NewTgtAirport(puObject *cb)
987 // strncpy( NewAirportId, current_options.get_airport_id().c_str(), 16 );
988 sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
989 TgtAptDialogInput->setValue( NewTgtAirportId );
991 FG_PUSH_PUI_DIALOG( TgtAptDialog );
994 void NewTgtAirportInit(void)
996 FG_LOG( FG_AUTOPILOT, FG_INFO, " enter NewTgtAirportInit()" );
997 // fgAPset_tgt_airport_id( current_options.get_airport_id() );
998 sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
999 FG_LOG( FG_AUTOPILOT, FG_INFO, " NewTgtAirportId " << NewTgtAirportId );
1000 // printf(" NewTgtAirportId %s\n", NewTgtAirportId);
1001 int len = 150 - puGetStringWidth( puGetDefaultLabelFont(),
1002 NewTgtAirportLabel ) / 2;
1004 TgtAptDialog = new puDialogBox (150, 50);
1006 TgtAptDialogFrame = new puFrame (0,0,350, 150);
1007 TgtAptDialogMessage = new puText (len, 110);
1008 TgtAptDialogMessage -> setLabel (NewTgtAirportLabel);
1010 TgtAptDialogInput = new puInput (50, 70, 300, 100);
1011 TgtAptDialogInput -> setValue (NewTgtAirportId);
1012 TgtAptDialogInput -> acceptInput();
1014 TgtAptDialogOkButton = new puOneShot (50, 10, 110, 50);
1015 TgtAptDialogOkButton -> setLegend (gui_msg_OK);
1016 TgtAptDialogOkButton -> setCallback (TgtAptDialog_OK);
1017 TgtAptDialogOkButton -> makeReturnDefault(TRUE);
1019 TgtAptDialogCancelButton = new puOneShot (140, 10, 210, 50);
1020 TgtAptDialogCancelButton -> setLegend (gui_msg_CANCEL);
1021 TgtAptDialogCancelButton -> setCallback (TgtAptDialog_Cancel);
1023 TgtAptDialogResetButton = new puOneShot (240, 10, 300, 50);
1024 TgtAptDialogResetButton -> setLegend (gui_msg_RESET);
1025 TgtAptDialogResetButton -> setCallback (TgtAptDialog_Reset);
1027 FG_FINALIZE_PUI_DIALOG( TgtAptDialog );
1028 printf("leave NewTgtAirportInit()");
1032 // Finally actual guts of AutoPilot
1033 void fgAPInit( fgAIRCRAFT *current_aircraft ) {
1036 FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" );
1038 APData = ( fgAPDataPtr ) calloc( sizeof( fgAPData ), 1 );
1040 if ( APData == NULL ) {
1041 // I couldn't get the mem. Dying
1042 FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying." );
1046 FG_LOG( FG_AUTOPILOT, FG_INFO, " Autopilot allocated " );
1048 APData->waypoint_hold = false ; // turn the target hold off
1049 APData->heading_hold = false ; // turn the heading hold off
1050 APData->altitude_hold = false ; // turn the altitude hold off
1052 // Initialize target location to startup location
1053 // FG_LOG( FG_AUTOPILOT, FG_INFO, " Autopilot setting startup location" );
1055 APData->TargetLatitude = fgAPget_latitude();
1057 APData->TargetLongitude = fgAPget_longitude();
1059 // FG_LOG( FG_AUTOPILOT, FG_INFO, " Autopilot setting TargetLatitudeStr" );
1060 MakeTargetLatLonStr( APData, APData->TargetLatitude, APData->TargetLongitude);
1062 APData->TargetHeading = 0.0; // default direction, due north
1063 APData->TargetAltitude = 3000; // default altitude in meters
1064 APData->alt_error_accum = 0.0;
1066 MakeTargetAltitudeStr( APData, 3000.0);
1067 MakeTargetHeadingStr( APData, 0.0 );
1069 // These eventually need to be read from current_aircaft somehow.
1073 // the maximum roll, in Deg
1074 APData->MaxRoll = 7;
1075 // the deg from heading to start rolling out at, in Deg
1076 APData->RollOut = 30;
1077 // how far can I move the aleron from center.
1078 APData->MaxAileron = .1;
1079 // Smoothing distance for alerion control
1080 APData->RollOutSmooth = 10;
1083 // the maximum roll, in Deg
1084 APData->MaxRoll = 20;
1086 // the deg from heading to start rolling out at, in Deg
1087 APData->RollOut = 20;
1089 // how far can I move the aleron from center.
1090 APData->MaxAileron = .2;
1092 // Smoothing distance for alerion control
1093 APData->RollOutSmooth = 10;
1095 //Remove at a later date
1096 APDataGlobal = APData;
1098 // Hardwired for now should be in options
1099 // 25% max control variablilty 0.5 / 2.0
1100 APData->disengage_threshold = 1.0;
1102 #if !defined( USING_SLIDER_CLASS )
1103 MaxRollAdjust = 2 * APData->MaxRoll;
1104 RollOutAdjust = 2 * APData->RollOut;
1105 MaxAileronAdjust = 2 * APData->MaxAileron;
1106 RollOutSmoothAdjust = 2 * APData->RollOutSmooth;
1107 #endif // !defined( USING_SLIDER_CLASS )
1109 get_control_values();
1111 // FG_LOG( FG_AUTOPILOT, FG_INFO, " calling NewTgtAirportInit" );
1112 NewTgtAirportInit();
1119 void fgAPReset( void ) {
1120 fgAPDataPtr APData = APDataGlobal;
1122 if ( fgAPTerrainFollowEnabled() )
1123 fgAPToggleTerrainFollow( );
1125 if ( fgAPAltitudeEnabled() )
1126 fgAPToggleAltitude();
1128 if ( fgAPHeadingEnabled() )
1129 fgAPToggleHeading();
1131 if ( fgAPAutoThrottleEnabled() )
1132 fgAPToggleAutoThrottle();
1134 APData->TargetHeading = 0.0; // default direction, due north
1135 MakeTargetHeadingStr( APData, APData->TargetHeading );
1137 APData->TargetAltitude = 3000; // default altitude in meters
1138 MakeTargetAltitudeStr( APData, 3000);
1140 APData->alt_error_accum = 0.0;
1143 get_control_values();
1145 sprintf( NewTgtAirportId, "%s", current_options.get_airport_id().c_str() );
1147 APData->TargetLatitude = fgAPget_latitude();
1148 APData->TargetLongitude = fgAPget_longitude();
1149 MakeTargetLatLonStr( APData,
1150 APData->TargetLatitude,
1151 APData->TargetLongitude);
1155 int fgAPRun( void ) {
1156 // Remove the following lines when the calling funcitons start
1157 // passing in the data pointer
1161 APData = APDataGlobal;
1164 // get control settings
1165 double aileron = controls.get_aileron();
1166 double elevator = controls.get_elevator();
1167 double elevator_trim = controls.get_elevator_trim();
1168 double rudder = controls.get_rudder();
1170 double lat = fgAPget_latitude();
1171 double lon = fgAPget_longitude();
1173 #ifdef FG_FORCE_AUTO_DISENGAGE
1174 // see if somebody else has changed them
1175 if( fabs(aileron - APData->old_aileron) > APData->disengage_threshold ||
1176 fabs(elevator - APData->old_elevator) > APData->disengage_threshold ||
1177 fabs(elevator_trim - APData->old_elevator_trim) >
1178 APData->disengage_threshold ||
1179 fabs(rudder - APData->old_rudder) > APData->disengage_threshold )
1181 // if controls changed externally turn autopilot off
1182 APData->waypoint_hold = false ; // turn the target hold off
1183 APData->heading_hold = false ; // turn the heading hold off
1184 APData->altitude_hold = false ; // turn the altitude hold off
1185 APData->terrain_follow = false; // turn the terrain_follow hold off
1186 // APData->auto_throttle = false; // turn the auto_throttle off
1188 // stash this runs control settings
1189 APData->old_aileron = aileron;
1190 APData->old_elevator = elevator;
1191 APData->old_elevator_trim = elevator_trim;
1192 APData->old_rudder = rudder;
1198 // waypoint hold enabled?
1199 if ( APData->waypoint_hold == true )
1201 double wp_course, wp_reverse, wp_distance;
1203 #ifdef DO_fgAP_CORRECTED_COURSE
1204 // compute course made good
1205 // this needs lots of special casing before use
1206 double course, reverse, distance, corrected_course;
1207 // need to test for iter
1208 geo_inverse_wgs_84( 0, //fgAPget_altitude(),
1216 #endif // DO_fgAP_CORRECTED_COURSE
1218 // compute course to way_point
1219 // need to test for iter
1220 if( ! geo_inverse_wgs_84( 0, //fgAPget_altitude(),
1223 APData->TargetLatitude,
1224 APData->TargetLongitude,
1229 #ifdef DO_fgAP_CORRECTED_COURSE
1230 corrected_course = course - wp_course;
1231 if( fabs(corrected_course) > 0.1 )
1232 printf("fgAP: course %f wp_course %f %f %f\n",
1233 course, wp_course, fabs(corrected_course), distance );
1234 #endif // DO_fgAP_CORRECTED_COURSE
1236 if ( wp_distance > 100 ) {
1237 // corrected_course = course - wp_course;
1238 APData->TargetHeading = NormalizeDegrees(wp_course);
1240 printf("APData->distance(%f) to close\n", wp_distance);
1241 // Real Close -- set heading hold to current heading
1242 // and Ring the arival bell !!
1243 NewTgtAirport(NULL);
1244 APData->waypoint_hold = false;
1246 APData->TargetHeading = fgAPget_heading();
1248 MakeTargetHeadingStr( APData, APData->TargetHeading );
1249 // Force this just in case
1250 APData->TargetDistance = wp_distance;
1251 MakeTargetDistanceStr( APData, wp_distance );
1252 // This changes the AutoPilot Heading Read Out
1253 // following cast needed
1254 ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
1256 APData->heading_hold = true;
1259 // heading hold enabled?
1260 if ( APData->heading_hold == true ) {
1267 NormalizeDegrees( APData->TargetHeading - fgAPget_heading() );
1268 // figure out how far off we are from desired heading
1270 // Now it is time to deterime how far we should be rolled.
1271 FG_LOG( FG_AUTOPILOT, FG_DEBUG, "RelHeading: " << RelHeading );
1274 // Check if we are further from heading than the roll out point
1275 if ( fabs( RelHeading ) > APData->RollOut ) {
1276 // set Target Roll to Max in desired direction
1277 if ( RelHeading < 0 ) {
1278 TargetRoll = 0 - APData->MaxRoll;
1280 TargetRoll = APData->MaxRoll;
1283 // We have to calculate the Target roll
1285 // This calculation engine thinks that the Target roll
1286 // should be a line from (RollOut,MaxRoll) to (-RollOut,
1287 // -MaxRoll) I hope this works well. If I get ambitious
1288 // some day this might become a fancier curve or
1291 TargetRoll = LinearExtrapolate( RelHeading, -APData->RollOut,
1292 -APData->MaxRoll, APData->RollOut,
1296 // Target Roll has now been Found.
1298 // Compare Target roll to Current Roll, Generate Rel Roll
1300 FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll );
1302 RelRoll = NormalizeDegrees( TargetRoll - fgAPget_roll() );
1304 // Check if we are further from heading than the roll out smooth point
1305 if ( fabs( RelRoll ) > APData->RollOutSmooth ) {
1306 // set Target Roll to Max in desired direction
1307 if ( RelRoll < 0 ) {
1308 AileronSet = 0 - APData->MaxAileron;
1310 AileronSet = APData->MaxAileron;
1313 AileronSet = LinearExtrapolate( RelRoll, -APData->RollOutSmooth,
1314 -APData->MaxAileron,
1315 APData->RollOutSmooth,
1316 APData->MaxAileron );
1319 controls.set_aileron( AileronSet );
1320 controls.set_rudder( AileronSet / 2.0 );
1321 // controls.set_rudder( 0.0 );
1324 // altitude hold or terrain follow enabled?
1325 if ( APData->altitude_hold || APData->terrain_follow ) {
1326 double speed, max_climb, error;
1327 double prop_error, int_error;
1328 double prop_adj, int_adj, total_adj;
1330 if ( APData->altitude_hold ) {
1331 // normal altitude hold
1332 APData->TargetClimbRate =
1333 ( APData->TargetAltitude - fgAPget_altitude() ) * 8.0;
1334 } else if ( APData->terrain_follow ) {
1335 // brain dead ground hugging with no look ahead
1336 APData->TargetClimbRate =
1337 ( APData->TargetAGL - fgAPget_agl() ) * 16.0;
1338 // cout << "target agl = " << APData->TargetAGL
1339 // << " current agl = " << fgAPget_agl()
1340 // << " target climb rate = " << APData->TargetClimbRate
1343 // just try to zero out rate of climb ...
1344 APData->TargetClimbRate = 0.0;
1347 speed = get_speed();
1349 if ( speed < min_climb ) {
1351 } else if ( speed < best_climb ) {
1352 max_climb = ((best_climb - min_climb) - (best_climb - speed))
1354 / (best_climb - min_climb);
1356 max_climb = ( speed - best_climb ) * 10.0 + ideal_climb_rate;
1359 // this first one could be optional if we wanted to allow
1360 // better climb performance assuming we have the airspeed to
1362 if ( APData->TargetClimbRate > ideal_climb_rate ) {
1363 APData->TargetClimbRate = ideal_climb_rate;
1366 if ( APData->TargetClimbRate > max_climb ) {
1367 APData->TargetClimbRate = max_climb;
1370 if ( APData->TargetClimbRate < -ideal_climb_rate ) {
1371 APData->TargetClimbRate = -ideal_climb_rate;
1374 error = fgAPget_climb() - APData->TargetClimbRate;
1375 // cout << "climb rate = " << fgAPget_climb()
1376 // << " error = " << error << endl;
1378 // accumulate the error under the curve ... this really should
1380 APData->alt_error_accum += error;
1382 // calculate integral error, and adjustment amount
1383 int_error = APData->alt_error_accum;
1384 // printf("error = %.2f int_error = %.2f\n", error, int_error);
1385 int_adj = int_error / 8000.0;
1387 // caclulate proportional error
1389 prop_adj = prop_error / 2000.0;
1391 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
1392 // if ( total_adj > 0.6 ) {
1394 // } else if ( total_adj < -0.2 ) {
1395 // total_adj = -0.2;
1397 if ( total_adj > 1.0 ) {
1399 } else if ( total_adj < -1.0 ) {
1403 controls.set_elevator( total_adj );
1406 // auto throttle enabled?
1407 if ( APData->auto_throttle ) {
1409 double prop_error, int_error;
1410 double prop_adj, int_adj, total_adj;
1412 error = APData->TargetSpeed - get_speed();
1414 // accumulate the error under the curve ... this really should
1416 APData->speed_error_accum += error;
1417 if ( APData->speed_error_accum > 2000.0 ) {
1418 APData->speed_error_accum = 2000.0;
1420 else if ( APData->speed_error_accum < -2000.0 ) {
1421 APData->speed_error_accum = -2000.0;
1424 // calculate integral error, and adjustment amount
1425 int_error = APData->speed_error_accum;
1427 // printf("error = %.2f int_error = %.2f\n", error, int_error);
1428 int_adj = int_error / 200.0;
1430 // caclulate proportional error
1432 prop_adj = 0.5 + prop_error / 50.0;
1434 total_adj = 0.9 * prop_adj + 0.1 * int_adj;
1435 if ( total_adj > 1.0 ) {
1438 else if ( total_adj < 0.0 ) {
1442 controls.set_throttle( FGControls::ALL_ENGINES, total_adj );
1445 #ifdef THIS_CODE_IS_NOT_USED
1446 if (APData->Mode == 2) // Glide slope hold
1451 // First, calculate Relative slope and normalize it
1452 RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch());
1454 // Now calculate the elevator offset from current angle
1455 if ( abs(RelSlope) > APData->SlopeSmooth )
1457 if ( RelSlope < 0 ) // set RelElevator to max in the correct direction
1458 RelElevator = -APData->MaxElevator;
1460 RelElevator = APData->MaxElevator;
1464 RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator);
1467 fgElevMove(RelElevator);
1470 #endif // THIS_CODE_IS_NOT_USED
1472 // stash this runs control settings
1473 // get_control_values();
1474 APData->old_aileron = controls.get_aileron();
1475 APData->old_elevator = controls.get_elevator();
1476 APData->old_elevator_trim = controls.get_elevator_trim();
1477 APData->old_rudder = controls.get_rudder();
1479 // for cross track error
1480 APData->old_lat = lat;
1481 APData->old_lon = lon;
1488 void fgAPSetMode( int mode)
1490 //Remove the following line when the calling funcitons start passing in the data pointer
1493 APData = APDataGlobal;
1496 fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode );
1498 APData->Mode = mode; // set the new mode
1502 void fgAPset_tgt_airport_id( const string id ) {
1503 FG_LOG( FG_AUTOPILOT, FG_INFO, "entering fgAPset_tgt_airport_id " << id );
1505 APData = APDataGlobal;
1506 APData->tgt_airport_id = id;
1507 FG_LOG( FG_AUTOPILOT, FG_INFO, "leaving fgAPset_tgt_airport_id "
1508 << APData->tgt_airport_id );
1511 string fgAPget_tgt_airport_id( void ) {
1512 fgAPDataPtr APData = APDataGlobal;
1513 return APData->tgt_airport_id;
1517 void fgAPToggleHeading( void ) {
1518 // Remove at a later date
1521 APData = APDataGlobal;
1524 if ( APData->heading_hold || APData->waypoint_hold ) {
1525 // turn off heading hold
1526 APData->heading_hold = false;
1527 APData->waypoint_hold = false;
1529 // turn on heading hold, lock at current heading
1530 APData->heading_hold = true;
1531 APData->TargetHeading = fgAPget_heading();
1532 MakeTargetHeadingStr( APData, APData->TargetHeading );
1533 ApHeadingDialogInput -> setValue ((float)APData->TargetHeading );
1536 get_control_values();
1537 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: ("
1538 << APData->heading_hold << ") " << APData->TargetHeading );
1541 void fgAPToggleWayPoint( void ) {
1542 // Remove at a later date
1545 APData = APDataGlobal;
1548 if ( APData->waypoint_hold ) {
1549 // turn off location hold
1550 APData->waypoint_hold = false;
1551 // set heading hold to current heading
1552 // APData->heading_hold = true;
1553 APData->TargetHeading = fgAPget_heading();
1555 double course, reverse, distance;
1556 // turn on location hold
1557 // turn on heading hold
1558 APData->old_lat = fgAPget_latitude();
1559 APData->old_lon = fgAPget_longitude();
1561 // need to test for iter
1562 if(!geo_inverse_wgs_84( fgAPget_altitude(),
1564 fgAPget_longitude(),
1565 APData->TargetLatitude,
1566 APData->TargetLongitude,
1570 APData->TargetHeading = course;
1571 APData->TargetDistance = distance;
1572 MakeTargetDistanceStr( APData, distance );
1575 APData->waypoint_hold = true;
1576 APData->heading_hold = true;
1579 // This changes the AutoPilot Heading
1580 // following cast needed
1581 ApHeadingDialogInput->setValue ((float)APData->TargetHeading );
1582 MakeTargetHeadingStr( APData, APData->TargetHeading );
1584 get_control_values();
1586 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetLocation: ( "
1587 << APData->waypoint_hold << " "
1588 << APData->TargetLatitude << " "
1589 << APData->TargetLongitude << " ) "
1594 void fgAPToggleAltitude( void ) {
1595 // Remove at a later date
1598 APData = APDataGlobal;
1601 if ( APData->altitude_hold ) {
1602 // turn off altitude hold
1603 APData->altitude_hold = false;
1605 // turn on altitude hold, lock at current altitude
1606 APData->altitude_hold = true;
1607 APData->terrain_follow = false;
1608 APData->TargetAltitude = fgAPget_altitude();
1609 APData->alt_error_accum = 0.0;
1610 // alt_error_queue.erase( alt_error_queue.begin(),
1611 // alt_error_queue.end() );
1612 float target_alt = APData->TargetAltitude;
1613 if ( current_options.get_units() == fgOPTIONS::FG_UNITS_FEET )
1614 target_alt *= METER_TO_FEET;
1616 ApAltitudeDialogInput->setValue(target_alt);
1617 MakeTargetAltitudeStr( APData, target_alt);
1620 get_control_values();
1621 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: ("
1622 << APData->altitude_hold << ") " << APData->TargetAltitude );
1626 void fgAPToggleAutoThrottle ( void ) {
1627 // Remove at a later date
1630 APData = APDataGlobal;
1633 if ( APData->auto_throttle ) {
1634 // turn off altitude hold
1635 APData->auto_throttle = false;
1637 // turn on terrain follow, lock at current agl
1638 APData->auto_throttle = true;
1639 APData->TargetSpeed = get_speed();
1640 APData->speed_error_accum = 0.0;
1643 get_control_values();
1644 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAutoThrottle: ("
1645 << APData->auto_throttle << ") " << APData->TargetSpeed );
1648 void fgAPToggleTerrainFollow( void ) {
1649 // Remove at a later date
1652 APData = APDataGlobal;
1655 if ( APData->terrain_follow ) {
1656 // turn off altitude hold
1657 APData->terrain_follow = false;
1659 // turn on terrain follow, lock at current agl
1660 APData->terrain_follow = true;
1661 APData->altitude_hold = false;
1662 APData->TargetAGL = fgAPget_agl();
1663 APData->alt_error_accum = 0.0;
1665 get_control_values();
1667 FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetTerrainFollow: ("
1668 << APData->terrain_follow << ") " << APData->TargetAGL );
1671 static double NormalizeDegrees( double Input ) {
1672 // normalize the input to the range (-180,180]
1673 // Input should not be greater than -360 to 360.
1674 // Current rules send the output to an undefined state.
1678 else if ( Input <= -180 )
1679 while ( Input <= -180 )
1684 static double LinearExtrapolate( double x, double x1, double y1, double x2, double y2 ) {
1685 // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2
1686 //assert(x1 != x2); // Divide by zero error. Cold abort for now
1689 // static double y = 0.0;
1690 // double dx = x2 -x1;
1691 // if( (dx < -FG_EPSILON ) || ( dx > FG_EPSILON ) )
1694 double m, b, y; // the constants to find in y=mx+b
1697 m = ( y2 - y1 ) / ( x2 - x1 ); // calculate the m
1699 b = y1 - m * x1; // calculate the b
1701 y = m * x + b; // the final calculation