Tips to get a more constant loop time

[laurensvalk]

For a balancing robot project I am running a control loop that repeats indefinitely. We would like to have an approximately constant loop time in the order of several milliseconds.

We know Linux is not a real time OS, but we would like to ask the Linux experts around here for some pointers to things we can do to improve the consistency of the loop time. As discussed below, we have more than 50% idle time, but the challenge is to allocate it properly.

First, some background information. The control loop will read sensors (1x gyro, 2x encoder), do some calculations, and set the duty cycle of two tacho motors. For the moment we are skipping the calculation step.

Everything is timed with the get_clock c function. (An external stopwatch was used to verify that the logged times and overall time actually make sense.) In the following, let's assume we would like a loop time as close as possible to 3ms.

If we cut out all the waiting loops and usleeps, we can run the loop that reads the sensors and sets the motor duty cycles 10,000 times in under 10 seconds. This is even with all the background processes running, so the control loop itself needs less than 1 ms to run. On average, the system can be idle for 2 ms.

For a very simple experiment, the timing is recorded as follows:

The loop begins by logging the current time. Then it reads sensors and sets motors. If it has run in under 3 ms, it reads the clock every 10 microseconds to wait until the 3ms loop time has elapsed. Then the loop runs again.

If a loop takes longer than 3ms, it will still finish the loop and then wait until the nearest multiple of 3ms before starting another loop.

The intervals (the difference between two subsequent logged time values) are plotted in the figure below, against the actual time. Most intervals are pretty close to the desired 3ms loop time. The 'negative' peaks occur directly after a 'positive peak'. For example, if a loop needs 4 ms, it will do the next one in 2ms, if possible.

(Update: The vertical label should read seconds: 0.003 s = 3ms. Update 2: The above plot does not correspond exactly to the above timing description [so don't bother to try and understand it exactly], but the issues with the delays hold in any case.)

Three types of delays appear to stand out as marked in the figure. Peak type 1 and 2 might be caused by another running process. I haven't yet found which ones these may be. Peak type 1 is the most serious. Peak type 3 occurs the most frequently. What would be a good way to identify the processes related to these delays?

We would like to ask for some tips to improve this behaviour. Here are some things that I imagine are possible, but I'm not sure if they're practical or even possible. If they are, which one is likely to give the best results? Please feel free to add more ideas as well.

• Identify some parallel processes and reduce their priority or even delay them if they're not critical to the robot balancing.
• Increase the priority of this process. (I experimented a bit with the niceness, but the results so far did not improve much.)
• Use an interrupt with a timer instead of a continuous loop
• Use something like the RT-Preempt patch
• Don't use EV3 or Linux for near real time applications. (I'm hoping this is not the answer, but do say so if you think it is.)

I'm steadily learning Linux and willing to learn more, but I'm new to this type of problem. I hope some experts around here can help.

Thanks!

[dlech]

Thanks for this detailed analysis to help us understand the EV3 better. Here are my thoughts on your thoughts.

Identify some parallel processes and reduce their priority or even delay them if they're not critical to the robot balancing.

If your process has higher priority already, I don't think reducing the priority of other processes will make any difference.

Increase the priority of this process. (I experimented a bit with the niceness, but the results so far did not improve much.)

I think the reason you did not see much difference is that the problem is not the other processes, it is the kernel.

Use an interrupt with a timer instead of a continuous loop

I don't think you would see any difference here. We do this with some things in the kernel and have the same problems. The most noticeable is the dimming of the LEDs. They use a hrtimer (a timer that triggers an interrupt). You can see the LEDs flicker when there is a "peak type 1" which happens when the kernel is busy handling other interrupts.

Use something like the RT-Preempt patch

This would be nice, but no one seems to be interested in paying anyone to work on the RT kernel, so maintenance has been minimal in recent years. The most recent RT kernel is 3.4 and we are using 3.16. You would have to downgrade to wheezy to use it. And on top of that, the 3.4 RT kernel is probably rather buggy.

There may be something else that I have not tried yet though. That is changing HZ in the kernel configuration from 100 to 1000. This controls how often the processor wakes up. You could try compiling your own kernel and see if this makes a difference.

Don't use EV3 or Linux for near real time applications. (I'm hoping this is not the answer, but do say so if you think it is.)

Unfortunately, yes if you want near real-time on the order of < 10ms, I think it is not going to happen on Linux - at least not without the RT patchset.

There are some tricks we do use on the EV3 to get around this though. I don't think it would be practical to try to use these from a user application, but I will tell you about them anyway.

