Linking a custom simulator in Python to a PX4 SITL?

jean.dupond · November 12, 2021, 6:56am

Hi,

We would like to link some own simulator (an alternative to jsbsim written in Python) with a PX4 SITL.

According to the PX4’s doc, the way to start a standalone PX4 SITL is like this:

$ make px4_sitl none_iris

(with “iris” to be adapted according to the situation)

After being launched the terminal indicates the simulator must connect to a TCP port:

(...)
  PWM_MIN: curr: 1000 -> new: 1075
  GPS_UBX_DYNMODEL: curr: 7 -> new: 6
* SYS_AUTOCONFIG: curr: 1 -> new: 0
INFO  [dataman] Unknown restart, data manager file './dataman' size is 11798680 bytes
INFO  [simulator] Waiting for simulator to accept connection on TCP port 4560

In Python then, I connect to the 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))

The connection works and the terminal indicates:

INFO  [simulator] Simulator connected on TCP port 4560.

But then it gets stuck there.

Logically the PX4 should also open a UDP port so that it can be monitored. For example, when using an existing simulator like flightgear with make px4_sitl_nolockstep flightgear_rascal I can connect to the UDP port without issue and monitor the drone attitude.

In the doc (Simulation | PX4 User Guide) it’s said the simulator must send MAVLink messages as HIL_SENSOR or HIL_GPS and receive HIL_ACTUATOR_CONTROLS from the PX4. Always with pymavlink I send all the defined messages, eg:

# Send HIL_GPS

vehicle.mav.hil_gps_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)
    fix_type            = 0             , # 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. (type:uint8_t)
    lat                 = 40            , # Latitude (WGS84) [degE7] (type:int32_t)
    lon                 = 17            , # Longitude (WGS84) [degE7] (type:int32_t)
    alt                 = 0             , # Altitude (MSL). Positive for up. [mm] (type:int32_t)
    eph                 = 0             , # GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX (type:uint16_t)
    epv                 = 0             , # GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX (type:uint16_t)
    vel                 = 0             , # GPS ground speed. If unknown, set to: 65535 [cm/s] (type:uint16_t)
    vn                  = 0             , # GPS velocity in north direction in earth-fixed NED frame [cm/s] (type:int16_t)
    ve                  = 0             , # GPS velocity in east direction in earth-fixed NED frame [cm/s] (type:int16_t)
    vd                  = 0             , # GPS velocity in down direction in earth-fixed NED frame [cm/s] (type:int16_t)
    cog                 = 65535         , # Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 [cdeg] (type:uint16_t)
    satellites_visible  = 0             , # Number of satellites visible. If unknown, set to 255 (type:uint8_t)
    id                  = 0             , # GPS ID (zero indexed). Used for multiple GPS inputs (type:uint8_t)
    yaw                 = 36000         , # Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north [cdeg] (type:uint16_t)
)

but the TCP connection doesn’t receive any HIL_ACTUATOR_CONTROLS message.

So:

After the simulator connects to the TCP port, nothing happens, even if needed MAVLink messages are sent from the simulator through this connection.
The UDP port is not opened.

Please, how to make it work?..

Thanks for the support

jiawan · March 7, 2022, 7:38pm

Hi Jean,
I’m having the same problem. Did you manage to solve this?
THX
Jia

jean.dupond · March 21, 2022, 6:25am

Hi Jia,

I opened o similar thread here, were I got help and published some script to start with:

github.com/PX4/PX4-user_guide

SITL with custom simulator

opened 07:33AM - 30 Nov 21 UTC

closed 07:53AM - 10 Dec 21 UTC

jean-dupond

