[UNRESOLVED] HITL/SITL delay problem

Hi,

I’m trying to setup sensor-level HIL simulation with the latest PX4 firmware and using my own simulator, but I’m having some issues.

The pitch and the roll react correctly to gyro rate data, but reacts very slowly to accelerometer data.
What I mean is that when I rotate the aircraft, the attitude displayed in QGC correctly reacts based on gyro rates. However this change is kind of like ‘dead reckoning’ and will diverge from the actual pitch, so it is supposed to also use accelerometer data to improve this estimate. What happens is that this accelerometer-based fix takes very long to take effect. For example, if I respawn the aircraft level on the ground, the pitch may take 15 seconds to come back to level. During flight, the pitch diverges from the actual pitch in a matter of seconds, and takes 15 seconds to come back to the actual pitch value if I pause the simulation. The same thing happens with the heading.

Any ideas on what could be causing this issue? It’s as if the filter is running too slow…

Implementation details:
*I’m using the latest version of the firwmare with the HIL Quad airframe
*HIL setup: I’m connecting the simulator and QGC through UDP to Mavproxy, and Mavproxy is connected to the Pixhawk through USB with a baudrate of 921600.
*I’m sending the HIL_SENSOR message with sensor data at a frequency of 500 to 1000 Hz
*I’m confident I’m calculating and sending the sensor data correctly (correct units, type, ranges, etc), and it is being received correctly by the PX4 because the attitude moves as it should, it just takes a long time to get to the correct attitude.

Thank you!

1 Like

I would like to reproduce this to try to help you but it’s a bit overwhelming. What pieces do I need to change? What are the commands to run it? The easier you can make that for someone like me trying to help, the higher the chances we can help.

@JulianOes Julian,

I’ve uploaded at the link below a script that replicates the problem, and down below I’m including instructions on how to setup the environment to run it.

script link

The script makes the pitch oscillate periodically between -45 degrees and 45 degrees, while keeping heading, roll, altitude and velocity at 0.

You’ll notice the aircraft attitude in QGC does not behave as expected. The roll and heading don’t stay at 0 but seem to move randomly.

Setup instructions:
-I’m using the official Cygwin PX4 SITL environment, and building/running the firmware with “make posix none”
-QGC UDP port: 14550, Script UDP port: 14560
-Install the Python dependencies from miniconda (or virtualenv)
conda create -n mavlink_env python=2.7.13
conda activate mavlink_env
pip install pymavlink
pip install numpy

1 Like

So your initial title is “HITL delay problem”, however the test instructions are for SITL. Does that mean that you can reproduce the problem in SITL as well (in the same way as HITL)?

Exactly, the same problem occurs in both SITL and HITL. I’m updating the title.

But HITL requires setting up MAVproxy so SITL can be easier to set for testing.

1 Like

I gave this a try and I can see that the attitude seems wrong somehow. Now my guess is that you have some mistake in any of the signs or coordinate frames.

Have you tried if the attitude initialization works without any movement, just static orientations? I would probably try that first.

Also, I looked at the values you send in hil_sensor_send and I could see that zacc is around +9, however, I would expect it to be -9 comparing to what PX4 usually receives from jMAVSim.

"zacc is around +9, however, I would expect it to be -9 "

You are right, that’s an error I made on the script as I was trying to simplify it from my original code (the original code does not have this bug). I’m sending a new version which fixes this. However, the issue persists after fixing this.

Script v2

“Have you tried if the attitude initialization works without any movement, just static orientations? I would probably try that first.”

That’s a good idea, the new script just sets the pitch fixed at 45 degrees. I think this isolates the issue better because now the rates are all at 0, and it is relying only on accelerometer (and magnetometer?) data to figure out the orientation.

You can see that the pitch in QGC does not go up to 45, and also takes a long time to get there. What could be going on?

Thank you for your help.

It’s expected that the EKF reacts very slowly to accelerometer data and mostly is based on gyro. I’m simplifying things here but basically the reason is that you can fly pretty well with gyro only data and only need the accel data over time to figure out where gravity points to. Basically you want to avoid getting confused by acceleration due to the movement of the drone.

This would mean however that your simulator accel data is not wrong as such but that the static test only shows if the signs are correct but not if the EKF reacts properly to a new attitude. For this, I guess, you need to add in the gyro data again.

I would try to simulate one gyro axis and check if you can match e.g. rotation per time.

Thank you very much for your help Julian, your feedback helped me figure it out.

I wasn’t expecting the state to be based so much on the rates and respond slowly to the accelerometer, so that was throwing me off on what to expect and how to find the problem.

The problem mainly was that I had not realized that resetting the simulation or causing the state to jump instantly at any point would throw the state estimation off so much that it can’t recover. Also I found a bug in the way the mag data was being calculated.

Thanks again for your help!!

1 Like

Unfortunately after further testing I’m still having a problem similar to the original problem, but with the heading.

Rotating the yaw 1 or 2 revolutions is enough for the heading shown in QGC to diverge from the heading in the simulation by about 45 degrees. If I pause the simulation and give it about a minute, the heading slowly reaches the correct value (based on magnetometer data), but during simulation the heading dead-reckoning based on yaw gyro diverges very quickly from the actual heading.

I’m not sure why this would be the case…

Is yaw always too fast or too slow? And are the rates and timestamps correct? It sounds like the integration of the angular rates diverges.

The yaw is always too slow, and the timestamps are consistent.

I have been investigating what you said, it seems to be that integration error is accumulating too fast, meaning it probably has something to do with the timing and time stamps of the data. But haven’t been able to solve it yet…

At what rate are you sending data?

I have tried sending from 500 to 800 Hz. One thing is that the frequency is not constant, some time steps may be somewhat longer than others. Could this be a problem?

Not sure, it sounds ok. You could try to generate a log some values and then do the same movement with real data and then comparing it. Or you can go into the EKF2 code and debug what the input is.