PX4 Mavlink Backend

File: px4_mavlink_backend.py
Author: Marcelo Jacinto (marcelo.jacinto@tecnico.ulisboa.pt)
Description: File that implements the Mavlink Backend for communication/control with/of the vehicle simulation
License: BSD-3-Clause. Copyright (c) 2023, Marcelo Jacinto. All rights reserved.

Classes:

`PX4MavlinkBackend`	The Mavlink Backend used to receive the vehicle's state and sensor data in order to send to PX4 through mavlink.
`PX4MavlinkBackendConfig`	An auxiliary data class used to store all the configurations for the mavlink communications.

class pegasus.simulator.logic.backends.px4_mavlink_backend.PX4MavlinkBackend

Bases: Backend

The Mavlink Backend used to receive the vehicle’s state and sensor data in order to send to PX4 through mavlink. It also receives via mavlink the thruster commands to apply to each vehicle rotor.

Methods:

`__del__`()	Gets called when the PX4MavlinkBackend object gets destroyed.
`__init__`([config])	Initialize the PX4MavlinkBackend
`handle_control`(time_usec, controls, mode, flags)	Method that when received a control message, compute the forces simulated force that should be applied on each rotor of the vehicle
`input_reference`()	Method that when implemented, should return a list of desired angular velocities to apply to the vehicle rotors
`poll_mavlink_messages`()	Method that is used to check if new mavlink messages were received
`re_initialize_interface`()	Auxiliar method used to get the MavlinkInterface to reset the MavlinkInterface to its initial state
`reset`()	For now does nothing.
`send_gps_msgs`(time_usec)	Method that is used to send simulated GPS data through the mavlink protocol.
`send_ground_truth`(time_usec)	Method that is used to send the groundtruth data of the vehicle through mavlink
`send_heartbeat`([mav_type])	Method that is used to publish an heartbear through mavlink protocol
`send_sensor_msgs`(time_usec)	Method that when invoked, will send the simulated sensor data through mavlink
`send_vision_msgs`(time_usec)	Method that is used to send simulated vision/mocap data through the mavlink protocol.
`start`()	Method that handles the begining of the simulation of vehicle.
`stop`()	Method that when called will handle the stopping of the simulation of vehicle.
`update`(dt)	Method that is called at every physics step to send data to px4 and receive the control inputs via mavlink
`update_bar_data`(data)	Gets called by the 'update_sensor' method to update the current Barometer data
`update_gps_data`(data)	Gets called by the 'update_sensor' method to update the current GPS data
`update_graphical_sensor`(sensor_type, data)	Method that when implemented, should handle the receival of graphical sensor data
`update_imu_data`(data)	Gets called by the 'update_sensor' method to update the current IMU data
`update_mag_data`(data)	Gets called by the 'update_sensor' method to update the current Vision data
`update_sensor`(sensor_type, data)	Method that is used as callback for the vehicle for every iteration that a sensor produces new data.
`update_state`(state)	Method that is used as callback and gets called at every physics step with the current state of the vehicle.
`update_vision_data`(data)	Method that 'in the future' will get called by the 'update_sensor' method to update the current Vision data This callback is currently not being called (TODO in a future simulator version) :param data: The data produced by an Vision sensor :type data: dict
`wait_for_first_hearbeat`()	Responsible for waiting for the first hearbeat.

__del__(): Gets called when the PX4MavlinkBackend object gets destroyed. When this happens, we make sure to close any mavlink connection open for this vehicle.

__init__(config=<pegasus.simulator.logic.backends.px4_mavlink_backend.PX4MavlinkBackendConfig object>)

Initialize the PX4MavlinkBackend

Parameters:: config (PX4MavlinkBackendConfig) – The configuration class for the PX4MavlinkBackend. Defaults to PX4MavlinkBackendConfig().

handle_control(time_usec, controls, mode, flags)

Method that when received a control message, compute the forces simulated force that should be applied on each rotor of the vehicle

Parameters:

time_usec (int) – The total time elapsed since the simulation started - Ignored argument
controls (list) – A list of ints which contains the thrust_control received via mavlink
flags – Ignored argument

input_reference(): Method that when implemented, should return a list of desired angular velocities to apply to the vehicle rotors

poll_mavlink_messages(): Method that is used to check if new mavlink messages were received

re_initialize_interface(): Auxiliar method used to get the MavlinkInterface to reset the MavlinkInterface to its initial state

reset(): For now does nothing. Here for compatibility purposes only

send_gps_msgs(time_usec)

Method that is used to send simulated GPS data through the mavlink protocol.