The first is a Fast Interrupt (FIQ). This is used for bit-banging the I2C on the 4 input ports. In ev3dev, we are also using it for sound playback, which is why audio on ev3dev sounds better than the official LEGO firmware. The thing about FIQs is that they completely bypass the kernel, so you have to be very careful. You are basically limited to manipulating hardware directly since you can't call any kernel functions.

The second is the Programmable Runtime Units (PRUs). One PRU used for 2 of the UARTs in the input ports of the EV3, but one is free. I would like to eventually use this for the I2C that is currently using FIQ (to take the load off the main CPU) and for bit-banging PWM using GPIO for dimming the LEDs like I mentioned earlier. But, there is a kernel driver that lets you load firmware into the PRU from a user program, so theoretically, you could write your app in the PRU assembly language and load it and run it from a Linux usermode program. Obviously, not very practical.

[laurensvalk]

Thanks for your detailed response. I'll further investigate the topics you mention here.

Another option is to account for the varying loop time in our code. This will work for some applications, but it will significantly complicate some algorithms and their theoretical stability properties (or even destabilize the system.)

While yesterday's post concerned mostly timing, I'll add some more plots to show the implications on measurements. Here is a plot of the gyro value of a segway type of robot freely swinging, in upside down orientation. (We do this to get a first estimate on the body inertia, which can approximately be found from the free swinging frequency.)

You can see the delays show up as gaps in the measurements. On zooming in (same plot), it appears we can distinguish between the kernel not updating the measurements and the user loop not running. In the former, you can see that the sensor values appear to stay constant.

[rhempel]

So in a real world system, you might end up having to use a predictor function that "fills in" the values as best it can when you know you have missed a reading or when you think the sensor is malfunctioning. Temporary sensor or timing loop failures have to be tolerated here. Then you have to determine the border between temporary and too long :-)

Nice graphs by the way - what tool are your using?

[laurensvalk]

Yes, that's the plan eventually. We'll likely explore the use of a Kalman filter. However, it usually assumes a constant sampling time, which will provide some challenges.

We're using MATLAB for the plots since we're also using it for other development (it has a fantastic toolbox to do symbolic derivations), but the free Matplotlib and something like iPython notebook can produce very similar results.

Thanks to ev3dev making these plots is a breeze. We press the run button in Eclipse CDT to compile and run the program and then we just grab the latest datalog.txt file from the brick over the usb network with a simple scp system command within the MATLAB script.

[dlech]

These most recent graphs are using the LEGO EV3 Gyro sensor, correct? Also, it looks like the gaps in the data are mostly periodic. Have you measured this to see if it is consistent?

[laurensvalk]

Yes, this is all the LEGO EV3 Gyro in GYRO-RATE mode. I did not yet check whether the gaps coincide with the "type 1" loop time spike and whether they're periodic.

Currently I'm seeing yet another discrepancy in the data. Somehow the sensor resolution becomes very course when it moves, making steps of +/- 6 degrees/sec rather than the specified 1 degree/sec resolution. That is, the value stays constant over many samples and then suddenly jumps by about 6 degrees/sec.

I think I need to take a few steps back and work with a simpler device first, such the motor encoder. I think the Medium motor should be able to hit the 1deg/msec mark. Since a motor moves quite constantly, I might be able to determine more accurately where the time delays originate.

I'll also see if I can recompile the kernel to get the ADC values at 1000 Hz, so that I can compare the HiTechnic and the LEGO EV3 gyro for response time and resolution. Then I can also try changing the CPU wakeup frequency as you suggested. Now seems to be a good time to learn how to recompile a kernel :)

[dlech]

Some of this may come from the sensor itself. I don't know how much you know about the internal workings of the EV3 UART sensors, but they send data to the EV3 when they want to rather than waiting for the EV3 to poll them. I just stuck the LEGO EV3 Gyro sensor on my logic analyzer to see what it looks like. Most of the time, the data is coming regularly every 1.6 ms.

However, the are sometimes gaps of of ~6 ms.

So, I am guessing this is the part where you said the "kernel is not updating sensor values". This is a true statement, but it is not updating the value because the sensor is not sending new information, not because the kernel is busy doing something else.

[dlech]

So, I think using a polled device rather than a UART sensor sounds like a good approach. As for compiling the kernel, I suggest you start here.

[laurensvalk]

Thanks, that's helpful information about the sensor. Indeed, I'll try to work with the HiTechnic Gyro first.

