diff --git a/sw/simulator/Makefile b/sw/simulator/Makefile index 774f2b0710..f023b22962 100644 --- a/sw/simulator/Makefile +++ b/sw/simulator/Makefile @@ -82,6 +82,14 @@ diffusion : stdlib.cmo diffusion.cmo clean : rm -f *.cm* *~ *.out .depend *.o *.a *.so gaia simhitl booz_sim + +# +# NPS +# +JSBSIM_SRC = /home/gustavo... + + + # # Dependencies # diff --git a/sw/simulator/booz_flight_model.h b/sw/simulator/booz_flight_model.h index 41935faefa..6fdbc3cd67 100644 --- a/sw/simulator/booz_flight_model.h +++ b/sw/simulator/booz_flight_model.h @@ -20,6 +20,7 @@ struct BoozFlightModel { VEC* state; /* user products */ + VEC* pos_ecef; VEC* pos_ltp; VEC* speed_ltp; VEC* accel_ltp; diff --git a/sw/simulator/nps_fdm.h b/sw/simulator/nps_fdm.h new file mode 100644 index 0000000000..506643e1bf --- /dev/null +++ b/sw/simulator/nps_fdm.h @@ -0,0 +1,80 @@ +#ifndef NPS_FDM +#define NPS_FDM + +#include "std.h" +#include + + + + +struct NpsFdmState { + + // generic vehicle state + bool_t on_ground; + + VEC* ecef_pos; + VEC* ecef_speed; + VEC* ecef_accel; + + VEC* ltp_to_body_quat; + VEC* ltp_body_rate; + VEC* ltp_body_accel; + + VEC* ecef_to_ltp_quat; + + // vehicle specific + union { + struct NpsDummy dummy; + struct NpsQuad quad; + struct NpsSFW fw; + }; + +}; + + + +struct NpsDummy { + /* force applied to the ball in Newton */ + double f_input; +}; + + +#define NPS_QUAD_MOTOR_FRONT 0 +#define NPS_QUAD_MOTOR_BACK 1 +#define NPS_QUAD_MOTOR_RIGHT 2 +#define NPS_QUAD_MOTOR_LEFT 3 +#define NPS_QUAD_MOTOR_NB 4 + +struct NpsQuad { + /* battery voltage in V */ + double battery; + /* motor commands [0:1] + VEC* motor_commands; + /* propeller rotation speed in rad/s */ + VEC* prop_omega; +}; + + + +#define NPS_SFW_ACTUATOR_THROTTLE 0 +#define NPS_SFW_ACTUATOR_AILERON_R 1 +#define NPS_SFW_ACTUATOR_AILERON_L 2 +#define NPS_SFW_ACTUATOR_NB 3 + +struct NpsSFW { + + double prop_omega; + /* + deflection of control surfaces in radian + normalized throttle [0:1] + */ + VEC* actuators; +} + + + + + + + +#endif /* NPS_FDM */ diff --git a/sw/simulator/nps_test1.c b/sw/simulator/nps_test1.c new file mode 100644 index 0000000000..ddcb266573 --- /dev/null +++ b/sw/simulator/nps_test1.c @@ -0,0 +1,41 @@ +#include "nps_fdm.h" + + +static struct NpsFdmState fdm_state; + + + +static void feed_inputs(JSBSim::FGFDMExec* fdmex) { + + double hover_force = 0.; + + fdmex->SetPropertyValue("/fdm/jsbsim/fcs/force_front", fdm_state.dummy.f_input); + + +} + +static void fetch_state(JSBSim::FGFDMExec* fdmex) { + + // double foo = fdmex->GetPropagate()->GetInertialVelocityMagnitude(); + // double foo = fdmex->GetPropagate()->GetGeodLatitudeRad(); + // cerr << "lat " << foo << endl ; + const JSBSim::FGColumnVector3 vel = fdmex->GetPropagate()->GetVel(); + cerr << "vel " << vel << endl ; + + // eulers->ve[EULER_PHI] = vel + + + // fdm_state.ecef_pos = + +} + + + + +int main(int argc, char** argv) { + + + + +} +