Optimizing on the speed of reading the ultrasonic sensor

[mindsqualls]

In this thread https://sourceforge.net/apps/phpbb/mind ... ?f=3&t=976 linusa wrote:

Do you have the feeling that "I2C is slow", as in "the ultrasonic sensor could really be faster, especially via Bluetooth"? I'm not sure how exactly you request input data from digital sensors using direct commands, but there might be a way for you to save 1 packet on average (which could be as much as 30ms gain in performance, depending on statistics). I suggested this for nxt-python a while ago: https://code.google.com/p/nxt-python/is ... tail?id=11

Short summary: Instead of doing it like this:

Code: Select all

1. Use LSWrite to tell the sensor which I2C register you need
2. In a loop, keep polling LSGetStatus until I2C is not busy anymore (no error is detected, statusbyte == 0).
3. Use LSRead to retrieve the sensor data.

Now, maybe you've noticed, you already get a statusbyte with LSRead, too. So you can completely omit the LSGetStatus command. The optimized version looks like this:

Code: Select all

1. Use LSWrite to tell the sensor which I2C register you need
2. In a loop, keep polling LSRead until I2C is not busy anymore (no error is detected, statusbyte == 0).
3. Once the loop is done, you already have the sensor's answer (payload of last LSRead command with statusbyte == 0).

In regular cases, this can save 1 direct command out of 3.

