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

fixing the sim

Browse Source
This commit is contained in:
Antoine Drouin
2009-07-27 17:04:15 +00:00
parent cd45978f01
commit 063d502513
15 changed files with 195 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/autopilot/booz2_simulator.makefile
-1
View File
@@ -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
conf/autopilot/booz2_simulator_nps.makefile
-2
View File
@@ -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
conf/autopilot/subsystems/booz2_gps.makefile
+4
View File
@@ -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
conf/telemetry/telemetry_booz2.xml
+1
View File
@@ -13,6 +13,7 @@
<message name="BOOZ2_NAV_REF" period="5."/>
<message name="BOOZ2_NAV_STATUS" period="1.6"/>
<message name="WP_MOVED" period="1.3"/>
<message name="BOOZ2_GPS" period=".25"/>
</mode>
<mode name="ppm">
sw/airborne/arm7/init_hw.h
+2 -2
View File
@@ -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
sw/airborne/booz/booz2_gps.c
+17 -13
View File
@@ -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);
sw/airborne/booz/booz2_gps.h
+18 -4
View File
@@ -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(); \
sw/airborne/booz/booz2_ins.c
+1 -1
View File
@@ -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);
sw/airborne/booz/booz2_telemetry.h
+3 -3
View File
@@ -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, \
sw/airborne/math/pprz_algebra_int.h
+2
View File
@@ -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;
sw/airborne/math/pprz_geodetic_double.c
+50 -25
View File
@@ -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);
}
sw/airborne/math/pprz_geodetic_double.h
+8
View File
@@ -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);
sw/simulator/nps/nps_autopilot_booz.c
+13 -7
View File
@@ -27,6 +27,7 @@ void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, cha
}
#include <stdio.h>
#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);
sw/simulator/nps/nps_fdm.h
+20 -10
View File
@@ -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;
sw/simulator/nps/nps_fdm_jsbsim.c
+56 -50
View File
@@ -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, &ltpdef, &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, &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);
/* 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(&ltpdef_d,&fdm.ecef_pos);
ltpdef_copy(&ltpdef, &ltpdef_d);
ltp_def_from_ecef_d(&ltpdef,&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);
}