Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 "std.h"
28#include "math/pprz_geodetic.h"
31#include "math/pprz_algebra.h"
33#include "math/pprz_isa.h"
34
35#include "generated/airframe.h"
36#include "generated/flight_plan.h"
37#include "generated/modules.h"
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#if (defined COMMAND_STEERING) && (defined COMMAND_THROTTLE) // STEERING ROVER PHYSICS
46
47// Friction coef, proportional to speed
48#ifndef NPS_ROVER_FRICTION
49#define NPS_ROVER_FRICTION 0.01f
50#endif
51
53
54// Maximum acceleration (m/s²)
55#ifndef NPS_ROVER_ACCELERATION
56#define NPS_ROVER_ACCELERATION 1.f
57#endif
58
60static float mu = NPS_ROVER_FRICTION;
61
62#endif
63
65
66#if (defined COMMAND_TURN) && (defined COMMAND_SPEED) // 2 wheels rover dynamic
67
68// Maximum speed (m/s)
69#ifndef NPS_ROVER_MAX_SPEED
70#define NPS_ROVER_MAX_SPEED 2.f
71#endif
72
73// Maximum turn rate (rad/s)
74#ifndef NPS_ROVER_TURN_RATE
75#define NPS_ROVER_TURN_RATE 3.f
76#endif
77
78// Accel 1st order time constant
79#ifndef NPS_ROVER_ACCEL_TAU
80#define NPS_ROVER_ACCEL_TAU 0.2f
81#endif
82
83// Turn rate 1st order time constant
84#ifndef NPS_ROVER_TURN_TAU
85#define NPS_ROVER_TURN_TAU 0.1f
86#endif
87
89static struct FirstOrderLowPass speed_filter;
90static struct FirstOrderLowPass turn_filter;
91
92#endif // 2 wheels
93
94// NpsFdm structure
95struct NpsFdm fdm;
96
97// Reference point
98static struct LtpDef_d ltpdef;
99
100// Static functions declaration
101static void init_ltp(void);
102
104static struct NedCoor_d rover_pos;
105static struct NedCoor_d rover_vel;
106static struct NedCoor_d rover_acc;
107
108
110void nps_fdm_init(double dt)
111{
112 fdm.init_dt = dt; // (1 / simulation freq)
113 fdm.curr_dt = dt;
114 fdm.time = 0;
115
116 fdm.on_ground = TRUE;
117 fdm.nan_count = 0;
118 fdm.pressure = -1;
119 fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE;
120 fdm.total_pressure = -1;
121 fdm.dynamic_pressure = -1;
122 fdm.temperature = -1;
123 fdm.ltpprz_to_body_eulers.psi = 0.0;
124 init_ltp();
125
126#if (defined COMMAND_TURN) && (defined COMMAND_SPEED) // 2 wheels rover dynamic
127 init_first_order_low_pass(&speed_filter, NPS_ROVER_ACCEL_TAU, fdm.curr_dt, 0.f);
128 init_first_order_low_pass(&turn_filter, NPS_ROVER_TURN_TAU, fdm.curr_dt, 0.f);
129#endif
130}
131
133{
134 // nothing to do
135}
136
137void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb __attribute__((unused)))
138{
139 fdm.time += fdm.curr_dt;
140
141 /****************************************************************************
142 PHYSICAL MODEL
143 -------------
144 The physical model of your rover goes here. This physics takes place in
145 the LTP plane (so we transfer every integration step to NED and ECEF at the end).
146 */
147
148 // From previous step...
150 ned_of_ecef_vect_d(&rover_vel, &ltpdef, &fdm.ecef_ecef_vel);
151 double phi = fdm.ltpprz_to_body_eulers.psi;
152
153 #if (defined COMMAND_STEERING) && (defined COMMAND_THROTTLE) // STEERING ROVER PHYSICS
154
155 // Steering rover cmds:
156 // COMMAND_STEERING -> delta parameter
157 // COMMAND_THROTTLE -> acceleration in heading direction
158
160 double delta = RadOfDeg(commands[COMMAND_STEERING] * MAX_DELTA);
161 double speed = FLOAT_VECT2_NORM(rover_vel);
162 double phi_d = tan(delta) / DRIVE_SHAFT_DISTANCE * speed;
163 double accel = NPS_ROVER_ACCELERATION * commands[COMMAND_THROTTLE];
164
165 // NED accel = R(psi) [accel; |V|*phi_d] - mu*V
166 rover_acc.x = accel * cos(phi) - speed * phi_d * sin(phi) - mu * rover_vel.x;
167 rover_acc.y = accel * sin(phi) + speed * phi_d * cos(phi) - mu * rover_vel.y;
168 // Velocities (EULER INTEGRATION)
169 rover_vel.x += rover_acc.x * fdm.curr_dt;
170 rover_vel.y += rover_acc.y * fdm.curr_dt;
171
172 #elif (defined COMMAND_TURN) && (defined COMMAND_SPEED) // 2 wheels rover dynamic
173
174 double phi_sp = NPS_ROVER_TURN_RATE * commands[COMMAND_TURN];
175 double phi_d = update_first_order_low_pass(&turn_filter, phi_sp);
176 double speed_sp = NPS_ROVER_MAX_SPEED * commands[COMMAND_SPEED];
177 double speed = update_first_order_low_pass(&speed_filter, speed_sp);
178 double vx = speed * cos(phi);
179 double vy = speed * sin(phi);
180 rover_acc.x = (vx - rover_vel.x) / fdm.curr_dt;
181 rover_acc.y = (vy - rover_vel.y) / fdm.curr_dt;
182 rover_vel.x = vx;
183 rover_vel.y = vy;
184
185 #else
186 #warning "The physics of this rover are not yet implemented in nps_fdm_rover!!"
187 #endif // STEERING ROVER PHYSICS
188
189 // Positions
190 rover_pos.x += rover_vel.x * fdm.curr_dt;
191 rover_pos.y += rover_vel.y * fdm.curr_dt;
192 // phi have to be contained in [-180º,180º). So:
193 phi += phi_d * fdm.curr_dt;
194 NormRadAngle(phi);
195 /****************************************************************************/
196 // Coordenates transformations |
197 // ----------------------------|
198
199 /* in ECEF */
201 ecef_of_ned_vect_d(&fdm.ecef_ecef_vel, &ltpdef, &rover_vel);
202 ecef_of_ned_vect_d(&fdm.ecef_ecef_accel, &ltpdef, &rover_acc);
203
204 /* in NED */
205 ned_of_ecef_point_d(&fdm.ltpprz_pos, &ltpdef, &fdm.ecef_pos);
206 ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, &ltpdef, &fdm.ecef_ecef_vel);
207 ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, &ltpdef, &fdm.ecef_ecef_accel);
208
209 /* Height above sea level */
210 fdm.hmsl = ltpdef.hmsl - rover_pos.z;
211
212 /* Eulers */
213 fdm.ltpprz_to_body_eulers.psi = phi;
214 fdm.ltp_to_body_eulers.psi = phi;
215
216 // Exporting Eulers to AHRS (in quaternions)
217 double_quat_of_eulers(&fdm.ltp_to_body_quat, &fdm.ltp_to_body_eulers);
218
219 // Angular vel & acc
220 fdm.body_ecef_rotaccel.r = (phi_d - fdm.body_ecef_rotvel.r) / fdm.curr_dt;
221 fdm.body_ecef_rotvel.r = phi_d;
222
223}
224
225
226/**************************
227 ** Generating LTP plane **
228 **************************/
229
230static void init_ltp(void)
231{
232 struct LlaCoor_d llh_nav0;
233 llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
234 llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
235 llh_nav0.alt = (double)(NAV_ALT0 + NAV_MSL0) / 1000.; /* Height above the ellipsoid */
236
237 struct EcefCoor_d ecef_nav0;
238 ecef_of_lla_d(&ecef_nav0, &llh_nav0);
239
240 ltp_def_from_ecef_d(&ltpdef, &ecef_nav0);
241 ltpdef.hmsl = (double)NAV_ALT0 / 1000.; /* Height above the geoid */
242
243 fdm.ecef_pos = ecef_nav0;
244 fdm.hmsl = ltpdef.hmsl;
245 fdm.ltp_g.x = 0.;
246 fdm.ltp_g.y = 0.;
247 fdm.ltp_g.z = 0.; // accel data are already with the correct format
248
249
250#ifdef AHRS_H_X
251 fdm.ltp_h.x = AHRS_H_X;
252 fdm.ltp_h.y = AHRS_H_Y;
253 fdm.ltp_h.z = AHRS_H_Z;
254 PRINT_CONFIG_MSG("Using magnetic field as defined in airframe file.")
255#else
256 fdm.ltp_h.x = 1.;
257 fdm.ltp_h.y = 0.;
258 fdm.ltp_h.z = 0.;
259#endif
260
261}
262
263
264/*****************************************************/
265// Atmosphere function (we don't need that features) //
266void nps_fdm_set_wind(double speed __attribute__((unused)),
267 double dir __attribute__((unused)))
268{
269}
270
271void nps_fdm_set_wind_ned(double wind_north __attribute__((unused)),
272 double wind_east __attribute__((unused)),
273 double wind_down __attribute__((unused)))
274{
275}
276
277void nps_fdm_set_turbulence(double wind_speed __attribute__((unused)),
278 int turbulence_severity __attribute__((unused)))
279{
280}
281
282void nps_fdm_set_temperature(double temp __attribute__((unused)),
283 double h __attribute__((unused)))
284{
285}
286
static void h(const real32_T x[7], const real32_T q[4], real32_T y[6])
void double_quat_of_eulers(struct DoubleQuat *q, struct DoubleEulers *e)
#define FLOAT_VECT2_NORM(_v)
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
double lon
in radians
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_ned_vect_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ned_of_ecef_point_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ecef_of_ned_point_d(struct EcefCoor_d *ecef, struct LtpDef_d *def, struct NedCoor_d *ned)
void ecef_of_lla_d(struct EcefCoor_d *ecef, struct LlaCoor_d *lla)
vector in EarthCenteredEarthFixed coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates Units: meters
#define PPRZ_ISA_SEA_LEVEL_PRESSURE
ISA sea level standard atmospheric pressure in Pascal.
Definition pprz_isa.h:50
struct FloatVect3 speed_sp
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
Simple first order low pass filter with bilinear transform.
static void init_first_order_low_pass(struct FirstOrderLowPass *filter, float tau, const float sample_time, float value)
Init first order low pass filter.
static float update_first_order_low_pass(struct FirstOrderLowPass *filter, const float value)
Update first order low pass filter state with a new value.
First order low pass filter structure.
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
static struct LtpDef_d ltpdef
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
void nps_fdm_init(double dt)
NPS FDM rover init.
static void init_ltp(void)
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...
static struct NedCoor_d rover_pos
Physical model structures.
void nps_fdm_set_temperature(double temp, double h)
Set temperature in degrees Celcius at given height h above MSL.
static struct NedCoor_d rover_vel
static struct NedCoor_d rover_acc
void nps_fdm_set_turbulence(double wind_speed, int turbulence_severity)
void nps_fdm_set_wind(double speed, double dir)
void nps_fdm_run_init_step(void)
Paparazzi generic algebra macros.
Paparazzi floating point algebra.
Paparazzi generic macros for geodetic calculations.
Paparazzi double-precision floating point math for geodetic calculations.
Paparazzi floating point math for geodetic calculations.
Paparazzi atmospheric pressure conversion utilities.
#define MAX_DELTA
Generated airframe.h from airframe.xml.
#define DRIVE_SHAFT_DISTANCE
static const ShellCommand commands[]
Definition shell_arch.c:77
static const float dir[]
API to get/set the generic vehicle states.
#define TRUE
Definition std.h:4