Initial revision. Contributed by Jeff Goeke-Smith - jgoeke@voyager.net
This commit is contained in:
curt
parent
575d57249c
commit
019d180f84
4 changed files with 702 additions and 0 deletions
7
Autopilot/Makefile.am
Normal file
7
Autopilot/Makefile.am
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
libdir = ${exec_prefix}/lib
|
||||
|
||||
lib_LTLIBRARIES = libAutopilot.la
|
||||
|
||||
libAutopilot_la_SOURCES = autopilot.c autopilot.h
|
||||
|
||||
INCLUDES += -I$(top_builddir) -I$(top_builddir)/Simulator
|
||||
319
Autopilot/Makefile.in
Normal file
319
Autopilot/Makefile.in
Normal file
|
|
@ -0,0 +1,319 @@
|
|||
# Makefile.in generated automatically by automake 1.2h from Makefile.am
|
||||
|
||||
# Copyright (C) 1994, 1995, 1996, 1997, 1998 Free Software Foundation, Inc.
|
||||
# This Makefile.in is free software; the Free Software Foundation
|
||||
# gives unlimited permission to copy and/or distribute it,
|
||||
# with or without modifications, as long as this notice is preserved.
|
||||
|
||||
# This program is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY, to the extent permitted by law; without
|
||||
# even the implied warranty of MERCHANTABILITY or FITNESS FOR A
|
||||
# PARTICULAR PURPOSE.
|
||||
|
||||
|
||||
SHELL = /bin/sh
|
||||
|
||||
srcdir = @srcdir@
|
||||
top_srcdir = @top_srcdir@
|
||||
VPATH = @srcdir@
|
||||
prefix = @prefix@
|
||||
exec_prefix = @exec_prefix@
|
||||
|
||||
bindir = @bindir@
|
||||
sbindir = @sbindir@
|
||||
libexecdir = @libexecdir@
|
||||
datadir = @datadir@
|
||||
sysconfdir = @sysconfdir@
|
||||
sharedstatedir = @sharedstatedir@
|
||||
localstatedir = @localstatedir@
|
||||
infodir = @infodir@
|
||||
mandir = @mandir@
|
||||
includedir = @includedir@
|
||||
oldincludedir = /usr/include
|
||||
|
||||
DISTDIR =
|
||||
|
||||
pkgdatadir = $(datadir)/@PACKAGE@
|
||||
pkglibdir = $(libdir)/@PACKAGE@
|
||||
pkgincludedir = $(includedir)/@PACKAGE@
|
||||
|
||||
top_builddir = ../..
|
||||
|
||||
ACLOCAL = @ACLOCAL@
|
||||
AUTOCONF = @AUTOCONF@
|
||||
AUTOMAKE = @AUTOMAKE@
|
||||
AUTOHEADER = @AUTOHEADER@
|
||||
|
||||
INSTALL = @INSTALL@
|
||||
INSTALL_PROGRAM = @INSTALL_PROGRAM@
|
||||
INSTALL_DATA = @INSTALL_DATA@
|
||||
INSTALL_SCRIPT = @INSTALL_SCRIPT@
|
||||
transform = @program_transform_name@
|
||||
|
||||
NORMAL_INSTALL = :
|
||||
PRE_INSTALL = :
|
||||
POST_INSTALL = :
|
||||
NORMAL_UNINSTALL = :
|
||||
PRE_UNINSTALL = :
|
||||
POST_UNINSTALL = :
|
||||
host_alias = @host_alias@
|
||||
host_triplet = @host@
|
||||
CC = @CC@
|
||||
CXX = @CXX@
|
||||
LD = @LD@
|
||||
LIBTOOL = @LIBTOOL@
|
||||
LN_S = @LN_S@
|
||||
MAINT = @MAINT@
|
||||
MAKEINFO = @MAKEINFO@
|
||||
NM = @NM@
|
||||
PACKAGE = @PACKAGE@
|
||||
RANLIB = @RANLIB@
|
||||
VERSION = @VERSION@
|
||||
|
||||
libdir = ${exec_prefix}/lib
|
||||
|
||||
lib_LTLIBRARIES = libAutopilot.la
|
||||
|
||||
libAutopilot_la_SOURCES = autopilot.c autopilot.h
|
||||
mkinstalldirs = $(SHELL) $(top_srcdir)/mkinstalldirs
|
||||
CONFIG_HEADER = ../../Include/config.h
|
||||
CONFIG_CLEAN_FILES =
|
||||
LTLIBRARIES = $(lib_LTLIBRARIES)
|
||||
|
||||
|
||||
DEFS = @DEFS@ -I. -I$(srcdir) -I../../Include
|
||||
CPPFLAGS = @CPPFLAGS@
|
||||
LDFLAGS = @LDFLAGS@
|
||||
LIBS = @LIBS@
|
||||
X_CFLAGS = @X_CFLAGS@
|
||||
X_LIBS = @X_LIBS@
|
||||
X_EXTRA_LIBS = @X_EXTRA_LIBS@
|
||||
X_PRE_LIBS = @X_PRE_LIBS@
|
||||
libAutopilot_la_LDFLAGS =
|
||||
libAutopilot_la_LIBADD =
|
||||
libAutopilot_la_OBJECTS = autopilot.lo
|
||||
CFLAGS = @CFLAGS@
|
||||
COMPILE = $(CC) $(DEFS) $(INCLUDES) $(CPPFLAGS) $(CFLAGS)
|
||||
LTCOMPILE = $(LIBTOOL) --mode=compile $(CC) $(DEFS) $(INCLUDES) $(CPPFLAGS) $(CFLAGS)
|
||||
LINK = $(LIBTOOL) --mode=link $(CC) $(CFLAGS) $(LDFLAGS) -o $@
|
||||
DIST_COMMON = Makefile.am Makefile.in
|
||||
|
||||
|
||||
DISTFILES = $(DIST_COMMON) $(SOURCES) $(HEADERS) $(TEXINFOS) $(EXTRA_DIST)
|
||||
|
||||
TAR = tar
|
||||
GZIP = --best
|
||||
DEP_FILES = .deps/autopilot.P
|
||||
SOURCES = $(libAutopilot_la_SOURCES)
|
||||
OBJECTS = $(libAutopilot_la_OBJECTS)
|
||||
|
||||
all: Makefile $(LTLIBRARIES)
|
||||
|
||||
.SUFFIXES:
|
||||
.SUFFIXES: .S .c .lo .o .s
|
||||
$(srcdir)/Makefile.in: @MAINT@ Makefile.am $(top_srcdir)/configure.in $(ACLOCAL_M4)
|
||||
cd $(top_srcdir) && $(AUTOMAKE) --gnu Simulator/Autopilot/Makefile
|
||||
|
||||
Makefile: $(srcdir)/Makefile.in $(top_builddir)/config.status $(BUILT_SOURCES)
|
||||
cd $(top_builddir) \
|
||||
&& CONFIG_FILES=$(subdir)/$@ CONFIG_HEADERS= $(SHELL) ./config.status
|
||||
|
||||
|
||||
mostlyclean-libLTLIBRARIES:
|
||||
|
||||
clean-libLTLIBRARIES:
|
||||
-test -z "$(lib_LTLIBRARIES)" || rm -f $(lib_LTLIBRARIES)
|
||||
|
||||
distclean-libLTLIBRARIES:
|
||||
|
||||
maintainer-clean-libLTLIBRARIES:
|
||||
|
||||
install-libLTLIBRARIES: $(lib_LTLIBRARIES)
|
||||
@$(NORMAL_INSTALL)
|
||||
$(mkinstalldirs) $(DESTDIR)$(libdir)
|
||||
@list='$(lib_LTLIBRARIES)'; for p in $$list; do \
|
||||
if test -f $$p; then \
|
||||
echo "$(LIBTOOL) --mode=install $(INSTALL_DATA) $$p $(DESTDIR)$(libdir)/$$p"; \
|
||||
$(LIBTOOL) --mode=install $(INSTALL_DATA) $$p $(DESTDIR)$(libdir)/$$p; \
|
||||
else :; fi; \
|
||||
done
|
||||
|
||||
uninstall-libLTLIBRARIES:
|
||||
@$(NORMAL_UNINSTALL)
|
||||
list='$(lib_LTLIBRARIES)'; for p in $$list; do \
|
||||
$(LIBTOOL) --mode=uninstall rm -f $(DESTDIR)$(libdir)/$$p; \
|
||||
done
|
||||
|
||||
.s.o:
|
||||
$(COMPILE) -c $<
|
||||
|
||||
.S.o:
|
||||
$(COMPILE) -c $<
|
||||
|
||||
mostlyclean-compile:
|
||||
-rm -f *.o core *.core
|
||||
|
||||
clean-compile:
|
||||
|
||||
distclean-compile:
|
||||
-rm -f *.tab.c
|
||||
|
||||
maintainer-clean-compile:
|
||||
|
||||
.s.lo:
|
||||
$(LIBTOOL) --mode=compile $(COMPILE) -c $<
|
||||
|
||||
.S.lo:
|
||||
$(LIBTOOL) --mode=compile $(COMPILE) -c $<
|
||||
|
||||
mostlyclean-libtool:
|
||||
-rm -f *.lo
|
||||
|
||||
clean-libtool:
|
||||
-rm -rf .libs _libs
|
||||
|
||||
distclean-libtool:
|
||||
|
||||
maintainer-clean-libtool:
|
||||
|
||||
libAutopilot.la: $(libAutopilot_la_OBJECTS) $(libAutopilot_la_DEPENDENCIES)
|
||||
$(LINK) -rpath $(libdir) $(libAutopilot_la_LDFLAGS) $(libAutopilot_la_OBJECTS) $(libAutopilot_la_LIBADD) $(LIBS)
|
||||
|
||||
tags: TAGS
|
||||
|
||||
ID: $(HEADERS) $(SOURCES) $(LISP)
|
||||
here=`pwd` && cd $(srcdir) \
|
||||
&& mkid -f$$here/ID $(SOURCES) $(HEADERS) $(LISP)
|
||||
|
||||
TAGS: $(HEADERS) $(SOURCES) $(TAGS_DEPENDENCIES) $(LISP)
|
||||
tags=; \
|
||||
here=`pwd`; \
|
||||
list='$(SOURCES) $(HEADERS)'; \
|
||||
unique=`for i in $$list; do echo $$i; done | \
|
||||
awk ' { files[$$0] = 1; } \
|
||||
END { for (i in files) print i; }'`; \
|
||||
test -z "$(ETAGS_ARGS)$$unique$(LISP)$$tags" \
|
||||
|| (cd $(srcdir) && etags $(ETAGS_ARGS) $$tags $$unique $(LISP) -o $$here/TAGS)
|
||||
|
||||
mostlyclean-tags:
|
||||
|
||||
clean-tags:
|
||||
|
||||
distclean-tags:
|
||||
-rm -f TAGS ID
|
||||
|
||||
maintainer-clean-tags:
|
||||
|
||||
distdir = $(top_builddir)/$(PACKAGE)-$(VERSION)/$(subdir)
|
||||
|
||||
subdir = Simulator/Autopilot
|
||||
|
||||
distdir: $(DISTFILES)
|
||||
here=`cd $(top_builddir) && pwd`; \
|
||||
top_distdir=`cd $(top_distdir) && pwd`; \
|
||||
distdir=`cd $(distdir) && pwd`; \
|
||||
cd $(top_srcdir) \
|
||||
&& $(AUTOMAKE) --include-deps --build-dir=$$here --srcdir-name=$(top_srcdir) --output-dir=$$top_distdir --gnu Simulator/Autopilot/Makefile
|
||||
@for file in $(DISTFILES); do \
|
||||
d=$(srcdir); \
|
||||
test -f $(distdir)/$$file \
|
||||
|| ln $$d/$$file $(distdir)/$$file 2> /dev/null \
|
||||
|| cp -p $$d/$$file $(distdir)/$$file; \
|
||||
done
|
||||
|
||||
DEPS_MAGIC := $(shell mkdir .deps > /dev/null 2>&1 || :)
|
||||
|
||||
-include $(DEP_FILES)
|
||||
|
||||
mostlyclean-depend:
|
||||
|
||||
clean-depend:
|
||||
|
||||
distclean-depend:
|
||||
|
||||
maintainer-clean-depend:
|
||||
-rm -rf .deps
|
||||
|
||||
%.o: %.c
|
||||
@echo '$(COMPILE) -c $<'; \
|
||||
$(COMPILE) -Wp,-MD,.deps/$(*F).P -c $<
|
||||
|
||||
%.lo: %.c
|
||||
@echo '$(LTCOMPILE) -c $<'; \
|
||||
$(LTCOMPILE) -Wp,-MD,.deps/$(*F).p -c $<
|
||||
@-sed -e 's/^\([^:]*\)\.o:/\1.lo \1.o:/' \
|
||||
< .deps/$(*F).p > .deps/$(*F).P
|
||||
@-rm -f .deps/$(*F).p
|
||||
info:
|
||||
dvi:
|
||||
check: all
|
||||
$(MAKE)
|
||||
installcheck:
|
||||
install-exec: install-libLTLIBRARIES
|
||||
@$(NORMAL_INSTALL)
|
||||
|
||||
install-data:
|
||||
@$(NORMAL_INSTALL)
|
||||
|
||||
install: install-exec install-data all
|
||||
@:
|
||||
|
||||
uninstall: uninstall-libLTLIBRARIES
|
||||
|
||||
install-strip:
|
||||
$(MAKE) INSTALL_PROGRAM='$(INSTALL_PROGRAM) -s' INSTALL_SCRIPT='$(INSTALL_PROGRAM)' install
|
||||
installdirs:
|
||||
$(mkinstalldirs) $(DATADIR)$(libdir)
|
||||
|
||||
|
||||
mostlyclean-generic:
|
||||
-test -z "$(MOSTLYCLEANFILES)" || rm -f $(MOSTLYCLEANFILES)
|
||||
|
||||
clean-generic:
|
||||
-test -z "$(CLEANFILES)" || rm -f $(CLEANFILES)
|
||||
|
||||
distclean-generic:
|
||||
-rm -f Makefile $(DISTCLEANFILES)
|
||||
-rm -f config.cache config.log stamp-h stamp-h[0-9]*
|
||||
-test -z "$(CONFIG_CLEAN_FILES)" || rm -f $(CONFIG_CLEAN_FILES)
|
||||
|
||||
maintainer-clean-generic:
|
||||
-test -z "$(MAINTAINERCLEANFILES)" || rm -f $(MAINTAINERCLEANFILES)
|
||||
-test -z "$(BUILT_SOURCES)" || rm -f $(BUILT_SOURCES)
|
||||
mostlyclean: mostlyclean-libLTLIBRARIES mostlyclean-compile \
|
||||
mostlyclean-libtool mostlyclean-tags mostlyclean-depend \
|
||||
mostlyclean-generic
|
||||
|
||||
clean: clean-libLTLIBRARIES clean-compile clean-libtool clean-tags \
|
||||
clean-depend clean-generic mostlyclean
|
||||
|
||||
distclean: distclean-libLTLIBRARIES distclean-compile distclean-libtool \
|
||||
distclean-tags distclean-depend distclean-generic clean
|
||||
-rm -f config.status
|
||||
-rm -f libtool
|
||||
|
||||
maintainer-clean: maintainer-clean-libLTLIBRARIES \
|
||||
maintainer-clean-compile maintainer-clean-libtool \
|
||||
maintainer-clean-tags maintainer-clean-depend \
|
||||
maintainer-clean-generic distclean
|
||||
@echo "This command is intended for maintainers to use;"
|
||||
@echo "it deletes files that may require special tools to rebuild."
|
||||
|
||||
.PHONY: mostlyclean-libLTLIBRARIES distclean-libLTLIBRARIES \
|
||||
clean-libLTLIBRARIES maintainer-clean-libLTLIBRARIES \
|
||||
uninstall-libLTLIBRARIES install-libLTLIBRARIES mostlyclean-compile \
|
||||
distclean-compile clean-compile maintainer-clean-compile \
|
||||
mostlyclean-libtool distclean-libtool clean-libtool \
|
||||
maintainer-clean-libtool tags mostlyclean-tags distclean-tags \
|
||||
clean-tags maintainer-clean-tags distdir mostlyclean-depend \
|
||||
distclean-depend clean-depend maintainer-clean-depend info dvi \
|
||||
installcheck install-exec install-data install uninstall all \
|
||||
installdirs mostlyclean-generic distclean-generic clean-generic \
|
||||
maintainer-clean-generic clean mostlyclean distclean maintainer-clean
|
||||
|
||||
|
||||
INCLUDES += -I$(top_builddir) -I$(top_builddir)/Simulator
|
||||
|
||||
# Tell versions [3.59,3.63) of GNU make to not export all variables.
|
||||
# Otherwise a system limit (for SysV at least) may be exceeded.
|
||||
.NOEXPORT:
|
||||
322
Autopilot/autopilot.c
Normal file
322
Autopilot/autopilot.c
Normal file
|
|
@ -0,0 +1,322 @@
|
|||
/**************************************************************************
|
||||
* autopilot.c -- autopilot subsystem
|
||||
*
|
||||
* Written by Jeff Goeke-Smith, started April 1998.
|
||||
*
|
||||
* Copyright (C) 1998 Jeff Goeke-Smith, jgoeke@voyager.net
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU General Public License as
|
||||
* published by the Free Software Foundation; either version 2 of the
|
||||
* License, or (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program; if not, write to the Free Software
|
||||
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
||||
*
|
||||
*
|
||||
*
|
||||
**************************************************************************/
|
||||
|
||||
|
||||
// I have no Idea how many of these are needed. Some one tell me.
|
||||
#include <config.h>
|
||||
#include <assert.h>
|
||||
|
||||
#ifdef HAVE_WINDOWS_H
|
||||
# include <windows.h>
|
||||
#endif
|
||||
|
||||
#include <GL/glut.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#ifdef HAVE_VALUES_H
|
||||
# include <values.h> // for MAXINT
|
||||
#endif
|
||||
|
||||
#include "autopilot.h"
|
||||
|
||||
#include <Include/fg_constants.h>
|
||||
#include <Aircraft/aircraft.h>
|
||||
#include <Main/fg_debug.h>
|
||||
#include <Math/fg_random.h>
|
||||
#include <Math/mat3.h>
|
||||
#include <Math/polar.h>
|
||||
#include <Scenery/scenery.h>
|
||||
#include <Time/fg_timer.h>
|
||||
#include <Weather/weather.h>
|
||||
#include <Controls/controls.h>
|
||||
|
||||
|
||||
// The below routines were copied right from hud.c ( I hate reinventing
|
||||
// the wheel more than necessary)
|
||||
//// The following routines obtain information concerntin the aircraft's
|
||||
//// current state and return it to calling instrument display routines.
|
||||
//// They should eventually be member functions of the aircraft.
|
||||
////
|
||||
|
||||
double get_throttleval( void )
|
||||
{
|
||||
fgCONTROLS *pcontrols;
|
||||
|
||||
pcontrols = current_aircraft.controls;
|
||||
return pcontrols->throttle[0]; // Hack limiting to one engine
|
||||
}
|
||||
|
||||
double get_aileronval( void )
|
||||
{
|
||||
fgCONTROLS *pcontrols;
|
||||
|
||||
pcontrols = current_aircraft.controls;
|
||||
return pcontrols->aileron;
|
||||
}
|
||||
|
||||
double get_elevatorval( void )
|
||||
{
|
||||
fgCONTROLS *pcontrols;
|
||||
|
||||
pcontrols = current_aircraft.controls;
|
||||
return pcontrols->elevator;
|
||||
}
|
||||
|
||||
double get_elev_trimval( void )
|
||||
{
|
||||
fgCONTROLS *pcontrols;
|
||||
|
||||
pcontrols = current_aircraft.controls;
|
||||
return pcontrols->elevator_trim;
|
||||
}
|
||||
|
||||
double get_rudderval( void )
|
||||
{
|
||||
fgCONTROLS *pcontrols;
|
||||
|
||||
pcontrols = current_aircraft.controls;
|
||||
return pcontrols->rudder;
|
||||
}
|
||||
|
||||
double get_speed( void )
|
||||
{
|
||||
fgFLIGHT *f;
|
||||
|
||||
f = current_aircraft.flight;
|
||||
return( FG_V_equiv_kts ); // Make an explicit function call.
|
||||
}
|
||||
|
||||
double get_aoa( void )
|
||||
{
|
||||
fgFLIGHT *f;
|
||||
|
||||
f = current_aircraft.flight;
|
||||
return( FG_Gamma_vert_rad * RAD_TO_DEG );
|
||||
}
|
||||
|
||||
double fgAPget_roll( void )
|
||||
{
|
||||
fgFLIGHT *f;
|
||||
|
||||
f = current_aircraft.flight;
|
||||
return( FG_Phi * RAD_TO_DEG );
|
||||
}
|
||||
|
||||
double get_pitch( void )
|
||||
{
|
||||
fgFLIGHT *f;
|
||||
|
||||
f = current_aircraft.flight;
|
||||
return( FG_Theta );
|
||||
}
|
||||
|
||||
double fgAPget_heading( void )
|
||||
{
|
||||
fgFLIGHT *f;
|
||||
|
||||
f = current_aircraft.flight;
|
||||
return( FG_Psi * RAD_TO_DEG );
|
||||
}
|
||||
|
||||
double get_altitude( void )
|
||||
{
|
||||
fgFLIGHT *f;
|
||||
// double rough_elev;
|
||||
|
||||
f = current_aircraft.flight;
|
||||
// rough_elev = mesh_altitude(FG_Longitude * RAD_TO_ARCSEC,
|
||||
// FG_Latitude * RAD_TO_ARCSEC);
|
||||
|
||||
return( FG_Altitude * FEET_TO_METER /* -rough_elev */ );
|
||||
}
|
||||
|
||||
double get_sideslip( void )
|
||||
{
|
||||
fgFLIGHT *f;
|
||||
|
||||
f = current_aircraft.flight;
|
||||
|
||||
return( FG_Beta );
|
||||
}
|
||||
|
||||
// End of copied section. ( thanks for the wheel :-)
|
||||
|
||||
// Local Prototype section
|
||||
|
||||
double LinearExtrapolate( double x,double x1, double y1, double x2, double y2);
|
||||
|
||||
// End Local ProtoTypes
|
||||
|
||||
fgAPDataPtr APDataGlobal; // global variable holding the AP info
|
||||
|
||||
|
||||
|
||||
void fgAPInit( fgAIRCRAFT *current_aircraft )
|
||||
{
|
||||
fgAPDataPtr APData ;
|
||||
|
||||
fgPrintf( FG_COCKPIT, 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);
|
||||
|
||||
APData->Mode = 0 ; // turn the AP off
|
||||
APData->Heading = 5 ; // 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->MaxAileron= .1; // how far can I move the aleron from center.
|
||||
APData->RollOutSmooth = 5; // Smoothing distance for alerion control
|
||||
|
||||
//Remove at a later date
|
||||
APDataGlobal = APData;
|
||||
|
||||
};
|
||||
|
||||
int fgAPRun( void )
|
||||
{
|
||||
|
||||
//Remove the following lines when the calling funcitons start passing in the data pointer
|
||||
fgAPDataPtr APData;
|
||||
|
||||
APData = APDataGlobal;
|
||||
// end section
|
||||
|
||||
if (APData->Mode == 0) // the autopilot is shut off
|
||||
return 0 ;
|
||||
|
||||
if (APData->Mode == 1) // heading hold mode
|
||||
{
|
||||
double RelHeading;
|
||||
double TargetRoll;
|
||||
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);
|
||||
|
||||
// Now it is time to deterime how far we should be rolled.
|
||||
fgPrintf( FG_COCKPIT, FG_INFO, "RelHeading:\n");
|
||||
|
||||
|
||||
if ( abs(RelHeading) > APData->RollOut ) // We are further from heading than the roll out point
|
||||
{
|
||||
if (RelHeading < 0 ) // set Target Roll to Max in desired direction
|
||||
TargetRoll = 0-APData->MaxRoll;
|
||||
else
|
||||
TargetRoll = APData->MaxRoll;
|
||||
}
|
||||
else // We have to calculate the Target roll
|
||||
{
|
||||
/*
|
||||
* This calculation engine thinks that the Target roll should be a line from (RollOut,MaxRoll) to
|
||||
* (-RollOut, -MaxRoll) I hope this works well. If I get ambitious some day this might become a
|
||||
* fancier curve or something.
|
||||
*/
|
||||
TargetRoll = LinearExtrapolate(RelHeading,-APData->RollOut,-APData->MaxRoll,APData->RollOut,APData->MaxRoll);
|
||||
};
|
||||
|
||||
// Target Roll has now been Found.
|
||||
|
||||
// Compare Target roll to Current Roll, Generate Rel Roll
|
||||
fgPrintf( FG_COCKPIT, FG_INFO, "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 ;
|
||||
|
||||
|
||||
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
|
||||
AileronSet = 0-APData->MaxAileron;
|
||||
else
|
||||
AileronSet = APData->MaxAileron;
|
||||
}
|
||||
|
||||
else
|
||||
AileronSet = LinearExtrapolate(RelRoll,-APData->RollOutSmooth,-APData->MaxAileron,APData->RollOutSmooth,APData->MaxAileron);
|
||||
|
||||
fgAileronSet(AileronSet);
|
||||
|
||||
//Cool, it is done.
|
||||
return 0;
|
||||
}
|
||||
|
||||
//every thing else failed. Not in a valid autopilot mode
|
||||
return -1;
|
||||
|
||||
}
|
||||
|
||||
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
|
||||
|
||||
}
|
||||
|
||||
|
||||
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
|
||||
|
||||
m=(y2-y1)/(x2-x1); // calculate the m
|
||||
|
||||
b= y1- m * x1; // calculate the b
|
||||
|
||||
y = m * x + b; // the final calculation
|
||||
|
||||
return (y);
|
||||
|
||||
};
|
||||
54
Autopilot/autopilot.h
Normal file
54
Autopilot/autopilot.h
Normal file
|
|
@ -0,0 +1,54 @@
|
|||
/**************************************************************************
|
||||
* autopilot.h -- autopilot defines and prototypes (very alpha)
|
||||
*
|
||||
* Written by Jeff Goeke-Smith, started April 1998.
|
||||
*
|
||||
* Copyright (C) 1998 Jeff Goeke-Smith - jgoeke@voyager.net
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU General Public License as
|
||||
* published by the Free Software Foundation; either version 2 of the
|
||||
* License, or (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program; if not, write to the Free Software
|
||||
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
|
||||
*
|
||||
*
|
||||
**************************************************************************/
|
||||
|
||||
|
||||
#ifndef _AUTOPILOT_H
|
||||
#define _AUTOPILOT_H
|
||||
|
||||
#include <Aircraft/aircraft.h>
|
||||
#include <Flight/flight.h>
|
||||
#include <Controls/controls.h>
|
||||
|
||||
|
||||
|
||||
typedef struct {
|
||||
int Mode ; // the current mode the AP is operating in
|
||||
double Heading; // the heading the AP should steer to.
|
||||
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
|
||||
|
||||
} fgAPData, *fgAPDataPtr ;
|
||||
|
||||
|
||||
// prototypes
|
||||
void fgAPInit( fgAIRCRAFT *current_aircraft );
|
||||
int fgAPRun( void );
|
||||
void fgAPSetMode( int mode);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
Loading…
Add table
Reference in a new issue