From 18e36a3302c1f0d4dc17387e981659e691ed9710 Mon Sep 17 00:00:00 2001 From: david Date: Thu, 25 Apr 2002 16:23:16 +0000 Subject: [PATCH] UFO FDM patches from Jonathan Polley: I have finished working the bugs out of my "Enhanced" version of the Magic Carpet FDM. The UFO FDM works the same as the Magic Carpet with the following exceptions: The aircraft's pitch is determined by the Elevator setting (with a pitch rate of 45 degrees per second). Climb is forward velocity * sin(pitch) Speed is forward velocity * soc(pitch) Roll is tied to the aileron (again, with a roll rate of 45 degrees per second) Turn rate is sin(roll) * 45 degrees. Yes, this does mean that turning is quite a bit more sluggish than climbing. If you are wondering why a UFO FDM, it is because it's best that I not replace the existing magic carpet and this FDM does behave like a UFO (it hovers, can spin and pitch while hovering, and does not obey the laws of physics ;) --- src/FDM/Makefile.am | 1 + src/FDM/UFO.cxx | 173 +++++++++++++++++++++++++++++++++++++++++++ src/FDM/UFO.hxx | 46 ++++++++++++ src/Main/fg_init.cxx | 3 + 4 files changed, 223 insertions(+) create mode 100644 src/FDM/UFO.cxx create mode 100644 src/FDM/UFO.hxx diff --git a/src/FDM/Makefile.am b/src/FDM/Makefile.am index 169e256ce..53b862ad7 100644 --- a/src/FDM/Makefile.am +++ b/src/FDM/Makefile.am @@ -12,6 +12,7 @@ libFlight_a_SOURCES = \ LaRCsim.cxx LaRCsim.hxx \ LaRCsimIC.cxx LaRCsimIC.hxx \ MagicCarpet.cxx MagicCarpet.hxx \ + UFO.cxx UFO.hxx \ NullFDM.cxx NullFDM.hxx if OLD_AUTOMAKE diff --git a/src/FDM/UFO.cxx b/src/FDM/UFO.cxx new file mode 100644 index 000000000..782f699f0 --- /dev/null +++ b/src/FDM/UFO.cxx @@ -0,0 +1,173 @@ +// UFO.cxx -- interface to the "UFO" flight model +// +// Written by Curtis Olson, started October 1999. +// Slightly modified from MagicCarpet.cxx by Jonathan Polley, April 2002 +// +// Copyright (C) 1999-2002 Curtis L. Olson - curt@flightgear.org +// +// 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. +// + + +#include +#include +#include + +#include +#include
+#include
+ +#include "UFO.hxx" + + +FGUFO::FGUFO( double dt ) { + set_delta_t( dt ); +} + + +FGUFO::~FGUFO() { +} + + +// Initialize the UFO flight model, dt is the time increment +// for each subsequent iteration through the EOM +void FGUFO::init() { + common_init(); +} + + +// Run an iteration of the EOM (equations of motion) +void FGUFO::update( int multiloop ) { + // cout << "FGLaRCsim::update()" << endl; + + double time_step = get_delta_t() * multiloop; + + // read the throttle + double Throttle = globals->get_controls()->get_throttle( 0 ); + + // read the state of the control surfaces + double Aileron = globals->get_controls()->get_aileron(); + double Elevator = globals->get_controls()->get_elevator(); + + // the velocity of the aircraft + double velocity = Throttle * 2000; // meters/sec + + double old_pitch = get_Theta(); + double pitch_rate = SGD_PI_4; // assume I will be pitching up + double target_pitch = -Elevator * SGD_PI_2; + + // if I am pitching down + if (old_pitch > target_pitch) + // set the pitch rate to negative (down) + pitch_rate *= -1; + + double pitch = old_pitch + (pitch_rate * time_step); + + // if I am pitching up + if (pitch_rate > 0.0) + { + // clip the pitch at the limit + if ( pitch > target_pitch) + { + pitch = target_pitch; + } + } + // if I am pitching down + else if (pitch_rate < 0.0) + { + // clip the pitch at the limit + if ( pitch < target_pitch) + { + pitch = target_pitch; + } + } + + double old_roll = get_Phi(); + double roll_rate = SGD_PI_4; + double target_roll = Aileron * SGD_PI_2; + + if (old_roll > target_roll) + roll_rate *= -1; + + double roll = old_roll + (roll_rate * time_step); + + // if I am rolling CW + if (roll_rate > 0.0) + { + // clip the roll at the limit + if ( roll > target_roll) + { + roll = target_roll; + } + } + // if I am rolling CCW + else if (roll_rate < 0.0) + { + // clip the roll at the limit + if ( roll < target_roll) + { + roll = target_roll; + } + } + + // the vertical speed of the aircraft + double real_climb_rate = sin (pitch) * SG_METER_TO_FEET * velocity; // feet/sec + _set_Climb_Rate( -Elevator * 10.0 ); + double climb = real_climb_rate * time_step; + + // the lateral speed of the aircraft + double speed = cos (pitch) * velocity; // meters/sec + double dist = speed * time_step; + double kts = velocity * SG_METER_TO_NM * 3600.0; + _set_V_equiv_kts( kts ); + _set_V_calibrated_kts( kts ); + _set_V_ground_speed( kts ); + + // angle of turn + double turn_rate = sin(roll) * SGD_PI_4; // radians/sec + double turn = turn_rate * time_step; + + // update (lon/lat) position + double lat2, lon2, az2; + if ( speed > SG_EPSILON ) { + geo_direct_wgs_84 ( get_Altitude(), + get_Latitude() * SGD_RADIANS_TO_DEGREES, + get_Longitude() * SGD_RADIANS_TO_DEGREES, + get_Psi() * SGD_RADIANS_TO_DEGREES, + dist, &lat2, &lon2, &az2 ); + + _set_Longitude( lon2 * SGD_DEGREES_TO_RADIANS ); + _set_Latitude( lat2 * SGD_DEGREES_TO_RADIANS ); + } + + // cout << "lon error = " << fabs(end.x()*SGD_RADIANS_TO_DEGREES - lon2) + // << " lat error = " << fabs(end.y()*SGD_RADIANS_TO_DEGREES - lat2) + // << endl; + + double sl_radius, lat_geoc; + sgGeodToGeoc( get_Latitude(), get_Altitude(), &sl_radius, &lat_geoc ); + + // update euler angles + _set_Euler_Angles( roll, pitch, + fmod(get_Psi() + turn, SGD_2PI) ); + _set_Euler_Rates(0,0,0); + + _set_Geocentric_Position( lat_geoc, get_Longitude(), + sl_radius + get_Altitude() + climb ); + // cout << "sea level radius (ft) = " << sl_radius << endl; + // cout << "(setto) sea level radius (ft) = " << get_Sea_level_radius() << endl; + _set_Sea_level_radius( sl_radius * SG_METER_TO_FEET); + _set_Altitude( get_Altitude() + climb ); +} diff --git a/src/FDM/UFO.hxx b/src/FDM/UFO.hxx new file mode 100644 index 000000000..aed107e75 --- /dev/null +++ b/src/FDM/UFO.hxx @@ -0,0 +1,46 @@ +// UFO.hxx -- interface to the "UFO" flight model +// +// Written by Curtis Olson, started October 1999. +// Slightly modified from MagicCarpet.hxx by Jonathan Polley, April 2002 +// +// Copyright (C) 1999-2002 Curtis L. Olson - curt@flightgear.org +// +// 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 _UFO_HXX +#define _UFO_HXX + + +#include "flight.hxx" + + +class FGUFO: public FGInterface { + +public: + FGUFO( double dt ); + ~FGUFO(); + + // reset flight params to a specific position + void init(); + + // update position based on inputs, positions, velocities, etc. + void update( int multiloop ); + +}; + + +#endif // _UFO_HXX diff --git a/src/Main/fg_init.cxx b/src/Main/fg_init.cxx index 9f8c6ebff..4b9b479db 100644 --- a/src/Main/fg_init.cxx +++ b/src/Main/fg_init.cxx @@ -89,6 +89,7 @@ #include #include #include +#include #include #include #include @@ -586,6 +587,8 @@ void fgInitFDM() { cur_fdm_state = new FGBalloonSim( dt ); } else if ( model == "magic" ) { cur_fdm_state = new FGMagicCarpet( dt ); + } else if ( model == "ufo" ) { + cur_fdm_state = new FGUFO( dt ); } else if ( model == "external" ) { cur_fdm_state = new FGExternal( dt ); } else if ( model.find("network,") == 0 ) { -- 2.39.5