Simulate arducopter with JSBSim

Hi,

I’m new here and I’m trying to simulate a quad-copter with a particular design with JSBSim and Ardupilot, now I’m trying to understand examples but I have some issues.
I’m on a Linux virtual machine with JSBSim 1.1.11 and the latest Ardupilot version.

I had run the Rascal example successfully with Arduplane, FlightGear and JSBSim.

cd ardupilot/ArduPlane
sim_vehicle.py -f jsbsim:Rascal --console --map

But when I want to simulate the “arducopter” example in Tools/autotest/aircraft, all is load like Rascal but I can’t control it.

In my terminal, I run these command :

cd ardupilot/ArduPlane
sim_vehicle.py -f jsbsim:arducopter --console --map

After the launch I load a mission (CMAC-circuit or CMAC-copter-circuit) and enter these command like my test with the Rascal. But with arducopter model, nothing is happening.

arm throttle
mode auto

Do you know if this model work ? or if I have made a mistake ?
Do you know if there is a better solution to simulate a very particular copter ?

Thanks in advance.

Hi,

If someone want to make the same things, I have the solution.

You have to select ArduCopter for the vehicle with sim_vehicle.py and send PWM commande with udp.

sim_vehicle.py -f JSON:127.0.0.1 --console --map

Now you can create your C++ app with JSBSim and copy pwm command to your motors.

I used the F450 model of JSBSim for my test, you just have to modified it a little.

You have all the details here :
https://jsbsim-team.github.io/jsbsim/

https://ardupilot.org/dev/docs/sitl-with-JSON.html

Hi Emilien,
I don’t know in depth ardupilot and jsbsim. i would need to try your solution. Could you give me some details of the C++ appù, in particular the part sending PWM commands to ardupilot.

I would be infinitely grateful.
Thank you very much.

Hi,

Ardupilot send PWM command thanks to the “JSON:127.0.0.1” on the sim_vehicle.py command.

I receive the pwm command thanks to this example : ardupilot/libraries/SITL/examples/JSON/C++ at master · ArduPilot/ardupilot · GitHub

You initialize the connection with ardupilot thanks to the InitSockets() function and init your JSBSIm qudcopter.

You use the ReceiveServoPacket() function from libAP_JSON to read the pwm motor command. After this you copy this table to the JSBSim motor variable. For F450 it’s somethings like this :

fdmex->GetFCS()->SetThrottleCmd(0, ThrottleCmd_ard[0]);
fdmex->GetFCS()->SetThrottleCmd(1, ThrottleCmd_ard[1]);
fdmex->GetFCS()->SetThrottleCmd(2, ThrottleCmd_ard[2]);
fdmex->GetFCS()->SetThrottleCmd(3, ThrottleCmd_ard[3]);

And you can copy JSBSim gyro value ect … to ardupilot with the SendState() function from libAP_JSON.
Here is an example to how get JSBSim variable.

fdmex->GetPropagate()->GetPQR(1); // roll  (rad/s)
fdmex->GetPropagate()->GetPQR(2); // pitch
fdmex->GetPropagate()->GetPQR(3); // yaw

If you have another question don’t hesitate.
I will do a full example when I have the time. With Arducopter param, JSBSim C++ code for linux.

Hi emilien thanks for the quick answer.
I’d still need your help. Could you show me the script that you have used to init the JSBSim.

Thanks a lot.

Hi Emilien

I have written the following code.
I didn’t understand how to use the servo_out array outputs from ArduPilot in the motor (FDMExec).

Could you help me to complete this part.

Thanks in advance.
Marco

#include <math.h>
#include <time.h>
#include
#include <stdlib.h>
#include <FGFDMExec.h>

#include “libAP_JSON.cpp”

uint16_t servo_out[16];

uint64_t micros() {
uint64_t us = std::chrono::duration_caststd::chrono::microseconds(std::chrono::high_resolution_clock::
now().time_since_epoch()).count();
return us;
}

