35#include "generated/airframe.h"
36#include "generated/flight_plan.h"
37#include "generated/modules.h"
42#error "The module nps_fdm_rover is designed for rovers and doesn't support other firmwares!!"
45#if (defined COMMAND_STEERING) && (defined COMMAND_THROTTLE)
48#ifndef NPS_ROVER_FRICTION
49#define NPS_ROVER_FRICTION 0.01f
55#ifndef NPS_ROVER_ACCELERATION
56#define NPS_ROVER_ACCELERATION 1.f
60static float mu = NPS_ROVER_FRICTION;
66#if (defined COMMAND_TURN) && (defined COMMAND_SPEED)
69#ifndef NPS_ROVER_MAX_SPEED
70#define NPS_ROVER_MAX_SPEED 2.f
74#ifndef NPS_ROVER_TURN_RATE
75#define NPS_ROVER_TURN_RATE 3.f
79#ifndef NPS_ROVER_ACCEL_TAU
80#define NPS_ROVER_ACCEL_TAU 0.2f
84#ifndef NPS_ROVER_TURN_TAU
85#define NPS_ROVER_TURN_TAU 0.1f
120 fdm.total_pressure = -1;
121 fdm.dynamic_pressure = -1;
122 fdm.temperature = -1;
123 fdm.ltpprz_to_body_eulers.psi = 0.0;
126#if (defined COMMAND_TURN) && (defined COMMAND_SPEED)
151 double phi =
fdm.ltpprz_to_body_eulers.psi;
153 #if (defined COMMAND_STEERING) && (defined COMMAND_THROTTLE)
163 double accel = NPS_ROVER_ACCELERATION *
commands[COMMAND_THROTTLE];
172 #elif (defined COMMAND_TURN) && (defined COMMAND_SPEED)
174 double phi_sp = NPS_ROVER_TURN_RATE *
commands[COMMAND_TURN];
178 double vx = speed * cos(phi);
179 double vy = speed * sin(phi);
186 #warning "The physics of this rover are not yet implemented in nps_fdm_rover!!"
193 phi += phi_d *
fdm.curr_dt;
213 fdm.ltpprz_to_body_eulers.psi = phi;
214 fdm.ltp_to_body_eulers.psi = phi;
220 fdm.body_ecef_rotaccel.r = (phi_d -
fdm.body_ecef_rotvel.r) /
fdm.curr_dt;
221 fdm.body_ecef_rotvel.r = phi_d;
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.;
241 ltpdef.hmsl = (double)NAV_ALT0 / 1000.;
243 fdm.ecef_pos = ecef_nav0;
251 fdm.ltp_h.x = AHRS_H_X;
252 fdm.ltp_h.y = AHRS_H_Y;
253 fdm.ltp_h.z = AHRS_H_Z;
267 double dir __attribute__((unused)))
272 double wind_east __attribute__((unused)),
273 double wind_down __attribute__((unused)))
278 int turbulence_severity __attribute__((unused)))
283 double h __attribute__((unused)))
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 alt
in meters above WGS84 reference ellipsoid
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.
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[]
API to get/set the generic vehicle states.