I have now tried this in my MindSqualls library (http://mindsqualls.net/) and here are my observations…

The suggested algorithm probably only works if you are sending a command to the sensor and expect exactly 1 byte in reply. Reading the distance from the ultrasonic sensor is such a case, but other commands like “Read product ID” and “Read sensor type” will return more bytes. But, since we are most interested in reading the distance we are in luck.

The first problem you encounter is that you get error on the LwRead() from the I2C bus when you try this. I encountered 2 types:

The first one was 0x20 (Pending communication transaction in progress). No problem, you can just ignore this and try to redo the LsRead() after a short pause.

The second, 0xDD (Communication buss error), is a bit more problematic. If the bus ends up in this state it will just continue to fail until the error is cleared. It is easy enough to solve this; you just need to do a LsWrite(). Any LsWrite() should do, and I simply chose to resend the original one (from step 1).

After that the algorithm was stable (but I am keeping it beta until I am 100% sure).

I also introduced a small wait-period in the algorithm. Having sent the LsWrite(), the data will not be ready in the sensor right way, so why not wait until the chances are better that it is before calling the LsRead()? The wait should not be too short, and not be too long either, but in between there is probably an optimal value.

I made some measurements and in my case about 22 milliseconds turned out to be the optimal length – other implementations may have another value. I will not say I am totally happy about this solution as the value may depend on a lot of variable circumstances e.g. the quality of the Bluetooth signal.

Anyway, adding this pause greatly reduced the calls to LsRead() which ended in an error and a second LsRead() a bit later.

The final algorithm ended up looking like this:

1. Use LsWrite() to tell the sensor which I2C register you need
2. Pause 22 ms.
3. Call LsRead() to get the value.
3a. If error == PendingCommunicationTransactionInProgress repeat from step 2.
3b. If error == CommunicationBusError repeat from step 1.
3c. If error == unknown Fail.
4. Return the value from LsRead.


Improvements:

Response-time: 104 ms -> 81 ms
Commands: 3.9 -> 2.1

[linusa]

The suggested algorithm probably only works if you are sending a command to the sensor and expect exactly 1 byte in reply. Reading the distance from the ultrasonic sensor is such a case, but other commands like “Read product ID” and “Read sensor type” will return more bytes.

I have to admit, I never tried to read "strings" like product ID from the sensors using this method. However, the way I described is currently the only way to read data via I2C in the RWTH - Mindstorms NXT Toolbox for MATLAB. It works with multiple sensors, including HiTechnic Compass, ColorV1 + V2, Accelerometer, + Codatex RFID. Those sensor's data lengths are longer than 1 byte. So I can't confirm your problems.

I also introduced a small wait-period in the algorithm. Having sent the LsWrite(), the data will not be ready in the sensor right way, so why not wait until the chances are better that it is before calling the LsRead()? The wait should not be too short, and not be too long either, but in between there is probably an optimal value.

Yep, good idea, I tested this waiting period myself, but I chose not to keep it inside production code.

Improvements:
Response-time: 104 ms -> 81 ms
Commands: 3.9 -> 2.1

That is really cool, unfortunately I don't remember the exact numbers for our improvements, and also I currently don't have an NXT to test it . But as I faintly remember, a 10 to 12 Hz polling rate of the US sensor via Bluetooth (which is about 100 to 83 ms latency) is what we achieved... Whenever I get the chance, I should really confirm this!


Maybe it helps if I post my corresponding code. I too discovered some possible error states etc. The function in question is called COM_ReadI2C, code here: http://www.mindstorms.rwth-aachen.de/tr ... _ReadI2C.m

Introductory comment (just describes what I posted in this thread before + my observations about adding a waiting pause as you did):

Code: Select all

%% Try requesting answer until it is ready

    % Usually one should issue the LSWrite command, then keep requesting
    % the I2C bus-status with LSGetStatus until it doesn't return an error
    % and until the amount of bytes one requested is availabled, and then
    % finally retrieve the data with LSRead.
    %
    % I found that the step with LSGetStatus can be omitted. Each direct
    % command, including LSRead, has its own error message included (the
    % statusbyte). If instead of using LSGetStatus and THEN using LSRead,
    % we just use LSRead straight ahead, this has 2 advantages:
    % - most of the time, the I2C sensor is ready anyway. So we save the
    % time (especially via Bluetooth) that LSGetStatus needs.
    % - if the sensor is not ready, we can keep polling it with LSRead. The
    % error messages are the same, but once they stop (and data is ready),
    % we already got the data as payload - having saved another call!
    %
    % Compared to the traditional way, this yields a speed up of about 100%
    % for both Bluetooth and USB.
    %
    %
    % Note that there is a tiny chance for further improvement, depending
    % on the sensor. If we add a short pause here in the magnitude of 1ms
    % to 10ms, it could lead to one failed LSRead being saved. Imagine
    % this: Instead of asking the sensor straight away if it's ready, and
    % then waiting 60ms for one more Bluetooth packet to arrive, we could
    % wait 10ms (if that is enough) and then get the sensors answer with
    % the next packet straight away.
    % Testing showed that this is NOT necessary and most of the time NOT
    % helping with Bluetooth, a fast computer and the ultrasonic sensor.
    % However, different sensors may show different behaviour... 

Algorithm:

Code: Select all

    % use a timeout so we don't get stuck
    startTime = clock();
    timeOut = 1; % in seconds
    status = -1; % initialize

    BytesRead = 0;
    
    % if we get error 224, it seems the sensor was not opened properly
    % or, alternatively, it was opened, but due to termination of a NXC
    % program, it was "closed" again... so let's warn about this, but only
    % once...
    ShowedWarning224 = false;
    
%% Actual request-loop
    timedOut = false;
    while ((status ~= 0) || ( BytesRead < 1)) 

        % note that we suppress statusbyte-error-warnings by requesting the
        % 3rd optional output argument of NXT_LSRead()
        [data BytesRead status] = NXT_LSRead(Port, handle);

        % discovery: if we get an error 221, it seems there is nothing that
        % can be done to retrieve the data. we will run into a timeout. so
        % why not retry the whole reading process?
        if (status == 221)  % 221 = "Communication bus error"
                        
            if isdebug()
                textOut(sprintf(['! COM_ReadI2C failed, recursion depth ' num2str(RecursionDepth) ' @ ' datestr(now, 'HH:MM:SS.FFF') '\n']))
            end%if
            
            % recursively retry this whole thing!
            ReturnBytes = COM_ReadI2C(Port, RequestLen, DeviceAddress, RegisterAddress, handle, RecursionDepth + 1);
            
            return 
            
        elseif (status == 224) % 224 = "Specified channel/connection not configured or busy"
            
            % for this error, we do nothing but showing a warning once...
            if (RecursionDepth == 1) && (ShowedWarning224 == false)
                warning('MATLAB:RWTHMindstormsNXT:Sensor:I2CnotConfiguredOrBusy', 'It seems the specified digital sensor is either busy or was not opened properly. If rebooting the NXT does not help, make sure the correct Open* command for this sensor has been executed, and no NXC/NBC program running on the brick has been terminated (or is currently being terminated) before/while sensor data are read. Trying to continue...');
                ShowedWarning224 = true;
            end%if
            
        end%if
        
        % already timed out?
        if etime(clock, startTime) > timeOut
            timedOut = true;
            break
        end%if
        
    end%while

Please note that that function can be called recursively to re-try retrieving data, a simple check at the beginning is:

Code: Select all

    % Check for maximum recursion depth! This can happen when this function
    % repeatedly cannot read valid data via I2C. We set it by hand to 3
    % tries. This condition will happen when for example you pull out the
    % sensorcable of the US sensor just while it is retrieving data...
    if RecursionDepth > 3
        return
    end%if

This probably exactly what you discovered:

The second, 0xDD (Communication buss error), is a bit more problematic. If the bus ends up in this state it will just continue to fail until the error is cleared. It is easy enough to solve this; you just need to do a LsWrite(). Any LsWrite() should do, and I simply chose to resend the original one (from step 1).

Maybe I didn't realize how easy this error is to circumvent. Anyway, this is why the whole COM_ReadI2C can be called recursively to re-try getting the data...



I can't tell whether the code I posted is always faster than the original version with LSGetStatus for all sensors, but here are some example calls from our sensor functions:

HiTechnic Color V1 + V2

Code: Select all

% see http://www.hitechnic.com for more register information
% retrieve 14 bytes from device 0x02, register 0x42 - 0x4F
% in mode 0 only the first 4 bytes are needed
lng = 14;
if mode == 0
       lng = 4;
end
  
%waitUntilI2CReady(port, handle);
data = COM_ReadI2C(port, lng, uint8(2), uint8(66), handle);

HiTechnic Compass:

Code: Select all

% retrieve 2 bytes from device 0x02, register 0x44   
    data = COM_ReadI2C(port, 2, uint8(2), uint8(68), handle);

HiTechnic Accelerator:

Code: Select all

% retrieve 6 bytes from device 0x02, register 0x42
    data = COM_ReadI2C(port, 6, uint8(2), uint8(66), handle);

Codatex RFID:

Code: Select all

%% Get sensor data
    waitUntilI2CReady(port, handle);
    % read 5 bytes from register 0x42
    data = COM_ReadI2C(port, 5, uint8(4), uint8(66));
    datahex = dec2hex(data);

That waitUntilI2CReady(), which just polls LSGetStatus, is needed as the RFID sensor behaves a bit differently (and has more complex commands) then the usual HiTechnic / LEGO digital sensors.

[mindsqualls]

Arrrrggghhh...... I have been struggling with this problem for a while now. Perhaps some fresh insight might help.

For some unfathomable (to me) reason the algorithm only works for the Bluetooth-case.

When it comes to USB, it seems to work, but this is really an illusion. What happens is that it consistently returns the value that it should have returned on the previous sensor-reading:

• If I point the ultrasonic sensor at my hand it will return 10 cm.
• If I point the sensor out into open air it retunes 10 cm once more, before it starts returning 255 cm (=infinity).
• And if I point the sensor at my hand again, it will return 255 cm once more before it starts returning 10 cm again.

When the .Net program is reading the sensor frequently enough you will not notice this delay, but it is genuine. If I e.g. read the sensor in 5 seconds intervals it becomes very obvious.

It is not only the ultrasonic sensor that misbehaves. I have also reproduced it consistently for the HiTechnic accelerator sensor, and I suspect that it is a general problem with the algorithm and I2C sensors.

As said, this is a problem for the USB-case – for Bluetooth it works just fine. And it is driving me nuts.

.oOo.

Well, luckily it is the Bluetooth-case where the performance enhancement is most welcome. So far I have chosen the pragmatic approach of using the new algorithm when the connection to the NXT is through Bluetooth and keeping the old algorithm when it is USB.

[linusa]

Oh oh, this sounds familiar. I'm afraid it's the exact symptoms the nxt-python developers describe: https://code.google.com/p/nxt-python/is ... tail?id=13 , so it seems you're on to something.

I'm very sorry I can't work on reproducing this bug with my original MATLAB code (the NXT for that purpose is occupied atm.). There could be a subtle difference between libusb and the Fantom driver. If the described behaviour also occurs with our toolbox, I'll have to deactivate the optimization for USB code, or accept the fact that the values return the value before the last value requested

Thanks for your continued interest in this...

Edit: Typo