From e64afb04125fdb01088254fa0899c32efe2ce736 Mon Sep 17 00:00:00 2001 From: curt Date: Thu, 7 May 1998 23:11:50 +0000 Subject: [PATCH] HUD updates from Jeff Goeke-Smith. --- Autopilot/autopilot.c | 102 ++++++++++++++++++++++++++++++------------ Autopilot/autopilot.h | 11 ++++- 2 files changed, 83 insertions(+), 30 deletions(-) diff --git a/Autopilot/autopilot.c b/Autopilot/autopilot.c index 8e380ba71..858055fb7 100644 --- a/Autopilot/autopilot.c +++ b/Autopilot/autopilot.c @@ -150,10 +150,12 @@ static double get_sideslip( void ) // Local Prototype section double LinearExtrapolate( double x,double x1, double y1, double x2, double y2); +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 @@ -161,23 +163,22 @@ void fgAPInit( fgAIRCRAFT *current_aircraft ) { fgAPDataPtr APData ; - fgPrintf( FG_COCKPIT, FG_INFO, "Init AutoPilot Subsystem\n" ); + fgPrintf( FG_AUTOPILOT, FG_INFO, "Init AutoPilot Subsystem\n" ); APData = (fgAPDataPtr)calloc(sizeof(fgAPData),1); if (APData == NULL) // I couldn't get the mem. Dying - // return ( NULL); - exit(1); + fgPrintf( FG_AUTOPILOT, FG_EXIT,"No ram for Autopilot. Dying.\n"); APData->Mode = 0 ; // turn the AP off APData->Heading = 0.0; // default direction, due north // These eventually need to be read from current_aircaft somehow. - APData->MaxRoll = 10; // the maximum roll, in Deg - APData->RollOut = 10; // the deg from heading to start rolling out at, in Deg + APData->MaxRoll = 7; // the maximum roll, in Deg + APData->RollOut = 30; // the deg from heading to start rolling out at, in Deg APData->MaxAileron= .1; // how far can I move the aleron from center. - APData->RollOutSmooth = 5; // Smoothing distance for alerion control + APData->RollOutSmooth = 10; // Smoothing distance for alerion control //Remove at a later date APDataGlobal = APData; @@ -203,17 +204,11 @@ int fgAPRun( void ) double RelRoll; double AileronSet; - RelHeading = APData->Heading - fgAPget_heading(); // figure out how far off we are from desired heading - if (RelHeading > 180) // Normalize the number to the range (-180,180] - RelHeading-= 360; // too much calc, sorry ^^^^^^^^^ - if (RelHeading <= -180) - RelHeading+=360; - - //assert(RelHeading <= 180); - //assert(RelHeading > -180); - + RelHeading = NormalizeDegrees( APData->Heading - fgAPget_heading()); + // figure out how far off we are from desired heading + // Now it is time to deterime how far we should be rolled. - fgPrintf( FG_COCKPIT, FG_DEBUG, "RelHeading:\n"); + fgPrintf( FG_AUTOPILOT, FG_DEBUG, "RelHeading: %f\n", RelHeading); if ( abs(RelHeading) > APData->RollOut ) // We are further from heading than the roll out point @@ -236,20 +231,10 @@ int fgAPRun( void ) // Target Roll has now been Found. // Compare Target roll to Current Roll, Generate Rel Roll - fgPrintf( FG_COCKPIT, FG_DEBUG, "TargetRoll:\n"); - - RelRoll = TargetRoll - fgAPget_roll(); - - if (RelRoll > 180) // Normalize the number to the range (-180,180] - RelRoll-= 360 ; // too much calc, sorry ^^^^^^^^^ - if (RelRoll <= -180) - RelRoll+=360 ; + fgPrintf( FG_COCKPIT, FG_BULK, "TargetRoll: %f\n", TargetRoll); + RelRoll = NormalizeDegrees(TargetRoll - fgAPget_roll()); - assert(RelRoll <= 180); - assert(RelRoll > -180); - - if ( abs(RelRoll) > APData->RollOutSmooth ) // We are further from heading than the roll out smooth point { if (RelRoll < 0 ) // set Target Roll to Max in desired direction @@ -267,6 +252,31 @@ int fgAPRun( void ) return 0; } + if (APData->Mode == 2) // Glide slope hold + { + double RelSlope; + double RelElevator; + + // First, calculate Relative slope and normalize it + RelSlope = NormalizeDegrees( APData->TargetSlope - get_pitch()); + + // Now calculate the elevator offset from current angle + if ( abs(RelSlope) > APData->SlopeSmooth ) + { + if ( RelSlope < 0 ) // set RelElevator to max in the correct direction + RelElevator = -APData->MaxElevator; + else + RelElevator = APData->MaxElevator; + } + + else + RelElevator = LinearExtrapolate(RelSlope,-APData->SlopeSmooth,-APData->MaxElevator,APData->SlopeSmooth,APData->MaxElevator); + + // set the elevator + fgElevMove(RelElevator); + + } + //every thing else failed. Not in a valid autopilot mode return -1; @@ -283,7 +293,29 @@ void fgAPSetMode( int mode) fgPrintf( FG_COCKPIT, FG_INFO, "APSetMode : %d\n", mode ); APData->Mode = mode; // set the new mode - APData->Heading = fgAPget_heading(); // Lock to current heading + +} + +void fgAPSetHeading( double Heading) +{ + // Set the heading for the autopilot subsystem + // Special Magic Number: + // -1= Set Heading To Current Heading, Define equivilent AP_CURRENT_HEADING + + // Remove at a later date + fgAPDataPtr APData; + + APData = APDataGlobal; + // end section + + if (Heading == AP_CURRENT_HEADING) { + APData->Heading = fgAPget_heading(); + } else { + APData->Heading = Heading; + } + + fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetHeading : %f\n", + APData->Heading); } @@ -303,3 +335,15 @@ double LinearExtrapolate( double x,double x1,double y1,double x2,double y2) return (y); }; + +double NormalizeDegrees(double Input) +{ + // normalize the input to the range (-180,180] + // Input should not be greater than -360 to 360. Current rules send the output to an undefined state. + if (Input > 180) + Input -= 360; + if (Input <= -180) + Input += 360; + + return (Input); +}; diff --git a/Autopilot/autopilot.h b/Autopilot/autopilot.h index 99c4b6287..ee2762c57 100644 --- a/Autopilot/autopilot.h +++ b/Autopilot/autopilot.h @@ -37,22 +37,32 @@ extern "C" { #endif +// Structures typedef struct { int Mode ; // the current mode the AP is operating in double Heading; // the heading the AP should steer to. + double TargetSlope; // the glide slope hold value + double MaxRoll ; // the max the plane can roll for the turn double RollOut; // when the plane should roll out // measured from Heading double MaxAileron; // how far to move the aleroin from center double RollOutSmooth; // deg to use for smoothing Aileron Control + double MaxElevator; // the maximum elevator allowed + double SlopeSmooth; // smoothing angle for elevator } fgAPData, *fgAPDataPtr ; +// Defines +#define AP_CURRENT_HEADING -1 + + // prototypes void fgAPInit( fgAIRCRAFT *current_aircraft ); int fgAPRun( void ); void fgAPSetMode( int mode); +void fgAPSetHeading( double Heading); #ifdef __cplusplus @@ -61,4 +71,3 @@ void fgAPSetMode( int mode); #endif // _AUTOPILOT_H - -- 2.39.2