FDM/slew.c

   1 /**************************************************************************
   2  * slew.c -- the "slew" flight model
   3  *
   4  * Written by Curtis Olson, started May 1997.
   5  *
   6  * $Id$
   7  * (Log is kept at end of this file)
   8  **************************************************************************/
   9
  10
  11 #include <math.h>
  12
  13 #include "slew.h"
  14 #include "flight.h"
  15 #include "../aircraft/aircraft.h"
  16 #include "../controls/controls.h"
  17
  18
  19 #ifndef PI2
  20 #define PI2  (M_PI + M_PI)
  21 #endif
  22
  23
  24 /* reset flight params to a specific position */
  25 void slew_init(double pos_x, double pos_y, double pos_z, double heading) {
  26     struct flight_params *f;
  27
  28     f = &current_aircraft.flight;
  29
  30     f->pos_x = pos_x;
  31     f->pos_y = pos_y;
  32     f->pos_z = pos_z;
  33
  34     f->vel_x = 0.0;
  35     f->vel_y = 0.0;
  36     f->vel_z = 0.0;
  37
  38     f->Phi = 0.0;
  39     f->Theta = 0.0;
  40     f->Psi = 0.0;
  41
  42     f->vel_Phi = 0.0;
  43     f->vel_Theta = 0.0;
  44     f->vel_Psi = 0.0;
  45
  46     f->Psi = heading;
  47 }
  48
  49
  50 /* update position based on inputs, positions, velocities, etc. */
  51 void slew_update() {
  52     struct flight_params *f;
  53     struct control_params *c;
  54
  55     f = &current_aircraft.flight;
  56     c = &current_aircraft.controls;
  57
  58     f->Psi += c->aileron;
  59     if ( f->Psi > PI2 ) {
  60         f->Psi -= PI2;
  61     } else if ( f->Psi < 0 ) {
  62         f->Psi += PI2;
  63     }
  64
  65     f->vel_x = -c->elev;
  66
  67     f->pos_x = f->pos_x + (cos(f->Psi) * f->vel_x);
  68     f->pos_y = f->pos_y + (sin(f->Psi) * f->vel_x);
  69 }
  70
  71
  72 /* $Log$
  73 /* Revision 1.1  1997/05/16 16:04:45  curt
  74 /* Initial revision.
  75 /*
  76  */