diff --git a/conf/autopilot/booz2_simulator.makefile b/conf/autopilot/booz2_simulator.makefile
index 64961b6943..1b0884fdc8 100644
--- a/conf/autopilot/booz2_simulator.makefile
+++ b/conf/autopilot/booz2_simulator.makefile
@@ -78,7 +78,6 @@ sim.srcs += $(SRC_BOOZ)/booz2_analog.c $(SRC_BOOZ_SIM)/booz2_analog_hw.c
-sim.srcs += $(SRC_BOOZ)/booz2_gps.c
sim.srcs += $(SRC_BOOZ)/booz2_autopilot.c
diff --git a/conf/autopilot/booz2_simulator_nps.makefile b/conf/autopilot/booz2_simulator_nps.makefile
index b39158d4fb..095e1794fc 100644
--- a/conf/autopilot/booz2_simulator_nps.makefile
+++ b/conf/autopilot/booz2_simulator_nps.makefile
@@ -83,8 +83,6 @@ sim.srcs += $(SRC_BOOZ)/booz2_analog.c $(SRC_BOOZ_SIM)/booz2_analog_hw.c
-sim.srcs += $(SRC_BOOZ)/booz2_gps.c
-
sim.srcs += $(SRC_BOOZ)/booz2_autopilot.c
sim.CFLAGS += -DAHRS_ALIGNER_LED=3
diff --git a/conf/autopilot/subsystems/booz2_gps.makefile b/conf/autopilot/subsystems/booz2_gps.makefile
index 7ba168f5da..cdf5cd35e2 100644
--- a/conf/autopilot/subsystems/booz2_gps.makefile
+++ b/conf/autopilot/subsystems/booz2_gps.makefile
@@ -2,3 +2,7 @@
ap.srcs += $(SRC_BOOZ)/booz2_gps.c
ap.CFLAGS += -DUSE_UART0 -DUART0_BAUD=B38400 -DUART0_VIC_SLOT=5
ap.CFLAGS += -DUSE_GPS -DGPS_LINK=Uart0 -DGPS_LED=4
+
+
+sim.CFLAGS += -DUSE_GPS
+sim.srcs += $(SRC_BOOZ)/booz2_gps.c
diff --git a/conf/telemetry/telemetry_booz2.xml b/conf/telemetry/telemetry_booz2.xml
index a0640e471b..63fcc7e702 100644
--- a/conf/telemetry/telemetry_booz2.xml
+++ b/conf/telemetry/telemetry_booz2.xml
@@ -13,6 +13,7 @@
+
diff --git a/sw/airborne/arm7/init_hw.h b/sw/airborne/arm7/init_hw.h
index 65af3dcc5e..dfa653caad 100644
--- a/sw/airborne/arm7/init_hw.h
+++ b/sw/airborne/arm7/init_hw.h
@@ -37,7 +37,7 @@
#ifdef PERIPHERALS_AUTO_INIT
-#ifdef LED
+#if defined LED || defined USE_LED
#include "led.h"
#endif
#if defined USE_UART0 || defined USE_UART1
@@ -110,7 +110,7 @@ static inline void hw_init(void) {
#ifdef PERIPHERALS_AUTO_INIT
-#ifdef LED
+#if defined LED || defined USE_LED
led_init();
#endif
#ifdef USE_UART0
diff --git a/sw/airborne/booz/booz2_gps.c b/sw/airborne/booz/booz2_gps.c
index 43ea2d99b0..5eb815f366 100644
--- a/sw/airborne/booz/booz2_gps.c
+++ b/sw/airborne/booz/booz2_gps.c
@@ -27,13 +27,17 @@
struct Booz_gps_state booz_gps_state;
+#ifdef SITL
+bool_t booz_gps_available;
+#endif
/* misc */
-volatile bool_t booz2_gps_msg_received;
volatile uint8_t booz2_gps_nb_ovrn;
void booz2_gps_init(void) {
-
+#ifdef SITL
+ booz_gps_available = FALSE;
+#endif
booz_gps_state.fix = BOOZ2_GPS_FIX_NONE;
#ifdef GPS_LED
LED_ON(GPS_LED);
@@ -47,17 +51,17 @@ void booz2_gps_read_ubx_message(void) {
if (ubx_class == UBX_NAV_ID) {
if (ubx_id == UBX_NAV_SOL_ID) {
- booz_gps_state.fix = UBX_NAV_SOL_GPSfix(ubx_msg_buf);
- booz_gps_state.ecef_pos.x = UBX_NAV_SOL_ECEF_X(ubx_msg_buf);
- booz_gps_state.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(ubx_msg_buf);
- booz_gps_state.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(ubx_msg_buf);
- booz_gps_state.pacc = UBX_NAV_SOL_Pacc(ubx_msg_buf);
- booz_gps_state.ecef_speed.x = UBX_NAV_SOL_ECEFVX(ubx_msg_buf);
- booz_gps_state.ecef_speed.y = UBX_NAV_SOL_ECEFVY(ubx_msg_buf);
- booz_gps_state.ecef_speed.z = UBX_NAV_SOL_ECEFVZ(ubx_msg_buf);
- booz_gps_state.sacc = UBX_NAV_SOL_Sacc(ubx_msg_buf);
- booz_gps_state.pdop = UBX_NAV_SOL_PDOP(ubx_msg_buf);
- booz_gps_state.num_sv = UBX_NAV_SOL_numSV(ubx_msg_buf);
+ booz_gps_state.fix = UBX_NAV_SOL_GPSfix(ubx_msg_buf);
+ booz_gps_state.ecef_pos.x = UBX_NAV_SOL_ECEF_X(ubx_msg_buf);
+ booz_gps_state.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(ubx_msg_buf);
+ booz_gps_state.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(ubx_msg_buf);
+ booz_gps_state.pacc = UBX_NAV_SOL_Pacc(ubx_msg_buf);
+ booz_gps_state.ecef_vel.x = UBX_NAV_SOL_ECEFVX(ubx_msg_buf);
+ booz_gps_state.ecef_vel.y = UBX_NAV_SOL_ECEFVY(ubx_msg_buf);
+ booz_gps_state.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(ubx_msg_buf);
+ booz_gps_state.sacc = UBX_NAV_SOL_Sacc(ubx_msg_buf);
+ booz_gps_state.pdop = UBX_NAV_SOL_PDOP(ubx_msg_buf);
+ booz_gps_state.num_sv = UBX_NAV_SOL_numSV(ubx_msg_buf);
#ifdef GPS_LED
if (booz_gps_state.fix == BOOZ2_GPS_FIX_3D) {
LED_OFF(GPS_LED);
diff --git a/sw/airborne/booz/booz2_gps.h b/sw/airborne/booz/booz2_gps.h
index 4d9e3de88c..fffce82cd7 100644
--- a/sw/airborne/booz/booz2_gps.h
+++ b/sw/airborne/booz/booz2_gps.h
@@ -29,7 +29,7 @@
struct Booz_gps_state {
struct EcefCoor_i ecef_pos; /* pos ECEF in cm */
- struct EcefCoor_i ecef_speed; /* speed ECEF in cm/s */
+ struct EcefCoor_i ecef_vel; /* speed ECEF in cm/s */
uint32_t pacc; /* position accuracy */
uint32_t sacc; /* speed accuracy */
uint16_t pdop; /* dilution of precision */
@@ -48,12 +48,26 @@ extern struct Booz_gps_state booz_gps_state;
#include "ubx_protocol.h"
#ifdef SITL
+extern bool_t booz_gps_available;
#define GPS_LINKChAvailable() (FALSE)
#define GPS_LINKGetch() (TRUE)
-#define Booz2GpsEvent(_sol_available_callback) { \
- _sol_available_callback(); \
+#include "nps_sensors.h"
+#define booz_gps_feed_value() { \
+ booz_gps_state.ecef_pos.x = sensors.gps.ecef_pos.x * 100.; \
+ booz_gps_state.ecef_pos.y = sensors.gps.ecef_pos.y * 100.; \
+ booz_gps_state.ecef_pos.z = sensors.gps.ecef_pos.z * 100.; \
+ booz_gps_state.ecef_vel.x = sensors.gps.ecef_vel.x * 100.; \
+ booz_gps_state.ecef_vel.y = sensors.gps.ecef_vel.y * 100.; \
+ booz_gps_state.ecef_vel.z = sensors.gps.ecef_vel.z * 100.; \
+ booz_gps_available = TRUE; \
}
-#else /* not SITL */
+#define Booz2GpsEvent(_sol_available_callback) { \
+ if (booz_gps_available) { \
+ _sol_available_callback(); \
+ booz_gps_available = FALSE; \
+ } \
+ }
+#else /* ! SITL */
#define Booz2GpsEvent(_sol_available_callback) { \
if (GpsBuffer()) { \
ReadGpsBuffer(); \
diff --git a/sw/airborne/booz/booz2_ins.c b/sw/airborne/booz/booz2_ins.c
index 081cf60d91..2afb318a39 100644
--- a/sw/airborne/booz/booz2_ins.c
+++ b/sw/airborne/booz/booz2_ins.c
@@ -134,7 +134,7 @@ void booz_ins_update_gps(void) {
booz_ins_ltp_initialised = TRUE;
}
ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_pos);
- ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_speed);
+ ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
#ifdef USE_HFF
b2ins_update_gps();
VECT2_SDIV(booz_ins_ltp_pos, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)), b2ins_pos_ltp);
diff --git a/sw/airborne/booz/booz2_telemetry.h b/sw/airborne/booz/booz2_telemetry.h
index 24a339fc3b..2a76166cb9 100644
--- a/sw/airborne/booz/booz2_telemetry.h
+++ b/sw/airborne/booz/booz2_telemetry.h
@@ -475,9 +475,9 @@ define PERIODIC_SEND_BOOZ2_VFF() {}
DOWNLINK_SEND_BOOZ2_GPS( &booz_gps_state.ecef_pos.x, \
&booz_gps_state.ecef_pos.y, \
&booz_gps_state.ecef_pos.z, \
- &booz_gps_state.ecef_speed.x, \
- &booz_gps_state.ecef_speed.y, \
- &booz_gps_state.ecef_speed.z, \
+ &booz_gps_state.ecef_vel.x, \
+ &booz_gps_state.ecef_vel.y, \
+ &booz_gps_state.ecef_vel.z, \
&booz_gps_state.pacc, \
&booz_gps_state.sacc, \
&booz_gps_state.pdop, \
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index e79a962969..f6e50b6fee 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -52,6 +52,8 @@ struct Int16Vect3 {
#define INT32_SPEED_OF_CM_S_NUM 41943
#define INT32_SPEED_OF_CM_S_DEN 8
+#define INT32_MAG_FRAC 11
+
#define INT32_ACCEL_FRAC 10
struct Int32Vect2 {
int32_t x;
diff --git a/sw/airborne/math/pprz_geodetic_double.c b/sw/airborne/math/pprz_geodetic_double.c
index d8fe9c95c1..91b4bcc2aa 100644
--- a/sw/airborne/math/pprz_geodetic_double.c
+++ b/sw/airborne/math/pprz_geodetic_double.c
@@ -25,31 +25,6 @@ void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef) {
}
-void ecef_of_enu_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu) {
- MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu);
- VECT3_ADD(*ecef, def->ecef);
-}
-
-void ecef_of_ned_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned) {
- struct EnuCoor_d enu;
- ENU_OF_TO_NED(enu, *ned);
- ecef_of_enu_point_d(ecef, def, &enu);
-}
-
-void ecef_of_enu_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu) {
- MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu);
-}
-
-void ecef_of_ned_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned) {
- struct EnuCoor_d enu;
- ENU_OF_TO_NED(enu, *ned);
- ecef_of_enu_vect_d(ecef, def, &enu);
-}
-
-
-
-
-
/* http://en.wikipedia.org/wiki/Geodetic_system */
void lla_of_ecef_d(struct LlaCoor_d* lla, struct EcefCoor_d* ecef) {
@@ -85,3 +60,53 @@ void lla_of_ecef_d(struct LlaCoor_d* lla, struct EcefCoor_d* ecef) {
lla->lon = atan2(ecef->y,ecef->x);
}
+
+void enu_of_ecef_point_d(struct EnuCoor_d* enu, struct LtpDef_d* def, struct EcefCoor_d* ecef) {
+ struct EcefCoor_d delta;
+ VECT3_DIFF(delta, *ecef, def->ecef);
+ MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, delta);
+}
+
+void ned_of_ecef_point_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef) {
+ struct EnuCoor_d enu;
+ enu_of_ecef_point_d(&enu, def, ecef);
+ ENU_OF_TO_NED(*ned, enu);
+}
+
+void enu_of_ecef_vect_d(struct EnuCoor_d* enu, struct LtpDef_d* def, struct EcefCoor_d* ecef) {
+ MAT33_VECT3_MUL(*enu, def->ltp_of_ecef, *ecef);
+}
+
+void ned_of_ecef_vect_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef) {
+ struct EnuCoor_d enu;
+ enu_of_ecef_vect_d(&enu, def, ecef);
+ ENU_OF_TO_NED(*ned, enu);
+}
+
+
+
+void ecef_of_enu_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu) {
+ MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu);
+ VECT3_ADD(*ecef, def->ecef);
+}
+
+void ecef_of_ned_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned) {
+ struct EnuCoor_d enu;
+ ENU_OF_TO_NED(enu, *ned);
+ ecef_of_enu_point_d(ecef, def, &enu);
+}
+
+void ecef_of_enu_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu) {
+ MAT33_VECT3_TRANSP_MUL(*ecef, def->ltp_of_ecef, *enu);
+}
+
+void ecef_of_ned_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned) {
+ struct EnuCoor_d enu;
+ ENU_OF_TO_NED(enu, *ned);
+ ecef_of_enu_vect_d(ecef, def, &enu);
+}
+
+
+
+
+
diff --git a/sw/airborne/math/pprz_geodetic_double.h b/sw/airborne/math/pprz_geodetic_double.h
index e1bb0f6a1e..cfae929cde 100644
--- a/sw/airborne/math/pprz_geodetic_double.h
+++ b/sw/airborne/math/pprz_geodetic_double.h
@@ -42,8 +42,16 @@ struct LtpDef_d {
extern void ltp_def_from_ecef_d(struct LtpDef_d* def, struct EcefCoor_d* ecef);
extern void lla_of_ecef_d(struct LlaCoor_d* out, struct EcefCoor_d* in);
+
+extern void enu_of_ecef_point_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef);
+extern void ned_of_ecef_point_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef);
+
+extern void enu_of_ecef_vect_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef);
+extern void ned_of_ecef_vect_d(struct NedCoor_d* ned, struct LtpDef_d* def, struct EcefCoor_d* ecef);
+
extern void ecef_of_enu_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu);
extern void ecef_of_ned_point_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned);
+
extern void ecef_of_enu_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct EnuCoor_d* enu);
extern void ecef_of_ned_vect_d(struct EcefCoor_d* ecef, struct LtpDef_d* def, struct NedCoor_d* ned);
diff --git a/sw/simulator/nps/nps_autopilot_booz.c b/sw/simulator/nps/nps_autopilot_booz.c
index 95c3a09f01..3002049f1a 100644
--- a/sw/simulator/nps/nps_autopilot_booz.c
+++ b/sw/simulator/nps/nps_autopilot_booz.c
@@ -27,6 +27,7 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha
}
#include
+#include "booz2_gps.h"
void nps_autopilot_run_step(double time __attribute__ ((unused))) {
@@ -53,6 +54,11 @@ void nps_autopilot_run_step(double time __attribute__ ((unused))) {
sim_overwrite_ahrs();
#endif /* BYPASS_AHRS */
+ if (nps_sensors_gps_available()) {
+ booz_gps_feed_value();
+ booz2_main_event();
+ }
+
booz2_main_periodic();
/* 25 */
@@ -94,14 +100,14 @@ static void sim_overwrite_ahrs(void) {
// printf("%f\n", fdm.ltpprz_to_body_eulers.phi);
- booz_ahrs.ltp_to_body_euler.phi = ANGLE_BFP_OF_REAL(fdm.ltpprz_to_body_eulers.phi);
- booz_ahrs.ltp_to_body_euler.theta = ANGLE_BFP_OF_REAL(fdm.ltpprz_to_body_eulers.theta);
- booz_ahrs.ltp_to_body_euler.psi = ANGLE_BFP_OF_REAL(fdm.ltpprz_to_body_eulers.psi);
+ booz_ahrs.ltp_to_body_euler.phi = ANGLE_BFP_OF_REAL(fdm.ltp_to_body_eulers.phi);
+ booz_ahrs.ltp_to_body_euler.theta = ANGLE_BFP_OF_REAL(fdm.ltp_to_body_eulers.theta);
+ booz_ahrs.ltp_to_body_euler.psi = ANGLE_BFP_OF_REAL(fdm.ltp_to_body_eulers.psi);
- booz_ahrs.ltp_to_body_quat.qi = QUAT1_BFP_OF_REAL(fdm.ltpprz_to_body_quat.qi);
- booz_ahrs.ltp_to_body_quat.qx = QUAT1_BFP_OF_REAL(fdm.ltpprz_to_body_quat.qx);
- booz_ahrs.ltp_to_body_quat.qy = QUAT1_BFP_OF_REAL(fdm.ltpprz_to_body_quat.qy);
- booz_ahrs.ltp_to_body_quat.qz = QUAT1_BFP_OF_REAL(fdm.ltpprz_to_body_quat.qz);
+ booz_ahrs.ltp_to_body_quat.qi = QUAT1_BFP_OF_REAL(fdm.ltp_to_body_quat.qi);
+ booz_ahrs.ltp_to_body_quat.qx = QUAT1_BFP_OF_REAL(fdm.ltp_to_body_quat.qx);
+ booz_ahrs.ltp_to_body_quat.qy = QUAT1_BFP_OF_REAL(fdm.ltp_to_body_quat.qy);
+ booz_ahrs.ltp_to_body_quat.qz = QUAT1_BFP_OF_REAL(fdm.ltp_to_body_quat.qz);
booz_ahrs.body_rate.p = RATE_BFP_OF_REAL(fdm.body_ecef_rotvel.p);
booz_ahrs.body_rate.q = RATE_BFP_OF_REAL(fdm.body_ecef_rotvel.q);
diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h
index 9947d43da9..c8f75accad 100644
--- a/sw/simulator/nps/nps_fdm.h
+++ b/sw/simulator/nps/nps_fdm.h
@@ -21,24 +21,34 @@ struct NpsFdm {
double time;
bool_t on_ground;
+ /* position */
struct EcefCoor_d ecef_pos;
- // struct EcefCoor_d ecef_inertial_vel;
- // struct EcefCoor_d ecef_inertial_accel;
- struct EcefCoor_d ecef_ecef_vel;
- struct EcefCoor_d ecef_ecef_accel;
- struct DoubleVect3 body_ecef_vel; /* aka UVW */
- struct DoubleVect3 body_ecef_accel;
-
struct NedCoor_d ltpprz_pos;
- struct NedCoor_d ltpprz_ecef_vel;
-
- struct NedCoor_d ltp_ecef_vel;
struct LlaCoor_d lla_pos;
+ /* 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 */
+ struct EcefCoor_d ecef_ecef_vel;
+ struct EcefCoor_d ecef_ecef_accel;
+ /* velocity and acceleration wrt ecef frame expressed in body frame */
+ struct DoubleVect3 body_ecef_vel; /* aka UVW */
+ struct DoubleVect3 body_ecef_accel;
+ /* velocity and acceleration wrt ecef frame expressed in ltp frame */
+ struct NedCoor_d ltp_ecef_vel;
+ struct NedCoor_d ltp_ecef_accel;
+ /* velocity and acceleration wrt ecef frame expressed in ltppprz frame */
+ struct NedCoor_d ltpprz_ecef_vel;
+ struct NedCoor_d ltpprz_ecef_accel;
+
+ /* attitude */
struct DoubleQuat ltp_to_body_quat;
struct DoubleEulers ltp_to_body_eulers;
struct DoubleQuat ltpprz_to_body_quat;
struct DoubleEulers ltpprz_to_body_eulers;
+
+ /* velocity and acceleration wrt ecef frame expressed in body frame */
struct DoubleRates body_ecef_rotvel;
struct DoubleRates body_ecef_rotaccel;
diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c
index 84fb864f9e..0346c83653 100644
--- a/sw/simulator/nps/nps_fdm_jsbsim.c
+++ b/sw/simulator/nps/nps_fdm_jsbsim.c
@@ -19,20 +19,19 @@ static void feed_jsbsim(double* commands);
static void fetch_state(void);
static void jsbsimvec_to_vec(DoubleVect3* fdm_vector, const FGColumnVector3* jsb_vector);
-static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, FGLocation* jsb_location);
-static void jsbsimquat_to_quat(DoubleQuat* fdm_quat, FGQuaternion* jsb_quat);
+static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, const FGLocation* jsb_location);
+static void jsbsimquat_to_quat(DoubleQuat* fdm_quat, const FGQuaternion* jsb_quat);
static void jsbsimvec_to_rate(DoubleRates* fdm_rate, const FGColumnVector3* jsb_vector);
//static void jsbsimloc_to_lla(LlaCoor_d* fdm_lla, FGLocation* jsb_location);
//static void rate_to_vec(DoubleVect3* vector, DoubleRates* rate);
-static void ltpdef_copy(struct LtpDef_f* ltpdef_f, struct LtpDef_d* ltpdef_d);
static void test123(LlaCoor_d* fdm_lla, FGPropagate* propagate);
static void init_jsbsim(double dt);
static void init_ltp(void);
struct NpsFdm fdm;
-FGFDMExec* FDMExec;
-struct LtpDef_f ltpdef;
+static FGFDMExec* FDMExec;
+static struct LtpDef_d ltpdef;
void nps_fdm_init(double dt) {
@@ -75,50 +74,66 @@ static void fetch_state(void) {
FGPropertyManager* node = FDMExec->GetPropertyManager()->GetNode("sim-time-sec");
fdm.time = node->getDoubleValue();
- // printf("%f\n", fdm.time);
-
- /* Commented are the calculus of acceleration in the case where
- jsbsim does not consider body rotational velocity */
-
- FGPropagate* propagate;
- FGPropagate::VehicleState* VState;
- // DoubleVect3 noninertial_accel;
- // DoubleVect3 dummy_vector;
-
- propagate = FDMExec->GetPropagate();
- VState = propagate->GetVState();
+ FGPropagate* propagate = FDMExec->GetPropagate();
fdm.on_ground = FDMExec->GetGroundReactions()->GetWOW();
- jsbsimloc_to_loc(&fdm.ecef_pos,&VState->vLocation);
- jsbsimvec_to_vec(&fdm.body_ecef_vel,&VState->vUVW);
- jsbsimvec_to_vec(&fdm.body_ecef_accel,&propagate->GetUVWdot());
- //jsbsimvec_to_vec(&noninertial_accel,&propagate->GetUVWdot());
-
- /* attitude */
- jsbsimquat_to_quat(&fdm.ltp_to_body_quat,&VState->vQtrn);
- /* convert to eulers */
- DOUBLE_EULERS_OF_QUAT(fdm.ltp_to_body_eulers, fdm.ltp_to_body_quat);
- jsbsimvec_to_rate(&fdm.body_ecef_rotvel,&VState->vPQR);
- jsbsimvec_to_rate(&fdm.body_ecef_rotaccel,&propagate->GetPQRdot());
- /* JSBSIM seems to call GetPQRdot body_ecef_rotaccel */
- // rate_to_vec(&dummy_vector,&fdm.body_ecef_rotvel);
- // DOUBLE_VECT3_CROSS_PRODUCT(fdm.body_ecef_accel,dummy_vector,fdm.body_ecef_vel);
- // DOUBLE_VECT3_SUM(fdm.body_ecef_accel,fdm.body_ecef_accel,noninertial_accel)
+ /*
+ * position
+ */
+ jsbsimloc_to_loc(&fdm.ecef_pos,&propagate->GetLocation());
- struct EcefCoor_f ecefpos_f;
- VECT3_COPY(ecefpos_f, fdm.ecef_pos);
- struct NedCoor_f ned;
- ned_of_ecef_point_f(&ned, <pdef, &ecefpos_f);
- VECT3_COPY(fdm.ltpprz_pos,ned);
+ /*
+ * linear speed and accelerations
+ */
+
+ /* in body frame */
+ 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 = propagate->GetUVWdot();
+ jsbsimvec_to_vec(&fdm.body_ecef_accel,&fg_body_ecef_accel);
+
+ /* in LTP frame */
+ const FGMatrix33& body_to_ltp = propagate->GetTb2l();
+ const FGColumnVector3& fg_ltp_ecef_vel = body_to_ltp * fg_body_ecef_vel;
+ jsbsimvec_to_vec((DoubleVect3*)&fdm.ltp_ecef_vel, &fg_ltp_ecef_vel);
+ 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);
+
+ /* in ECEF frame */
+ const FGMatrix33& body_to_ecef = propagate->GetTb2ec();
+ 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);
+ 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);
+
+ /* in LTP pprz */
+ 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);
+
+ /* lla */
//jsbsimloc_to_lla(&fdm.lla_pos, &VState->vLocation);
test123(&fdm.lla_pos, propagate);
+ /*
+ * attitude
+ */
+ const FGQuaternion jsb_quat = propagate->GetQuaternion();
+ jsbsimquat_to_quat(&fdm.ltp_to_body_quat, &jsb_quat);
+ /* convert to eulers */
+ DOUBLE_EULERS_OF_QUAT(fdm.ltp_to_body_eulers, fdm.ltp_to_body_quat);
/* the "false" pprz lpt */
- /* FIXME: use jsbsim lpt for now */
+ /* FIXME: use jsbsim ltp for now */
EULERS_COPY(fdm.ltpprz_to_body_eulers, fdm.ltp_to_body_eulers);
QUAT_COPY(fdm.ltpprz_to_body_quat, fdm.ltp_to_body_quat);
+
+ /*
+ * rotational speed and accelerations
+ */
+ jsbsimvec_to_rate(&fdm.body_ecef_rotvel,&propagate->GetPQR());
+ jsbsimvec_to_rate(&fdm.body_ecef_rotaccel,&propagate->GetPQRdot());
}
@@ -175,14 +190,12 @@ static void init_ltp(void) {
FGPropagate* propagate;
FGPropagate::VehicleState* VState;
- struct LtpDef_d ltpdef_d;
propagate = FDMExec->GetPropagate();
VState = propagate->GetVState();
jsbsimloc_to_loc(&fdm.ecef_pos,&VState->vLocation);
- ltp_def_from_ecef_d(<pdef_d,&fdm.ecef_pos);
- ltpdef_copy(<pdef, <pdef_d);
+ ltp_def_from_ecef_d(<pdef,&fdm.ecef_pos);
fdm.ltp_g.x = 0.;
fdm.ltp_g.y = 0.;
@@ -195,7 +208,7 @@ static void init_ltp(void) {
}
-static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, FGLocation* jsb_location){
+static void jsbsimloc_to_loc(EcefCoor_d* fdm_location, const FGLocation* jsb_location){
fdm_location->x = jsb_location->Entry(1);
fdm_location->y = jsb_location->Entry(2);
@@ -211,7 +224,7 @@ static void jsbsimvec_to_vec(DoubleVect3* fdm_vector, const FGColumnVector3* jsb
}
-static void jsbsimquat_to_quat(DoubleQuat* fdm_quat, FGQuaternion* jsb_quat){
+static void jsbsimquat_to_quat(DoubleQuat* fdm_quat, const FGQuaternion* jsb_quat){
fdm_quat->qi = jsb_quat->Entry(1);
fdm_quat->qx = jsb_quat->Entry(2);
@@ -246,10 +259,3 @@ void jsbsimloc_to_lla(LlaCoor_d* fdm_lla, FGLocation* jsb_location) {
}
#endif
-void ltpdef_copy(struct LtpDef_f* ltpdef_f, struct LtpDef_d* ltpdef_d) {
-
- VECT3_COPY(ltpdef_f->ecef, ltpdef_d->ecef);
- LLA_COPY(ltpdef_f->lla, ltpdef_d->lla);
- MAT33_COPY(ltpdef_f->ltp_of_ecef, ltpdef_d->ltp_of_ecef);
-
-}