Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

updated timing to use autopilot PERIODIC_FREQUENCY as base, jsbsim steps based on SIM_DT, fixed out of ground timestep check to include vehicle radius, added some debug printf's based on DEBUG_NPS_JSBSIM and DEBUG_NPS_TIME, checks if simulation runs will get behind real time at higher time factors, tried to fix display output timing, still needs to be fixed

Browse Source
This commit is contained in:
Stephen Dwyer
2012-06-21 23:21:55 -06:00
committed by Felix Ruess
parent 22014e87c8
commit 2de9b81ed1
2 changed files with 74 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/nps/nps_fdm_jsbsim.c
+40 -3
View File
@@ -43,7 +43,9 @@
/// Macro to convert from feet to metres
#define MetersOfFeet(_f) ((_f)/3.2808399)
/// Minimum JSBSim timestep
/** Minimum JSBSim timestep
* Around 1/10000 seems to be good for ground impacts
*/
#define MIN_DT (1.0/10240.0)
using namespace JSBSim;
@@ -74,16 +76,27 @@ static struct LtpDef_d ltpdef;
/// The largest distance between vehicle CG and contact point
double vehicle_radius_max;
/// Timestep used for higher fidelity near the ground
double min_dt;
void nps_fdm_init(double dt) {
fdm.init_dt = dt;
fdm.curr_dt = dt;
//Sets up the high fidelity timestep as a multiple of the normal timestep
for (min_dt = (1.0/dt); min_dt < (1/MIN_DT); min_dt += (1/dt)){}
min_dt = (1/min_dt);
init_jsbsim(dt);
FDMExec->RunIC();
init_ltp();
#if DEBUG_NPS_JSBSIM
printf("fdm.time,fg_body_ecef_accel1,fg_body_ecef_accel2,fg_body_ecef_accel3,fdm.body_ecef_accel.x,fdm.body_ecef_accel.y,fdm.body_ecef_accel.z,fg_ltp_ecef_accel1,fg_ltp_ecef_accel2,fg_ltp_ecef_accel3,fdm.ltp_ecef_accel.x,fdm.ltp_ecef_accel.y,fdm.ltp_ecef_accel.z,fg_ecef_ecef_accel1,fg_ecef_ecef_accel2,fg_ecef_ecef_accel3,fdm.ecef_ecef_accel.x,fdm.ecef_ecef_accel.y,fdm.ecef_ecef_accel.z,fdm.ltpprz_ecef_accel.z,fdm.ltpprz_ecef_accel.y,fdm.ltpprz_ecef_accel.z,fdm.agl\n");
#endif
fetch_state();
}
@@ -114,11 +127,11 @@ void nps_fdm_run_step(double* commands) {
double numDT_to_impact = (fdm.agl - vehicle_radius_max) / (fdm.curr_dt * fdm.ltp_ecef_vel.z);
// If impact imminent within next timestep, use high sim rate
if (numDT_to_impact <= 1.0) {
fdm.curr_dt = MIN_DT;
fdm.curr_dt = min_dt;
}
}
// If the vehicle is moving upwards and out of the ground, reset timestep
else if ((fdm.ltp_ecef_vel.z <= 0) && (fdm.agl > 0)) {
else if ((fdm.ltp_ecef_vel.z <= 0) && ((fdm.agl + vehicle_radius_max) > 0)) {
fdm.curr_dt = fdm.init_dt;
}
@@ -164,6 +177,10 @@ static void fetch_state(void) {
FGPropertyManager* node = FDMExec->GetPropertyManager()->GetNode("simulation/sim-time-sec");
fdm.time = node->getDoubleValue();
#if DEBUG_NPS_JSBSIM
printf("%f,",fdm.time);
#endif
FGPropagate* propagate = FDMExec->GetPropagate();
FGAccelerations* accelerations = FDMExec->GetAccelerations();
@@ -185,6 +202,10 @@ static void fetch_state(void) {
const FGColumnVector3& fg_body_ecef_accel = accelerations->GetUVWdot();
jsbsimvec_to_vec(&fdm.body_ecef_accel,&fg_body_ecef_accel);
#if DEBUG_NPS_JSBSIM
printf("%f,%f,%f,%f,%f,%f,",(&fg_body_ecef_accel)->Entry(1),(&fg_body_ecef_accel)->Entry(2),(&fg_body_ecef_accel)->Entry(3),fdm.body_ecef_accel.x,fdm.body_ecef_accel.y,fdm.body_ecef_accel.z);
#endif
/* in LTP frame */
const FGMatrix33& body_to_ltp = propagate->GetTb2l();
const FGColumnVector3& fg_ltp_ecef_vel = body_to_ltp * fg_body_ecef_vel;
@@ -192,6 +213,10 @@ static void fetch_state(void) {
const FGColumnVector3& fg_ltp_ecef_accel = body_to_ltp * fg_body_ecef_accel;
jsbsimvec_to_vec((DoubleVect3*)&fdm.ltp_ecef_accel, &fg_ltp_ecef_accel);
#if DEBUG_NPS_JSBSIM
printf("%f,%f,%f,%f,%f,%f,",(&fg_ltp_ecef_accel)->Entry(1),(&fg_ltp_ecef_accel)->Entry(2),(&fg_ltp_ecef_accel)->Entry(3),fdm.ltp_ecef_accel.x,fdm.ltp_ecef_accel.y,fdm.ltp_ecef_accel.z);
#endif
/* in ECEF frame */
const FGMatrix33& body_to_ecef = propagate->GetTb2ec();
const FGColumnVector3& fg_ecef_ecef_vel = body_to_ecef * fg_body_ecef_vel;
@@ -199,11 +224,19 @@ static void fetch_state(void) {
const FGColumnVector3& fg_ecef_ecef_accel = body_to_ecef * fg_body_ecef_accel;
jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel);
#if DEBUG_NPS_JSBSIM
printf("%f,%f,%f,%f,%f,%f,",(&fg_ecef_ecef_accel)->Entry(1),(&fg_ecef_ecef_accel)->Entry(2),(&fg_ecef_ecef_accel)->Entry(3),fdm.ecef_ecef_accel.x,fdm.ecef_ecef_accel.y,fdm.ecef_ecef_accel.z);
#endif
/* in LTP pprz */
ned_of_ecef_point_d(&fdm.ltpprz_pos, &ltpdef, &fdm.ecef_pos);
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_vel, &ltpdef, &fdm.ecef_ecef_vel);
ned_of_ecef_vect_d(&fdm.ltpprz_ecef_accel, &ltpdef, &fdm.ecef_ecef_accel);
#if DEBUG_NPS_JSBSIM
printf("%f,%f,%f,",fdm.ltpprz_ecef_accel.z,fdm.ltpprz_ecef_accel.y,fdm.ltpprz_ecef_accel.z);
#endif
/* llh */
llh_from_jsbsim(&fdm.lla_pos, propagate);
@@ -213,6 +246,10 @@ static void fetch_state(void) {
lla_of_ecef_d(&fdm.lla_pos_pprz, &fdm.ecef_pos);
fdm.agl = MetersOfFeet(propagate->GetDistanceAGL());
#if DEBUG_NPS_JSBSIM
printf("%f\n",fdm.agl);
#endif
/*
* attitude
*/
sw/simulator/nps/nps_main.c
+34 -2
View File
@@ -13,12 +13,17 @@
#include "nps_ivy.h"
#include "nps_flightgear.h"
#define SIM_DT (1./512.)
#if SYS_TIME_RESOLUTION
#define SIM_DT (SYS_TIME_RESOLUTION)
#else
#define SIM_DT (1./(double)(PERIODIC_FREQUENCY))
#endif /* SYS_TIME_RESOLUTION */
#define DISPLAY_DT (1./30.)
#define HOST_TIMEOUT_MS 40
#define HOST_TIME_FACTOR 1.
static struct {
double real_initial_time;
double scaled_initial_time;
double host_time_factor;
double sim_time;
@@ -82,6 +87,7 @@ static void nps_main_init(void) {
nps_main.display_time = 0.;
struct timeval t;
gettimeofday (&t, NULL);
nps_main.real_initial_time = time_to_double(&t);
nps_main.scaled_initial_time = time_to_double(&t);
nps_main.host_time_factor = HOST_TIME_FACTOR;
@@ -107,6 +113,10 @@ static void nps_main_init(void) {
if (nps_main.fg_host)
nps_flightgear_init(nps_main.fg_host, nps_main.fg_port);
#if DEBUG_NPS_TIME
printf("host_time_factor,host_time_elapsed,host_time_now,scaled_initial_time,sim_time_before,display_time_before,sim_time_after,display_time_after\n");
#endif
}
@@ -169,15 +179,37 @@ static gboolean nps_main_periodic(gpointer data __attribute__ ((unused))) {
host_time_now = time_to_double(&tv_now);
double host_time_elapsed = nps_main.host_time_factor *(host_time_now - nps_main.scaled_initial_time);
#if DEBUG_NPS_TIME
printf("%f,%f,%f,%f,%f,%f,",nps_main.host_time_factor,host_time_elapsed,host_time_now,nps_main.scaled_initial_time,nps_main.sim_time,nps_main.display_time);
#endif
int cnt = 0;
static int prev_cnt = 0;
static int grow_cnt = 0;
while (nps_main.sim_time <= host_time_elapsed) {
nps_main_run_sim_step();
nps_main.sim_time += SIM_DT;
if (nps_main.display_time < nps_main.sim_time) {
if (nps_main.display_time < (host_time_now - nps_main.real_initial_time)) {
nps_main_display();
nps_main.display_time += DISPLAY_DT;
}
cnt++;
}
/* Check to make sure the simulation doesn't get too far behind real time looping */
if (cnt > (prev_cnt)) {grow_cnt++;}
else { grow_cnt--;}
if (grow_cnt < 0) {grow_cnt = 0;}
prev_cnt = cnt;
if (grow_cnt > 10) {
printf("Warning: The time factor is too large for efficient operation! Please reduce the time factor.\n");
}
#if DEBUG_NPS_TIME
printf("%f,%f\n",nps_main.sim_time,nps_main.display_time);
#endif
return TRUE;
}