From 6f55cae2cc58ee6663e912fd8eebba22c6879035 Mon Sep 17 00:00:00 2001 From: curt Date: Tue, 27 Apr 1999 15:42:23 +0000 Subject: [PATCH] Changes by NHV so that the autopilot can be driven by the GUI. --- Simulator/Autopilot/autopilot.cxx | 519 +++++++++++++++++++++++++----- Simulator/Autopilot/autopilot.hxx | 7 + 2 files changed, 454 insertions(+), 72 deletions(-) diff --git a/Simulator/Autopilot/autopilot.cxx b/Simulator/Autopilot/autopilot.cxx index 413aa7830..5dc892520 100644 --- a/Simulator/Autopilot/autopilot.cxx +++ b/Simulator/Autopilot/autopilot.cxx @@ -36,6 +36,7 @@ #include #include
+#include // The below routines were copied right from hud.c ( I hate reinventing // the wheel more than necessary) @@ -89,12 +90,12 @@ static double get_sideslip( void ) static double fgAPget_agl( void ) { - double agl; + double agl; - agl = current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER - - scenery.cur_elev; + agl = current_aircraft.fdm_state->get_Altitude() * FEET_TO_METER + - scenery.cur_elev; - return( agl ); + return( agl ); } // End of copied section. ( thanks for the wheel :-) @@ -106,16 +107,16 @@ double NormalizeDegrees( double Input); // End Local ProtoTypes -fgAPDataPtr APDataGlobal; // global variable holding the AP info - // I want this gone. Data should be in aircraft structure +fgAPDataPtr APDataGlobal; // global variable holding the AP info +// I want this gone. Data should be in aircraft structure bool fgAPHeadingEnabled( void ) { fgAPDataPtr APData; - + APData = APDataGlobal; - + // heading hold enabled? return APData->heading_hold; } @@ -123,17 +124,27 @@ bool fgAPHeadingEnabled( void ) bool fgAPAltitudeEnabled( void ) { fgAPDataPtr APData; - + APData = APDataGlobal; - + // altitude hold or terrain follow enabled? - return APData->altitude_hold || APData->terrain_follow ; + return APData->altitude_hold; +} + +bool fgAPTerrainFollowEnabled( void ) +{ + fgAPDataPtr APData; + + APData = APDataGlobal; + + // altitude hold or terrain follow enabled? + return APData->terrain_follow ; } bool fgAPAutoThrottleEnabled( void ) { fgAPDataPtr APData; - + APData = APDataGlobal; // autothrottle enabled? @@ -189,56 +200,426 @@ void fgAPAutoThrottleAdjust( double inc ) APData->TargetSpeed = target; } -void fgAPInit( fgAIRCRAFT *current_aircraft ) +void fgAPReset(void) +{ + if( fgAPTerrainFollowEnabled() ) + fgAPToggleTerrainFollow( ); + + if( fgAPAltitudeEnabled() ) + fgAPToggleAltitude(); + + if( fgAPHeadingEnabled() ) + fgAPToggleHeading(); + + if( fgAPAutoThrottleEnabled() ) + fgAPToggleAutoThrottle(); +} + +#define mySlider puSlider + +/// These statics will eventually go into the class +/// they are just here while I am experimenting -- NHV :-) + +static double MaxRollAdjust; // MaxRollAdjust = 2 * APData->MaxRoll; +static double RollOutAdjust; // RollOutAdjust = 2 * APData->RollOut; +static double MaxAileronAdjust; // MaxAileronAdjust = 2 * APData->MaxAileron; +static double RollOutSmoothAdjust; // RollOutSmoothAdjust = 2 * APData->RollOutSmooth; + +static float MaxRollValue; // 0.1 -> 1.0 +static float RollOutValue; +static float MaxAileronValue; +static float RollOutSmoothValue; + +static float TmpMaxRollValue; // for cancel operation +static float TmpRollOutValue; +static float TmpMaxAileronValue; +static float TmpRollOutSmoothValue; + +static puDialogBox *APAdjustDialog; +static puFrame *APAdjustFrame; +static puText *APAdjustDialogMessage; + +static int DialogX = 40; +static int DialogY = 100; + +static puOneShot *APAdjustOkButton; +static puOneShot *APAdjustResetButton; +static puOneShot *APAdjustCancelButton; + +//static puButton *APAdjustDragButton; + +static puText *APAdjustMaxRollTitle; +static puText *APAdjustRollOutTitle; +static puText *APAdjustMaxAileronTitle; +static puText *APAdjustRollOutSmoothTitle; + +static puText *APAdjustMaxAileronText; +static puText *APAdjustMaxRollText; +static puText *APAdjustRollOutText; +static puText *APAdjustRollOutSmoothText; + +static mySlider *APAdjustHS0; +static mySlider *APAdjustHS1; +static mySlider *APAdjustHS2; +static mySlider *APAdjustHS3; + +static char SliderText[4][8]; + + +#define fgAP_CLAMP(val,min,max) \ +( (val) = (val) > (max) ? (max) : (val) < (min) ? (min) : (val) ) + + static void maxroll_adj(puObject *hs) +{ + float val ; + fgAPDataPtr APData; + APData = APDataGlobal; + + hs -> getValue ( &val ) ; + fgAP_CLAMP ( val, 0.1, 1.0 ) ; + // printf ( "maxroll_adj( %p ) %f %f\n", hs, val, MaxRollAdjust * val ) ; + APData->MaxRoll = MaxRollAdjust * val; + sprintf(SliderText[0],"%05.2f", APData->MaxRoll ); + APAdjustMaxRollText -> setLabel ( SliderText[0] ) ; +} + +static void rollout_adj(puObject *hs) +{ + float val ; + fgAPDataPtr APData; + APData = APDataGlobal; + + hs -> getValue ( &val ) ; + fgAP_CLAMP ( val, 0.1, 1.0 ) ; + // printf ( "rollout_adj( %p ) %f %f\n", hs, val, RollOutAdjust * val ) ; + APData->RollOut = RollOutAdjust * val; + sprintf(SliderText[1],"%05.2f", APData->RollOut ); + APAdjustRollOutText -> setLabel ( SliderText[1] ); +} + +static void maxaileron_adj( puObject *hs ) +{ + float val ; + fgAPDataPtr APData; + APData = APDataGlobal; + + hs -> getValue ( &val ) ; + fgAP_CLAMP ( val, 0.1, 1.0 ) ; + // printf ( "maxaileron_adj( %p ) %f %f\n", hs, val, MaxAileronAdjust * val ) ; + APData->MaxAileron = MaxAileronAdjust * val; + sprintf(SliderText[3],"%05.2f", APData->MaxAileron ); + APAdjustMaxAileronText -> setLabel ( SliderText[3] ); +} + +static void rolloutsmooth_adj( puObject *hs ) +{ + float val ; + fgAPDataPtr APData; + APData = APDataGlobal; + + hs -> getValue ( &val ) ; + fgAP_CLAMP ( val, 0.1, 1.0 ) ; + // printf ( "rolloutsmooth_adj( %p ) %f %f\n", hs, val, RollOutSmoothAdjust * val ) ; + APData->RollOutSmooth = RollOutSmoothAdjust * val; + sprintf(SliderText[2],"%5.2f", APData->RollOutSmooth ); + APAdjustRollOutSmoothText -> setLabel ( SliderText[2] ); + +} + +static void goAwayAPAdjust (puObject *) +{ + puPopLiveInterface ( ) ; + // puPopInterface ( ) ; + puPopGroup ( ) ; + APAdjustDialog -> hide(); +} + +void cancelAPAdjust(puObject *) +{ + fgAPDataPtr APData = APDataGlobal; + + APData->MaxRoll = TmpMaxRollValue; + APData->RollOut = TmpRollOutValue; + APData->MaxAileron = TmpMaxAileronValue; + APData->RollOutSmooth = TmpRollOutSmoothValue; + + puPopLiveInterface ( ) ; + // puPopInterface ( ) ; + puPopGroup ( ) ; + + + APAdjustDialog -> hide(); +} + +void resetAPAdjust(puObject *self) +{ + fgAPDataPtr APData = APDataGlobal; + + APData->MaxRoll = MaxRollAdjust / 2; + APData->RollOut = RollOutAdjust / 2; + APData->MaxAileron = MaxAileronAdjust / 2; + APData->RollOutSmooth = RollOutSmoothAdjust / 2; + + puPopLiveInterface ( ) ; + // puPopInterface ( ) ; + puPopGroup ( ) ; + + APAdjustDialog -> hide(); + fgAPAdjust(self); +} + +void fgAPAdjust( puObject * ) +{ + fgAPDataPtr APData = APDataGlobal; + + TmpMaxRollValue = APData->MaxRoll; + TmpRollOutValue = APData->RollOut; + TmpMaxAileronValue = APData->MaxAileron; + TmpRollOutSmoothValue = APData->RollOutSmooth; + + MaxRollValue = APData->MaxRoll / MaxRollAdjust; + RollOutValue = APData->RollOut / RollOutAdjust; + MaxAileronValue = APData->MaxAileron / MaxAileronAdjust; + RollOutSmoothValue = APData->RollOutSmooth / RollOutSmoothAdjust; + + APAdjustHS0 -> setValue ( MaxRollValue ) ; + APAdjustHS1 -> setValue ( RollOutValue ) ; + APAdjustHS2 -> setValue ( RollOutSmoothValue ) ; + APAdjustHS3 -> setValue ( MaxAileronValue ) ; + + // puPushInterface ( APAdjustDialog ) ; + puPushGroup ( APAdjustDialog ) ; + puPushLiveInterface ( APAdjustDialog ) ; + + APAdjustDialog -> reveal(); +} + +#ifdef NOT_USED +void dragAPAdjust(puObject *self) { - fgAPDataPtr APData ; - FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" ); + return; + + int LastX, LastY; + int DeltaX, DeltaY, X, Y; + int button, count; + + // button = guiGetMouseButton(); + guiGetMouse(&LastX, &LastY); + + button = 0; + printf("button %d LastX %d LastY %d\n", button, LastX, LastY); - APData = (fgAPDataPtr)calloc(sizeof(fgAPData),1); + // count = 0; + // while( guiGetMouseButton() == button ) + for(count=0;count<1000;count++) + { + guiGetMouse(&X, &Y); + printf(" X %d Y %d\n", X, Y); + } + + DeltaX = X - LastX; + DeltaY = Y - LastY; + + // LiveInterface is Dialog Box + puInterface *Interface = puGetBaseLiveInterface(); + puObject *dlist = Interface->getFirstChild() ; + // printf("%s %p %s %p\n", + // Interface->getTypeString(), Interface, dlist->getTypeString(), dlist); - if (APData == NULL) { - // I couldn't get the mem. Dying - FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying."); - exit(-1); + Interface -> getPosition ( &X, &Y ) ; + Interface -> setPosition ( X + DeltaX, Y + DeltaY ) ; + // Interface -> recalc_bbox(); + + for ( puObject *bo = dlist ; bo != NULL ; bo = bo->next ) + { + bo -> getPosition ( &X, &Y ) ; + bo -> setPosition ( X + DeltaX, Y + DeltaY ) ; + bo -> recalc_bbox(); + // printf("%s %p X %d Y %d\n", bo->getTypeString(), bo, X, Y); } + + Interface->recalc_bbox(); + + puPopLiveInterface ( ) ; + // puPopInterface ( ) ; + puPopGroup ( ) ; + + APAdjustDialog -> hide(); + fgAPAdjust(dlist); +} +#endif // NOT_USED + +// Done once at system initialization +void fgAPAdjustInit( void ) +{ +#define HORIZONTAL FALSE + + fgAPDataPtr APData = APDataGlobal; + + TmpMaxRollValue = APData->MaxRoll; + TmpRollOutValue = APData->RollOut; + TmpMaxAileronValue = APData->MaxAileron; + TmpRollOutSmoothValue = APData->RollOutSmooth; + + MaxRollValue = APData->MaxRoll / MaxRollAdjust; + RollOutValue = APData->RollOut / RollOutAdjust; + MaxAileronValue = APData->MaxAileron / MaxAileronAdjust; + RollOutSmoothValue = APData->RollOutSmooth / RollOutSmoothAdjust; + + APAdjustDialog = new puDialogBox (DialogX, DialogY); + { + int horiz_slider_height = puGetStringHeight () + + puGetStringDescender () + + PUSTR_TGAP + PUSTR_BGAP+5; + + APAdjustFrame = new puFrame (0,0,230, 85+4*horiz_slider_height); + + APAdjustDialogMessage = new puText (40, 52 + 4*horiz_slider_height); + APAdjustDialogMessage -> setLabel ("AutoPilot Adjust"); + + // APAdjustDragButton = new puButton (0, 75+4*horiz_slider_height, + // 230, 85+4*horiz_slider_height); + // APAdjustDragButton -> setLegend ("*"); + // APAdjustDragButton -> setCallback (dragAPAdjust); + + int slider_y = 55; + APAdjustHS0 = new mySlider ( 10, slider_y, 210, HORIZONTAL ) ; + APAdjustHS0 -> setDelta ( 0.1 ) ; + APAdjustHS0 -> setValue ( MaxRollValue ) ; + APAdjustHS0 -> setCBMode ( PUSLIDER_DELTA ) ; + APAdjustHS0 -> setCallback ( maxroll_adj ) ; - APData->heading_hold = false ; // turn the heading hold off - APData->altitude_hold = false ; // turn the altitude hold off + sprintf(SliderText[0],"%05.2f", APData->MaxRoll ); + APAdjustMaxRollTitle = new puText ( 15, slider_y ) ; + APAdjustMaxRollTitle -> setLabel ( "MaxRoll" ) ; + APAdjustMaxRollText = new puText ( 160, slider_y ) ; + APAdjustMaxRollText -> setLabel ( SliderText[0] ) ; + + slider_y += horiz_slider_height; + + APAdjustHS1 = new mySlider ( 10, slider_y, 210, HORIZONTAL ) ; + APAdjustHS1 -> setDelta ( 0.1 ) ; + APAdjustHS1 -> setValue ( RollOutValue ) ; + APAdjustHS1 -> setCBMode ( PUSLIDER_DELTA ) ; + APAdjustHS1 -> setCallback ( rollout_adj ) ; + + sprintf(SliderText[1],"%05.2f", APData->RollOut ); + APAdjustRollOutTitle = new puText ( 15, slider_y ) ; + APAdjustRollOutTitle -> setLabel ( "AdjustRollOut" ) ; + APAdjustRollOutText = new puText ( 160, slider_y ) ; + APAdjustRollOutText -> setLabel ( SliderText[1] ); + + slider_y += horiz_slider_height; + + APAdjustHS2 = new mySlider ( 10, slider_y, 210, HORIZONTAL ) ; + APAdjustHS2 -> setDelta ( 0.1 ) ; + APAdjustHS2 -> setValue ( RollOutSmoothValue ) ; + APAdjustHS2 -> setCBMode ( PUSLIDER_DELTA ) ; + APAdjustHS2 -> setCallback ( rolloutsmooth_adj ) ; + + sprintf(SliderText[2],"%5.2f", APData->RollOutSmooth ); + APAdjustRollOutSmoothTitle = new puText ( 15, slider_y ) ; + APAdjustRollOutSmoothTitle -> setLabel ( "RollOutSmooth" ) ; + APAdjustRollOutSmoothText = new puText ( 160, slider_y ) ; + APAdjustRollOutSmoothText -> setLabel ( SliderText[2] ); + + slider_y += horiz_slider_height; + + APAdjustHS3 = new mySlider ( 10, slider_y, 210, HORIZONTAL ) ; + APAdjustHS3 -> setDelta ( 0.1 ) ; + APAdjustHS3 -> setValue ( MaxAileronValue ) ; + APAdjustHS3 -> setCBMode ( PUSLIDER_DELTA ) ; + APAdjustHS3 -> setCallback ( maxaileron_adj ) ; + + sprintf(SliderText[3],"%05.2f", APData->MaxAileron ); + APAdjustMaxAileronTitle = new puText ( 15, slider_y ) ; + APAdjustMaxAileronTitle -> setLabel ( "MaxAileron" ) ; + APAdjustMaxAileronText = new puText ( 160, slider_y ) ; + APAdjustMaxAileronText -> setLabel ( SliderText[3] ); + + APAdjustOkButton = new puOneShot (10, 10, 60, 50); + APAdjustOkButton -> setLegend ("OK"); + APAdjustOkButton -> makeReturnDefault (TRUE ); + APAdjustOkButton -> setCallback (goAwayAPAdjust); + + APAdjustCancelButton = new puOneShot (70, 10, 150, 50); + APAdjustCancelButton -> setLegend ("Cancel"); + APAdjustCancelButton -> makeReturnDefault (TRUE ); + APAdjustCancelButton -> setCallback (cancelAPAdjust); + + APAdjustResetButton = new puOneShot (160, 10, 220, 50); + APAdjustResetButton -> setLegend ("Reset"); + APAdjustResetButton -> makeReturnDefault (TRUE ); + APAdjustResetButton -> setCallback (resetAPAdjust); + } + APAdjustDialog -> close(); + // APAdjustDialog -> reveal(); + // Take it off the Stack + puPopLiveInterface ( ) ; + // puPopInterface ( ) ; + puPopGroup ( ) ; +#undef HORIZONTAL +} - APData->TargetHeading = 0.0; // default direction, due north - APData->TargetAltitude = 3000; // default altitude in meters - APData->alt_error_accum = 0.0; +void fgAPInit( fgAIRCRAFT *current_aircraft ) +{ + fgAPDataPtr APData ; + + FG_LOG( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem" ); + + APData = (fgAPDataPtr)calloc(sizeof(fgAPData),1); + + if (APData == NULL) { + // I couldn't get the mem. Dying + FG_LOG( FG_AUTOPILOT, FG_ALERT, "No ram for Autopilot. Dying."); + exit(-1); + } + + APData->heading_hold = false ; // turn the heading hold off + APData->altitude_hold = false ; // turn the altitude hold off + + APData->TargetHeading = 0.0; // default direction, due north + APData->TargetAltitude = 3000; // default altitude in meters + APData->alt_error_accum = 0.0; - // These eventually need to be read from current_aircaft somehow. + // These eventually need to be read from current_aircaft somehow. #if 0 - // Original values - // the maximum roll, in Deg - APData->MaxRoll = 7; - // the deg from heading to start rolling out at, in Deg - APData->RollOut = 30; - // how far can I move the aleron from center. - APData->MaxAileron= .1; - // Smoothing distance for alerion control - APData->RollOutSmooth = 10; + // Original values + // the maximum roll, in Deg + APData->MaxRoll = 7; + // the deg from heading to start rolling out at, in Deg + APData->RollOut = 30; + // how far can I move the aleron from center. + APData->MaxAileron= .1; + // Smoothing distance for alerion control + APData->RollOutSmooth = 10; #endif - // the maximum roll, in Deg - APData->MaxRoll = 20; + // the maximum roll, in Deg + APData->MaxRoll = 20; - // the deg from heading to start rolling out at, in Deg - APData->RollOut = 20; + // the deg from heading to start rolling out at, in Deg + APData->RollOut = 20; - // how far can I move the aleron from center. - APData->MaxAileron= .2; + // how far can I move the aleron from center. + APData->MaxAileron= .2; - // Smoothing distance for alerion control - APData->RollOutSmooth = 10; + // Smoothing distance for alerion control + APData->RollOutSmooth = 10; - //Remove at a later date - APDataGlobal = APData; - + //Remove at a later date + APDataGlobal = APData; + +#if !defined( USING_SLIDER_CLASS ) + MaxRollAdjust = 2 * APData->MaxRoll; + RollOutAdjust = 2 * APData->RollOut; + MaxAileronAdjust = 2 * APData->MaxAileron; + RollOutSmoothAdjust = 2 * APData->RollOutSmooth; +#endif // !defined( USING_SLIDER_CLASS ) + + fgAPAdjustInit( ) ; }; int fgAPRun( void ) @@ -247,25 +628,25 @@ int fgAPRun( void ) // passing in the data pointer fgAPDataPtr APData; - + APData = APDataGlobal; // end section - + // heading hold enabled? if ( APData->heading_hold == true ) { double RelHeading; double TargetRoll; double RelRoll; double AileronSet; - + RelHeading = NormalizeDegrees( APData->TargetHeading - fgAPget_heading()); // 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 ); - - + + // Check if we are further from heading than the roll out point if ( fabs(RelHeading) > APData->RollOut ) { // set Target Roll to Max in desired direction @@ -287,13 +668,13 @@ int fgAPRun( void ) -APData->MaxRoll, APData->RollOut, APData->MaxRoll ); } - + // Target Roll has now been Found. // Compare Target roll to Current Roll, Generate Rel Roll FG_LOG( FG_COCKPIT, FG_BULK, "TargetRoll: " << TargetRoll ); - + RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll()); // Check if we are further from heading than the roll out smooth point @@ -310,7 +691,7 @@ int fgAPRun( void ) APData->RollOutSmooth, APData->MaxAileron ); } - + controls.set_aileron( AileronSet ); controls.set_rudder( 0.0 ); } @@ -409,7 +790,7 @@ int fgAPRun( void ) controls.set_throttle( FGControls::ALL_ENGINES, total_adj ); } - /* + /* if (APData->Mode == 2) // Glide slope hold { double RelSlope; @@ -435,7 +816,7 @@ int fgAPRun( void ) } */ - + // Ok, we are done return 0; @@ -446,12 +827,12 @@ void fgAPSetMode( int mode) { //Remove the following line when the calling funcitons start passing in the data pointer fgAPDataPtr APData; - + APData = APDataGlobal; // end section - + fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode ); - + APData->Mode = mode; // set the new mode } */ @@ -476,7 +857,7 @@ void fgAPToggleHeading( void ) FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetHeading: (" << APData->heading_hold << ") " << APData->TargetHeading ); } - + void fgAPToggleAltitude( void ) { @@ -502,7 +883,7 @@ void fgAPToggleAltitude( void ) FG_LOG( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: (" << APData->altitude_hold << ") " << APData->TargetAltitude ); } - + void fgAPToggleAutoThrottle ( void ) { @@ -554,16 +935,12 @@ double LinearExtrapolate( double x,double x1,double y1,double x2,double y2) // This procedure extrapolates the y value for the x posistion on a line defined by x1,y1; x2,y2 //assert(x1 != x2); // Divide by zero error. Cold abort for now - double m, b, y; // the constants to find in y=mx+b - + double m, b, y; // the constants to find in y=mx+b m=(y2-y1)/(x2-x1); // calculate the m + b= y1- m * x1; // calculate the b + y = m * x + b; // the final calculation - b= y1- m * x1; // calculate the b - - y = m * x + b; // the final calculation - - return (y); - + return y; }; double NormalizeDegrees(double Input) @@ -577,5 +954,3 @@ double NormalizeDegrees(double Input) return (Input); }; - - diff --git a/Simulator/Autopilot/autopilot.hxx b/Simulator/Autopilot/autopilot.hxx index e6b954145..438feaf9b 100644 --- a/Simulator/Autopilot/autopilot.hxx +++ b/Simulator/Autopilot/autopilot.hxx @@ -65,18 +65,25 @@ typedef struct { // prototypes void fgAPInit( fgAIRCRAFT *current_aircraft ); int fgAPRun( void ); + void fgAPToggleHeading( void ); void fgAPToggleAltitude( void ); void fgAPToggleTerrainFollow( void ); void fgAPToggleAutoThrottle( void ); +bool fgAPTerrainFollowEnabled( void ); bool fgAPAltitudeEnabled( void ); bool fgAPHeadingEnabled( void ); bool fgAPAutoThrottleEnabled( void ); + void fgAPAltitudeAdjust( double inc ); void fgAPHeadingAdjust( double inc ); void fgAPAutoThrottleAdjust( double inc ); +void fgAPReset(void); + +class puObject; +void fgAPAdjust( puObject * ); #endif // _AUTOPILOT_HXX -- 2.39.5