mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-31 03:57:45 +08:00
[sim] moving the old ocaml simulator to NPS (#3167)
- the exact same basic model is now a NPS FDM - sim target still woks, it is just an alias to NPS with the proper FDM - the old ocaml files are removed - AHRS and INS are bypassed, since the accelerations are not well calculated by the model
This commit is contained in:
Gautier Hattenberger
committed by
GitHub
GitHub
parent
dfb08fa733
commit
41451d5422
@@ -0,0 +1,320 @@
|
||||
#ifndef FLIGHT_GEAR_H
|
||||
#define FLIGHT_GEAR_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#define FG_NET_CTRLS_VERSION 27
|
||||
#define FG_NET_CTRLS_MAX_ENGINES 4
|
||||
#define FG_NET_CTRLS_MAX_WHEELS 16
|
||||
#define FG_NET_CTRLS_MAX_TANKS 8
|
||||
#define FG_NET_CTRLS_RESERVED_SPACE 25
|
||||
|
||||
struct FGNetCtrls {
|
||||
uint32_t version; // increment when data values change
|
||||
|
||||
// Aero controls
|
||||
double aileron; // -1 ... 1
|
||||
double elevator; // -1 ... 1
|
||||
double rudder; // -1 ... 1
|
||||
double aileron_trim; // -1 ... 1
|
||||
double elevator_trim; // -1 ... 1
|
||||
double rudder_trim; // -1 ... 1
|
||||
double flaps; // 0 ... 1
|
||||
double spoilers;
|
||||
double speedbrake;
|
||||
|
||||
// Aero control faults
|
||||
uint32_t flaps_power; // true = power available
|
||||
uint32_t flap_motor_ok;
|
||||
|
||||
// Engine controls
|
||||
uint32_t num_engines; // number of valid engines
|
||||
uint32_t master_bat[FG_NET_CTRLS_MAX_ENGINES];
|
||||
uint32_t master_alt[FG_NET_CTRLS_MAX_ENGINES];
|
||||
uint32_t magnetos[FG_NET_CTRLS_MAX_ENGINES];
|
||||
uint32_t starter_power[FG_NET_CTRLS_MAX_ENGINES];// true = starter power
|
||||
double throttle[FG_NET_CTRLS_MAX_ENGINES]; // 0 ... 1
|
||||
double mixture[FG_NET_CTRLS_MAX_ENGINES]; // 0 ... 1
|
||||
double condition[FG_NET_CTRLS_MAX_ENGINES]; // 0 ... 1
|
||||
uint32_t fuel_pump_power[FG_NET_CTRLS_MAX_ENGINES];// true = on
|
||||
double prop_advance[FG_NET_CTRLS_MAX_ENGINES]; // 0 ... 1
|
||||
uint32_t feed_tank_to[4];
|
||||
uint32_t reverse[4];
|
||||
|
||||
// Engine faults
|
||||
uint32_t engine_ok[FG_NET_CTRLS_MAX_ENGINES];
|
||||
uint32_t mag_left_ok[FG_NET_CTRLS_MAX_ENGINES];
|
||||
uint32_t mag_right_ok[FG_NET_CTRLS_MAX_ENGINES];
|
||||
uint32_t spark_plugs_ok[FG_NET_CTRLS_MAX_ENGINES]; // false = fouled plugs
|
||||
uint32_t oil_press_status[FG_NET_CTRLS_MAX_ENGINES];// 0 = normal, 1 = low, 2 = full fail
|
||||
uint32_t fuel_pump_ok[FG_NET_CTRLS_MAX_ENGINES];
|
||||
|
||||
// Fuel management
|
||||
uint32_t num_tanks; // number of valid tanks
|
||||
uint32_t fuel_selector[FG_NET_CTRLS_MAX_TANKS]; // false = off, true = on
|
||||
uint32_t xfer_pump[5]; // specifies transfer from array
|
||||
// value tank to tank specified by
|
||||
// int value
|
||||
uint32_t cross_feed; // false = off, true = on
|
||||
|
||||
// Brake controls
|
||||
double brake_left;
|
||||
double brake_right;
|
||||
double copilot_brake_left;
|
||||
double copilot_brake_right;
|
||||
double brake_parking;
|
||||
|
||||
// Landing Gear
|
||||
uint32_t gear_handle; // true=gear handle down; false= gear handle up
|
||||
|
||||
// Switches
|
||||
uint32_t master_avionics;
|
||||
|
||||
// nav and Comm
|
||||
double comm_1;
|
||||
double comm_2;
|
||||
double nav_1;
|
||||
double nav_2;
|
||||
|
||||
// wind and turbulance
|
||||
double wind_speed_kt;
|
||||
double wind_dir_deg;
|
||||
double turbulence_norm;
|
||||
|
||||
// temp and pressure
|
||||
double temp_c;
|
||||
double press_inhg;
|
||||
|
||||
// other information about environment
|
||||
double hground; // ground elevation (meters)
|
||||
double magvar; // local magnetic variation in degs.
|
||||
|
||||
// hazards
|
||||
uint32_t icing; // icing status could me much
|
||||
// more complex but I'm
|
||||
// starting simple here.
|
||||
|
||||
// simulation control
|
||||
uint32_t speedup; // integer speedup multiplier
|
||||
uint32_t freeze; // 0=normal
|
||||
// 0x01=master
|
||||
// 0x02=position
|
||||
// 0x04=fuel
|
||||
|
||||
// --- New since FlightGear 0.9.10 (FG_NET_CTRLS_VERSION = 27)
|
||||
|
||||
// --- Add new variables just before this line.
|
||||
|
||||
uint32_t reserved[FG_NET_CTRLS_RESERVED_SPACE]; // 100 bytes reserved for future use.
|
||||
};
|
||||
|
||||
|
||||
#define FG_NET_FDM_VERSION 24
|
||||
#define FG_NET_FDM_MAX_ENGINES 4
|
||||
#define FG_NET_FDM_MAX_WHEELS 3
|
||||
#define FG_NET_FDM_MAX_TANKS 4
|
||||
|
||||
#ifndef _NET_FDM_HXX
|
||||
|
||||
struct FGNetFDM {
|
||||
|
||||
uint32_t version; // increment when data values change
|
||||
uint32_t padding; // padding
|
||||
|
||||
// Positions
|
||||
double longitude; // geodetic (radians)
|
||||
double latitude; // geodetic (radians)
|
||||
double altitude; // above sea level (meters)
|
||||
float agl; // above ground level (meters)
|
||||
float phi; // roll (radians)
|
||||
float theta; // pitch (radians)
|
||||
float psi; // yaw or true heading (radians)
|
||||
float alpha; // angle of attack (radians)
|
||||
float beta; // side slip angle (radians)
|
||||
|
||||
// Velocities
|
||||
float phidot; // roll rate (radians/sec)
|
||||
float thetadot; // pitch rate (radians/sec)
|
||||
float psidot; // yaw rate (radians/sec)
|
||||
float vcas; // calibrated airspeed
|
||||
float climb_rate; // feet per second
|
||||
float v_north; // north velocity in local/body frame, fps
|
||||
float v_east; // east velocity in local/body frame, fps
|
||||
float v_down; // down/vertical velocity in local/body frame, fps
|
||||
float v_body_u; // ECEF velocity in body frame
|
||||
float v_body_v; // ECEF velocity in body frame
|
||||
float v_body_w; // ECEF velocity in body frame
|
||||
|
||||
// Accelerations
|
||||
float A_X_pilot; // X accel in body frame ft/sec^2
|
||||
float A_Y_pilot; // Y accel in body frame ft/sec^2
|
||||
float A_Z_pilot; // Z accel in body frame ft/sec^2
|
||||
// Stall
|
||||
float stall_warning; // 0.0 - 1.0 indicating the amount of stall
|
||||
float slip_deg; // slip ball deflection
|
||||
|
||||
// Pressure
|
||||
|
||||
// Engine status
|
||||
uint32_t num_engines; // Number of valid engines
|
||||
uint32_t eng_state[FG_NET_FDM_MAX_ENGINES]; // Engine state (off, cranking, running)
|
||||
float rpm[FG_NET_FDM_MAX_ENGINES]; // Engine RPM rev/min
|
||||
float fuel_flow[FG_NET_FDM_MAX_ENGINES]; // Fuel flow gallons/hr
|
||||
float fuel_px[FG_NET_FDM_MAX_ENGINES]; // Fuel pressure psi
|
||||
float egt[FG_NET_FDM_MAX_ENGINES]; // Exhuast gas temp deg F
|
||||
float cht[FG_NET_FDM_MAX_ENGINES]; // Cylinder head temp deg F
|
||||
float mp_osi[FG_NET_FDM_MAX_ENGINES]; // Manifold pressure
|
||||
float tit[FG_NET_FDM_MAX_ENGINES]; // Turbine Inlet Temperature
|
||||
float oil_temp[FG_NET_FDM_MAX_ENGINES]; // Oil temp deg F
|
||||
float oil_px[FG_NET_FDM_MAX_ENGINES]; // Oil pressure psi
|
||||
|
||||
// Consumables
|
||||
uint32_t num_tanks; // Max number of fuel tanks
|
||||
float fuel_quantity[FG_NET_FDM_MAX_TANKS];
|
||||
|
||||
// Gear status
|
||||
uint32_t num_wheels;
|
||||
uint32_t wow[FG_NET_FDM_MAX_WHEELS];
|
||||
float gear_pos[FG_NET_FDM_MAX_WHEELS];
|
||||
float gear_steer[FG_NET_FDM_MAX_WHEELS];
|
||||
float gear_compression[FG_NET_FDM_MAX_WHEELS];
|
||||
|
||||
// Environment
|
||||
uint32_t cur_time; // current unix time
|
||||
// FIXME: make this uint64_t before 2038
|
||||
int32_t warp; // offset in seconds to unix time
|
||||
float visibility; // visibility in meters (for env. effects)
|
||||
|
||||
// Control surface positions (normalized values)
|
||||
float elevator;
|
||||
float elevator_trim_tab;
|
||||
float left_flap;
|
||||
float right_flap;
|
||||
float left_aileron;
|
||||
float right_aileron;
|
||||
float rudder;
|
||||
float nose_wheel;
|
||||
float speedbrake;
|
||||
float spoilers;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
struct FGNetMiniFDM {
|
||||
uint32_t version; // increment when data values change
|
||||
|
||||
// Positions
|
||||
double longitude; // geodetic (radians)
|
||||
double latitude; // geodetic (radians)
|
||||
double altitude; // above sea level (meters)
|
||||
double agl; // above ground level (meters)
|
||||
double phi; // roll (radians)
|
||||
double theta; // pitch (radians)
|
||||
double psi; // yaw or true heading (radians)
|
||||
|
||||
// Velocities
|
||||
double vcas;
|
||||
double climb_rate; // feet per second
|
||||
|
||||
// Consumables
|
||||
uint32_t num_tanks; // Max number of fuel tanks
|
||||
double fuel_quantity[FG_NET_FDM_MAX_TANKS];
|
||||
|
||||
// Environment
|
||||
uint32_t cur_time; // current unix time
|
||||
int32_t warp; // offset in seconds to unix time
|
||||
};
|
||||
|
||||
#if FG_2_4
|
||||
#define FG_NET_GUI_VERSION 7
|
||||
#else
|
||||
#define FG_NET_GUI_VERSION 8
|
||||
#endif /*FG_2_4*/
|
||||
|
||||
#define FG_NET_GUI_MAX_TANKS 4
|
||||
|
||||
// Prior to FG_NET_GUI_VERSION 8, OS X needed #pragma pack(4) to
|
||||
// properly display FG visualization data. In version 8 they added
|
||||
// a padding1 element to ensure proper data alignment, so this is
|
||||
// no longer required. The rest of this struct is based on FG source
|
||||
// in src/Network/net_gui.hxx
|
||||
|
||||
#if FG_2_4
|
||||
#ifdef __x86_64__
|
||||
#pragma pack(push)
|
||||
#ifdef __APPLE__
|
||||
#pragma pack(4)
|
||||
#else
|
||||
#pragma pack(8)
|
||||
#endif /*__APPLE__*/
|
||||
#endif /*__x86_64__*/
|
||||
#endif /*FG_2_4*/
|
||||
struct FGNetGUI {
|
||||
uint32_t version; // increment when data values change
|
||||
uint32_t padding1;
|
||||
|
||||
// Positions
|
||||
double longitude; // geodetic (radians)
|
||||
double latitude; // geodetic (radians)
|
||||
float altitude; // above sea level (meters)
|
||||
float agl; // above ground level (meters)
|
||||
float phi; // roll (radians)
|
||||
float theta; // pitch (radians)
|
||||
float psi; // yaw or true heading (radians)
|
||||
|
||||
// Velocities
|
||||
float vcas;
|
||||
float climb_rate; // feet per second
|
||||
|
||||
// Consumables
|
||||
uint32_t num_tanks; // Max number of fuel tanks
|
||||
float fuel_quantity[FG_NET_GUI_MAX_TANKS];
|
||||
|
||||
// Environment
|
||||
uint32_t cur_time; // current unix time
|
||||
// FIXME: make this uint64_t before 2038
|
||||
uint32_t warp; // offset in seconds to unix time
|
||||
float ground_elev; // ground elev (meters)
|
||||
|
||||
// Approach
|
||||
float tuned_freq; // currently tuned frequency
|
||||
float nav_radial; // target nav radial
|
||||
uint32_t in_range; // tuned navaid is in range?
|
||||
float dist_nm; // distance to tuned navaid in nautical miles
|
||||
float course_deviation_deg; // degrees off target course
|
||||
float gs_deviation_deg; // degrees off target glide slope
|
||||
};
|
||||
#if FG_2_4
|
||||
#ifdef __x86_64__
|
||||
#pragma pack(push)
|
||||
#pragma pack(pop)
|
||||
#endif /*__x86_64__*/
|
||||
#endif /*FG_2_4*/
|
||||
|
||||
|
||||
#define FG_ENVIRONMENT_FOOTER_MAGIC 0x12345678
|
||||
// described in file fg_environment.xml (generic protocol)
|
||||
struct FGEnvironment{
|
||||
double elapsed_sec; // elapsed sim seconds
|
||||
float wind_from_north; // wind from north in m/s
|
||||
float wind_from_east; // wind from east in m/s
|
||||
float wind_from_down; // wind from down in m/s
|
||||
float wind_from_heading; // wind N-E heading in degrees
|
||||
float wind_speed; // wind N-E speed in m/s
|
||||
uint32_t footer_magic; // magic footer 0x12345678
|
||||
};
|
||||
|
||||
|
||||
|
||||
extern void net_fdm_dump (struct FGNetFDM* fdm);
|
||||
extern void net_fdm_ntoh (struct FGNetFDM* fdm);
|
||||
extern void net_fdm_init (struct FGNetFDM* fdm);
|
||||
|
||||
extern void net_gui_init (struct FGNetGUI* gui);
|
||||
extern void net_gui_hton (struct FGNetGUI* gui);
|
||||
extern void net_gui_dump (struct FGNetGUI* gui);
|
||||
|
||||
extern void net_ctrls_dump(struct FGNetCtrls* ctrls);
|
||||
extern void net_ctrls_ntoh(struct FGNetCtrls* ctrls);
|
||||
|
||||
#endif /* FLIGHT_GEAR_H */
|
||||
@@ -27,7 +27,7 @@ extern "C" {
|
||||
#endif
|
||||
|
||||
#include "std.h"
|
||||
#include "../flight_gear.h"
|
||||
#include "flight_gear.h"
|
||||
#include "math/pprz_geodetic_double.h"
|
||||
#include "math/pprz_algebra_double.h"
|
||||
|
||||
|
||||
@@ -0,0 +1,309 @@
|
||||
/*
|
||||
* Copyright (C) 2023 Gautier Hattenberger <gautier.hattenberger@enac.fr>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "nps_fdm.h"
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
|
||||
#include "std.h"
|
||||
#include "math/pprz_geodetic.h"
|
||||
#include "math/pprz_geodetic_double.h"
|
||||
#include "math/pprz_geodetic_float.h"
|
||||
#include "math/pprz_algebra.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
#include "math/pprz_isa.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "generated/flight_plan.h"
|
||||
|
||||
#include "state.h"
|
||||
|
||||
// Check if rover firmware
|
||||
#ifndef FIXEDWING_FIRMWARE
|
||||
#error "The module nps_fdm_fixedwing_sim is a basic flight model for fixedwing only"
|
||||
#endif
|
||||
|
||||
#ifndef ROLL_RESPONSE_FACTOR
|
||||
#define ROLL_RESPONSE_FACTOR 15.
|
||||
#endif
|
||||
|
||||
#ifndef PITCH_RESPONSE_FACTOR
|
||||
#define PITCH_RESPONSE_FACTOR 1.
|
||||
#endif
|
||||
|
||||
#ifndef YAW_RESPONSE_FACTOR
|
||||
#define YAW_RESPONSE_FACTOR 1.
|
||||
#endif
|
||||
|
||||
#ifndef WEIGHT
|
||||
#define WEIGHT 1.
|
||||
#endif
|
||||
|
||||
#ifndef ROLL_MAX_SETPOINT
|
||||
#define ROLL_MAX_SETPOINT RadOfDeg(40.)
|
||||
#endif
|
||||
|
||||
#ifndef ROLL_RATE_MAX_SETPOINT
|
||||
#define ROLL_RATE_MAX_SETPOINT RadOfDeg(15.)
|
||||
#endif
|
||||
|
||||
#ifndef NOMINAL_AIRSPEED
|
||||
#error "Please define NOMINAL_AIRSPEED in your airframe file"
|
||||
#endif
|
||||
|
||||
#ifndef MAXIMUM_AIRSPEED
|
||||
#define MAXIMUM_AIRSPEED (NOMINAL_AIRSPEED * 1.5)
|
||||
#endif
|
||||
|
||||
#ifndef MAXIMUM_POWER
|
||||
#define MAXIMUM_POWER (5. * MAXIMUM_AIRSPEED * WEIGHT)
|
||||
#endif
|
||||
|
||||
#ifndef AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE
|
||||
#define AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE 0.45
|
||||
#endif
|
||||
|
||||
// NpsFdm structure
|
||||
struct NpsFdm fdm;
|
||||
|
||||
// Reference point
|
||||
static struct LtpDef_d ltpdef;
|
||||
|
||||
// Static functions declaration
|
||||
static void init_ltp(void);
|
||||
|
||||
struct fixedwing_sim_state {
|
||||
struct EnuCoor_d pos;
|
||||
struct EnuCoor_d speed;
|
||||
struct DoubleEulers attitude;
|
||||
struct DoubleEulers rates;
|
||||
double airspeed;
|
||||
double delta_roll;
|
||||
double delta_pitch;
|
||||
double delta_thrust;
|
||||
struct EnuCoor_d wind;
|
||||
};
|
||||
|
||||
static struct fixedwing_sim_state sim_state;
|
||||
|
||||
|
||||
/** NPS FDM rover init ***************************/
|
||||
void nps_fdm_init(double dt)
|
||||
{
|
||||
fdm.init_dt = dt; // (1 / simulation freq)
|
||||
fdm.curr_dt = dt;
|
||||
fdm.time = dt;
|
||||
|
||||
fdm.on_ground = TRUE;
|
||||
|
||||
fdm.nan_count = 0;
|
||||
fdm.pressure = -1;
|
||||
fdm.pressure_sl = PPRZ_ISA_SEA_LEVEL_PRESSURE;
|
||||
fdm.total_pressure = -1;
|
||||
fdm.dynamic_pressure = -1;
|
||||
fdm.temperature = -1;
|
||||
|
||||
fdm.ltpprz_to_body_eulers.phi = 0.0;
|
||||
fdm.ltpprz_to_body_eulers.theta = 0.0;
|
||||
fdm.ltpprz_to_body_eulers.psi = 0.0;
|
||||
|
||||
VECT3_ASSIGN(sim_state.pos, 0., 0., 0.);
|
||||
VECT3_ASSIGN(sim_state.speed, 0., 0., 0.);
|
||||
EULERS_ASSIGN(sim_state.attitude, 0., 0., 0.);
|
||||
EULERS_ASSIGN(sim_state.rates, 0., 0., 0.);
|
||||
VECT3_ASSIGN(sim_state.wind, 0., 0., 0.);
|
||||
sim_state.airspeed = 0.;
|
||||
sim_state.delta_roll = 0.;
|
||||
sim_state.delta_pitch = 0.;
|
||||
sim_state.delta_thrust = 0.;
|
||||
|
||||
init_ltp();
|
||||
}
|
||||
|
||||
|
||||
/** Minimum complexity flight dynamic model
|
||||
* In legacy Paparazzi simulator, was implemented in OCaml
|
||||
* and correspond to the 'sim' target
|
||||
*
|
||||
* Johnson, E.N., Fontaine, S.G., and Kahn, A.D.,
|
||||
* “Minimum Complexity Uninhabited Air Vehicle Guidance And Flight Control System,”
|
||||
* Proceedings of the 20th Digital Avionics Systems Conference, 2001.
|
||||
* http://www.ae.gatech.edu/~ejohnson/JohnsonFontaineKahn.pdf
|
||||
|
||||
* Johnson, E.N. and Fontaine, S.G.,
|
||||
* “Use Of Flight Simulation To Complement Flight Testing Of Low-Cost UAVs,”
|
||||
* Proceedings of the AIAA Modeling and Simulation Technology Conference, 2001.
|
||||
* http://www.ae.gatech.edu/~ejohnson/AIAA%202001-4059.pdf
|
||||
*/
|
||||
void nps_fdm_run_step(bool launch, double *commands, int commands_nb __attribute__((unused)))
|
||||
{
|
||||
|
||||
// commands
|
||||
sim_state.delta_roll = commands[COMMAND_ROLL] * MAX_PPRZ;
|
||||
sim_state.delta_pitch = -commands[COMMAND_PITCH] * MAX_PPRZ; // invert back to correct sign
|
||||
sim_state.delta_thrust = commands[COMMAND_THROTTLE];
|
||||
|
||||
static bool already_launched = false;
|
||||
if (launch && !already_launched) {
|
||||
sim_state.airspeed = NOMINAL_AIRSPEED;
|
||||
already_launched = true;
|
||||
}
|
||||
if (sim_state.airspeed == 0. && sim_state.delta_thrust > 0.) {
|
||||
sim_state.airspeed = NOMINAL_AIRSPEED;
|
||||
}
|
||||
|
||||
if (sim_state.pos.z >= -3. && sim_state.airspeed > 0.) {
|
||||
double v2 = sim_state.airspeed * sim_state.airspeed;
|
||||
double vn2 = NOMINAL_AIRSPEED * NOMINAL_AIRSPEED;
|
||||
|
||||
// roll dynamic
|
||||
double c_l = 4e-5 * sim_state.delta_roll;
|
||||
double phi_dot_dot = ROLL_RESPONSE_FACTOR * c_l * v2 / vn2 - sim_state.rates.phi;
|
||||
sim_state.rates.phi = sim_state.rates.phi + phi_dot_dot * fdm.curr_dt;
|
||||
BoundAbs(sim_state.rates.phi, ROLL_RATE_MAX_SETPOINT);
|
||||
sim_state.attitude.phi = sim_state.attitude.phi + sim_state.rates.phi * fdm.curr_dt;
|
||||
NormRadAngle(sim_state.attitude.phi);
|
||||
BoundAbs(sim_state.attitude.phi, ROLL_MAX_SETPOINT);
|
||||
|
||||
// yaw dynamic
|
||||
sim_state.rates.psi = PPRZ_ISA_GRAVITY / sim_state.airspeed * tan(YAW_RESPONSE_FACTOR * sim_state.attitude.phi);
|
||||
sim_state.attitude.psi = sim_state.attitude.psi + sim_state.rates.psi * fdm.curr_dt;
|
||||
NormRadAngle(sim_state.attitude.psi);
|
||||
|
||||
// Aerodynamic pitching moment coeff, proportional to elevator
|
||||
// No Thrust moment, so null (0) for steady flight
|
||||
double c_m = 5e-7 * sim_state.delta_pitch;
|
||||
double theta_dot_dot = PITCH_RESPONSE_FACTOR * c_m * v2 - sim_state.rates.theta;
|
||||
sim_state.rates.theta = sim_state.rates.theta + theta_dot_dot * fdm.curr_dt;
|
||||
sim_state.attitude.theta = sim_state.attitude.theta + sim_state.rates.theta * fdm.curr_dt;
|
||||
|
||||
// Flight path angle
|
||||
double gamma = atan2(sim_state.speed.z, sim_state.airspeed);
|
||||
|
||||
// Cz proportional to angle of attack
|
||||
double alpha = sim_state.attitude.theta - gamma;
|
||||
double c_z = 0.2 * alpha + PPRZ_ISA_GRAVITY / vn2;
|
||||
|
||||
// Lift
|
||||
double lift = c_z * v2;
|
||||
double z_dot_dot = lift / WEIGHT * cos(sim_state.attitude.theta) * cos(sim_state.attitude.phi) - PPRZ_ISA_GRAVITY;
|
||||
sim_state.speed.z = sim_state.speed.z + z_dot_dot * fdm.curr_dt;
|
||||
sim_state.pos.z = sim_state.pos.z + sim_state.speed.z * fdm.curr_dt;
|
||||
|
||||
// Constant Cx, drag to get expected cruise and maximum throttle
|
||||
double cruise_th = AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE;
|
||||
double drag = cruise_th + (v2 - vn2) * (1. - cruise_th) / (MAXIMUM_AIRSPEED*MAXIMUM_AIRSPEED - vn2);
|
||||
double airspeed_dot = MAXIMUM_POWER / sim_state.airspeed * (sim_state.delta_thrust - drag) / WEIGHT - PPRZ_ISA_GRAVITY * sin(gamma);
|
||||
sim_state.airspeed = sim_state.airspeed + airspeed_dot * fdm.curr_dt;
|
||||
sim_state.airspeed = Max(sim_state.airspeed, 10.); // avoid stall
|
||||
|
||||
// Wind effect (FIXME apply to forces ?)
|
||||
sim_state.speed.x = sim_state.airspeed * sin(sim_state.attitude.psi) + sim_state.wind.x;
|
||||
sim_state.speed.y = sim_state.airspeed * cos(sim_state.attitude.psi) + sim_state.wind.y;
|
||||
sim_state.pos.x = sim_state.pos.x + sim_state.speed.x * fdm.curr_dt;
|
||||
sim_state.pos.y = sim_state.pos.y + sim_state.speed.y * fdm.curr_dt;
|
||||
sim_state.pos.z = sim_state.pos.z + sim_state.wind.z * fdm.curr_dt;
|
||||
}
|
||||
|
||||
/****************************************************************************/
|
||||
// Coordenates transformations |
|
||||
// ----------------------------|
|
||||
|
||||
/* in ECEF */
|
||||
ecef_of_enu_point_d(&fdm.ecef_pos, <pdef, &sim_state.pos);
|
||||
lla_of_ecef_d(&fdm.lla_pos, &fdm.ecef_pos);
|
||||
ecef_of_enu_vect_d(&fdm.ecef_ecef_vel, <pdef, &sim_state.speed);
|
||||
//ecef_of_enu_vect_d(&fdm.ecef_ecef_accel, <pdef, &rover_acc);
|
||||
|
||||
/* in NED */
|
||||
ned_of_ecef_point_d(&fdm.ltpprz_pos, <pdef, &fdm.ecef_pos);
|
||||
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, <pdef, &fdm.ecef_ecef_vel);
|
||||
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, <pdef, &fdm.ecef_ecef_accel);
|
||||
fdm.hmsl = -fdm.ltpprz_pos.z + NAV_ALT0 / 1000.;
|
||||
|
||||
/* Eulers */
|
||||
fdm.ltp_to_body_eulers = sim_state.attitude;
|
||||
// Exporting Eulers to AHRS (in quaternions)
|
||||
double_quat_of_eulers(&fdm.ltp_to_body_quat, &fdm.ltp_to_body_eulers);
|
||||
|
||||
// Angular vel & acc
|
||||
fdm.body_ecef_rotvel.p = sim_state.rates.phi;
|
||||
fdm.body_ecef_rotvel.q = sim_state.rates.theta;
|
||||
fdm.body_ecef_rotvel.r = sim_state.rates.psi;
|
||||
}
|
||||
|
||||
|
||||
/**************************
|
||||
** Generating LTP plane **
|
||||
**************************/
|
||||
|
||||
static void init_ltp(void)
|
||||
{
|
||||
|
||||
struct LlaCoor_d llh_nav0; /* Height above the ellipsoid */
|
||||
llh_nav0.lat = RadOfDeg((double)NAV_LAT0 / 1e7);
|
||||
llh_nav0.lon = RadOfDeg((double)NAV_LON0 / 1e7);
|
||||
llh_nav0.alt = (double)(NAV_ALT0) / 1000.0;
|
||||
|
||||
struct EcefCoor_d ecef_nav0;
|
||||
|
||||
ecef_of_lla_d(&ecef_nav0, &llh_nav0);
|
||||
|
||||
ltp_def_from_ecef_d(<pdef, &ecef_nav0);
|
||||
fdm.ecef_pos = ecef_nav0;
|
||||
|
||||
// accel and mag data should not be used
|
||||
// AHRS and INS are bypassed
|
||||
fdm.ltp_g.x = 0.;
|
||||
fdm.ltp_g.y = 0.;
|
||||
fdm.ltp_g.z = 0.;
|
||||
|
||||
fdm.ltp_h.x = 1.;
|
||||
fdm.ltp_h.y = 0.;
|
||||
fdm.ltp_h.z = 0.;
|
||||
|
||||
}
|
||||
|
||||
|
||||
void nps_fdm_set_wind(double speed, double dir)
|
||||
{
|
||||
sim_state.wind.x = speed * sin(dir);
|
||||
sim_state.wind.y = speed * cos(dir);
|
||||
sim_state.wind.z = 0.;
|
||||
}
|
||||
|
||||
void nps_fdm_set_wind_ned(double wind_north, double wind_east, double wind_down)
|
||||
{
|
||||
sim_state.wind.x = wind_east;
|
||||
sim_state.wind.y = wind_north;
|
||||
sim_state.wind.z = -wind_down;
|
||||
}
|
||||
|
||||
void nps_fdm_set_turbulence(double wind_speed __attribute__((unused)),
|
||||
int turbulence_severity __attribute__((unused)))
|
||||
{
|
||||
}
|
||||
|
||||
void nps_fdm_set_temperature(double temp __attribute__((unused)),
|
||||
double h __attribute__((unused)))
|
||||
{
|
||||
}
|
||||
|
||||
@@ -13,7 +13,7 @@
|
||||
#include <pthread.h>
|
||||
|
||||
#include "std.h"
|
||||
#include "../flight_gear.h"
|
||||
#include "flight_gear.h"
|
||||
#include "nps_main.h"
|
||||
#include "nps_fdm.h"
|
||||
#include "nps_atmosphere.h"
|
||||
|
||||
Reference in New Issue
Block a user