SITL with custom simulator

[jean-dupond]

Hi,

I need to link one homemade simulator in Python with PX4 SITL (later HITL).

My simulator contains full drone and world models. My plan is to receive commands from PX4 and to return the drone state (position, attitude, acceleration, everything needed).

I tried to follow the dedicated documentation but despite of my efforts it doesn't work. So I suspect the documentation to be incomplete.

What I need is a lockstep SITL. The documentation says:

Simulator MAVLink API PX4 inputs are: HIL_SENSOR, HIL_GPS, etc... mavlink messages PX4 outputs are: HIL_ACTUATOR_CONTROLS message

SITL Simulation Environment PX4 SITL and the simulator communicate through a TCP connection on port 4560

Starting/Building SITL Simulation To start PX4 with a custom simulator: make px4_sitl none_iris

Lockstep Simulation A lockstep simulation runs as follow:

1. The simulator must send a HIL_SENSOR message.
2. The PX4 returns a HIL_ACTUATOR_CONTROLS message.

I followed all of this information but can't have it work. What I do:

1. I launch a SITL standalone PX4: make px4_sitl none_iris

2. I wait until it says it waits for the simulator to connect: INFO [simulator] Waiting for simulator to accept connection on TCP port 4560

3. In Python, I connect to the TCP port with pymavlink:

from pymavlink import mavutil
vehicle = mavutil.mavlink_connection('tcpin:localhost:4560')
vehicle.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % (vehicle.target_system, vehicle.target_component))

4. PX4 says the simulator is connected: INFO [simulator] Simulator connected on TCP port 4560.

5. I sent a HIL_SENSOR message

# HIL_SENSOR

vehicle.mav.hil_sensor_send(
    time_usec           = 1636636640    , # Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. [us] (type:uint64_t)
    xacc                = 0             , # X acceleration [m/s/s] (type:float)
    yacc                = 0             , # Y acceleration [m/s/s] (type:float)
    zacc                = 0             , # Z acceleration [m/s/s] (type:float)
    xgyro               = 0             , # Angular speed around X axis in body frame [rad/s] (type:float)
    ygyro               = 0             , # Angular speed around Y axis in body frame [rad/s] (type:float)
    zgyro               = 0             , # Angular speed around Z axis in body frame [rad/s] (type:float)
    xmag                = 0             , # X Magnetic field [gauss] (type:float)
    ymag                = 0             , # Y Magnetic field [gauss] (type:float)
    zmag                = 0             , # Z Magnetic field [gauss] (type:float)
    abs_pressure        = 1             , # Absolute pressure [hPa] (type:float)
    diff_pressure       = 0             , # Differential pressure (airspeed) [hPa] (type:float)
    pressure_alt        = 0             , # Altitude calculated from pressure (type:float)
    temperature         = 15            , # Temperature [degC] (type:float)
    fields_updated      = 31            , # Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. (type:uint32_t)
    id                  = 0             , # Sensor ID (zero indexed). Used for multiple sensor inputs (type:uint8_t)
)

6. I wait for an answer from PX4

print("Waiting a message...")
msg = vehicle.recv_match(blocking=True)
print(msg)

But nothing happens. In addition, no UDP connection is not initiated by PX4 to monitor it.

Is there something missing in the documentation regarding the use of custom simulators?

[hamishwillee]

@julianoes Can you advise?

[julianoes]

I would try without lockstep first to see if you can get the communication going. You can read about how to disable lockstep here: https://docs.px4.io/master/en/simulation/#disable-lockstep-simulation

I would also add printfs in https://github.com/PX4/PX4-Autopilot/blob/master/src/modules/simulator/simulator_mavlink.cpp to check if your messages arrive.

[jean-dupond]

Hi julianoes,