int main(int argc, char **argv)
{

cout << "JSBSim and Ardupilot JSON interface" << endl;

// init the ArduPilot connection
libAP_JSON ap;
if (ap.InitSockets("127.0.0.1", 9002))
{
    cout << "started socket" << endl;
}



// init the JSBSim FDM
JSBSim::FGFDMExec FDMExec;

FDMExec.LoadScript(SGPath(argv[1]));

FDMExec.RunIC();

// 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;


// send and receive data from AP
while (FDMExec.Run()) {
    double timestamp = (double) micros() / 1000000.0;

    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;
    }

    //FDMExec.GetFCS()->SetThrottleCmd(0, 0);
    ??????????????????????????????????????????




    ??????????????????????????????????????????
    
    // send with the required
    ap.SendState(timestamp,
                 0, 0, 0,    // gyro
                 0, 0, -9.81, // accel
                 0, 0, 0,    // position
                 0, 0, 0,    // attitude
                 0, 0, 0);    // velocity

    usleep(1000); // run this example at about 1000 Hz. Realistically it is about 800 Hz.
}

return 0;

}

Hi,

Instead of FDMExec.LaodScript, you have to use FDMExec.LoadModel and run the Initial condition like this:

bool result = fdmex->LoadModel(SGPath("aircraft"),
        SGPath("engine"),
        SGPath("systems"),
        "F450");
fdmex->GetIC()->Load(SGPath("init"));
fdmex->RunIC();

After this, in the while 1 for copy Throttle commands, it’s with these lines:

fdmex->GetFCS()->SetThrottleCmd(0, (servo_out[0]-1000)/1000);
fdmex->GetFCS()->SetThrottleCmd(1, (servo_out[1]-1000)/1000);
fdmex->GetFCS()->SetThrottleCmd(2, (servo_out[2]-1000)/1000);
fdmex->GetFCS()->SetThrottleCmd(3, (servo_out[3]-1000)/1000);

And for the sendstate function, instead of timestamp, use FDMExec->GetSimTime(); and you can delete the usleep function.

Hi Emilien
thanks again.

I still have two issues to overcame:

1)At the following point

FDMExec->GetIC()->Load(SGPath(“init”));

the program interrupts with the following log.

Could not open file: Path “aircraft/F450/init.xml”
File: Path “aircraft/F450/init” could not be read.
terminate called after throwing an instance of ‘JSBSim::BaseException’
what(): File: Path “aircraft/F450/init” could not be read.
Aborted (core dumped)

How must be the init.xml file? Or where can I find an example of the this file.

Second question.

Which methods of FDMExec provide me the info gyro, accel, position, attitude and velocity needed in the SendState method.

Thanks in advance.
Marco

The following is a first try!
Where can I get the acceleration?

What a disaster!

    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

    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
    
    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 pos_x = FDMExec->GetPropagate()->GetInertialPosition(1);
    double pos_y = FDMExec->GetPropagate()->GetInertialPosition(2);
    double pos_z = FDMExec->GetPropagate()->GetInertialPosition(3);


    // send with the required
    ap.SendState(FDMExec->GetSimTime(),
                 gyro_x, gyro_y, gyro_z,    // gyro
                 0, 0, -9.81, // accel
                 pos_x, pos_y, pos_z,    // position
                 phi, theta, psi,    // attitude
                 Vx, Vy, Vz);    // velocity

Hi,

Sorry, for load script function, replace “init” by “initGrnd”, I’ve changed its name.

For send state to Ardupilot, your gyro, velocities, attitude are good.
For accel body I use FDMExec->GetAcceleration()->GetBodyAccel(idex)

And for the position it’s more complicated, you have to use the latitude and longitude and calculate de delta.

First, you have to save the initial position after the RunIc()

    pos_x_o = fdmex->GetPropagate()->GetLatitudeDeg();
    position_y_o = fdmex->GetPropagate()->GetLongitudeDeg();
    position_z_o = fdmex->GetPropagate()->GetAltitudeASLmeters();

