From 1d2f0053ea6f058964fab6c7c64635b0ef1cc504 Mon Sep 17 00:00:00 2001 From: Davide iafrate Date: Fri, 27 Mar 2026 02:12:02 +0000 Subject: [PATCH 1/4] fix: add mag/baro HIL sensor simulation, fix integrator in PX4 multiroto MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Send magnetometer and barometer data in HIL_SENSOR with correct fields_updated bitmask (ACCEL|GYRO|MAG|BARO = 7167) — previously only ACCEL+GYRO were sent, causing PX4 to time out on MAG and BARO and fall back to SIH internal sensors - Switch physics integrator from Radau (implicit, expensive) to RK45 with max_step=0.05 to avoid double-stepping at typical engine delta_times (~9ms) Co-Authored-By: Claude Sonnet 4.6 --- rotorpy/vehicles/px4_multirotor.py | 66 ++++++++++++++++++++++++------ 1 file changed, 53 insertions(+), 13 deletions(-) diff --git a/rotorpy/vehicles/px4_multirotor.py b/rotorpy/vehicles/px4_multirotor.py index 50a60a1..1ba7400 100644 --- a/rotorpy/vehicles/px4_multirotor.py +++ b/rotorpy/vehicles/px4_multirotor.py @@ -57,7 +57,7 @@ def __init__( lockstep=True, integrator_kwargs=None ): - integrator_kwargs = integrator_kwargs if integrator_kwargs is not None else {'method':'Radau', 'rtol':1e-3, 'atol':1e-6, 'max_step':0.01} + integrator_kwargs = integrator_kwargs if integrator_kwargs is not None else {'method':'RK45', 'rtol':1e-2, 'atol':1e-4, 'max_step':0.05} # If no initial state passed, initialize to hover at origin if initial_state is None: initial_state = { @@ -88,6 +88,7 @@ def __init__( self._autopilot_controller = autopilot_controller self._lockstep_enabled = lockstep + self._last_control = {'cmd_motor_speeds': np.zeros(quad_params['num_rotors'])} @staticmethod def enu_to_geodetic( @@ -220,29 +221,67 @@ def _send_hil_state_quaternion(self, state, statedot): int(a_frd_mg[0]), int(a_frd_mg[1]), int(a_frd_mg[2]) ) + # Earth magnetic field at reference origin (lat=40°N, lon=-74.3°W) in NED frame, gauss. + # Values approximate WMM at sea level for New York area. + _MAG_NED_GAUSS = np.array([0.198, -0.030, 0.448]) + + def _mag_body_frd(self, state) -> np.ndarray: + """Return Earth magnetic field vector in body FRD frame (gauss).""" + # NED -> ENU: [N, E, D] -> [E, N, -D] + mag_enu = np.array([self._MAG_NED_GAUSS[1], + self._MAG_NED_GAUSS[0], + -self._MAG_NED_GAUSS[2]]) + # Rotate from ENU world frame to FLU body frame via quaternion [qx, qy, qz, qw] + qx, qy, qz, qw = state['q'] + R_WB = np.array([ + [1 - 2*(qy**2 + qz**2), 2*(qx*qy - qz*qw), 2*(qx*qz + qy*qw)], + [2*(qx*qy + qz*qw), 1 - 2*(qx**2 + qz**2), 2*(qy*qz - qx*qw)], + [2*(qx*qz - qy*qw), 2*(qy*qz + qx*qw), 1 - 2*(qx**2 + qy**2)], + ]) + mag_flu = R_WB.T @ mag_enu + # FLU -> FRD: negate Y and Z + return np.array([mag_flu[0], -mag_flu[1], -mag_flu[2]]) + + def _baro(self, state): + """Return (abs_pressure_hpa, pressure_alt_m, temperature_c) via standard atmosphere.""" + alt_m = float(state['x'][2]) # ENU z = altitude above reference + P0 = 101325.0 # Pa, sea-level standard pressure + T0 = 288.15 # K, sea-level standard temperature + abs_pressure_pa = P0 * (1.0 - alt_m / 44330.0) ** 5.2561 + abs_pressure_hpa = abs_pressure_pa / 100.0 + pressure_alt_m = 44330.0 * (1.0 - (abs_pressure_pa / P0) ** (1.0 / 5.2561)) + temperature_c = T0 - 0.0065 * alt_m - 273.15 + return abs_pressure_hpa, pressure_alt_m, temperature_c + def _send_hil_sensor(self, state, statedot): """ Send HIL_SENSOR message to PX4. - + Args: state: Current vehicle state statedot: State derivative (computed externally) """ # Get IMU measurements - a_ned, omega_ned = self._imu(state, statedot) + a_frd, omega_frd = self._imu(state, statedot) + + # Magnetometer: Earth field rotated into body FRD frame (gauss) + mag_frd = self._mag_body_frd(state) + + # Barometer: standard atmosphere from ENU altitude + abs_pressure_hpa, pressure_alt_m, temperature_c = self._baro(state) - # Only flag accel/gyro as updated (exclude mag and baro-related fields) - updated_bitmask = SensorSource.ACCEL | SensorSource.GYRO + updated_bitmask = (SensorSource.ACCEL | SensorSource.GYRO + | SensorSource.MAG | SensorSource.BARO) self.conn.mav.hil_sensor_send( int(self.t * 1e6), - *tuple(a_ned), - *tuple(omega_ned), - *(0.0, 0.0, 0.0), # Magnetometer (body frame) - 0.0, # Abs pressure - 0, # Differential pressure - 0.0, # Altitude from pressure - 25, # Temperature + *tuple(a_frd), + *tuple(omega_frd), + *tuple(mag_frd), + abs_pressure_hpa, + 0.0, # Differential pressure (not simulated) + pressure_alt_m, + temperature_c, fields_updated=updated_bitmask, ) @@ -258,9 +297,10 @@ def step(self, state, control, t_step): if self._autopilot_controller: px4_control = self._fetch_latest_px4_control(blocking=self._lockstep_enabled) if px4_control is not None: + self._last_control = px4_control control = px4_control else: - control = {'cmd_motor_speeds': np.zeros(self.num_rotors)} + control = self._last_control else: # In this case we use the control provided by the external controller pass From 94dd048fb2a07a1af325d356787eb749fe7757ac Mon Sep 17 00:00:00 2001 From: Davide iafrate Date: Sun, 29 Mar 2026 23:34:09 +0000 Subject: [PATCH 2/4] feat: make torch an optional dependency for the batched simulator torch, torchdiffeq, roma, and opt-einsum are only required for the batched simulator (BatchedMultirotor, simulate_batch, BatchedSE3Control, etc.). Moving them to an optional extra avoids forcing all users to install PyTorch (~2 GB) when they only need the standard single-drone simulator. - pyproject.toml: remove the four packages from core dependencies and add a new `batched` extra; also include them in `all` - Wrap top-level torch/roma/torchdiffeq imports with try/except in every file that mixes batched and non-batched classes, so those modules remain importable without the extra installed Install the batched simulator with: pip install rotorpy[batched] Co-Authored-By: Claude Sonnet 4.6 --- pyproject.toml | 14 ++++++++++---- rotorpy/controllers/quadrotor_control.py | 7 +++++-- rotorpy/sensors/imu.py | 5 ++++- rotorpy/simulate.py | 7 +++++-- rotorpy/trajectories/circular_traj.py | 5 ++++- rotorpy/trajectories/hover_traj.py | 5 ++++- rotorpy/trajectories/lissajous_traj.py | 5 ++++- rotorpy/trajectories/minsnap.py | 5 ++++- rotorpy/trajectories/traj_template.py | 5 ++++- rotorpy/vehicles/multirotor.py | 9 ++++++--- rotorpy/wind/default_winds.py | 5 ++++- rotorpy/wind/dryden_winds.py | 5 ++++- 12 files changed, 58 insertions(+), 19 deletions(-) diff --git a/pyproject.toml b/pyproject.toml index e70e45e..26efe2a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -29,14 +29,16 @@ dependencies = [ 'pandas', 'tqdm', 'gymnasium', - 'roma', # For batched sim - 'torch>=1.11.0', # For batched sim - 'torchdiffeq', # For batched sim - 'opt-einsum', # For batched sim 'timed_count', # Only for ardupilot sitl example ] [project.optional-dependencies] +batched = [ + 'torch>=1.11.0', + 'torchdiffeq', + 'roma', + 'opt-einsum', +] learning = [ 'stable_baselines3', 'tensorboard', @@ -58,6 +60,10 @@ px4 = [ 'pymavlink', ] all = [ + "torch>=1.11.0", + "torchdiffeq", + "roma", + "opt-einsum", "stable_baselines3", "tensorboard", "pytest", diff --git a/rotorpy/controllers/quadrotor_control.py b/rotorpy/controllers/quadrotor_control.py index e203485..9d8e05c 100644 --- a/rotorpy/controllers/quadrotor_control.py +++ b/rotorpy/controllers/quadrotor_control.py @@ -1,6 +1,9 @@ import numpy as np -import torch -import roma +try: + import torch + import roma +except ImportError: + pass from scipy.spatial.transform import Rotation class SE3Control(object): diff --git a/rotorpy/sensors/imu.py b/rotorpy/sensors/imu.py index f07148b..b2edf40 100644 --- a/rotorpy/sensors/imu.py +++ b/rotorpy/sensors/imu.py @@ -1,6 +1,9 @@ import numpy as np from scipy.spatial.transform import Rotation -import torch +try: + import torch +except ImportError: + pass import copy class Imu: diff --git a/rotorpy/simulate.py b/rotorpy/simulate.py index 4674a87..1552168 100644 --- a/rotorpy/simulate.py +++ b/rotorpy/simulate.py @@ -2,8 +2,11 @@ from enum import Enum import copy import numpy as np -import roma -import torch +try: + import roma + import torch +except ImportError: + pass from numpy.linalg import norm from scipy.spatial.transform import Rotation from time import perf_counter diff --git a/rotorpy/trajectories/circular_traj.py b/rotorpy/trajectories/circular_traj.py index a09a950..e6e37e5 100644 --- a/rotorpy/trajectories/circular_traj.py +++ b/rotorpy/trajectories/circular_traj.py @@ -1,5 +1,8 @@ import numpy as np -import torch +try: + import torch +except ImportError: + pass import sys class ThreeDCircularTraj(object): diff --git a/rotorpy/trajectories/hover_traj.py b/rotorpy/trajectories/hover_traj.py index 0b1b67c..653962b 100644 --- a/rotorpy/trajectories/hover_traj.py +++ b/rotorpy/trajectories/hover_traj.py @@ -1,5 +1,8 @@ import numpy as np -import torch +try: + import torch +except ImportError: + pass class HoverTraj(object): """ diff --git a/rotorpy/trajectories/lissajous_traj.py b/rotorpy/trajectories/lissajous_traj.py index 2b5db6a..d45833d 100644 --- a/rotorpy/trajectories/lissajous_traj.py +++ b/rotorpy/trajectories/lissajous_traj.py @@ -1,5 +1,8 @@ import numpy as np -import torch +try: + import torch +except ImportError: + pass """ Lissajous curves are defined by trigonometric functions parameterized in time. diff --git a/rotorpy/trajectories/minsnap.py b/rotorpy/trajectories/minsnap.py index 83308a6..dda4c8d 100644 --- a/rotorpy/trajectories/minsnap.py +++ b/rotorpy/trajectories/minsnap.py @@ -5,7 +5,10 @@ import cvxopt from scipy.linalg import block_diag from typing import List -import torch +try: + import torch +except ImportError: + pass def cvxopt_solve_qp(P, q, G=None, h=None, A=None, b=None): """ diff --git a/rotorpy/trajectories/traj_template.py b/rotorpy/trajectories/traj_template.py index e11516b..45d68e0 100644 --- a/rotorpy/trajectories/traj_template.py +++ b/rotorpy/trajectories/traj_template.py @@ -2,7 +2,10 @@ Imports """ import numpy as np -import torch +try: + import torch +except ImportError: + pass class TrajTemplate(object): """ diff --git a/rotorpy/vehicles/multirotor.py b/rotorpy/vehicles/multirotor.py index 8bc2171..1e28c27 100644 --- a/rotorpy/vehicles/multirotor.py +++ b/rotorpy/vehicles/multirotor.py @@ -8,9 +8,12 @@ from scipy.spatial.transform import Rotation as R # imports for Batched Dynamics -import torch -from torchdiffeq import odeint -import roma +try: + import torch + from torchdiffeq import odeint + import roma +except ImportError: + pass import time diff --git a/rotorpy/wind/default_winds.py b/rotorpy/wind/default_winds.py index 5a66e0d..a2d0e22 100644 --- a/rotorpy/wind/default_winds.py +++ b/rotorpy/wind/default_winds.py @@ -1,6 +1,9 @@ import numpy as np import sys -import torch +try: + import torch +except ImportError: + pass import math import random diff --git a/rotorpy/wind/dryden_winds.py b/rotorpy/wind/dryden_winds.py index fe045cc..1bb0bef 100644 --- a/rotorpy/wind/dryden_winds.py +++ b/rotorpy/wind/dryden_winds.py @@ -1,5 +1,8 @@ import numpy as np -import torch +try: + import torch +except ImportError: + pass import os import sys From 8253fe611b5e151b0270cbe039669321945f74e9 Mon Sep 17 00:00:00 2001 From: Davide iafrate Date: Wed, 1 Apr 2026 01:40:13 +0000 Subject: [PATCH 3/4] fix: refine control handling in PX4Multirotor step method --- rotorpy/vehicles/px4_multirotor.py | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/rotorpy/vehicles/px4_multirotor.py b/rotorpy/vehicles/px4_multirotor.py index 1ba7400..abf2829 100644 --- a/rotorpy/vehicles/px4_multirotor.py +++ b/rotorpy/vehicles/px4_multirotor.py @@ -285,8 +285,8 @@ def _send_hil_sensor(self, state, statedot): fields_updated=updated_bitmask, ) - def step(self, state, control, t_step): - + def step(self, state, control, t_step): + # Compute state derivative once for state and messages # and send both HIL messages statedot = self.statedot(state, control, 0.0) @@ -298,14 +298,13 @@ def step(self, state, control, t_step): px4_control = self._fetch_latest_px4_control(blocking=self._lockstep_enabled) if px4_control is not None: self._last_control = px4_control - control = px4_control else: - control = self._last_control + pass # Do not modify _last_control else: # In this case we use the control provided by the external controller - pass - - state = super().step(state, control, t_step) + self._last_control = control + + state = super().step(state, self._last_control, t_step) self.state = state self.t += t_step From 449ecc1c74d1d1d65b9e961533a0fd0da8e12774 Mon Sep 17 00:00:00 2001 From: Davide iafrate Date: Tue, 7 Apr 2026 07:12:13 +0000 Subject: [PATCH 4/4] feat: PX4Multirotor performance and lockstep improvements - Add fixed-step RK4 integrator to Multirotor base class (7x faster than solve_ivp for small timesteps, identical accuracy at dt<=4ms). PX4Multirotor enables it by default. - Precompute IMU measurements once per step and pass to HIL messages, avoiding redundant computation. - Add configurable lockstep_timeout parameter (default 0.002s) with retry loop in _fetch_latest_px4_control. Allows DTPS bridge mode to use longer timeouts for higher-latency round trips. - Add per-step timing instrumentation (printed every 500 steps). --- rotorpy/vehicles/multirotor.py | 41 ++++++++++---- rotorpy/vehicles/px4_multirotor.py | 91 +++++++++++++++++++++++------- 2 files changed, 100 insertions(+), 32 deletions(-) diff --git a/rotorpy/vehicles/multirotor.py b/rotorpy/vehicles/multirotor.py index 1e28c27..c9ee71c 100644 --- a/rotorpy/vehicles/multirotor.py +++ b/rotorpy/vehicles/multirotor.py @@ -179,12 +179,15 @@ def __init__(self, quad_params, initial_state = {'x': np.array([0,0,0]), self.aero = aero - # Integrator settings. + # Integrator settings. if integrator_kwargs is None: self.integrator_kwargs = {'method':'RK45'} else: self.integrator_kwargs = integrator_kwargs + # Fixed-step RK4 option (much faster than solve_ivp for small timesteps) + self.use_fixed_step = False + def extract_geometry(self): """ Extracts the geometry in self.rotors for efficient use later on in the computation of @@ -233,19 +236,22 @@ def step(self, state, control, t_step): # The true motor speeds can not fall below min and max speeds. cmd_rotor_speeds = np.clip(cmd_rotor_speeds, self.rotor_speed_min, self.rotor_speed_max) - # Form autonomous ODE for constant inputs and integrate one time step. - def s_dot_fn(t, s): - return self._s_dot_fn(t, s, cmd_rotor_speeds) s = Multirotor._pack_state(state) - # Integrate - sol = scipy.integrate.solve_ivp( - s_dot_fn, - (0.0, t_step), - s, - **self.integrator_kwargs - ) - s = sol['y'][:, -1] + if self.use_fixed_step: + # Fixed-step RK4: 4 function evaluations, no adaptive overhead + s = self._rk4_step(s, cmd_rotor_speeds, t_step) + else: + # Adaptive RK45 via scipy + def s_dot_fn(t, s): + return self._s_dot_fn(t, s, cmd_rotor_speeds) + sol = scipy.integrate.solve_ivp( + s_dot_fn, + (0.0, t_step), + s, + **self.integrator_kwargs + ) + s = sol['y'][:, -1] # Unpack the state vector. state = Multirotor._unpack_state(s) @@ -263,6 +269,17 @@ def s_dot_fn(t, s): return state + def _rk4_step(self, s, cmd_rotor_speeds, dt): + """ + Single fixed-step RK4 integration. 7x faster than solve_ivp for small + timesteps with identical accuracy at dt <= 4ms. + """ + k1 = self._s_dot_fn(0, s, cmd_rotor_speeds) + k2 = self._s_dot_fn(0, s + 0.5 * dt * k1, cmd_rotor_speeds) + k3 = self._s_dot_fn(0, s + 0.5 * dt * k2, cmd_rotor_speeds) + k4 = self._s_dot_fn(0, s + dt * k3, cmd_rotor_speeds) + return s + (dt / 6.0) * (k1 + 2*k2 + 2*k3 + k4) + def _s_dot_fn(self, t, s, cmd_rotor_speeds): """ Compute derivative of state for quadrotor given fixed control inputs as diff --git a/rotorpy/vehicles/px4_multirotor.py b/rotorpy/vehicles/px4_multirotor.py index abf2829..980e96e 100644 --- a/rotorpy/vehicles/px4_multirotor.py +++ b/rotorpy/vehicles/px4_multirotor.py @@ -6,6 +6,8 @@ from typing import Tuple import math +import time +import statistics # Constants R_EARTH = 6378137.0 # meters @@ -55,6 +57,7 @@ def __init__( mavlink_url="tcpin:localhost:4560", autopilot_controller=True, lockstep=True, + lockstep_timeout=0.002, integrator_kwargs=None ): integrator_kwargs = integrator_kwargs if integrator_kwargs is not None else {'method':'RK45', 'rtol':1e-2, 'atol':1e-4, 'max_step':0.05} @@ -76,11 +79,14 @@ def __init__( enable_ground=enable_ground, integrator_kwargs=integrator_kwargs ) + # Use fixed-step RK4 for faster physics (7x vs solve_ivp, identical accuracy at dt<=4ms) + self.use_fixed_step = True # Simulated IMU (with noise) self.imu = Imu() self._enable_imu_noise = True # Always add a bit of noise to avoid stale detection self.t = 0.0 + print("PX4Multirotor: Initializing MAVLink connection... on {}".format(mavlink_url)) self.conn = mavutil.mavlink_connection(mavlink_url) self.conn.wait_heartbeat() @@ -88,6 +94,7 @@ def __init__( self._autopilot_controller = autopilot_controller self._lockstep_enabled = lockstep + self._lockstep_timeout = lockstep_timeout self._last_control = {'cmd_motor_speeds': np.zeros(quad_params['num_rotors'])} @staticmethod @@ -166,10 +173,20 @@ def geodetic_to_mavlink(lat_deg: float, lon_deg: float, alt_msl_m: float) -> Tup def _fetch_latest_px4_control(self, blocking : bool = True): """Fetch the latest HIL_ACTUATOR_CONTROLS message from PX4 and update control inputs.""" - - msg = self.conn.recv_match(type='HIL_ACTUATOR_CONTROLS', blocking=blocking, timeout=0.01) - if msg is not None: - return {'cmd_motor_speeds': [c * self.rotor_speed_max for c in msg.controls[:self.num_rotors]]} + # Drain all queued messages non-blocking first + latest = None + while True: + msg = self.conn.recv_match(type='HIL_ACTUATOR_CONTROLS', blocking=False) + if msg is None: + break + latest = msg + # If no message found and blocking requested, poll with retry until timeout + if latest is None and blocking: + deadline = time.perf_counter() + self._lockstep_timeout + while latest is None and time.perf_counter() < deadline: + latest = self.conn.recv_match(type='HIL_ACTUATOR_CONTROLS', blocking=True, timeout=0.01) + if latest is not None: + return {'cmd_motor_speeds': [c * self.rotor_speed_max for c in latest.controls[:self.num_rotors]]} def _enu_to_ned_cmps(self, v_enu): v_n = float(v_enu[1]) @@ -190,25 +207,22 @@ def _imu(self, state, statedot): omega_frd = np.array([omega_flu[0], -omega_flu[1], -omega_flu[2]], dtype=float) return a_frd, omega_frd - def _send_hil_state_quaternion(self, state, statedot): + def _send_hil_state_quaternion(self, state, a_frd_gt): """ Send HIL_STATE_QUATERNION message to PX4. - + Args: state: Current vehicle state - statedot: State derivative (from the `Multirotor.statedot` method) + a_frd_gt: Ground-truth acceleration in FRD frame (precomputed) """ # Convert cartesian ENU position to geodetic coordinates (latitude, longitude and height) lat_deg, lon_deg, height_meters = self.enu_to_geodetic(*state['x']) lat_e7, lon_e7, alt_mm = self.geodetic_to_mavlink(lat_deg, lon_deg, height_meters) - + # Convert quaternion from rotorpy to aerospace convention using ArduPilot's method quaternion_flu2ned = Ardupilot._quaternion_rotorpy_to_aerospace(state['q']) vx_cms, vy_cms, vz_cms = self._enu_to_ned_cmps(state['v']) - # Send the ground truth acceleration in the state message (without imu noise) - a_flu_gt = self.imu.measurement(state, statedot, with_noise=False)["accel"] - a_frd_gt = np.array([a_flu_gt[0], -a_flu_gt[1], -a_flu_gt[2]], dtype=float) a_frd_mg = np.clip(np.round(a_frd_gt / 9.80665 * 1000.0), INT_MIN, INT_MAX).astype(np.int16) self.conn.mav.hil_state_quaternion_send( @@ -253,17 +267,15 @@ def _baro(self, state): temperature_c = T0 - 0.0065 * alt_m - 273.15 return abs_pressure_hpa, pressure_alt_m, temperature_c - def _send_hil_sensor(self, state, statedot): + def _send_hil_sensor(self, state, a_frd, omega_frd): """ Send HIL_SENSOR message to PX4. Args: state: Current vehicle state - statedot: State derivative (computed externally) + a_frd: Accelerometer reading in FRD frame (precomputed) + omega_frd: Gyroscope reading in FRD frame (precomputed) """ - # Get IMU measurements - a_frd, omega_frd = self._imu(state, statedot) - # Magnetometer: Earth field rotated into body FRD frame (gauss) mag_frd = self._mag_body_frd(state) @@ -286,12 +298,21 @@ def _send_hil_sensor(self, state, statedot): ) def step(self, state, control, t_step): + _t0 = time.perf_counter() - # Compute state derivative once for state and messages - # and send both HIL messages + # Compute state derivative once for messages statedot = self.statedot(state, control, 0.0) - self._send_hil_state_quaternion(state, statedot) - self._send_hil_sensor(state, statedot) + + # Compute IMU measurements once (noisy for HIL_SENSOR, ground-truth for HIL_STATE) + a_frd_noisy, omega_frd_noisy = self._imu(state, statedot) + a_flu_gt = self.imu.measurement(state, statedot, with_noise=False)["accel"] + a_frd_gt = np.array([a_flu_gt[0], -a_flu_gt[1], -a_flu_gt[2]], dtype=float) + _t_statedot = time.perf_counter() + + # Send both HIL messages with precomputed data + self._send_hil_state_quaternion(state, a_frd_gt) + self._send_hil_sensor(state, a_frd_noisy, omega_frd_noisy) + _t_hil_send = time.perf_counter() # Use PX4 commands only if autopilot_controller is True if self._autopilot_controller: @@ -303,9 +324,39 @@ def step(self, state, control, t_step): else: # In this case we use the control provided by the external controller self._last_control = control + _t_px4_fetch = time.perf_counter() state = super().step(state, self._last_control, t_step) self.state = state self.t += t_step + _t_rk4 = time.perf_counter() + + # Accumulate timing samples; print summary every 500 steps (~2s at 250Hz) + if not hasattr(self, '_step_timing'): + self._step_timing = {'statedot': [], 'hil_send': [], 'px4_fetch': [], 'rk4': [], 'total': []} + self._step_count = 0 + self._step_count += 1 + self._step_timing['statedot'].append((_t_statedot - _t0) * 1e3) + self._step_timing['hil_send'].append((_t_hil_send - _t_statedot) * 1e3) + self._step_timing['px4_fetch'].append((_t_px4_fetch - _t_hil_send) * 1e3) + self._step_timing['rk4'].append((_t_rk4 - _t_px4_fetch) * 1e3) + self._step_timing['total'].append((_t_rk4 - _t0) * 1e3) + + if self._step_count % 500 == 0: + def _fmt(vals): + avg = statistics.mean(vals) + p99 = sorted(vals)[int(len(vals) * 0.99)] + return f"avg={avg:.2f}ms p99={p99:.2f}ms" + t = self._step_timing + print( + f"[PX4Multirotor.step timing | n={self._step_count}]\n" + f" statedot : {_fmt(t['statedot'])}\n" + f" hil_send : {_fmt(t['hil_send'])}\n" + f" px4_fetch: {_fmt(t['px4_fetch'])} ← lockstep wait\n" + f" rk4 : {_fmt(t['rk4'])}\n" + f" total : {_fmt(t['total'])}" + ) + for key in self._step_timing: + self._step_timing[key].clear() return state