/***************************************************************************
TITLE: ls_step
----------------------------------------------------------------------------
FUNCTION: Integration routine for equations of motion
(vehicle states)
----------------------------------------------------------------------------
MODULE STATUS: developmental
----------------------------------------------------------------------------
GENEALOGY: Written 920802 by Bruce Jackson. Based upon equations
given in reference [1] and a Matrix-X/System Build block
diagram model of equations of motion coded by David Raney
at NASA-Langley in June of 1992.
----------------------------------------------------------------------------
DESIGNED BY: Bruce Jackson
CODED BY: Bruce Jackson
MAINTAINED BY:
----------------------------------------------------------------------------
MODIFICATION HISTORY:
DATE PURPOSE BY
921223 Modified calculation of Phi and Psi to use the "atan2" routine
rather than the "atan" to allow full circular angles.
"atan" limits to +/- pi/2. EBJ
940111 Changed from oldstyle include file ls_eom.h; also changed
from DATA to SCALAR type. EBJ
950207 Initialized Alpha_dot and Beta_dot to zero on first pass; calculated
thereafter. EBJ
950224 Added logic to avoid adding additional increment to V_east
in case V_east already accounts for rotating earth.
EBJ
CURRENT RCS HEADER:
$Header$
$Log$
Revision 1.4 1998/08/24 20:09:27 curt
Code optimization tweaks from Norman Vine.
Revision 1.3 1998/07/12 03:11:04 curt
Removed some printf()'s.
Fixed the autopilot integration so it should be able to update it's control
positions every time the internal flight model loop is run, and not just
once per rendered frame.
Added a routine to do the necessary stuff to force an arbitrary altitude
change.
Gave the Navion engine just a tad more power.
Revision 1.2 1998/01/19 18:40:28 curt
Tons of little changes to clean up the code and to remove fatal errors
when building with the c++ compiler.
Revision 1.1 1997/05/29 00:09:59 curt
Initial Flight Gear revision.
* Revision 1.5 1995/03/02 20:24:13 bjax
* Added logic to avoid adding additional increment to V_east
* in case V_east already accounts for rotating earth. EBJ
*
* Revision 1.4 1995/02/07 20:52:21 bjax
* Added initialization of Alpha_dot and Beta_dot to zero on first
* pass; they get calculated by ls_aux on next pass... EBJ
*
* Revision 1.3 1994/01/11 19:01:12 bjax
* Changed from DATA to SCALAR type; also fixed header files (was ls_eom.h)
*
* Revision 1.2 1993/06/02 15:03:09 bjax
* Moved initialization of geocentric position to subroutine ls_geod_to_geoc.
*
* Revision 1.1 92/12/30 13:16:11 bjax
* Initial revision
*
----------------------------------------------------------------------------
REFERENCES:
[ 1] McFarland, Richard E.: "A Standard Kinematic Model
for Flight Simulation at NASA-Ames", NASA CR-2497,
January 1975
[ 2] ANSI/AIAA R-004-1992 "Recommended Practice: Atmos-
pheric and Space Flight Vehicle Coordinate Systems",
February 1992
----------------------------------------------------------------------------
CALLED BY:
----------------------------------------------------------------------------
CALLS TO: None.
----------------------------------------------------------------------------
INPUTS: State derivatives
----------------------------------------------------------------------------
OUTPUTS: States
--------------------------------------------------------------------------*/
#include "ls_types.h"
#include "ls_constants.h"
#include "ls_generic.h"
#include "ls_accel.h"
#include "ls_aux.h"
#include "ls_model.h"
#include "ls_step.h"
#include "ls_geodesy.h"
#include "ls_gravity.h"
/* #include "ls_sim_control.h" */
#include <math.h>
extern SCALAR Simtime; /* defined in ls_main.c */
void ls_step( SCALAR dt, int Initialize ) {
static int inited = 0;
SCALAR dth;
static SCALAR v_dot_north_past, v_dot_east_past, v_dot_down_past;
static SCALAR latitude_dot_past, longitude_dot_past, radius_dot_past;
static SCALAR p_dot_body_past, q_dot_body_past, r_dot_body_past;
SCALAR p_local_in_body, q_local_in_body, r_local_in_body;
SCALAR epsilon, inv_eps, local_gnd_veast;
SCALAR e_dot_0, e_dot_1, e_dot_2, e_dot_3;
static SCALAR e_0, e_1, e_2, e_3;
static SCALAR e_dot_0_past, e_dot_1_past, e_dot_2_past, e_dot_3_past;
SCALAR cos_Lat_geocentric, inv_Radius_to_vehicle;
/* I N I T I A L I Z A T I O N */
if ( (inited == 0) || (Initialize != 0) )
{
/* Set past values to zero */
v_dot_north_past = v_dot_east_past = v_dot_down_past = 0;
latitude_dot_past = longitude_dot_past = radius_dot_past = 0;
p_dot_body_past = q_dot_body_past = r_dot_body_past = 0;
e_dot_0_past = e_dot_1_past = e_dot_2_past = e_dot_3_past = 0;
/* Initialize geocentric position from geodetic latitude and altitude */
ls_geod_to_geoc( Latitude, Altitude, &Sea_level_radius, &Lat_geocentric);
Earth_position_angle = 0;
Lon_geocentric = Longitude;
Radius_to_vehicle = Altitude + Sea_level_radius;
/* Correct eastward velocity to account for earths' rotation, if necessary */
local_gnd_veast = OMEGA_EARTH*Sea_level_radius*cos(Lat_geocentric);
if( fabs(V_east - V_east_rel_ground) < 0.8*local_gnd_veast )
V_east = V_east + local_gnd_veast;
/* Initialize quaternions and transformation matrix from Euler angles */
e_0 = cos(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5)
+ sin(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5);
e_1 = cos(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5)
- sin(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5);
e_2 = cos(Psi*0.5)*sin(Theta*0.5)*cos(Phi*0.5)
+ sin(Psi*0.5)*cos(Theta*0.5)*sin(Phi*0.5);
e_3 =-cos(Psi*0.5)*sin(Theta*0.5)*sin(Phi*0.5)
+ sin(Psi*0.5)*cos(Theta*0.5)*cos(Phi*0.5);
T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
/* Calculate local gravitation acceleration */
ls_gravity( Radius_to_vehicle, Lat_geocentric, &Gravity );
/* Initialize vehicle model */
ls_aux();
ls_model(0.0, 0);
/* Calculate initial accelerations */
ls_accel();
/* Initialize auxiliary variables */
ls_aux();
Alpha_dot = 0.;
Beta_dot = 0.;
/* set flag; disable integrators */
inited = -1;
dt = 0;
}
/* Update time */
dth = 0.5*dt;
Simtime = Simtime + dt;
/* L I N E A R V E L O C I T I E S */
/* Integrate linear accelerations to get velocities */
/* Using predictive Adams-Bashford algorithm */
V_north = V_north + dth*(3*V_dot_north - v_dot_north_past);
V_east = V_east + dth*(3*V_dot_east - v_dot_east_past );
V_down = V_down + dth*(3*V_dot_down - v_dot_down_past );
/* record past states */
v_dot_north_past = V_dot_north;
v_dot_east_past = V_dot_east;
v_dot_down_past = V_dot_down;
/* Calculate trajectory rate (geocentric coordinates) */
inv_Radius_to_vehicle = 1.0/Radius_to_vehicle;
cos_Lat_geocentric = cos(Lat_geocentric);
if ( cos_Lat_geocentric != 0) {
Longitude_dot = V_east/(Radius_to_vehicle*cos_Lat_geocentric);
}
Latitude_dot = V_north*inv_Radius_to_vehicle;
Radius_dot = -V_down;
/* A N G U L A R V E L O C I T I E S A N D P O S I T I O N S */
/* Integrate rotational accelerations to get velocities */
P_body = P_body + dth*(3*P_dot_body - p_dot_body_past);
Q_body = Q_body + dth*(3*Q_dot_body - q_dot_body_past);
R_body = R_body + dth*(3*R_dot_body - r_dot_body_past);
/* Save past states */
p_dot_body_past = P_dot_body;
q_dot_body_past = Q_dot_body;
r_dot_body_past = R_dot_body;
/* Calculate local axis frame rates due to travel over curved earth */
P_local = V_east*inv_Radius_to_vehicle;
Q_local = -V_north*inv_Radius_to_vehicle;
R_local = -V_east*tan(Lat_geocentric)*inv_Radius_to_vehicle;
/* Transform local axis frame rates to body axis rates */
p_local_in_body = T_local_to_body_11*P_local + T_local_to_body_12*Q_local + T_local_to_body_13*R_local;
q_local_in_body = T_local_to_body_21*P_local + T_local_to_body_22*Q_local + T_local_to_body_23*R_local;
r_local_in_body = T_local_to_body_31*P_local + T_local_to_body_32*Q_local + T_local_to_body_33*R_local;
/* Calculate total angular rates in body axis */
P_total = P_body - p_local_in_body;
Q_total = Q_body - q_local_in_body;
R_total = R_body - r_local_in_body;
/* Transform to quaternion rates (see Appendix E in [2]) */
e_dot_0 = 0.5*( -P_total*e_1 - Q_total*e_2 - R_total*e_3 );
e_dot_1 = 0.5*( P_total*e_0 - Q_total*e_3 + R_total*e_2 );
e_dot_2 = 0.5*( P_total*e_3 + Q_total*e_0 - R_total*e_1 );
e_dot_3 = 0.5*( -P_total*e_2 + Q_total*e_1 + R_total*e_0 );
/* Integrate using trapezoidal as before */
e_0 = e_0 + dth*(e_dot_0 + e_dot_0_past);
e_1 = e_1 + dth*(e_dot_1 + e_dot_1_past);
e_2 = e_2 + dth*(e_dot_2 + e_dot_2_past);
e_3 = e_3 + dth*(e_dot_3 + e_dot_3_past);
/* calculate orthagonality correction - scale quaternion to unity length */
epsilon = sqrt(e_0*e_0 + e_1*e_1 + e_2*e_2 + e_3*e_3);
inv_eps = 1/epsilon;
e_0 = inv_eps*e_0;
e_1 = inv_eps*e_1;
e_2 = inv_eps*e_2;
e_3 = inv_eps*e_3;
/* Save past values */
e_dot_0_past = e_dot_0;
e_dot_1_past = e_dot_1;
e_dot_2_past = e_dot_2;
e_dot_3_past = e_dot_3;
/* Update local to body transformation matrix */
T_local_to_body_11 = e_0*e_0 + e_1*e_1 - e_2*e_2 - e_3*e_3;
T_local_to_body_12 = 2*(e_1*e_2 + e_0*e_3);
T_local_to_body_13 = 2*(e_1*e_3 - e_0*e_2);
T_local_to_body_21 = 2*(e_1*e_2 - e_0*e_3);
T_local_to_body_22 = e_0*e_0 - e_1*e_1 + e_2*e_2 - e_3*e_3;
T_local_to_body_23 = 2*(e_2*e_3 + e_0*e_1);
T_local_to_body_31 = 2*(e_1*e_3 + e_0*e_2);
T_local_to_body_32 = 2*(e_2*e_3 - e_0*e_1);
T_local_to_body_33 = e_0*e_0 - e_1*e_1 - e_2*e_2 + e_3*e_3;
/* Calculate Euler angles */
Theta = asin( -T_local_to_body_13 );
if( T_local_to_body_11 == 0 )
Psi = 0;
else
Psi = atan2( T_local_to_body_12, T_local_to_body_11 );
if( T_local_to_body_33 == 0 )
Phi = 0;
else
Phi = atan2( T_local_to_body_23, T_local_to_body_33 );
/* Resolve Psi to 0 - 359.9999 */
if (Psi < 0 ) Psi = Psi + 2*PI;
/* L I N E A R P O S I T I O N S */
/* Trapezoidal acceleration for position */
Lat_geocentric = Lat_geocentric + dth*(Latitude_dot + latitude_dot_past );
Lon_geocentric = Lon_geocentric + dth*(Longitude_dot + longitude_dot_past);
Radius_to_vehicle = Radius_to_vehicle + dth*(Radius_dot + radius_dot_past );
Earth_position_angle = Earth_position_angle + dt*OMEGA_EARTH;
/* Save past values */
latitude_dot_past = Latitude_dot;
longitude_dot_past = Longitude_dot;
radius_dot_past = Radius_dot;
/* end of ls_step */
}
/*************************************************************************/