From 8177db2e97b56beecb126381806442830216f515 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 9 Mar 2015 22:55:04 +0100 Subject: [PATCH] [nps] fdm_jsbsim: use GetAccelBody for accelerometer simulation --- .../subsystems/fixedwing/fdm_crrcsim.makefile | 2 ++ sw/simulator/nps/nps_fdm.h | 24 +++++++++++++------ sw/simulator/nps/nps_fdm_jsbsim.cpp | 13 +++++++--- sw/simulator/nps/nps_sensor_accel.c | 13 ++++++---- 4 files changed, 38 insertions(+), 14 deletions(-) diff --git a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile index fdc75eceded..c3e4cae15b8 100644 --- a/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile +++ b/conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile @@ -23,6 +23,8 @@ nps.LDFLAGS += $(shell pkg-config glib-2.0 --libs) -lm -lglibivy $(shell pcre-co nps.CFLAGS += -I$(SRC_FIRMWARE) -I$(SRC_BOARD) -I$(PAPARAZZI_SRC)/sw/simulator -I$(PAPARAZZI_HOME)/conf/simulator/nps nps.LDFLAGS += $(shell sdl-config --libs) +nps.CFLAGS += -DNPS_ACCEL_FROM_UVWDOT + # # add the simulator directory to the make searchpath # diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h index 96c21889c12..387ebe668ec 100644 --- a/sw/simulator/nps/nps_fdm.h +++ b/sw/simulator/nps/nps_fdm.h @@ -59,22 +59,32 @@ struct NpsFdm { struct LlaCoor_d lla_pos_geoc; //geocentric lla from jsbsim double agl; //AGL from jsbsim in m - /* velocity and acceleration wrt inertial frame expressed in ecef frame */ - // struct EcefCoor_d ecef_inertial_vel; - // struct EcefCoor_d ecef_inertial_accel; - /* velocity and acceleration wrt ecef frame expressed in ecef frame */ + /** acceleration in body frame, wrt ECI inertial frame */ + struct DoubleVect3 body_inertial_accel; + + /** velocity in ECEF frame, wrt ECEF frame */ struct EcefCoor_d ecef_ecef_vel; + /** acceleration in ECEF frame, wrt ECEF frame */ struct EcefCoor_d ecef_ecef_accel; - /* velocity and acceleration wrt ecef frame expressed in body frame */ + + /** velocity in body frame, wrt ECEF frame */ struct DoubleVect3 body_ecef_vel; /* aka UVW */ + /** acceleration in body frame, wrt ECEF frame */ struct DoubleVect3 body_ecef_accel; - /* velocity and acceleration wrt ecef frame expressed in ltp frame */ + + /** velocity in LTP frame, wrt ECEF frame */ struct NedCoor_d ltp_ecef_vel; + /** acceleration in LTP frame, wrt ECEF frame */ struct NedCoor_d ltp_ecef_accel; - /* velocity and acceleration wrt ecef frame expressed in ltppprz frame */ + + /** velocity in ltppprz frame, wrt ECEF frame */ struct NedCoor_d ltpprz_ecef_vel; + /** accel in ltppprz frame, wrt ECEF frame */ struct NedCoor_d ltpprz_ecef_accel; + /** acceleration in body frame as measured by an accelerometer (incl. gravity) */ + struct DoubleVect3 body_accel; + /* attitude */ struct DoubleQuat ecef_to_body_quat; struct DoubleQuat ltp_to_body_quat; diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp index 9f519656585..657fadfcff1 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.cpp +++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp @@ -286,7 +286,13 @@ static void fetch_state(void) { const FGColumnVector3& fg_body_ecef_vel = propagate->GetUVW(); jsbsimvec_to_vec(&fdm.body_ecef_vel, &fg_body_ecef_vel); const FGColumnVector3& fg_body_ecef_accel = accelerations->GetUVWdot(); - jsbsimvec_to_vec(&fdm.body_ecef_accel,&fg_body_ecef_accel); + jsbsimvec_to_vec(&fdm.body_ecef_accel, &fg_body_ecef_accel); + + const FGColumnVector3& fg_body_inertial_accel = accelerations->GetUVWidot(); + jsbsimvec_to_vec(&fdm.body_inertial_accel, &fg_body_inertial_accel); + + const FGColumnVector3& fg_body_accel = accelerations->GetBodyAccel(); + jsbsimvec_to_vec(&fdm.body_accel, &fg_body_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); @@ -304,9 +310,10 @@ static void fetch_state(void) { #endif /* in ECEF frame */ - const FGMatrix33& body_to_ecef = propagate->GetTb2ec(); - const FGColumnVector3& fg_ecef_ecef_vel = body_to_ecef * fg_body_ecef_vel; + const FGColumnVector3& fg_ecef_ecef_vel = propagate->GetECEFVelocity(); jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel); + + const FGMatrix33& body_to_ecef = propagate->GetTb2ec(); 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); diff --git a/sw/simulator/nps/nps_sensor_accel.c b/sw/simulator/nps/nps_sensor_accel.c index 5743f09f5b8..cca6a22ef09 100644 --- a/sw/simulator/nps/nps_sensor_accel.c +++ b/sw/simulator/nps/nps_sensor_accel.c @@ -30,22 +30,27 @@ void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru if (time < accel->next_update) return; +#if NPS_ACCEL_FROM_UVWDOT /* transform gravity to body frame */ struct DoubleVect3 g_body; double_quat_vmult(&g_body, &fdm.ltp_to_body_quat, &fdm.ltp_g); - // printf(" g_body %f %f %f\n", g_body.x, g_body.y, g_body.z); + // printf(" g_body % .3f % .3f % .3f\n", g_body.x, g_body.y, g_body.z); - // printf(" accel_fdm %f %f %f\n", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z); + // printf(" accel_fdm % .3f % .3f % .3f\n", fdm.body_ecef_accel.x, fdm.body_ecef_accel.y, fdm.body_ecef_accel.z); /* substract gravity to acceleration in body frame */ struct DoubleVect3 accelero_body; VECT3_DIFF(accelero_body, fdm.body_ecef_accel, g_body); +#else + struct DoubleVect3 accelero_body; + VECT3_COPY(accelero_body, fdm.body_accel); +#endif - // printf(" accelero body %f %f %f\n", accelero_body.x, accelero_body.y, accelero_body.z); + // printf(" accel body % .3f %.3f % .3f\n", accelero_body.x, accelero_body.y, accelero_body.z); /* transform to imu frame */ struct DoubleVect3 accelero_imu; - MAT33_VECT3_MUL(accelero_imu, *body_to_imu, accelero_body ); + MAT33_VECT3_MUL(accelero_imu, *body_to_imu, accelero_body); /* compute accelero readings */ MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu);