Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
nps_fdm.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009 Antoine Drouin <poinix@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
22 #ifndef NPS_FDM
23 #define NPS_FDM
24 
25 #ifdef __cplusplus
26 extern "C" {
27 #endif
28 
29 #include "std.h"
30 #include "flight_gear.h"
33 
34 /*
35  Notations for fdm variables
36  ---------------------------
37  coordinate system [ frame ] name
38 
39  ecef_inertial_vel is the time derivative of position
40  with respect to inertial frame expressed in ECEF ( Earth Centered Earth Fixed)
41  coordinate system.
42 */
43 
44 struct NpsFdm {
45 
46  double time;
47  double init_dt;
48  double curr_dt;
49  bool on_ground;
50  int nan_count;
51 
52  /* position */
53  struct EcefCoor_d ecef_pos;
54  struct NedCoor_d ltpprz_pos;
55  struct LlaCoor_d lla_pos;
56  double hmsl;
57  // for debugging
58  struct LlaCoor_d lla_pos_pprz; //lla converted by pprz from ecef
59  struct LlaCoor_d lla_pos_geod; //geodetic lla from jsbsim
60  struct LlaCoor_d lla_pos_geoc; //geocentric lla from jsbsim
61  double agl; //AGL from jsbsim in m
62 
67 
69  struct DoubleVect3 body_ecef_vel; /* aka UVW */
72 
74  struct NedCoor_d ltp_ecef_vel;
77 
82 
85 
87  struct DoubleVect3 body_accel;
88 
89  /* attitude */
95 
96  /* angular velocity and acceleration in body frame, wrt ECEF frame */
99 
100  /* angular velocity and acceleration in body frame, wrt inertial ECI frame */
103 
104  struct DoubleVect3 ltp_g;
105  struct DoubleVect3 ltp_h;
106 
107  struct DoubleVect3 wind;
108 
109  double airspeed;
110  double pressure;
111  double total_pressure;
113  double temperature;
114  double pressure_sl;
115  double aoa;
116  double sideslip;
117 
118  // Control surface positions (normalized values)
119  float elevator;
120  float flap;
123  float rudder;
124 
125  //engine state for first engine
127  uint32_t eng_state[FG_NET_FDM_MAX_ENGINES];// Engine state (off, cranking, running)
128  float rpm[FG_NET_FDM_MAX_ENGINES]; // Engine RPM rev/min
129 
130 };
131 
132 extern struct NpsFdm fdm;
133 
134 extern void nps_fdm_init(double dt);
135 extern void nps_fdm_run_step(bool launch, double *commands, int commands_nb);
136 extern void nps_fdm_set_wind(double speed, double dir);
137 extern void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down);
138 extern void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity);
140 extern void nps_fdm_set_temperature(double temp, double h);
141 
142 #ifdef __cplusplus
143 } /* extern "C" */
144 #endif
145 
146 #endif /* NPS_FDM */
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
#define FG_NET_FDM_MAX_ENGINES
Definition: flight_gear.h:113
euler angles
Roation quaternion.
angular rates
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
vector in North East Down coordinates Units: meters
float rudder
Definition: nps_fdm.h:123
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
double time
Definition: nps_fdm.h:46
double aoa
angle of attack in rad
Definition: nps_fdm.h:115
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
void nps_fdm_init(double dt)
NPS FDM rover init.
struct DoubleVect3 ltp_g
Definition: nps_fdm.h:104
double pressure_sl
pressure at sea level in Pascal
Definition: nps_fdm.h:114
float elevator
Definition: nps_fdm.h:119
struct DoubleQuat ecef_to_body_quat
Definition: nps_fdm.h:90
struct DoubleQuat ltpprz_to_body_quat
Definition: nps_fdm.h:93
double init_dt
Definition: nps_fdm.h:47
double total_pressure
total atmospheric pressure in Pascal
Definition: nps_fdm.h:111
double hmsl
Definition: nps_fdm.h:56
struct EcefCoor_d ecef_pos
Definition: nps_fdm.h:53
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
struct LlaCoor_d lla_pos_pprz
Definition: nps_fdm.h:58
struct DoubleVect3 body_inertial_accel
acceleration in body frame, wrt ECI inertial frame
Definition: nps_fdm.h:84
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Minimum complexity flight dynamic model In legacy Paparazzi simulator, was implemented in OCaml and c...
struct DoubleRates body_inertial_rotvel
Definition: nps_fdm.h:101
double airspeed
equivalent airspeed in m/s
Definition: nps_fdm.h:109
struct NedCoor_d ltp_ecef_vel
velocity in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:74
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:81
struct DoubleVect3 body_ecef_vel
velocity in body frame, wrt ECEF frame
Definition: nps_fdm.h:69
double agl
Definition: nps_fdm.h:61
void nps_fdm_set_temperature(double temp, double h)
Set temperature in degrees Celcius at given height h above MSL.
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105
double dynamic_pressure
dynamic pressure in Pascal
Definition: nps_fdm.h:112
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
struct EcefCoor_d ecef_ecef_accel
acceleration in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:66
double pressure
current (static) atmospheric pressure in Pascal
Definition: nps_fdm.h:110
double curr_dt
Definition: nps_fdm.h:48
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
struct EcefCoor_d ecef_ecef_vel
velocity in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:64
struct DoubleVect3 body_ecef_accel
acceleration in body frame, wrt ECEF frame
Definition: nps_fdm.h:71
struct DoubleRates body_inertial_rotaccel
Definition: nps_fdm.h:102
double temperature
current temperature in degrees Celcius
Definition: nps_fdm.h:113
struct DoubleRates body_ecef_rotaccel
Definition: nps_fdm.h:98
float left_aileron
Definition: nps_fdm.h:121
float right_aileron
Definition: nps_fdm.h:122
struct DoubleEulers ltpprz_to_body_eulers
Definition: nps_fdm.h:94
float rpm[FG_NET_FDM_MAX_ENGINES]
Definition: nps_fdm.h:128
uint32_t eng_state[FG_NET_FDM_MAX_ENGINES]
Definition: nps_fdm.h:127
struct LlaCoor_d lla_pos
Definition: nps_fdm.h:55
double sideslip
sideslip angle in rad
Definition: nps_fdm.h:116
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
struct DoubleVect3 body_accel
acceleration in body frame as measured by an accelerometer (incl.
Definition: nps_fdm.h:87
struct LlaCoor_d lla_pos_geoc
Definition: nps_fdm.h:60
struct DoubleVect3 wind
velocity in m/s in NED
Definition: nps_fdm.h:107
void nps_fdm_set_wind(double speed, double dir)
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
bool on_ground
Definition: nps_fdm.h:49
int nan_count
Definition: nps_fdm.h:50
float flap
Definition: nps_fdm.h:120
uint32_t num_engines
Definition: nps_fdm.h:126
struct LlaCoor_d lla_pos_geod
Definition: nps_fdm.h:59
struct NedCoor_d ltp_ecef_accel
acceleration in LTP frame, wrt ECEF frame
Definition: nps_fdm.h:76
Definition: nps_fdm.h:44
Paparazzi double precision floating point algebra.
Paparazzi double-precision floating point math for geodetic calculations.
static const float dir[]
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78