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

[nps] fdm_jsbsim: use GetAccelBody for accelerometer simulation

Browse Source
This commit is contained in:
Felix Ruess
2015-03-09 22:55:04 +01:00
parent 06b9456ad2
commit 8177db2e97
4 changed files with 38 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/fixedwing/fdm_crrcsim.makefile
+2
View File
@@ -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.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.LDFLAGS += $(shell sdl-config --libs)
nps.CFLAGS += -DNPS_ACCEL_FROM_UVWDOT
# #
# add the simulator directory to the make searchpath # add the simulator directory to the make searchpath
# #
sw/simulator/nps/nps_fdm.h
+17 -7
View File
@@ -59,22 +59,32 @@ struct NpsFdm {
struct LlaCoor_d lla_pos_geoc; //geocentric lla from jsbsim struct LlaCoor_d lla_pos_geoc; //geocentric lla from jsbsim
double agl; //AGL from jsbsim in m double agl; //AGL from jsbsim in m
/* velocity and acceleration wrt inertial frame expressed in ecef frame */ /** acceleration in body frame, wrt ECI inertial frame */
// struct EcefCoor_d ecef_inertial_vel; struct DoubleVect3 body_inertial_accel;
// struct EcefCoor_d ecef_inertial_accel;
/* velocity and acceleration wrt ecef frame expressed in ecef frame */ /** velocity in ECEF frame, wrt ECEF frame */
struct EcefCoor_d ecef_ecef_vel; struct EcefCoor_d ecef_ecef_vel;
/** acceleration in ECEF frame, wrt ECEF frame */
struct EcefCoor_d ecef_ecef_accel; 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 */ struct DoubleVect3 body_ecef_vel; /* aka UVW */
/** acceleration in body frame, wrt ECEF frame */
struct DoubleVect3 body_ecef_accel; 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; struct NedCoor_d ltp_ecef_vel;
/** acceleration in LTP frame, wrt ECEF frame */
struct NedCoor_d ltp_ecef_accel; 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; struct NedCoor_d ltpprz_ecef_vel;
/** accel in ltppprz frame, wrt ECEF frame */
struct NedCoor_d ltpprz_ecef_accel; struct NedCoor_d ltpprz_ecef_accel;
/** acceleration in body frame as measured by an accelerometer (incl. gravity) */
struct DoubleVect3 body_accel;
/* attitude */ /* attitude */
struct DoubleQuat ecef_to_body_quat; struct DoubleQuat ecef_to_body_quat;
struct DoubleQuat ltp_to_body_quat; struct DoubleQuat ltp_to_body_quat;
sw/simulator/nps/nps_fdm_jsbsim.cpp
+10 -3
View File
@@ -286,7 +286,13 @@ static void fetch_state(void) {
const FGColumnVector3& fg_body_ecef_vel = propagate->GetUVW(); const FGColumnVector3& fg_body_ecef_vel = propagate->GetUVW();
jsbsimvec_to_vec(&fdm.body_ecef_vel, &fg_body_ecef_vel); jsbsimvec_to_vec(&fdm.body_ecef_vel, &fg_body_ecef_vel);
const FGColumnVector3& fg_body_ecef_accel = accelerations->GetUVWdot(); 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 #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); 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 #endif
/* in ECEF frame */ /* in ECEF frame */
const FGMatrix33& body_to_ecef = propagate->GetTb2ec(); const FGColumnVector3& fg_ecef_ecef_vel = propagate->GetECEFVelocity();
const FGColumnVector3& fg_ecef_ecef_vel = body_to_ecef * fg_body_ecef_vel;
jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_vel, &fg_ecef_ecef_vel); 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; 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); jsbsimvec_to_vec((DoubleVect3*)&fdm.ecef_ecef_accel, &fg_ecef_ecef_accel);
sw/simulator/nps/nps_sensor_accel.c
+9 -4
View File
@@ -30,22 +30,27 @@ void nps_sensor_accel_run_step(struct NpsSensorAccel* accel, double time, stru
if (time < accel->next_update) if (time < accel->next_update)
return; return;
#if NPS_ACCEL_FROM_UVWDOT
/* transform gravity to body frame */ /* transform gravity to body frame */
struct DoubleVect3 g_body; struct DoubleVect3 g_body;
double_quat_vmult(&g_body, &fdm.ltp_to_body_quat, &fdm.ltp_g); 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 */ /* substract gravity to acceleration in body frame */
struct DoubleVect3 accelero_body; struct DoubleVect3 accelero_body;
VECT3_DIFF(accelero_body, fdm.body_ecef_accel, g_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 */ /* transform to imu frame */
struct DoubleVect3 accelero_imu; 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 */ /* compute accelero readings */
MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu); MAT33_VECT3_MUL(accel->value, accel->sensitivity, accelero_imu);