After, you can calculate the delta:
Radius is the radius of the earth
After, it is a calculation of the perimeter of an arc of a circle.

    double radius = conversion_ft_m(fdmex->GetPropagate()->GetRadius());
    pos_x = (fdmex->GetPropagate()->GetLatitudeDeg()-pos_x_o)*2*M_PI*radius/360.0;
    pos_y = (fdmex->GetPropagate()->GetLongitudeDeg()-pos_y_o)*2*M_PI*radius/360.0*cos(position_origine[0]*M_PI/180.0);
    pos_z = -(fdmex->GetPropagate()->GetAltitudeASLmeters()- pos_z_o); // altitude

Hi Emilien,

is it normal that before the while loop I have the following State

######################################
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)
####################################

and as soon as the simulation starts (after the FDMExec->Run()) and I receive the first servos message I get the following output without copy pwm command to the motors ( FDMExec->GetFCS()->SetThrottleCmd …)

###################################
State Report at sim time: 65.608333 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)
#####################################

int main(int argc, char **argv) {

cout << "JSBSim and Ardupilot JSON interface" << endl;

// init the ArduPilot connection
libAP_JSON ap;
if (ap.InitSockets("127.0.0.1", 9002))
{
    cout << "started socket" << endl;
}

JSBSim::FGFDMExec* FDMExec = new JSBSim::FGFDMExec();  

bool result = FDMExec->LoadModel(SGPath("aircraft"),
    SGPath("engine"),
    SGPath("systems"),
    "F450");

FDMExec->GetIC()->Load(SGPath("initGrnd"));
FDMExec->RunIC();

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;

while (FDMExec->Run()) {

    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;
    }


    // Dump the simulation state (position, orientation, etc.)
    FDMExec->GetPropagate()->DumpState();

    cout << "FDMExec->GetSimTime()=" << FDMExec->GetSimTime() << endl;

    ap.SendState(FDMExec->GetSimTime(),
                 0, 0, 0,    // gyro
                 0, 0, 0,    // accel
                 0, 0, 0,    // position
                 0, 0, 0,    // attitude
                 0, 0, 0);    // velocity

}

return 0;

}

It’s very strange, why your simulation go to 65s after the first run?
Maybe you can try to set a delta time for the simulation with FDMExec->Setdt(1/400) after the load model.

Sorry, I’ve solved. The problem was in the loop.

Now I’ve to complete the SendState.
What do you mean with the position_origine[0] in the following code?

double radius = conversion_ft_m(fdmex->GetPropagate()->GetRadius());
pos_x = (fdmex->GetPropagate()->GetLatitudeDeg()-pos_x_o)2M_PIradius/360.0;
pos_y = (fdmex->GetPropagate()->GetLongitudeDeg()-pos_y_o)2M_PI
radius/360.0*cos(position_origine[0]*M_PI/180.0);
pos_z = -(fdmex->GetPropagate()->GetAltitudeASLmeters()- pos_z_o); // altitude

You can replace position_origine[0] by pos_x_o.
I have made a mistake when I copy the code to this discuss and make it more easyer to understand.

Maybe you can replace position_origine[0] by pos_x, I have not try this but I think it will be more precise.

Not pos_x, but FDMExec->GetPropagate()->GetLatitude()

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
 
}

I have the same thing when my drone craches after a long fall .
Try to modify the initgrnd.xml, change the elevation to 0 and altitude to 0.2 or 0.1.
This will set the elevation of the terrain to sea level and place the drone at 20cm.
With this you delete the possible fall after lunch but if you find the solution to completely delete this bug when the drone fall I will take it.

If that’s not enough, you can try to see the csv output file (I don’t remember if this output is already set), I use this when I have bugs, you can plot the altitude with excel and see exactly when the bug appear.

Hi Emilien,
with the initgrnd.xml modifications the model runs “correctly”, that is without NaN value.

