diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h index ef444f46ca..88ade0089b 100644 --- a/sw/simulator/nps/nps_fdm.h +++ b/sw/simulator/nps/nps_fdm.h @@ -106,6 +106,12 @@ struct NpsFdm { struct DoubleVect3 wind; ///< velocity in m/s in NED + double airspeed; ///< equivalent airspeed in m/s + double pressure; ///< current (static) atmospheric pressure in Pascal + double total_pressure; ///< total atmospheric pressure in Pascal + double dynamic_pressure; ///< dynamic pressure in Pascal + double temperature; ///< current temperature in degrees Celcius + // Control surface positions (normalized values) float elevator; float flap; diff --git a/sw/simulator/nps/nps_fdm_crrcsim.c b/sw/simulator/nps/nps_fdm_crrcsim.c index ecb211db71..35af2bb807 100644 --- a/sw/simulator/nps/nps_fdm_crrcsim.c +++ b/sw/simulator/nps/nps_fdm_crrcsim.c @@ -120,6 +120,10 @@ void nps_fdm_init(double dt) fdm.init_dt = dt; fdm.curr_dt = dt; fdm.nan_count = 0; + fdm.pressure = -1; + fdm.total_pressure = -1; + fdm.dynamic_pressure = -1; + fdm.temperature = -1; init_ltp(); @@ -385,6 +389,13 @@ static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer) fdm->ltp_ecef_vel = vel; ecef_of_ned_vect_d(&fdm->ecef_ecef_vel, <pdef, &vel); + /* No airspeed from CRRCSIM? + * use ground speed for now, since we also don't know wind + */ + struct DoubleVect3 ltp_airspeed; + VECT3_COPY(ltp_airspeed, vel); + fdm.airspeed = double_vect3_norm(<p_airspeed); + /* gps position (1e7 deg to rad and 1e3 m to m) */ struct LlaCoor_d pos; pos.lon = (double)LongOfBuf(buffer, 15) * 1.74533e-9; diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp index 5a14828f99..46e1293fd8 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.cpp +++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp @@ -40,6 +40,8 @@ #include #include #include +#include +#include #include #include @@ -67,6 +69,9 @@ #define MetersOfFeet(_f) ((_f)/3.2808399) #define FeetOfMeters(_m) ((_m)*3.2808399) +#define PascalOfPsf(_p) ((_p) * 47.8802588889) +#define CelsiusOfRankine(_r) (((_r) - 491.67) / 1.8) + /** Name of the JSBSim model. * Defaults to the AIRFRAME_NAME */ @@ -436,6 +441,15 @@ static void fetch_state(void) const FGColumnVector3 &fg_wind_ned = FDMExec->GetWinds()->GetTotalWindNED(); jsbsimvec_to_vec(&fdm.wind, &fg_wind_ned); + /* + * Equivalent Airspeed, atmospheric pressure and temperature. + */ + fdm.airspeed = MetersOfFeet(FDMExec->GetAuxiliary()->GetVequivalentFPS()); + fdm.pressure = PascalOfPsf(FDMExec->GetAtmosphere()->GetPressure()); + fdm.total_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->GetTotalPressure()); + fdm.dynamic_pressure = PascalOfPsf(FDMExec->GetAuxiliary()->Getqbar()); + fdm.temperature = CelsiusOfRankine(FDMExec->GetAtmosphere()->GetTemperature()); + /* * Control surface positions * diff --git a/sw/simulator/nps/nps_sensor_airspeed.c b/sw/simulator/nps/nps_sensor_airspeed.c index 96449452bc..bf3ebfcb82 100644 --- a/sw/simulator/nps/nps_sensor_airspeed.c +++ b/sw/simulator/nps/nps_sensor_airspeed.c @@ -67,14 +67,8 @@ void nps_sensor_airspeed_run_step(struct NpsSensorAirspeed *airspeed, double tim return; } - /* super simple approximation for now: - * airspeed = ground speed + wind - */ - struct DoubleVect3 ltp_air_vel; - VECT3_SUM(ltp_air_vel, fdm.ltpprz_ecef_vel, fdm.wind); - double speed = double_vect3_norm(<p_air_vel); - /* sensor offset */ - airspeed->value = speed + airspeed->offset; + /* equivalent airspeed + sensor offset */ + airspeed->value = fdm.airspeed + airspeed->offset; /* add noise with std dev meters/second */ airspeed->value += get_gaussian_noise() * airspeed->noise_std_dev; /* can't be negative, min is zero */