Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-29 02:38:07 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[nps] use angular velocity wrt inertial frame for gyro sim

Browse Source
This commit is contained in:
Felix Ruess
2015-03-10 14:30:03 +01:00
parent 16f9c93b89
commit d56a4a5381
4 changed files with 18 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/simulator/nps/nps_fdm.h
+8 -4
View File
@@ -59,9 +59,6 @@ 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
/** acceleration in body frame, wrt ECI inertial frame */
struct DoubleVect3 body_inertial_accel;
/** velocity in ECEF frame, wrt 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 */ /** acceleration in ECEF frame, wrt ECEF frame */
@@ -82,6 +79,9 @@ struct NpsFdm {
/** accel in ltppprz frame, wrt ECEF frame */ /** accel in ltppprz frame, wrt ECEF frame */
struct NedCoor_d ltpprz_ecef_accel; struct NedCoor_d ltpprz_ecef_accel;
/** acceleration in body frame, wrt ECI inertial frame */
struct DoubleVect3 body_inertial_accel;
/** acceleration in body frame as measured by an accelerometer (incl. gravity) */ /** acceleration in body frame as measured by an accelerometer (incl. gravity) */
struct DoubleVect3 body_accel; struct DoubleVect3 body_accel;
@@ -92,10 +92,14 @@ struct NpsFdm {
struct DoubleQuat ltpprz_to_body_quat; struct DoubleQuat ltpprz_to_body_quat;
struct DoubleEulers ltpprz_to_body_eulers; struct DoubleEulers ltpprz_to_body_eulers;
/* velocity and acceleration wrt ecef frame expressed in body frame */ /* angular velocity and acceleration in body frame, wrt ECEF frame */
struct DoubleRates body_ecef_rotvel; struct DoubleRates body_ecef_rotvel;
struct DoubleRates body_ecef_rotaccel; struct DoubleRates body_ecef_rotaccel;
/* angular velocity and acceleration in body frame, wrt inertial ECI frame */
struct DoubleRates body_inertial_rotvel;
struct DoubleRates body_inertial_rotaccel;
struct DoubleVect3 ltp_g; struct DoubleVect3 ltp_g;
struct DoubleVect3 ltp_h; struct DoubleVect3 ltp_h;
sw/simulator/nps/nps_fdm_crrcsim.c
+6 -6
View File
@@ -436,9 +436,9 @@ void decode_imupacket(struct NpsFdm * fdm, byte* buffer)
/* angular rate (0.9387340515702713e4 rad/s to rad/s) */ /* angular rate (0.9387340515702713e4 rad/s to rad/s) */
fdm->body_ecef_rotvel.p = (double)ShortOfBuf(buffer,9)*1.06526e-04; fdm->body_inertial_rotvel.p = (double)ShortOfBuf(buffer,9)*1.06526e-04;
fdm->body_ecef_rotvel.q = (double)ShortOfBuf(buffer,11)*1.06526e-04; fdm->body_inertial_rotvel.q = (double)ShortOfBuf(buffer,11)*1.06526e-04;
fdm->body_ecef_rotvel.r = (double)ShortOfBuf(buffer,13)*1.06526e-04; fdm->body_inertial_rotvel.r = (double)ShortOfBuf(buffer,13)*1.06526e-04;
/* magnetic field in Gauss */ /* magnetic field in Gauss */
//fdm->mag.x = (double)ShortOfBuf(buffer,15)*6.10352e-05; //fdm->mag.x = (double)ShortOfBuf(buffer,15)*6.10352e-05;
@@ -454,9 +454,9 @@ void decode_imupacket(struct NpsFdm * fdm, byte* buffer)
fdm->body_ecef_accel.x, fdm->body_ecef_accel.x,
fdm->body_ecef_accel.y, fdm->body_ecef_accel.y,
fdm->body_ecef_accel.z, fdm->body_ecef_accel.z,
fdm->body_ecef_rotvel.p, fdm->body_inertial_rotvel.p,
fdm->body_ecef_rotvel.q, fdm->body_inertial_rotvel.q,
fdm->body_ecef_rotvel.r); fdm->body_inertial_rotvel.r);
#endif #endif
} }
sw/simulator/nps/nps_fdm_jsbsim.cpp
+3
View File
@@ -352,6 +352,9 @@ static void fetch_state(void) {
jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR()); jsbsimvec_to_rate(&fdm.body_ecef_rotvel, &propagate->GetPQR());
jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot()); jsbsimvec_to_rate(&fdm.body_ecef_rotaccel, &accelerations->GetPQRdot());
jsbsimvec_to_rate(&fdm.body_inertial_rotvel, &propagate->GetPQRi());
jsbsimvec_to_rate(&fdm.body_inertial_rotaccel, &accelerations->GetPQRidot());
/* /*
* wind * wind
sw/simulator/nps/nps_sensor_gyro.c
+1 -1
View File
@@ -33,7 +33,7 @@ void nps_sensor_gyro_run_step(struct NpsSensorGyro* gyro, double time, struct Do
return; return;
/* transform body rates to IMU frame */ /* transform body rates to IMU frame */
struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_ecef_rotvel); struct DoubleVect3* rate_body = (struct DoubleVect3*)(&fdm.body_inertial_rotvel);
struct DoubleVect3 rate_imu; struct DoubleVect3 rate_imu;
MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body ); MAT33_VECT3_MUL(rate_imu, *body_to_imu, *rate_body );
/* compute gyros readings */ /* compute gyros readings */