fly/flightgear
1
0
Fork 0
Code Activity

Auto pilot tweaks. It looks like I actually got something that is functional.

Browse source 
It's far from perfect and still could use a lot of refining, but it basically
seems to work.
This commit is contained in:
curt 1998-09-29 21:54:23 +00:00
parent c464527318
commit eb81ebae2f
1 changed files with 26 additions and 22 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

48
Autopilot/autopilot.cxx
View file

@ -31,13 +31,14 @@
#include <assert.h> #include <assert.h>
#include <stdlib.h> #include <stdlib.h>
#include <list> // #include <list>
#include <Include/fg_stl_config.h> // #include <Include/fg_stl_config.h>
#include <Scenery/scenery.hxx> #include <Scenery/scenery.hxx>
#ifdef NEEDNAMESPACESTD // #ifdef NEEDNAMESPACESTD
using namespace std; // using namespace std;
#endif // #endif
#include "autopilot.hxx" #include "autopilot.hxx"
@ -45,7 +46,7 @@ using namespace std;
#include <Debug/fg_debug.h> #include <Debug/fg_debug.h>
static list < double > alt_error_queue; // static list < double > alt_error_queue;
// The below routines were copied right from hud.c ( I hate reinventing // The below routines were copied right from hud.c ( I hate reinventing
@ -300,12 +301,14 @@ int fgAPRun( void )
double prop_adj, int_adj, total_adj; double prop_adj, int_adj, total_adj;
// normal altitude hold // normal altitude hold
// APData->TargetClimbRate = APData->TargetClimbRate =
// (APData->TargetAltitude - fgAPget_altitude()) * 8.0; (APData->TargetAltitude - fgAPget_altitude()) * 12.0;
// brain dead ground hugging with no look ahead // brain dead ground hugging with no look ahead
APData->TargetClimbRate = // APData->TargetClimbRate = ( 500 - fgAPget_agl() ) * 12.0;
( 500 - fgAPget_agl() ) * 8.0;
// just try to zero out rate of climb ...
// APData->TargetClimbRate = 0.0;
if ( APData->TargetClimbRate > 200.0 ) { if ( APData->TargetClimbRate > 200.0 ) {
APData->TargetClimbRate = 200.0; APData->TargetClimbRate = 200.0;
@ -317,30 +320,30 @@ int fgAPRun( void )
error = fgAPget_climb() - APData->TargetClimbRate; error = fgAPget_climb() - APData->TargetClimbRate;
// push current error onto queue and add into accumulator // push current error onto queue and add into accumulator
alt_error_queue.push_back(error); // alt_error_queue.push_back(error);
APData->alt_error_accum += error; APData->alt_error_accum += error;
// if queue size larger than 60 ... pop front and subtract // if queue size larger than 60 ... pop front and subtract
// from accumulator // from accumulator
size = alt_error_queue.size(); // size = alt_error_queue.size();
if ( size > 300 ) { // if ( size > 300 ) {
APData->alt_error_accum -= alt_error_queue.front(); // APData->alt_error_accum -= alt_error_queue.front();
alt_error_queue.pop_front(); // alt_error_queue.pop_front();
size--; // size--;
} // }
// calculate integral error, and adjustment amount // calculate integral error, and adjustment amount
int_error = APData->alt_error_accum / 10; int_error = APData->alt_error_accum;
printf("error = %.2f int_error = %.2f\n", error, int_error); printf("error = %.2f int_error = %.2f\n", error, int_error);
int_adj = int_error / 2000.0; int_adj = int_error / 1500.0;
// caclulate proportional error // caclulate proportional error
prop_error = error; prop_error = error;
prop_adj = prop_error / 1000.0; prop_adj = prop_error / 2000.0;
total_adj = 0.9 * prop_adj + 0.1 * int_adj; total_adj = 0.9 * prop_adj + 0.1 * int_adj;
if ( total_adj > 0.5 ) { total_adj = 0.5; } if ( total_adj > 0.5 ) { total_adj = 0.5; }
if ( total_adj < -0.5 ) { total_adj = -0.5; } if ( total_adj < -0.3 ) { total_adj = -0.3; }
fgElevSet( total_adj ); fgElevSet( total_adj );
} }
@ -431,7 +434,8 @@ void fgAPToggleAltitude( void )
APData->altitude_hold = 1; APData->altitude_hold = 1;
APData->TargetAltitude = fgAPget_altitude(); APData->TargetAltitude = fgAPget_altitude();
APData->alt_error_accum = 0.0; APData->alt_error_accum = 0.0;
alt_error_queue.erase( alt_error_queue.begin(), alt_error_queue.end() ); // alt_error_queue.erase( alt_error_queue.begin(),
// alt_error_queue.end() );
} }
fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: (%d) %.2f\n", fgPrintf( FG_COCKPIT, FG_INFO, " fgAPSetAltitude: (%d) %.2f\n",