As you can see, I have other issues to overcame.

  1. Why I have the plane icon even if I have launched ardupilot with the command
    sim_vehicle.py -v ArduCopter -f JSON --map --console

  2. Which command could I try and how can I have the classic mode (STABILIZED, AUTO, GUIDED… ) instead of Mode(0)…

  3. With no commands in Ardupilot the UAV still moves slowly (towards right) and after 2 mins the height has strange value (no sense!)

Have you other good suggestions!

Thanks.

Hi Emilien,
I’d like to share with you a workaround I’m trying.

I want to run ArduPilot and JSBsim in the traditional way, that is with the following command

cd ardupilot/ArduCopter
sim_vehicle.py -f jsbsim:F450 --console --map

To do this, first I had to modify vehicleinfo.py to add the following option:

      "jsbsim": {
            "waf_target": "bin/arducopter",
            "default_params_filename": ["default_params/copter-jsbsim.parm"],
            "external": True,
        },

Secondly I had to modify the send_servos function in SIM_JSBSim.cpp

void JSBSim::send_servos(const struct sitl_input &input)
{
char *buf = nullptr;

// ARDUPLANE
/*
float aileron  = filtered_servo_angle(input, 0);
float elevator = filtered_servo_angle(input, 1);
float throttle = filtered_servo_range(input, 2);
float rudder   = filtered_servo_angle(input, 3);

if (frame == FRAME_ELEVON) {
printf("frame == FRAME_ELEVON!\n");
    // fake an elevon plane
    float ch1 = aileron;
    float ch2 = elevator;
    aileron  = (ch2-ch1)/2.0f;
    // the minus does away with the need for RC2_REVERSED=-1
    elevator = -(ch2+ch1)/2.0f;
} else if (frame == FRAME_VTAIL) {
printf("frame == FRAME_VTAIL!\n");
    // fake a vtail plane
    float ch1 = elevator;
    float ch2 = rudder;
    // this matches VTAIL_OUTPUT==2
    elevator = (ch2-ch1)/2.0f;
    rudder   = (ch2+ch1)/2.0f;
}
float wind_speed_fps = input.wind.speed / FEET_TO_METERS;

asprintf(&buf,
         "set fcs/aileron-cmd-norm %f\n"
         "set fcs/elevator-cmd-norm %f\n"
         "set fcs/rudder-cmd-norm %f\n"
         "set fcs/throttle-cmd-norm %f\n"
         "set atmosphere/psiw-rad %f\n"
         "set atmosphere/wind-mag-fps %f\n"
         "set atmosphere/turbulence/milspec/windspeed_at_20ft_AGL-fps %f\n"
         "set atmosphere/turbulence/milspec/severity %f\n"
         "iterate 1\n",
         aileron, elevator, rudder, throttle,
         radians(input.wind.direction),
         wind_speed_fps,
         wind_speed_fps/3,
         input.wind.turbulence);
 */


//  ARDUCOPTER
float throttle0 = filtered_servo_range(input, 0);
float throttle1 = filtered_servo_range(input, 1);
float throttle2 = filtered_servo_range(input, 2);
float throttle3 = filtered_servo_range(input, 3);

asprintf(&buf,
         "set fcs/throttle-cmd-norm %f\n"
         "set fcs/throttle-cmd-norm[1] %f\n"
         "set fcs/throttle-cmd-norm[2] %f\n"
         "set fcs/throttle-cmd-norm[3] %f\n"
         "iterate 1\n",
         throttle0, throttle1, throttle2, throttle3); 


But the result wasn’t good.
After the command

mode GUIDED
arm throttle
takeoff 10

the drone becomes unstable and after 10 second crashes.

Do yuo have any ideas?

Thank you
Marco

Hi,
Your solution is amazing and more simple !

I think most of your issues are from a bad .param configuration in arducopter.

Which file are you using ?
If it’s copter.param you have to change FRAME_TYPE to 1 (X quadcopter) and change the PID value with smaller value.

If you have a reel fly data log from a similar quadcopter, you can extract param of the quadcopter with MAVExplorer and load these param in the simulation (disable arming check and all security)