Hi Emilien,
before going on with the SendState to Ardupilot, I’m doing some tests on the model.
After 10 sec of simulation, without copy pwm command to the motors ( FDMExec->GetFCS()->SetThrottleCmd …) I get all NAN.
Here the log and my code.
Do you think that my F450 model could be wrong or I have missed somthing in the implementation?
Is there some good model with which I can make some test?
Thanks again.
State Report at sim time: 0.000000 seconds
Position
ECI: 20925646.32546, 0, 0 (x,y,z, in ft)
ECEF: 20925646.325460 , 0.000000 , 0.000000 (x,y,z, in ft)
Local: 0.000000, 0.000000, 0.000000 (geodetic lat, lon, alt ASL in deg and ft)
Orientation
ECI: 0, -90, 0 (phi, theta, psi in deg)
Local: 0, 1.272221872585407e-14, 0 (phi, theta, psi in deg)
Velocity
ECI: 0, 1525.922194545818, 0 (x,y,z in ft/s)
ECEF: 0, 0, 0 (x,y,z in ft/s)
Local: 0.000000 , 0.000000 , 0.000000 (n,e,d in ft/sec)
Body: 0.000000 , 0.000000 , 0.000000 (u,v,w in ft/sec)
Body Rates (relative to given frame, expressed in body frame)
ECI: 0.004178074132240403, 0, 9.277188200408133e-19 (p,q,r in deg/s)
ECEF: 0, 0, 0 (p,q,r in deg/s)
---- JSBSim Execution beginning … ----
[libAP_JSON] Broken ArduPilot connection (no packets received)
[libAP_JSON] ArduPilot controller has reset
[libAP_JSON] Connected to ArduPilot controller @ 127.0.0.1:58823
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 2
[libAP_JSON] Missed 4294967293 input frames
FDMExec->GetSimTime()=1.008333
State Report at sim time: 1.008333 seconds
Position
ECI: 20925647.36178803, 1538.637780605836, 0.9496520700431462 (x,y,z, in ft)
ECEF: 20925647.418355 , -0.000511 , 0.949652 (x,y,z, in ft)
Local: 0.000003, -0.000000, 1.092895 (geodetic lat, lon, alt ASL in deg and ft)
Orientation
ECI: -0.006808742676163626, -26.44913021254384, 0.01352356432380844 (phi, theta, psi in deg)
Local: 0.0140951876559937, 63.55087088450278, 0.0187159192786267 (phi, theta, psi in deg)
Velocity
ECI: 154.7746847613654, 1525.919448446042, -2.802783430857132 (x,y,z in ft/s)
ECEF: 154.8868833715895, -0.01421032077126442, -2.802783430857133 (x,y,z in ft/s)
Local: -2.802791 , -0.014210 , -154.886883 (n,e,d in ft/sec)
Body: 137.426526 , -0.030883 , -71.496513 (u,v,w in ft/sec)
Body Rates (relative to given frame, expressed in body frame)
ECI: -67.07018287430262, -331866.1244805319, -2.22166407109872 (p,q,r in deg/s)
ECEF: -67.07204380136496, -331866.1244800874, -2.225404826868024 (p,q,r in deg/s)
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
FDMExec->GetSimTime()=2.008333
State Report at sim time: 2.008333 seconds
Position
ECI: 20925767.72712725, 3064.556521178257, -1.523711472964928 (x,y,z, in ft)
ECEF: 20925767.951528 , -0.021687 , -1.523711 (x,y,z, in ft)
Local: -0.000004, -0.000000, 121.626068 (geodetic lat, lon, alt ASL in deg and ft)
Orientation
ECI: -179.9517065465413, -3.659452225381625, 179.2670541035965 (phi, theta, psi in deg)
Local: -11.41199979272025, -86.26631648516998, 11.43661043594352 (phi, theta, psi in deg)
Velocity
ECI: 90.83142458006481, 1525.917903854014, -2.21547202136259 (x,y,z in ft/s)
ECEF: 91.05489266451822, -0.02647842809559027, -2.215472021362589 (x,y,z in ft/s)
Local: -2.215465 , -0.026478 , -91.054893 (n,e,d in ft/sec)
Body: -91.003376 , 1.148594 , -3.601226 (u,v,w in ft/sec)
Body Rates (relative to given frame, expressed in body frame)
ECI: -4261.423580524435, -331088.3762349032, -2.221673664252495 (p,q,r in deg/s)
ECEF: -4261.423847194509, -331088.3762313888, -2.217504110538333 (p,q,r in deg/s)
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
FDMExec->GetSimTime()=3.008333
State Report at sim time: 3.008333 seconds
Position
ECI: 20925836.19104943, 4590.472938432715, -3.543477518938277 (x,y,z, in ft)
ECEF: 20925836.694552 , -0.051388 , -3.543478 (x,y,z, in ft)
Local: -0.000010, -0.000000, 190.369093 (geodetic lat, lon, alt ASL in deg and ft)
Orientation
ECI: 1.020601105958028, 34.98864379838599, 1.786041456378311 (phi, theta, psi in deg)
Local: 177.9297833138694, 54.97217319121542, 177.4682030722056 (phi, theta, psi in deg)
Velocity
ECI: 48.20441418129395, 1525.914483791318, -1.864858592276852 (x,y,z in ft/s)
ECEF: 48.53915085030894, -0.03220406050469359, -1.864858592276853 (x,y,z in ft/s)
Local: -1.864850 , -0.032204 , -48.539151 (n,e,d in ft/sec)
Body: 40.815933 , -1.065836 , 26.314473 (u,v,w in ft/sec)
Body Rates (relative to given frame, expressed in body frame)
ECI: -8445.960028892652, -330304.9167754501, -2.221678039781552 (p,q,r in deg/s)
ECEF: -8445.957633126172, -330304.9168364193, -2.225100449630733 (p,q,r in deg/s)
FDMExec->GetSimTime()=4.008333
State Report at sim time: 4.008333 seconds
Position
ECI: 20925866.77098924, 6116.384423697858, -5.358739800117285 (x,y,z, in ft)
ECEF: 20925867.664863 , -0.084981 , -5.358740 (x,y,z, in ft)
Local: -0.000015, -0.000000, 221.339403 (geodetic lat, lon, alt ASL in deg and ft)
Orientation
ECI: -174.7089211588933, -67.45474602612701, 174.2771561505573 (phi, theta, psi in deg)
Local: -0.9198734762831681, -22.42607522786314, 2.377381002535568 (phi, theta, psi in deg)
Velocity
ECI: 13.70423946826454, 1525.907877078303, -1.779015598189861 (x,y,z in ft/s)
ECEF: 14.15024376629609, -0.03452861739494022, -1.779015598189864 (x,y,z in ft/s)
Local: -1.779012 , -0.034529 , -14.150244 (n,e,d in ft/sec)
Body: -7.042570 , 0.238386 , -12.399228 (u,v,w in ft/sec)
Body Rates (relative to given frame, expressed in body frame)
ECI: -12620.22324065335, -329470.6006933129, -2.221680044694698 (p,q,r in deg/s)
ECEF: -12620.22709942647, -329470.6005455902, -2.220084942373226 (p,q,r in deg/s)
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
FDMExec->GetSimTime()=5.000000
State Report at sim time: 5.000000 seconds
Position
ECI: 20925864.51460714, 7629.57164253755, -7.097335040895596 (x,y,z, in ft)
ECEF: 20925865.905478 , -0.119221 , -7.097335 (x,y,z, in ft)
Local: -0.000020, -0.000000, 219.580019 (geodetic lat, lon, alt ASL in deg and ft)
Orientation
ECI: -23.93334727621221, -82.84123877849245, 24.10237119707075 (phi, theta, psi in deg)
Local: 0.3156041551482364, 6.532841217226703, 2.933756812343095 (phi, theta, psi in deg)
Velocity
ECI: -18.1684427315697, 1525.897996048127, -1.734531521487428 (x,y,z in ft/s)
ECEF: -17.61209904674509, -0.03368763101712979, -1.734531521487428 (x,y,z in ft/s)
Local: -1.734538 , -0.033688 , 17.612098 (n,e,d in ft/sec)
Body: -3.726505 , 0.150428 , 17.299893 (u,v,w in ft/sec)
Body Rates (relative to given frame, expressed in body frame)
ECI: -16748.92463023603, -328590.9912259106, -2.221681032715559 (p,q,r in deg/s)
ECEF: -16748.92877574063, -328590.9910146893, -2.222156932583856 (p,q,r in deg/s)
[libAP_JSON] Drained n packets: 2
[libAP_JSON] Missed 4294967293 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
FDMExec->GetSimTime()=6.000000
State Report at sim time: 6.000000 seconds
Position
ECI: 20925831.15497937, 9155.464653083704, -8.78248641777585 (x,y,z, in ft)
ECEF: 20925833.157828 , -0.149966 , -8.782486 (x,y,z, in ft)
Local: -0.000024, -0.000000, 186.832369 (geodetic lat, lon, alt ASL in deg and ft)
Orientation
ECI: 175.8932594185197, 48.34685276439868, 174.5132396957695 (phi, theta, psi in deg)
Local: -176.7478139962079, -41.41791106149832, 175.1166388481605 (phi, theta, psi in deg)
Velocity
ECI: -47.86475894514667, 1525.888183555957, -1.635192452296097 (x,y,z in ft/s)
ECEF: -47.19714819409259, -0.02683895225644788, -1.635192452296099 (x,y,z in ft/s)
Local: -1.635212 , -0.026839 , 47.197148 (n,e,d in ft/sec)
Body: 32.443188 , -2.112511 , -34.252332 (u,v,w in ft/sec)
Body Rates (relative to given frame, expressed in body frame)
ECI: -20900.80029899745, -327651.2252102206, -2.221681551748254 (p,q,r in deg/s)
ECEF: -20900.79717721602, -327651.2254090827, -2.218911851759178 (p,q,r in deg/s)
FDMExec->GetSimTime()=7.000000
State Report at sim time: 7.000000 seconds
Position
ECI: 20925770.54465128, 10681.348333698, -10.30861665633206 (x,y,z, in ft)
ECEF: 20925773.270744 , -0.171363 , -10.308617 (x,y,z, in ft)
Local: -0.000028, -0.000000, 126.945287 (geodetic lat, lon, alt ASL in deg and ft)
Orientation
ECI: -0.9539883115938425, -12.23257520293844, 4.485876782073739 (phi, theta, psi in deg)
Local: 19.24199015547204, 76.99248935411063, 19.71818357281452 (phi, theta, psi in deg)
Velocity
ECI: -72.30505082696212, 1525.879315868528, -1.397202860506101 (x,y,z in ft/s)
ECEF: -71.52617181567054, -0.01542649223828207, -1.397202860506104 (x,y,z in ft/s)
Local: -1.397238 , -0.015427 , 71.526171 (n,e,d in ft/sec)
Body: -69.988070 , 5.312925 , 13.834325 (u,v,w in ft/sec)
Body Rates (relative to given frame, expressed in body frame)
ECI: -25040.44492839982, -326658.4834922447, -2.221681827589946 (p,q,r in deg/s)
ECEF: -25040.44581365212, -326658.4834242613, -2.225764475026543 (p,q,r in deg/s)
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
FDMExec->GetSimTime()=8.000000
State Report at sim time: 8.000000 seconds
Position
ECI: 20925688.69716367, 12207.22345142419, -11.61875133403097 (x,y,z, in ft)
ECEF: 20925692.257770 , -0.180208 , -11.618751 (x,y,z, in ft)
Local: -0.000032, -0.000000, 45.932314 (geodetic lat, lon, alt ASL in deg and ft)
Orientation
ECI: -177.5990557880517, -25.06350487866678, 174.3531678867542 (phi, theta, psi in deg)
Local: -10.81259383591101, -64.34153348875473, 11.94996709980832 (phi, theta, psi in deg)
Velocity
ECI: -90.36570059356623, 1525.87094090658, -1.221902388839226 (x,y,z in ft/s)
ECEF: -89.47555229832454, -0.002146154558834493, -1.221902388839233 (x,y,z in ft/s)
Local: -1.221952 , -0.002147 , 89.475552 (n,e,d in ft/sec)
Body: 80.134631 , -7.223928 , 39.161502 (u,v,w in ft/sec)
Body Rates (relative to given frame, expressed in body frame)
ECI: -29167.19196036109, -325612.8128641316, -2.221681974927023 (p,q,r in deg/s)
ECEF: -29167.1937302874, -325612.8127055841, -2.217900635616547 (p,q,r in deg/s)
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
FDMExec->GetSimTime()=9.000000
State Report at sim time: 9.000000 seconds
Position
ECI: nan, -nan, -nan (x,y,z, in ft)
ECEF: -nan , -nan , -nan (x,y,z, in ft)
Local: nan, -nan, -nan (geodetic lat, lon, alt ASL in deg and ft)
Orientation
ECI: -nan, nan, -nan (phi, theta, psi in deg)
Local: -nan, nan, -nan (phi, theta, psi in deg)
Velocity
ECI: nan, -nan, -nan (x,y,z in ft/s)
ECEF: -nan, -nan, -nan (x,y,z in ft/s)
Local: -nan , nan , -nan (n,e,d in ft/sec)
Body: -nan , -nan , -nan (u,v,w in ft/sec)
Body Rates (relative to given frame, expressed in body frame)
ECI: -nan, -nan, -nan (p,q,r in deg/s)
ECEF: -nan, -nan, -nan (p,q,r in deg/s)
[libAP_JSON] Drained n packets: 1
[libAP_JSON] Missed 4294967294 input frames
FDMExec->GetSimTime()=10.000000
JSBSim::FGFDMExec* FDMExec = new JSBSim::FGFDMExec();
if ( ! FDMExec->LoadModel(SGPath("aircraft"),
SGPath("engine"),
SGPath("systems"),
"F450")) {
cerr << " flight sim could not be started, could not open airplane model" << endl << endl;
exit(-1);
}
//FDMExec->GetIC()->Load(SGPath("initGrnd"));
FDMExec->RunIC();
bool flying=true;//Tracks whether the plane is flying or has crashed.
double pos_x_o = FDMExec->GetPropagate()->GetLatitudeDeg();
double pos_y_o = FDMExec->GetPropagate()->GetLongitudeDeg();
double pos_z_o = FDMExec->GetPropagate()->GetAltitudeASLmeters();
cout << "[pos_x_o, pos_y_o, pos_z_o]=[" << pos_x_o << "," << pos_y_o << "," << pos_z_o << "]" << endl;
// PRINT SIMULATION CONFIGURATION
FDMExec->PrintSimulationConfiguration();
// Dump the simulation state (position, orientation, etc.)
FDMExec->GetPropagate()->DumpState();
cout << endl << JSBSim::FGFDMExec::fggreen << JSBSim::FGFDMExec::highint
<< "---- JSBSim Execution beginning ... ----"
<< JSBSim::FGFDMExec::reset << endl << endl;
//Get the propogation pointer, auxiliary pointer, and flight control
JSBSim::FGPropagate* propagate=FDMExec->GetPropagate();
JSBSim::FGAuxiliary* auxiliary=FDMExec->GetAuxiliary();
JSBSim::FGFCS* fcs=FDMExec->GetFCS();
double printTime = 1.0;
while (flying) {
if (ap.ReceiveServoPacket(servo_out))
{
/* cout << "servo_out PWM: [";
for (int i = 0; i < MAX_SERVO_CHANNELS - 1; ++i)
{
cout << servo_out[i] << ", ";
}
cout << servo_out[MAX_SERVO_CHANNELS - 1] << "]" << endl;
*/
}
if (!ap.ap_online) {
continue;
}
//fcs->SetThrottleCmd(0, (servo_out[0]-1000)/1000);
//fcs->SetThrottleCmd(1, (servo_out[1]-1000)/1000);
//fcs->SetThrottleCmd(2, (servo_out[2]-1000)/1000);
//fcs->SetThrottleCmd(3, (servo_out[3]-1000)/1000);
// Step the flight simulator
FDMExec->Run();
if (FDMExec->GetSimTime() > printTime) {
cout << "FDMExec->GetSimTime()=" << FDMExec->GetSimTime() << endl;
// Dump the simulation state (position, orientation, etc.)
FDMExec->GetPropagate()->DumpState();
printTime += 1.0;
}
/*double gyro_x = FDMExec->GetPropagate()->GetPQR(1); // roll (rad/s)
double gyro_y = FDMExec->GetPropagate()->GetPQR(2); // pitch
double gyro_z = FDMExec->GetPropagate()->GetPQR(3); // yaw
//cout << "[gyro_x, gyro_y, gyro_z]=[" << gyro_x << "," << gyro_y << "," << gyro_z << "]" << endl;
double accel_x = FDMExec->GetAccelerations()->GetBodyAccel(1);
double accel_y = FDMExec->GetAccelerations()->GetBodyAccel(2);
double accel_z = FDMExec->GetAccelerations()->GetBodyAccel(3);
double phi = FDMExec->GetPropagate()->GetEuler(1); //(Phi, Theta, Psi)
double theta = FDMExec->GetPropagate()->GetEuler(2);
double psi = FDMExec->GetPropagate()->GetEuler(3);
//FDMExec->GetPropagate()->GetQuaterniondot(); //(Phi, Theta, Psi)
double Vx = FDMExec->GetPropagate()->GetVel(1) * FEET_TO_METERS; //Vx
double Vy = FDMExec->GetPropagate()->GetVel(2) * FEET_TO_METERS; //Vy
double Vz = FDMExec->GetPropagate()->GetVel(3) * FEET_TO_METERS; //Vz
// cout << "[Vx, Vy, Vz]=[" << Vx << "," << Vy << "," << Vz << "]" << endl;
ap.SendState(FDMExec->GetSimTime(),
gyro_x, gyro_y, gyro_z, // gyro
accel_x, accel_y, accel_z, // accel
0, 0, 0, // position
phi, theta, psi, // attitude
Vx, Vy, Vz); // velocity
*/
ap.SendState(FDMExec->GetSimTime(),
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0); // velocity
//check for crash. If crashed, set flying to false
}