Parameters:: time_usec (int) – The total time elapsed since the simulation started

send_ground_truth(time_usec)

Method that is used to send the groundtruth data of the vehicle through mavlink

Parameters:: time_usec (int) – The total time elapsed since the simulation started

send_heartbeat(mav_type=pymavlink.mavutil.mavlink.MAV_TYPE_GENERIC)

Method that is used to publish an heartbear through mavlink protocol

Parameters:: mav_type (int) – The ID that indicates the type of vehicle. Defaults to MAV_TYPE_GENERIC=0

send_sensor_msgs(time_usec)

Method that when invoked, will send the simulated sensor data through mavlink

Parameters:: time_usec (int) – The total time elapsed since the simulation started

send_vision_msgs(time_usec)

Method that is used to send simulated vision/mocap data through the mavlink protocol.

Parameters:: time_usec (int) – The total time elapsed since the simulation started

start(): Method that handles the begining of the simulation of vehicle. It will try to open the mavlink connection interface and also attemp to launch px4 in a background process if that option as specified in the config class

stop(): Method that when called will handle the stopping of the simulation of vehicle. It will make sure that any open mavlink connection will be closed and also that the PX4 background process gets killed (if it was auto-initialized)

update(dt)

Method that is called at every physics step to send data to px4 and receive the control inputs via mavlink

Parameters:: dt (float) – The time elapsed between the previous and current function calls (s).

update_bar_data(data)

Gets called by the ‘update_sensor’ method to update the current Barometer data

Parameters:: data (dict) – The data produced by an Barometer sensor

update_gps_data(data)

Gets called by the ‘update_sensor’ method to update the current GPS data

Parameters:: data (dict) – The data produced by an GPS sensor

update_graphical_sensor(sensor_type, data)

Method that when implemented, should handle the receival of graphical sensor data

Parameters:

sensor_type (str) – A name that describes the type of sensor (for example MonocularCamera)
data (dict) – A dictionary that contains the data produced by the sensor

update_imu_data(data)

Gets called by the ‘update_sensor’ method to update the current IMU data

Parameters:: data (dict) – The data produced by an IMU sensor

update_mag_data(data)

Gets called by the ‘update_sensor’ method to update the current Vision data

Parameters:: data (dict) – The data produced by an Vision sensor

update_sensor(sensor_type, data)

Method that is used as callback for the vehicle for every iteration that a sensor produces new data. Only the IMU, GPS, Barometer and Magnetometer sensor data are stored to be sent through mavlink. Every other sensor data that gets passed to this function is discarded.

Parameters:

sensor_type (str) – A name that describes the type of sensor
data (dict) – A dictionary that contains the data produced by the sensor

update_state(state)

Method that is used as callback and gets called at every physics step with the current state of the vehicle. This state is then stored in order to be sent as groundtruth via mavlink

Parameters:: state (State) – The current state of the vehicle.

update_vision_data(data): Method that ‘in the future’ will get called by the ‘update_sensor’ method to update the current Vision data This callback is currently not being called (TODO in a future simulator version) :param data: The data produced by an Vision sensor :type data: dict

wait_for_first_hearbeat(): Responsible for waiting for the first hearbeat. This method is locking and will only return if an hearbeat is received via mavlink. When this first heartbeat is received poll for mavlink messages

class pegasus.simulator.logic.backends.px4_mavlink_backend.PX4MavlinkBackendConfig

Bases: BackendConfig

An auxiliary data class used to store all the configurations for the mavlink communications.

Methods:

__init__([config])

Initialize the PX4MavlinkBackendConfig class

__init__(config={})

Initialize the PX4MavlinkBackendConfig class

Parameters:: config (dict) – A Dictionary that contains all the parameters for configuring the Mavlink interface - it can be empty or only have some of the parameters used by this backend.

Examples

The dictionary default parameters are

>>> {"vehicle_id": 0,
>>>  "connection_type": "tcpin",
>>>  "connection_ip": "localhost",
>>>  "connection_baseport": 4560,
>>>  "px4_autolaunch": True,
>>>  "px4_dir": "PegasusInterface().px4_path",
>>>  "px4_vehicle_model": "gazebo-classic_iris",
>>>  "enable_lockstep": True,
>>>  "num_rotors": 4,
>>>  "input_offset": [0.0, 0.0, 0.0, 0.0],
>>>  "input_scaling": [1000.0, 1000.0, 1000.0, 1000.0],
>>>  "zero_position_armed": [100.0, 100.0, 100.0, 100.0],
>>>  "update_rate": 250.0
>>> }