diff --git a/sw/simulator/sim_ac_fw.c b/sw/simulator/sim_ac_fw.c index f3e0a02f99..dca40c5395 100644 --- a/sw/simulator/sim_ac_fw.c +++ b/sw/simulator/sim_ac_fw.c @@ -22,6 +22,7 @@ * */ +#include #include "sim_ac_jsbsim.h" #include "main_ap.h" #include "main_fbw.h" @@ -46,293 +47,46 @@ void autopilot_event_task(void) { event_task_fbw(); } +bool check_crash_jsbsim(JSBSim::FGFDMExec* FDMExec) { + + //gear/unit/WOW + return false; + +} void copy_inputs_to_jsbsim(JSBSim::FGFDMExec* FDMExec) { - FGPropagate* Propagate; - - Propagate = FDMExec->GetPropagate(); - - // FDMExec->SetPropertyValue(CommandLineProperties[i], CommandLinePropertyValues[i]); - - cout << "Input " << Propagate->GetEuler(FGJSBBase::ePhi) << endl; - cout << "Input " << Propagate->GetEuler(FGJSBBase::eTht) << endl; - cout << "Input " << Propagate->GetEuler(FGJSBBase::ePsi) << endl; + //fcs/front_motor + //fcs/back_motor + //fcs/right_motor + //fcs/left_motor } void copy_outputs_from_jsbsim(JSBSim::FGFDMExec* FDMExec) { FGPropertyManager* cur_node; - double time; + double cur_value, factor=1; + char buf[64]; + const char* state[] = {"sim-time-sec", + "lat-gc-deg","long-gc-deg","h-sl-ft", + "u-fps","v-fps","w-fps", + "theta-deg","phi-deg","psi-true-deg", + "p-rad_sec","q-rad_sec","r-rad_sec"}; + int i=0; cur_node = FDMExec->GetPropertyManager()->GetNode("sim-time-sec"); - time = cur_node->getDoubleValue(); + cur_value = cur_node->getDoubleValue(); + cout << state[i] << cur_value << endl; + + for (i=1; i<12+1; i++) { + sprintf(buf,"ic/%s",state[i]); + if (strstr(state[i],"rad_")!=NULL) factor=RAD2DEG; + if (strstr(state[i],"fps")!=NULL || strstr(state[i],"ft")!=NULL) factor=FT2M; + cur_node = FDMExec->GetPropertyManager()->GetNode(buf); + cur_value = factor*(cur_node->getDoubleValue()); + cout << state[i] << " " << cur_value << endl; + } - cout << "Time " << time << endl; - printf("%f \n",time); } -/* -// Convert from the JSBsim generic_ struct to the FGInterface struct - -bool FGJSBsim::copy_from_JSBsim() -{ - unsigned int i, j; - - _set_Inertias( MassBalance->GetMass(), - MassBalance->GetIxx(), - MassBalance->GetIyy(), - MassBalance->GetIzz(), - MassBalance->GetIxz() ); - - _set_CG_Position( MassBalance->GetXYZcg(1), - MassBalance->GetXYZcg(2), - MassBalance->GetXYZcg(3) ); - - _set_Accels_Body( Aircraft->GetBodyAccel(1), - Aircraft->GetBodyAccel(2), - Aircraft->GetBodyAccel(3) ); - - _set_Accels_CG_Body_N ( Aircraft->GetNcg(1), - Aircraft->GetNcg(2), - Aircraft->GetNcg(3) ); - - _set_Accels_Pilot_Body( Auxiliary->GetPilotAccel(1), - Auxiliary->GetPilotAccel(2), - Auxiliary->GetPilotAccel(3) ); - - _set_Nlf( Aircraft->GetNlf() ); - - // Velocities - - _set_Velocities_Local( Propagate->GetVel(FGJSBBase::eNorth), - Propagate->GetVel(FGJSBBase::eEast), - Propagate->GetVel(FGJSBBase::eDown) ); - - _set_Velocities_Wind_Body( Propagate->GetUVW(1), - Propagate->GetUVW(2), - Propagate->GetUVW(3) ); - - // Make the HUD work ... - _set_Velocities_Ground( Propagate->GetVel(FGJSBBase::eNorth), - Propagate->GetVel(FGJSBBase::eEast), - -Propagate->GetVel(FGJSBBase::eDown) ); - - _set_V_rel_wind( Auxiliary->GetVt() ); - - _set_V_equiv_kts( Auxiliary->GetVequivalentKTS() ); - - _set_V_calibrated_kts( Auxiliary->GetVcalibratedKTS() ); - - _set_V_ground_speed( Auxiliary->GetVground() ); - - _set_Omega_Body( Propagate->GetPQR(FGJSBBase::eP), - Propagate->GetPQR(FGJSBBase::eQ), - Propagate->GetPQR(FGJSBBase::eR) ); - - _set_Euler_Rates( Auxiliary->GetEulerRates(FGJSBBase::ePhi), - Auxiliary->GetEulerRates(FGJSBBase::eTht), - Auxiliary->GetEulerRates(FGJSBBase::ePsi) ); - - _set_Mach_number( Auxiliary->GetMach() ); - - // Positions of Visual Reference Point - FGLocation l = Auxiliary->GetLocationVRP(); - _updateGeocentricPosition( l.GetLatitude(), l.GetLongitude(), - l.GetRadius() - get_Sea_level_radius() ); - - _set_Altitude_AGL( Propagate->GetDistanceAGL() ); - { - double loc_cart[3] = { l(FGJSBBase::eX), l(FGJSBBase::eY), l(FGJSBBase::eZ) }; - double contact[3], d[3], sd, t; - is_valid_m(&t, d, &sd); - get_agl_ft(t, loc_cart, SG_METER_TO_FEET*2, contact, d, d, &sd); - double rwrad - = FGColumnVector3( contact[0], contact[1], contact[2] ).Magnitude(); - _set_Runway_altitude( rwrad - get_Sea_level_radius() ); - } - - _set_Euler_Angles( Propagate->GetEuler(FGJSBBase::ePhi), - Propagate->GetEuler(FGJSBBase::eTht), - Propagate->GetEuler(FGJSBBase::ePsi) ); - - _set_Alpha( Auxiliary->Getalpha() ); - _set_Beta( Auxiliary->Getbeta() ); - - - _set_Gamma_vert_rad( Auxiliary->GetGamma() ); - - _set_Earth_position_angle( Inertial->GetEarthPositionAngle() ); - - _set_Climb_Rate( Propagate->Gethdot() ); - - const FGMatrix33& Tl2b = Propagate->GetTl2b(); - for ( i = 1; i <= 3; i++ ) { - for ( j = 1; j <= 3; j++ ) { - _set_T_Local_to_Body( i, j, Tl2b(i,j) ); - } - } - - // Copy the engine values from JSBSim. - for ( i=0; i < Propulsion->GetNumEngines(); i++ ) { - SGPropertyNode * node = fgGetNode("engines/engine", i, true); - char buf[30]; - sprintf(buf, "engines/engine[%d]/thruster", i); - SGPropertyNode * tnode = fgGetNode(buf, true); - FGThruster * thruster = Propulsion->GetEngine(i)->GetThruster(); - - switch (Propulsion->GetEngine(i)->GetType()) { - case FGEngine::etPiston: - { // FGPiston code block - FGPiston* eng = (FGPiston*)Propulsion->GetEngine(i); - node->setDoubleValue("egt-degf", eng->getExhaustGasTemp_degF()); - node->setDoubleValue("oil-temperature-degf", eng->getOilTemp_degF()); - node->setDoubleValue("oil-pressure-psi", eng->getOilPressure_psi()); - node->setDoubleValue("mp-osi", eng->getManifoldPressure_inHg()); - // NOTE: mp-osi is not in ounces per square inch. - // This error is left for reasons of backwards compatibility with - // existing FlightGear sound and instrument configurations. - node->setDoubleValue("mp-inhg", eng->getManifoldPressure_inHg()); - node->setDoubleValue("cht-degf", eng->getCylinderHeadTemp_degF()); - node->setDoubleValue("rpm", eng->getRPM()); - } // end FGPiston code block - break; - case FGEngine::etRocket: - { // FGRocket code block - FGRocket* eng = (FGRocket*)Propulsion->GetEngine(i); - } // end FGRocket code block - break; - case FGEngine::etTurbine: - { // FGTurbine code block - FGTurbine* eng = (FGTurbine*)Propulsion->GetEngine(i); - node->setDoubleValue("n1", eng->GetN1()); - node->setDoubleValue("n2", eng->GetN2()); - node->setDoubleValue("egt_degf", 32 + eng->GetEGT()*9/5); - node->setBoolValue("augmentation", eng->GetAugmentation()); - node->setBoolValue("water-injection", eng->GetInjection()); - node->setBoolValue("ignition", eng->GetIgnition()); - node->setDoubleValue("nozzle-pos-norm", eng->GetNozzle()); - node->setDoubleValue("inlet-pos-norm", eng->GetInlet()); - node->setDoubleValue("oil-pressure-psi", eng->getOilPressure_psi()); - node->setBoolValue("reversed", eng->GetReversed()); - node->setBoolValue("cutoff", eng->GetCutoff()); - node->setDoubleValue("epr", eng->GetEPR()); - globals->get_controls()->set_reverser(i, eng->GetReversed() ); - globals->get_controls()->set_cutoff(i, eng->GetCutoff() ); - globals->get_controls()->set_water_injection(i, eng->GetInjection() ); - globals->get_controls()->set_augmentation(i, eng->GetAugmentation() ); - } // end FGTurbine code block - break; - case FGEngine::etTurboprop: - { // FGTurboProp code block - FGTurboProp* eng = (FGTurboProp*)Propulsion->GetEngine(i); - node->setDoubleValue("n1", eng->GetN1()); - //node->setDoubleValue("n2", eng->GetN2()); - node->setDoubleValue("itt_degf", 32 + eng->GetITT()*9/5); - node->setBoolValue("ignition", eng->GetIgnition()); - node->setDoubleValue("nozzle-pos-norm", eng->GetNozzle()); - node->setDoubleValue("inlet-pos-norm", eng->GetInlet()); - node->setDoubleValue("oil-pressure-psi", eng->getOilPressure_psi()); - node->setBoolValue("reversed", eng->GetReversed()); - node->setBoolValue("cutoff", eng->GetCutoff()); - node->setBoolValue("starting", eng->GetEngStarting()); - node->setBoolValue("generator-power", eng->GetGeneratorPower()); - node->setBoolValue("damaged", eng->GetCondition()); - node->setBoolValue("ielu-intervent", eng->GetIeluIntervent()); - node->setDoubleValue("oil-temperature-degf", eng->getOilTemp_degF()); -// node->setBoolValue("onfire", eng->GetFire()); - globals->get_controls()->set_reverser(i, eng->GetReversed() ); - globals->get_controls()->set_cutoff(i, eng->GetCutoff() ); - } // end FGTurboProp code block - break; - case FGEngine::etElectric: - { // FGElectric code block - FGElectric* eng = (FGElectric*)Propulsion->GetEngine(i); - node->setDoubleValue("rpm", eng->getRPM()); - } // end FGElectric code block - break; - } - - { // FGEngine code block - FGEngine* eng = Propulsion->GetEngine(i); - node->setDoubleValue("fuel-flow-gph", eng->getFuelFlow_gph()); - node->setDoubleValue("thrust_lb", thruster->GetThrust()); - node->setDoubleValue("fuel-flow_pph", eng->getFuelFlow_pph()); - node->setBoolValue("running", eng->GetRunning()); - node->setBoolValue("starter", eng->GetStarter()); - node->setBoolValue("cranking", eng->GetCranking()); - globals->get_controls()->set_starter(i, eng->GetStarter() ); - } // end FGEngine code block - - switch (thruster->GetType()) { - case FGThruster::ttNozzle: - { // FGNozzle code block - FGNozzle* noz = (FGNozzle*)thruster; - } // end FGNozzle code block - break; - case FGThruster::ttPropeller: - { // FGPropeller code block - FGPropeller* prop = (FGPropeller*)thruster; - tnode->setDoubleValue("rpm", thruster->GetRPM()); - tnode->setDoubleValue("pitch", prop->GetPitch()); - tnode->setDoubleValue("torque", prop->GetTorque()); - tnode->setBoolValue("feathered", prop->GetFeather()); - } // end FGPropeller code block - break; - case FGThruster::ttRotor: - { // FGRotor code block - FGRotor* rotor = (FGRotor*)thruster; - } // end FGRotor code block - break; - case FGThruster::ttDirect: - { // Direct code block - } // end Direct code block - break; - } - - } - - // Copy the fuel levels from JSBSim if fuel - // freeze not enabled. - if ( ! Propulsion->GetFuelFreeze() ) { - for (i = 0; i < Propulsion->GetNumTanks(); i++) { - SGPropertyNode * node = fgGetNode("/consumables/fuel/tank", i, true); - FGTank* tank = Propulsion->GetTank(i); - double contents = tank->GetContents(); - double temp = tank->GetTemperature_degC(); - node->setDoubleValue("level-gal_us", contents/6.6); - node->setDoubleValue("level-lbs", contents); - if (temp != -9999.0) node->setDoubleValue("temperature_degC", temp); - } - } - - update_gear(); - - stall_warning->setDoubleValue( Aerodynamics->GetStallWarn() ); - - elevator_pos_pct->setDoubleValue( FCS->GetDePos(ofNorm) ); - left_aileron_pos_pct->setDoubleValue( FCS->GetDaLPos(ofNorm) ); - right_aileron_pos_pct->setDoubleValue( FCS->GetDaRPos(ofNorm) ); - rudder_pos_pct->setDoubleValue( -1*FCS->GetDrPos(ofNorm) ); - flap_pos_pct->setDoubleValue( FCS->GetDfPos(ofNorm) ); - speedbrake_pos_pct->setDoubleValue( FCS->GetDsbPos(ofNorm) ); - spoilers_pos_pct->setDoubleValue( FCS->GetDspPos(ofNorm) ); - tailhook_pos_pct->setDoubleValue( FCS->GetTailhookPos() ); - wing_fold_pos_pct->setDoubleValue( FCS->GetWingFoldPos() ); - - // force a sim crashed if crashed (altitude AGL < 0) - if (get_Altitude_AGL() < -100.0) { - State->SuspendIntegration(); - crashed = true; - } - - return true; - } - - */ - -//#include "gps.h" -//static void sim_gps_feed_data(void) { -//} - diff --git a/sw/simulator/sim_ac_jsbsim.c b/sw/simulator/sim_ac_jsbsim.c index 4dcbddbe90..d0907f17ec 100644 --- a/sw/simulator/sim_ac_jsbsim.c +++ b/sw/simulator/sim_ac_jsbsim.c @@ -58,19 +58,16 @@ static void ivy_transport_init(void); static void sim_init(void) { - + double dt; - dt = DT; - // *** SET UP JSBSIM *** // FDMExec = new JSBSim::FGFDMExec(); - FDMExec->SetAircraftPath(RootDir + "aircraft"); - FDMExec->SetEnginePath(RootDir + "engine"); - FDMExec->SetSystemsPath(RootDir + "systems"); - FDMExec->GetPropertyManager()->Tie("sim-time-sec", &dt); + State = FDMExec->GetState(); + State->Setsim_time(0.); + State->Setdt(DT); #ifdef JSBSIM_ROOT_DIR RootDir = JSBSIM_ROOT_DIR; @@ -103,15 +100,6 @@ static void sim_init(void) { exit(-1); } - FDMExec->Run(); // MAKE AN INITIAL RUN - State = new JSBSim::FGState(FDMExec); - State->Setdt(0.2); - - //Debug - dt = FDMExec->GetDeltaT(); - cout << "DT" << dt << endl; - // - // init sensors ? or discribe them in jSBSim ivy_transport_init(); diff --git a/sw/simulator/sim_ac_jsbsim.h b/sw/simulator/sim_ac_jsbsim.h index bef1d40284..932b04a536 100644 --- a/sw/simulator/sim_ac_jsbsim.h +++ b/sw/simulator/sim_ac_jsbsim.h @@ -32,11 +32,15 @@ #include "airframe.h" #include "flight_plan.h" +#define RAD2DEG 57.29578 +#define FT2M 0.3048 + void autopilot_init(void); void autopilot_periodic_task(void); void autopilot_event_task(void); void copy_inputs_to_jsbsim(JSBSim::FGFDMExec* FDMExec); void copy_outputs_from_jsbsim(JSBSim::FGFDMExec* FDMExec); +bool check_crash_jsbsim(JSBSim::FGFDMExec* FDMExec); #endif // SIM_AC_JSBSIM_H