Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
nps_fdm_rover.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2021 Jesús Bautista <jesusbautistavillar@gmail.com>
3  * Hector García <noeth3r@gmail.com>
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, see
19  * <http://www.gnu.org/licenses/>.
20  */
21 
22 #include "nps_fdm.h"
23 
24 #include <stdlib.h>
25 #include <stdio.h>
26 
27 #include "math/pprz_geodetic.h"
30 #include "math/pprz_algebra.h"
32 #include "math/pprz_isa.h"
33 
34 #include "generated/airframe.h"
35 #include "generated/flight_plan.h"
36 
38 #include "state.h"
39 
40 // Check if rover firmware
41 #ifndef ROVER_FIRMWARE
42 #error "The module nps_fdm_rover is designed for rovers and doesn't support other firmwares!!"
43 #endif
44 
45 // NpsFdm structure
46 struct NpsFdm fdm;
47 
48 // Reference point
49 static struct LtpDef_d ltpdef;
50 
51 // Static functions declaration
52 static void init_ltp(void);
53 
55 static struct EnuCoor_d rover_pos;
56 static struct EnuCoor_d rover_vel;
57 static struct EnuCoor_d rover_acc;
58 
60 static float mu = 0.01;
61 
62 
64 void nps_fdm_init(double dt)
65 {
66  fdm.init_dt = dt; // (1 / simulation freq)
67  fdm.curr_dt = 0.001; // ¿Configurable from GCS?
68  fdm.time = dt;
69 
70  fdm.on_ground = TRUE;
71 
72  fdm.nan_count = 0;
73  fdm.pressure = -1;
75  fdm.total_pressure = -1;
76  fdm.dynamic_pressure = -1;
77  fdm.temperature = -1;
78 
80  init_ltp();
81 }
82 
83 void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb __attribute__((unused)))
84 {
85 
86  /****************************************************************************
87  PHYSICAL MODEL
88  -------------
89  The physical model of your rover goes here. This physics takes place in
90  the LTP plane (so we transfer every integration step to NED and ECEF at the end).
91  */
92 
93  #ifdef COMMAND_STEERING // STEERING ROVER PHYSICS
94 
95  // Steering rover cmds:
96  // COMMAND_STEERING -> delta parameter
97  // COMMAND_TRHOTTLE -> acceleration in heading direction
98 
99  double delta = RadOfDeg(commands[COMMAND_STEERING] * MAX_DELTA);
100 
102  // From previous step...
105  double speed = FLOAT_VECT2_NORM(rover_vel);
106  double phi = fdm.ltpprz_to_body_eulers.psi;
107  double phi_d = tan(delta) / DRIVE_SHAFT_DISTANCE * speed;
108 
109  // Setting accelerations
110  rover_acc.x = commands[COMMAND_THROTTLE] * cos(phi) - speed * (sin(phi) * phi_d + cos(phi) * mu);
111  rover_acc.y = commands[COMMAND_THROTTLE] * sin(phi) + speed * (cos(phi) * phi_d - sin(phi) * mu);
112  double phi_dd = tan(delta) / DRIVE_SHAFT_DISTANCE * commands[COMMAND_THROTTLE];
113 
114  // Velocities (EULER INTEGRATION)
117  phi_d = tan(delta) / DRIVE_SHAFT_DISTANCE * speed;
118 
119  // Positions
122  phi += phi_d * fdm.curr_dt;
123 
124  // phi have to be contained in [-180º,180º). So:
125  phi = (phi > M_PI)? - 2*M_PI + phi : (phi < -M_PI)? 2*M_PI + phi : phi;
126 
127  #else
128  #warning "The physics of this rover are not yet implemented in nps_fdm_rover!!"
129  #endif // STEERING ROVER PHYSICS
130 
131 
132  /****************************************************************************/
133  // Coordenates transformations |
134  // ----------------------------|
135 
136  /* in ECEF */
140 
141  /* in NED */
145 
146  /* Eulers */
148 
149  // ENU to NED and exporting heading (psi euler angle)
150  fdm.ltp_to_body_eulers.psi = - phi + M_PI / 2;
151 
152  // Exporting Eulers to AHRS (in quaternions)
154 
155  // Angular vel & acc
156  fdm.body_ecef_rotvel.r = phi_d;
157  fdm.body_ecef_rotaccel.r = phi_dd;
158 
159 }
160 
161 
162 /**************************
163  ** Generating LTP plane **
164  **************************/
165 
166 static void init_ltp(void)
167 {
168 
169  struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
170  llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
171  llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
172 
173  struct EcefCoor_d ecef_nav0;
174 
175  ecef_of_lla_d(&ecef_nav0, &llh_nav0);
176 
177  ltp_def_from_ecef_d(&ltpdef, &ecef_nav0);
178  fdm.ecef_pos = ecef_nav0;
179 
180  fdm.ltp_g.x = 0.;
181  fdm.ltp_g.y = 0.;
182  fdm.ltp_g.z = 0.; // accel data are already with the correct format
183 
184 
185 #ifdef AHRS_H_X
186  fdm.ltp_h.x = AHRS_H_X;
187  fdm.ltp_h.y = AHRS_H_Y;
188  fdm.ltp_h.z = AHRS_H_Z;
189  PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file.")
190 #else
191  fdm.ltp_h.x = 0.4912;
192  fdm.ltp_h.y = 0.1225;
193  fdm.ltp_h.z = 0.8624;
194 #endif
195 
196 }
197 
198 
199 /*****************************************************/
200 // Atmosphere function (we don't need that features) //
201 void nps_fdm_set_wind(double speed __attribute__((unused)),
202  double dir __attribute__((unused)))
203 {
204 }
205 
206 void nps_fdm_set_wind_ned(double wind_north __attribute__((unused)),
207  double wind_east __attribute__((unused)),
208  double wind_down __attribute__((unused)))
209 {
210 }
211 
212 void nps_fdm_set_turbulence(double wind_speed __attribute__((unused)),
213  int turbulence_severity __attribute__((unused)))
214 {
215 }
216 
217 void nps_fdm_set_temperature(double temp __attribute__((unused)),
218  double h __attribute__((unused)))
219 {
220 }
221 
fdm
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
Definition: nps_fdm_rover.c:46
mu
static float mu
Physical model parameters.
Definition: nps_fdm_rover.c:60
init_ltp
static void init_ltp(void)
Definition: nps_fdm_rover.c:166
NpsFdm::ltp_h
struct DoubleVect3 ltp_h
Definition: nps_fdm.h:105
ltp_def_from_ecef_d
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:34
nps_fdm_set_wind_ned
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
Definition: nps_fdm_rover.c:206
nps_fdm_set_wind
void nps_fdm_set_wind(double speed, double dir)
Definition: nps_fdm_rover.c:201
EnuCoor_d
vector in East North Up coordinates Units: meters
Definition: pprz_geodetic_double.h:76
pprz_geodetic.h
Paparazzi generic macros for geodetic calculations.
DoubleVect3::z
double z
Definition: pprz_algebra_double.h:49
LlaCoor_d
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_double.h:58
NpsFdm
Definition: nps_fdm.h:44
double_quat_of_eulers
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
Definition: pprz_algebra_double.c:49
h
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
Definition: UKF_Wind_Estimator.c:821
EnuCoor_d::x
double x
in meters
Definition: pprz_geodetic_double.h:77
ecef_of_lla_d
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
Definition: pprz_geodetic_double.c:120
NpsFdm::ltpprz_ecef_vel
struct NedCoor_d ltpprz_ecef_vel
velocity in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:79
rover_acc
static struct EnuCoor_d rover_acc
Definition: nps_fdm_rover.c:57
nps_fdm_run_step
void nps_fdm_run_step(bool launch, double *commands, int commands_nb)
Update the simulation state.
Definition: nps_fdm_rover.c:83
ecef_of_enu_point_d
void ecef_of_enu_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
Definition: pprz_geodetic_double.c:168
DoubleRates::r
double r
in rad/s^2
Definition: pprz_algebra_double.h:88
DoubleEulers::psi
double psi
in radians
Definition: pprz_algebra_double.h:79
NpsFdm::on_ground
bool on_ground
Definition: nps_fdm.h:49
commands
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
NpsFdm::ltpprz_ecef_accel
struct NedCoor_d ltpprz_ecef_accel
accel in ltppprz frame, wrt ECEF frame
Definition: nps_fdm.h:81
nps_fdm.h
pprz_algebra_float.h
Paparazzi floating point algebra.
NpsFdm::ltp_to_body_quat
struct DoubleQuat ltp_to_body_quat
Definition: nps_fdm.h:91
NpsFdm::ltpprz_to_body_eulers
struct DoubleEulers ltpprz_to_body_eulers
Definition: nps_fdm.h:94
nps_fdm_init
void nps_fdm_init(double dt)
NPS FDM rover init.
Definition: nps_fdm_rover.c:64
NpsFdm::body_ecef_rotvel
struct DoubleRates body_ecef_rotvel
Definition: nps_fdm.h:97
NpsFdm::ecef_ecef_vel
struct EcefCoor_d ecef_ecef_vel
velocity in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:64
pprz_geodetic_float.h
Paparazzi floating point math for geodetic calculations.
nps_fdm_set_temperature
void nps_fdm_set_temperature(double temp, double h)
Set temperature in degrees Celcius at given height h above MSL.
Definition: nps_fdm_rover.c:217
NpsFdm::time
double time
Definition: nps_fdm.h:46
DoubleVect3::x
double x
Definition: pprz_algebra_double.h:47
NpsFdm::ltp_g
struct DoubleVect3 ltp_g
Definition: nps_fdm.h:104
dir
static const float dir[]
Definition: shift_tracking.c:91
NpsFdm::total_pressure
double total_pressure
total atmospheric pressure in Pascal
Definition: nps_fdm.h:111
FLOAT_VECT2_NORM
#define FLOAT_VECT2_NORM(_v)
Definition: pprz_algebra_float.h:132
rover_pos
static struct EnuCoor_d rover_pos
Physical model structures.
Definition: nps_fdm_rover.c:55
NpsFdm::body_ecef_rotaccel
struct DoubleRates body_ecef_rotaccel
Definition: nps_fdm.h:98
LlaCoor_d::lat
double lat
in radians
Definition: pprz_geodetic_double.h:59
pprz_geodetic_double.h
Paparazzi double-precision floating point math for geodetic calculations.
LtpDef_d
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_double.h:97
NpsFdm::ecef_ecef_accel
struct EcefCoor_d ecef_ecef_accel
acceleration in ECEF frame, wrt ECEF frame
Definition: nps_fdm.h:66
rover_vel
static struct EnuCoor_d rover_vel
Definition: nps_fdm_rover.c:56
PRINT_CONFIG_MSG
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
rover_guidance_steering.h
ltpdef
static struct LtpDef_d ltpdef
Definition: nps_fdm_rover.c:49
NpsFdm::ltp_to_body_eulers
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
NpsFdm::init_dt
double init_dt
Definition: nps_fdm.h:47
NpsFdm::ltpprz_pos
struct NedCoor_d ltpprz_pos
Definition: nps_fdm.h:54
ned_of_ecef_point_d
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:147
EcefCoor_d
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_double.h:49
NpsFdm::ecef_pos
struct EcefCoor_d ecef_pos
Definition: nps_fdm.h:53
EnuCoor_d::y
double y
in meters
Definition: pprz_geodetic_double.h:78
NpsFdm::temperature
double temperature
current temperature in degrees Celcius
Definition: nps_fdm.h:113
DRIVE_SHAFT_DISTANCE
#define DRIVE_SHAFT_DISTANCE
Definition: rover_guidance_steering.h:87
ned_of_ecef_vect_d
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:159
enu_of_ecef_vect_d
void enu_of_ecef_vect_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:154
NpsFdm::pressure
double pressure
current (static) atmospheric pressure in Pascal
Definition: nps_fdm.h:110
nps_fdm_set_turbulence
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
Definition: nps_fdm_rover.c:212
NpsFdm::dynamic_pressure
double dynamic_pressure
dynamic pressure in Pascal
Definition: nps_fdm.h:112
pprz_algebra.h
Paparazzi generic algebra macros.
DoubleVect3::y
double y
Definition: pprz_algebra_double.h:48
state.h
LlaCoor_d::lon
double lon
in radians
Definition: pprz_geodetic_double.h:60
TRUE
#define TRUE
Definition: std.h:4
NpsFdm::curr_dt
double curr_dt
Definition: nps_fdm.h:48
NpsFdm::nan_count
int nan_count
Definition: nps_fdm.h:50
enu_of_ecef_point_d
void enu_of_ecef_point_d(struct EnuCoor_d *enu, struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:140
MAX_DELTA
#define MAX_DELTA
Generated airframe.h from airframe.xml.
Definition: rover_guidance_steering.h:60
PPRZ_ISA_SEA_LEVEL_PRESSURE
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition: pprz_isa.h:50
NpsFdm::pressure_sl
double pressure_sl
pressure at sea level in Pascal
Definition: nps_fdm.h:114
pprz_isa.h
Paparazzi atmospheric pressure conversion utilities.
ecef_of_enu_vect_d
void ecef_of_enu_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct EnuCoor_d *enu)
Definition: pprz_geodetic_double.c:181