Kernel seemed to compile just fine---without changing any settings so far, though the brick is not getting past the penguin boot screen just yet. I'll try again later this week.

[rhempel]

New issue for kernel compile problems please :-) But you did build a new kernel package, transferred it to the brick, and then dpkg -i new-kernel-package right?

[laurensvalk]

I didn't mean to hijack the post for kernel help, but rather to update on progress :)

It looks like I should have been using the latest image with the 1 May changes. I'll report back later with sensor info (or elsewhere, if I need help with the kernel compilation process.)

Thanks a lot for the input so far!

[rhempel]

Laurens, another idea worth checking is if the data gaps coincide with the log file writes. Slow down the sample rate - do the gaps spread out? Consider having your program store the data in an array (we have 64MB of RAM!) and then write the data at the end - do the gaps disappear?

[laurensvalk]

Thanks for your comment. Logging the array data to file only at the end is exactly the method used in all of the experiments logged so far, so I'm afraid that's not it.

Now that the ADC giving output more quickly (see #231) I hope to get a better idea about the cause of the delays.

Perhaps another related question: What is a good approach to pause/sleep a user program in a way that other processes or the kernel can use it?

That is, once I'm done reading the sensors (in 1 ms), I don't need to do anything until the clock hits 3ms and the next loop iteration begins. Waiting for this with a while loop with a short usleep in it seems inefficient?

[theZiz]

usleep does give the processor time "back to the kernel", so another thread can get time on the cpu. However the time you pass to usleep is not exactly the time your program while pause. It will pause at least this amount of time. So if you are unlucky, you have to wait much longer.

You could even be unlucky and give away your time on the processor just some µs before the 3 ms are passed, so maybe you could cycle and usleep e.g. for 2,5 ms, but if you recognize, only 500µs are left until your next sensor polling step, you could do a busy cycle (without usleep) until the right time slot arives.

[laurensvalk]

Thanks for the info. I suppose that explains why the loop interval timings appear much more exact when I disable the usleep in the waiting loop. But perhaps it leads to larger delays later on.

[theZiz]

I read, that the completely fair sheduler of recent linux gives time slices between 750µs and 6ms. So I think, if you get processor time, you have at least 750µs for free use (probably except for unexpected interupts, but you can never plan with them). However, I am not sure, whether this helps you at all.

[laurensvalk]

I think we have a winner, for now anyway. Here is a shot comparing the LEGO EV3 Gyro and the HiTechnic one. Amplitude scaled for comparison.

The HiTechnic sensor has a finer resolution and is less affected by delays. Presumably the delay is reduced because there are fewer possibilities of delays down the line.

The peak time in some of the loops (see top post, and here e.g. at 3.9 seconds) is still there, but this is a nice step forward. Thanks everyone for the help so far.

[laurensvalk]

I should add that the measurements where made by attaching the two sensors rigidly to the same robot, and reading the sensor value files of both sensors immediately after one another. The order of reading the files did not seem to have a significant effect. The * marks the time logged at the start of each loop iteration. The 'robot' is just a pendulum that freely swings back and forth on an axle.

Now it is interesting to compare two types of delays.

1. Here it seems the user program loop is just not running.

   

2. Here the same appears to happen at first, but then the loop continues while the LEGO EV3 Gyro stays constant. This may very well be the effect that David found with the logic analyzer, where the uart sensor is not sending any new data.

   

[laurensvalk]

For completeness, I'll also add an I2C sensor to this thread, although I won't be using it in my project.

The lower limit for the I2C sensor implementation appears to be the poll_ms attribute with a lower limit of 50.

[psiorx]

I'm also developing a balancing robot that needs high rate sensor feedback. I'm fusing HiTechnic Gyro readings with HiTechnic accelerometer readings to generate an angle estimate that doesn't drift.

However, the specs on the accelerometer allow for a sampling rate of 100 Hz but is this achievable ev3dev? The poll_ms hard cap at 50 ms only allows for a sampling rate of 20 Hz. Is there any way in the ev3dev framework to poll an i2c sensor at this rate?

[dlech]

I'm fusing HiTechnic Gyro readings with HiTechnic accelerometer readings to generate an angle estimate that doesn't drift.

I've thought many times about doing something like this. I'd be interested to know how well it works.

The poll_ms hard cap at 50 ms only allows for a sampling rate of 20 Hz.

I will look into removing this hard cap in the next kernel release.

Is there any way in the ev3dev framework to poll an i2c sensor at this rate?

You could compile your own kernel. Or you might be able to use the direct attribute of the sensor. To do this, set poll_ms to 0 to disable polling and then construct your own I2C messages using the direct attribute. Or you could unbind the ev3dev driver and use the standard Linux i2c library.

[psiorx]

Thanks for the response.

As far as the sensor fusion goes, I've done it on the original EV3 kernel using their lms2012.h and it worked extremely well. Using a simple complementary filter gave great results. The robot could balance until it ran out of battery with no noticeable drift after more than an hour of balancing.

Here's a good resource on implementing such a filter:

https://b94be14129454da9cf7f056f5f8b89a9b17da0be.googledrive.com/host/0B0ZbiLZrqVa6Y2d3UjFVWDhNZms/filter.pdf

I don't see a direct attribute on the sensor. Maybe I'm using an older kernel.

I've tried probing the relevant /dev/i2c device using i2c-tools to no avail. In my case, the accelerometer is mapped on /dev/i2c-4 but running i2cdetect -r 4 shows no values in any of the registers. Is this because the ev3dev driver is shadowing the standard Linux driver functionality? How would I go about unbinding it?

[dlech]

i2c-tools should complain that the device is in use if you are probing the correct port and address because as you guessed the kernel driver has taken ownership of the i2c device.

To unbind the driver run echo 4-0001 > /sys/bus/i2c/drivers/nxt-i2c-sensor/unbind where 4 is input port plus 2 and 0001 is the I2C address of the sensor.

[laurensvalk]

To keep you updated --- the robot is now working. Here is a report on the controller design. It's written for people with a background in control theory, but chapter 2 about the technical implementation (including the use of ev3dev and the code development process) may be interesting for some of you.

I plan to write a blog post later this summer, with some background information on the controller design methods used. I would also like to implement our MATLAB controller design scripts in Python, so that other people can run them too.

Ultimately, the inconsistent loop time did not seem to affect the controllers too much, but there is still room for improvement here. We ended up using a 10 msec sampling time. It can be shown (by studying the frequencies of the system dynamics) that this is still more than sufficient to keep the robot balancing. We need only about 4 ms to read sensors, do calculations, and store data, so a clever scheduling mechanism might be able to get each loop iteration to start consistently in intervals of 10 msec.

I'm also developing a balancing robot that needs high rate sensor feedback. I'm fusing HiTechnic Gyro readings with HiTechnic accelerometer readings to generate an angle estimate that doesn't drift.

Another way to do this would be to use a Kalman filter and only the gyro sensor. It can be shown that the wheel position, the robot body angle, the wheel speed, the body angular velocity and the sensor offset are all observable from only the gyro sensor and the motor rotation sensor, which is quite interesting. This is also discussed in the report.

If the robot encounter any heat guns while travelling, it should survive.

[psiorx]

Great job! I'm excited to read the report.

@laurensvalk It's very interesting to me that all of those quantities are observable via the gyro reading only. Bayes filters never cease to surprise me. Thanks for posting the update!

In Hitechnic's HTWay blog, they use a low-pass filter to continuously maintain an estimate of the gyro offset.

[dlech]

The future of realtime linux is looking better. I was checking out the BeagleBone Black and TI has a realtime kernel (4.1.y) for it.

[bmegli]

Quite recent work (2014):

"A Platform for LEGO Mindstorms EV3 Based on an RTOS with MMU Support" Yixiao Li, Takuya Ishikawa, Yutaka Matsubara, and Hiroaki Takada

http://www.mpi-sws.org/~bbb/proceedings/ospert14-proceedings.pdf

[dlech]

It has recently come to light that there are two different versions of the LEGO EV3 Gyro sensor. These can be identified by the manufacturing date code stamped on them. The older sensors with date code ending in "N2" and "N3" have a number of observed bugs.

@laurensvalk, do you know which version of the LEGO EV3 gyro sensor was used in your data above? Could you repeat the experiment with the older and newer versions of the sensor to see if there is a difference?

[laurensvalk]

I'm afraid I don't know for sure which gyro I used back then. There's a fair chance it was N2 or N3, because I mostly have older EV3 sets from the time of release (2013).

Unfortunately, most of my hardware is temporarily in storage. I do have an emergency life supply here, but this includes only 1 gyro (N2 type). I'll try to pick up some other gyros next time, if I have them.

The code essentially just polled the sensor files as fast as possible, while keeping file handles open. I used C for the least amount of overhead. Data was stored in an array and saved to a text file after the experiment was complete.