question

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](https://docs.px4.io/master/en/simulation/#simulator-mavlink-api) 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](https://docs.px4.io/master/en/simulation/#simulator-mavlink-api)** PX4 inputs are: **HIL_SENSOR**, **HIL_GPS**, etc... mavlink messages PX4 outputs are: **HIL_ACTUATOR_CONTROLS** message **[SITL Simulation Environment](https://docs.px4.io/master/en/simulation/#sitl-simulation-environment)** PX4 SITL and the simulator communicate through a TCP connection on port 4560 **[Starting/Building SITL Simulation](https://docs.px4.io/master/en/simulation/#starting-building-sitl-simulation)** To start PX4 with a custom simulator: ```make px4_sitl none_iris``` **[Lockstep Simulation](https://docs.px4.io/master/en/simulation/#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?

Good luck

jiawan · April 6, 2022, 6:42pm

Hi Jean,
Thanks very much for the reply and the very useful page.
Now I got the basic communication runs. The main problem for me is that I didn’t add any noise to the sensor data.
BTW, how far did you get with your own simulator (just out of curiosity)?
Best
Jia

jean.dupond · April 7, 2022, 8:13am

Strange for the noise issue. Downloaded the script again and launched it and for me it works:

Waiting to connect...
1 <== COMMAND_LONG {target_system : 0, target_component : 0, command : 511, confirmation : 0, param1 : 115.0, param2 : 5000.0, param3 : 0.0, param4 : 0.0, param5 : 0.0, param6 : 0.0, param7 : 0.0}
2 <== HEARTBEAT {type : 0, autopilot : 12, base_mode : 0, custom_mode : 0, system_status : 0, mavlink_version : 3}
3 --> HEARTBEAT { type : 0 , autopilot : 0 , base_mode : 0 , custom_mode : 0 , system_status : 0 , mavlink_version : 3 }
4 --> HIL_STATE_QUATERNION { time_usec : 1649318095859497 , attitude_quaternion : [1.0, 0.0, 0.0, 0.0] , rollspeed : 0.0 , pitchspeed : 0.0 , yawspeed : 0.0 , lat : 473977422 , lon : 85455939 , alt : 488003 , vx : 0 , vy : 0 , vz : 0 , ind_airspeed : 0 , true_airspeed : 28 , xacc : 0 , yacc : 0 , zacc : 0 }
5 --> HIL_SENSOR { time_usec : 1649318095859497 , xacc : 0.0869873774433021 , yacc : -0.018836107911570643 , zacc : -9.736397137273249 , xgyro : -0.0011116222528289566 , ygyro : -0.012375404965121658 , zgyro : -0.008219845225238226 , xmag : 0.22019787082162118 , ymag : 0.01429072615776518 , zmag : 0.4204610766437646 , abs_pressure : 955.9799941836161 , diff_pressure : 0.0 , pressure_alt : 488.00686896581556 , temperature : 0.0 , fields_updated : 7167 , id : 0 }
6 --> HIL_GPS { time_usec : 1649318095863497 , fix_type : 3 , lat : 473977419 , lon : 85455942 , alt : 488023 , eph : 30 , epv : 40 , vel : 0 , vn : 0 , ve : 0 , vd : 0 , cog : 0 , satellites_visible : 10 , id : 0 , yaw : 0 }
7 --> HIL_SENSOR { time_usec : 1649318095863497 , xacc : 0.0752508736385988 , yacc : 0.016565314824787358 , zacc : -9.84150226301041 , xgyro : 0.010630751974958321 , ygyro : 0.014001448757552356 , zgyro : 0.005044346631781527 , xmag : 0.20944257943860095 , ymag : 0.009386470910912645 , zmag : 0.4310428877361716 , abs_pressure : 955.9999709503526 , diff_pressure : 0.0 , pressure_alt : 487.79657919856135 , temperature : 0.0 , fields_updated : 7167 , id : 0 }
8 --> HIL_STATE_QUATERNION { time_usec : 1649318095867497 , attitude_quaternion : [1.0, 0.0, 0.0, 0.0] , rollspeed : 0.0 , pitchspeed : 0.0 , yawspeed : 0.0 , lat : 473977420 , lon : 85455942 , alt : 488025 , vx : 0 , vy : 0 , vz : 0 , ind_airspeed : 0 , true_airspeed : 30 , xacc : 0 , yacc : 0 , zacc : 0 }
9 --> HIL_SENSOR { time_usec : 1649318095867497 , xacc : -0.09792464618859963 , yacc : 0.07661836489885276 , zacc : -9.745354685040779 , xgyro : -0.002319270345839266 , ygyro : -0.00216241057202192 , zgyro : -0.010848278853378322 , xmag : 0.20846972973600333 , ymag : 0.0167925816317799 , zmag : 0.43064261491068967 , abs_pressure : 955.9730280679087 , diff_pressure : 0.0 , pressure_alt : 488.11231697176646 , temperature : 0.0 , fields_updated : 7167 , id : 0 }
10 --> HIL_SENSOR { time_usec : 1649318095871497 , xacc : -0.0833811074557665 , yacc : -0.025542583234145422 , zacc : -9.868183840341352 , xgyro : -0.019197703466759863 , ygyro : -0.009403836443466025 , zgyro : -0.015447651202046933 , xmag : 0.2070071156095079 , ymag : 0.014988646777674208 , zmag : 0.42259694970827966 , abs_pressure : 955.9772094477092 , diff_pressure : 0.0 , pressure_alt : 487.9135687667235 , temperature : 0.0 , fields_updated : 7167 , id : 0 }
11 --> HIL_STATE_QUATERNION { time_usec : 1649318095875497 , attitude_quaternion : [1.0, 0.0, 0.0, 0.0] , rollspeed : 0.0 , pitchspeed : 0.0 , yawspeed : 0.0 , lat : 473977421 , lon : 85455940 , alt : 487997 , vx : 0 , vy : 0 , vz : 0 , ind_airspeed : 0 , true_airspeed : 6 , xacc : 0 , yacc : 0 , zacc : 0 }
12 --> HIL_SENSOR { time_usec : 1649318095875497 , xacc : 0.0956422009959455 , yacc : 0.04505997811838998 , zacc : -9.85112514639019 , xgyro : -0.016386304108879274 , ygyro : -0.01852849170240611 , zgyro : -0.013144381020113128 , xmag : 0.2118406592834444 , ymag : 0.01605693716858827 , zmag : 0.4224176215166938 , abs_pressure : 955.9605251453867 , diff_pressure : 0.0 , pressure_alt : 488.1613093371457 , temperature : 0.0 , fields_updated : 7167 , id : 0 }
13 --> HIL_SENSOR { time_usec : 1649318095879497 , xacc : -0.07078192987561797 , yacc : 0.09297171213041788 , zacc : -9.902695513797285 , xgyro : -0.004591493814822436 , ygyro : 0.0013962442963718934 , zgyro : -0.007217065778046181 , xmag : 0.21547586397289487 , ymag : 0.013000935844679904 , zmag : 0.42294048281635055 , abs_pressure : 955.9897149770569 , diff_pressure : 0.0 , pressure_alt : 487.76562067997287 , temperature : 0.0 , fields_updated : 7167 , id : 0 }

Else yes, I could implement the IRIS drone in my simulator, inspired by what I found into PX4-Autopilot/Tools/sitl_gazebo/ (IRIS model in sitl_gazebo/models/iris, useful data on how the propulsion is implemented in sitl_gazebo/src/gazebo_motor_model.cpp and sitl_gazebo/include/common.h).

Just to know: gyroscopic moments are very important and should not be neglected (else the drone just won’t work), so the simulator must handle them.

jiawan · May 3, 2022, 7:26am

Hi Jean, sorry I didn’t make it clear. I mean the problem with my code is that there was no noise. So I couldn’t make it work. After reading the links you posted, I fixed it (adding some noise).

BTW, did you try the HITL simulation? To make a HITL simulator is my final goal, but couldn’t make any progress even with the jMAVSim.