Didn't mention, but already edited the simulator_mavlink.ccp file and yes, messages arrive:

    if (fds[0].revents & POLLIN) {

      int len = ::recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, (socklen_t *)&_addrlen);

      if (len > 0) {
        mavlink_message_t msg;
        //---------------//
        //--v--DEBUG--v--//
        printf("DEBUG: Message received\n");
        //--^--DEBUG--^--//
        //---------------//
        for (int i = 0; i < len; i++) {
          if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &mavlink_status)) {
            handle_message(&msg);
          }
        }
      }
    }

Output:

INFO  [simulator] Waiting for simulator to accept connection on TCP port 4560
INFO  [simulator] Simulator connected on TCP port 4560.
DEBUG: Message received

About deactivating the lockstep, I also tested it but like this:

make px4_sitl_nolockstep none_iris

Then if the PX4 starts and opens a UDP port for monitoring, still nothing arrives through the TCP port.

Outputs from PX4 SITL:

INFO  [dataman] Unknown restart, data manager file './dataman' size is 11798680 bytes
INFO  [simulator] Waiting for simulator to accept connection on TCP port 4560
INFO  [commander] LED: open /dev/led0 failed (22)
INFO  [init] Mixer: etc/mixers/quad_w.main.mix on /dev/pwm_output0
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 18570 remote port 14550
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14580 remote port 14540
INFO  [mavlink] mode: Onboard, data rate: 4000 B/s on udp port 14280 remote port 14030
INFO  [logger] logger started (mode=all)
INFO  [logger] Start file log (type: full)
INFO  [logger] [logger] ./log/2021-12-01/07_59_32.ulg
INFO  [logger] Opened full log file: ./log/2021-12-01/07_59_32.ulg
INFO  [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
INFO  [px4] Startup script returned successfully
pxh> INFO  [simulator] Simulator connected on TCP port 4560.
DEBUG: Message received

Output from my Python script:

Waiting to connect...
Heartbeat from system (system 1 component 0)
No GPS_RAW_INT message received
Waiting a message...

and it get stucks.

[julianoes]

Now you have to - as I usually say - printf your way through. So now you check if the message received is the correct one, and you check what happens in the system.

Oh, and one more note:

The system starts with a "freewheeling" period where the simulation sends sensor messages including time and therefore runs PX4 until it has initialized and responds with an actuator message.

You need to send HIL_SENSOR with the timestamp increasing for quite some time, so that PX4 can initialize the estimator and everything. Only after a while will it start sending actuators back. Once it sends it back they two go into lockstep.

[hamishwillee]

Thanks Julian

Do you think it would help to move "The system starts with a "freewheeling" period where the simulation sends sensor messages including time and therefore runs PX4 until it has initialized and responds with an actuator message" as point 1 and add your note that it might need to happen for some time before the first actuator response is recieved>

My thinking is that it wouldn't hinder anyone reading this for general interest, but it would help people doing their own simulators.

[julianoes]

Sure that makes sense.

[hamishwillee]

Just discussed this offline. My suggestion does not really help.

[hamishwillee]

I'm going to leave this open for a little while in the hope that @jean-dupond is able to debug, and might be able to find some missing instruction to add. If there is no feedback when I come back to this in a couple of weeks I'll close it.

[jean-dupond]

Hi all,

Unfortunatelly I'm still stuck at the same point.

The system starts with a "freewheeling" period where the simulation sends sensor messages including time and therefore runs PX4 until it has initialized and responds with an actuator message.

(https://docs.px4.io/master/en/simulation/#lockstep-simulation)[https://docs.px4.io/master/en/simulation/#lockstep-simulation]

After connecting to the TCP port I sent in an infinite loop all messages defined in the doc ( HIL_SENSOR, HIL_GPS, HIL_OPTICAL_FLOW, HIL_STATE_QUATERNION, and HIL_RC_INPUTS_RAW ) with the timestamp increasing, but doesn't change anything.

Populating the PX4 code of printf() doesn't help much too. After lauching PX4 SITL in lockstep mode and connecting to the TCP port, seems messages are received, recognised, processed. But can't understand what triggers the PX4 start (that is, the opening of UDP ports to communicate with the ground station).

[julianoes]

The estimator and controllers needs to start in order for actuator_outputs to be sent back to the simulator.

I'm not sure what you mean with

PX4 start (that is, the opening of UDP ports to communicate with the ground station

Why do you need the communication to the ground station at this point?

[jean-dupond]

Why do you need the communication to the ground station at this point?

Well, the aim is not to replace QGroundControl, but only the simulator (the world's physics). So still QGroundControl is needed to upload a mission.

For example what printf lines shown while launching SITL with flightgear: PX4 won't send any HIL_ACTUATOR_CONTROLS message before a mission is uploaded (from QGroundControl) and the virtual drone starts flying. So I need PX4 to open a UDP port to upload a mission before expecting any actuator message through the TCP from it.

Isn't it like this?

[Jaeyoung-Lim]

@jean-dupond If you are sending identical sensor values, (particularly accel and gyro) the estimator will assume stale values and not use it

Are you adding noise to your sensor data?

[julianoes]

@jean-dupond I would look at jmavim or gazebo for reference implementations. I don't know how the flight gear one works, and it's probably without lockstep.

Are you adding noise to your sensor data?

Good point. It might not work without because PX4 thinks it's stale data.

[jean-dupond]

Hi all,

Thanks for the advises so far.

Fortunately it seems I'm getting closer and closer, but it still not work...

(Trying to give you only the useful data so that my posts is not endless...)

Now I am launching jMAVSim (make px4_sitl jmavsim) with the PX4 code full of printf lines. I can see how the "conversation" between PX4 and jMAVSim starts and am trying to reproduce something similar with Python as the simulator (so, without jMAVSim).

The "conversation" is as following:

1. PX4 sends a REQU_HIL_STATE_QUAT
2. PX4 sends an HEARTBEAT
3. jMAVSim sends an HEARTBEAT
4. jMAVSim sends a HIL_SENSOR message
5. jMAVSim sends a SYSTEM_TIME message
6. jMAVSim sends in loop one HIL_STATE_QUATERNION and two HIL_SENSOR messages

After few seconds, PX4 start sending HIL_ACTUATOR_CONTROLS messages.

So far I have tried to redo exactly the same with my Python script instead of jMAVSIM (with make px4_sitl none_iris).

I added noise in my Python script and solved some issue I had with timestamp unit sent to PX4 (had it in [s] instead of [us]).

Results are:

• PX4 confirms the simulator (my Python script) is connected through the TCP port (as before)
• printf outputs give quiet similar results while comparing logs with jMAVSim and my Python script
• QGroundControl connects to the PX4 SITL, however...

while with jMAVSim QGroundControl can locate the drone on the map after few seconds, I am stuck with a "No GPS Lock for Vehicle" message and, in the PX4's output log I can't see any ACTUATOR CONTROL sent from PX4, even after a long time.

Files to reproduce the issue are here (I am using a fresh git clone - last commit 2 Dec 2021): px4_pysim_test.zip

Here are some screeshots showing the situation:

printf lines in the simulator_mavlink.cpp file

PX4 SITL Log comparision: left = jMavSim ; right = my Python script

QGroundControl with jMavSim

QGroundControl with my Python script

Sorry for the long post, and thanks for helping.

[julianoes]

So you only send complete sensor noise/garbage to PX4, right? My guess is that the estimator can't do the initial attitude estimation because there is no clear accel and mag orientation. Try sending noise around at least x = 0, y = 0, z = 9.81.

[jean-dupond]

Hello, thanks @julianoes. I realized I gave oriented acceleration in the STATE_QUATERNION message only through the x/y/zacc entries (unit is [mG], meaning a "normal" value at rest would be x=0;y=0;z=1000, right?), but in the HIL_SENSOR message I gave indeed 0;0;0.

Else yes, my aim is to make it work first "by hand" before plugging it to the Python simulator I developed (to see what I must change in the simulator to be compatible with PX4 SITL).

Now I give a same value in both HIL_SENSOR and STATE_QUATERNION (respectively 0;0;9.81 [m/s2] and 0;0;1000 [mG]). I also played with the signs of z accels. For example in my Python simulator at rest the acceleration is negative in Z (while load factor in Z is positive).

But... no improvement.

Updated script here: px4_pysim_test.zip

Example of dummy data sent to PX4:

Velocity:
    (int) vx    [cm/s]: 0
    (int) vy    [cm/s]: 0
    (int) vz    [cm/s]: 0
    (int) ind_airspeed  [cm/s]: 0
    (int) true_airspeed     [cm/s]: 0
Ang. Velocity:
    (float) xgyro   [rad/s]: -0.0005650935108646715
    (float) ygyro   [rad/s]: -0.004852044166749646
    (float) zgyro   [rad/s]: -0.0007004715053733545
    (float) rollspeed   [rad/s]: -0.0008589121975504078
    (float) pitchspeed  [rad/s]: 0.004608458053681133
    (float) yawspeed    [rad/s]: 0.00187634294677806
Accel.:
    (float) xacc    [m/s2]: 0.003448883619322791
    (float) yacc    [m/s2]: 0.0030685466211583866
    (float) zacc    [m/s2]: 9.813063112842915
    (int) xacc  [mG]: 0
    (int) yacc  [mG]: 0
    (int) zacc  [mG]: 1000
Position:
    (float) abs_pressure    [hPa]: 1013.0372288933186
    (float) diff_pressure   [hPa]: -0.02221315554358838
    (float) pressure_alt    [?]: 3.1863939480025746e-05
    (int) lat   [degE7]: 399999631
    (int) lon   [degE7]: 169999800
    (int) alt   [mm]: 0
Attitude:
    (float) attitude_quaternion     [1]: [0.9960755821766781, 2.619293344717777e-06, 0.0012551466649958676, 0.002229107596490446]
Air:
    (float) abs_pressure    [hPa]: 1013.0372288933186
    (float) diff_pressure   [hPa]: -0.02221315554358838
    (float) pressure_alt    [?]: 3.1863939480025746e-05
    (float) temperature     [degC]: 14.97323385101384
Magneto:
    (float) xmag    [gauss]: 0.3991511908219839
    (float) ymag    [gauss]: 0.0030339483048868856
    (float) zmag    [gauss]: 0.0021670995833398964

Velocity:
    (int) vx    [cm/s]: 0
    (int) vy    [cm/s]: 0
    (int) vz    [cm/s]: 0
    (int) ind_airspeed  [cm/s]: 0
    (int) true_airspeed     [cm/s]: 0
Ang. Velocity:
    (float) xgyro   [rad/s]: 0.0031318596107133712
    (float) ygyro   [rad/s]: -0.0020878175004089305
    (float) zgyro   [rad/s]: 0.002556596648006284
    (float) rollspeed   [rad/s]: -0.001104361568673512
    (float) pitchspeed  [rad/s]: -0.0015294345574458801
    (float) yawspeed    [rad/s]: -0.0014028302404435744
Accel.:
    (float) xacc    [m/s2]: 0.0047495151118576075
    (float) yacc    [m/s2]: -0.00427956789167005
    (float) zacc    [m/s2]: 9.80819089837088
    (int) xacc  [mG]: 0
    (int) yacc  [mG]: 0
    (int) zacc  [mG]: 1000
Position:
    (float) abs_pressure    [hPa]: 1012.9603661437395
    (float) diff_pressure   [hPa]: -0.024887505725759394
    (float) pressure_alt    [?]: 0.015778218478806394
    (int) lat   [degE7]: 399999769
    (int) lon   [degE7]: 169999805
    (int) alt   [mm]: 16
Attitude:
    (float) attitude_quaternion     [1]: [1.0042574417686387, -0.004278084464768942, -0.0013816917822760178, 0.001551125893375198]
Air:
    (float) abs_pressure    [hPa]: 1012.9603661437395
    (float) diff_pressure   [hPa]: -0.024887505725759394
    (float) pressure_alt    [?]: 0.015778218478806394
    (float) temperature     [degC]: 15.046439041509725
Magneto:
    (float) xmag    [gauss]: 0.4043993920391593
    (float) ymag    [gauss]: 0.000662482884762865
    (float) zmag    [gauss]: 0.0018204204667350875

Velocity:
    (int) vx    [cm/s]: 0
    (int) vy    [cm/s]: 0
    (int) vz    [cm/s]: 0
    (int) ind_airspeed  [cm/s]: 0
    (int) true_airspeed     [cm/s]: 0
Ang. Velocity:
    (float) xgyro   [rad/s]: -0.002421445504837425
    (float) ygyro   [rad/s]: -0.004829062428998616
    (float) zgyro   [rad/s]: 0.0038505754316678063
    (float) rollspeed   [rad/s]: -0.002645309027595152
    (float) pitchspeed  [rad/s]: 0.0018081366817844014
    (float) yawspeed    [rad/s]: -0.0037928575828114174
Accel.:
    (float) xacc    [m/s2]: -0.0028454368189977162
    (float) yacc    [m/s2]: 0.0009376338054545407
    (float) zacc    [m/s2]: 9.807661605000295
    (int) xacc  [mG]: 0
    (int) yacc  [mG]: 0
    (int) zacc  [mG]: 1000
Position:
    (float) abs_pressure    [hPa]: 1012.9588256465714
    (float) diff_pressure   [hPa]: 0.041483207969807805
    (float) pressure_alt    [?]: 0.029214862555163378
    (int) lat   [degE7]: 399999627
    (int) lon   [degE7]: 170000326
    (int) alt   [mm]: 29
Attitude:
    (float) attitude_quaternion     [1]: [1.0048483392302165, -0.001767264813533257, -0.00316114585189599, -0.0023994124140765995]
Air:
    (float) abs_pressure    [hPa]: 1012.9588256465714
    (float) diff_pressure   [hPa]: 0.041483207969807805
    (float) pressure_alt    [?]: 0.029214862555163378
    (float) temperature     [degC]: 14.973357353849307
Magneto:
    (float) xmag    [gauss]: 0.404116587956381
    (float) ymag    [gauss]: 0.004280872321975508
    (float) zmag    [gauss]: -0.00456474083958884

[Jaeyoung-Lim]

@jean-dupond Can you confirm that you are also setting the time in HIL_SENSOR message?

[jean-dupond]

@jean-dupond Can you confirm that you are also setting the time in HIL_SENSOR message?

Yes, I can, since it is visible in the PX4 output thanks to printf lines (cf. previous post).

[Jaeyoung-Lim]

@jean-dupond then why is your time not changing? It appears that two consecutive messages have the same timestamp while the jmavsim progresses 4ms at a time

[jean-dupond]

@jean-dupond then why is your time not changing?

Good point. Updated the script.

Actually I was sending three messages (2x HIL_SENSOR + 1 HIL_STATE_QUATERNION) in a loop, and updated the timestamp only once per loop - so 3 messages had same timestamp.

I updated the script to send actual time stamp for each individual message, and added a sleep command to have roughly 4000 us between messages.

But still no improvement . . .

Updated scripts: px4_pysim_test.zip

Log comparison (left = jMAVSim ; right = my Python script)

**After a while, jMAVSim starts receiving ACTUATOR_CONTROLS message (in green below), not my script...***

[Jaeyoung-Lim]

@jean-dupond I would not print HIL_GPS as it doesnt influence anything, but print accel and gyro data with timestamps of HIL_SENSOR

Also it seems like your simulation time is not equally progressing 4000ms at a time but seems quite irregular. Is this intentional?

[jean-dupond]

Hi @Jaeyoung-Lim,

The irregularity of timestamps was not on purpose, simply I was waiting 4000ms between two messages in my script with a sleep command but then it takes time to execute message sending I guess.

I updated the script to have precisely 4000ms between messages and now it fits exactly the output from with jMAVSim (regarding timestamps).

Problem still not solved, but I will try now to "printf" the HIL_SENSOR data as you suggest.

Once again, thanks for the support... Very useful (and encouraging)

[jean-dupond]

Hi all,

New printf lines in the PX4 code could help solve few things (like some wrong units or wrong value of the fields_updated entry in the HIL_SENSOR message).

Now PX4 is finally sending HIL_ACTUATOR_CONTROLS messages to my Python dummy simulator (big step achieved...).

But then still QGroundControl shows the "No GPS Lock for Vehicle" even if GPS data is sent to PX4 (and even after waiting a long time).

Else, I switched to a new approach with jMAVSim. Instead of populating the PX4 code of printf lines, I intercept the communication between jMAVSim and PX4 from outside. Simply, in the Tools/jmavsim_run.sh file I change the TCP port from 4560 to (say) 4559. Then, in a Python script, I connect to both 4560 and 4559 ports, listen at both and forward all messages from one to the other (after printing them of course...).

Like this I registered all messages exchanged between jMAVSim and PX4 from start to a successful lift-off after a mission was uploaded with QGroundControl.

I extracted from that everything send by jMAVSim: message types, mean values and approx. noise amplitude for each entry.

I also rewrote my dummy Python simulator script to reproduce a similar behavior.

I am reproducing preciselly the timestamp frequency from jMAVSim: 4ms between HIL_SENSOR messages, 8ms between HIL_STATE_QUATERNION messages, 52ms between HIL_GPS messages, 1000ms between each HEARTBEAT, 4000ms between SYSTEM_TIME messages (jMAVSim didn't send anything else).

But still QGroundControl has the "No GPS Lock for Vehicle" message, and I can't understand what I do wrong.

Only thing I noticed is that jMAVSim starts sending HIL_GPS messages after some time, and I can't figure out what triggers that. From my side, I am sending HIL_GPS messages as soon as the simulator connects to PX4.

Files to reproduce what I do are here: dummy_sim_test.zip

Here are some graphs comparing entries from jMAVSim and from my dummy Python simulator.

Other graphs are in the ZIP archive linked above.

[jean-dupond]

Hi all,

Just after posting that I made another try and... finally it works :-)

I was sending a fix_type of 0 in the HIL_GPS messages (which is the initial value sent by jMAVSim).

Putting 3 instead solved the issue (wrongly though it was to activate some optional correction - while a value > 1 is mandatory).

Thanks all for the very useful time and support. Next step for me will be to plug my real Python simulator (that is, to build a Flight model of the IRIS drone and play with it).

Thank you very much again,

[jean-dupond]

Hi all,

I cleaned my scripts and publish them here, so that anyone else can use them as a starting point in the case of developing a own simulator like me.

The archive contains:

• the script intercepting MAVLink messages between jMAVSim and PX4 SITL to see exactly which messages are sent ;
• and the "dummy simulator" script reproducing a similar behavior without jMAVSim.

px4_sitl_python_sim_testing.zip

Maybe these scripts could be improved by someone mastering pymavlink better than me and put somewhere in the documentation?

Else, I think this thread can be closed.

[julianoes]

I was sending a fix_type of 0 in the HIL_GPS messages (which is the initial value sent by jMAVSim).

Aaaahha! Thanks for publishing this. I think we should probably try to have some sort of template/example simulator repo for someone to start from, so they wouldn't have to go through the debugging that you just went through.

[hamishwillee]

Aaaahha! Thanks for publishing this. I think we should probably try to have some sort of template/example simulator repo for someone to start from, so they wouldn't have to go through the debugging that you just went through.

That would be cool. If not, can we summarise the main details/gotchas in a set of bullet points?