Announcement

Collapse
No announcement yet.

Gyro Read Latency... Development team update.

Collapse
X
  • Filter
  • Time
  • Show
Clear All
new posts

  • Gyro Read Latency... Development team update.

    Hi All....

    This post specifically addresses the issues raised about how quickly (slowly) gyro updates are available to an opmode.
    It does not address ODS issues that may or may not be related.

    The planned schedule is to migrate the current 2.4 Beta to release status this Monday.
    At the same time (or very soon afterwards) a new 2.5 beta will be made available on the GitHub site.

    There are three topics I wanted to discuss here.

    1) There were some inefficiencies in the MR Gyro I2C handling code which were slowing down the transfer of fresh heading information from the Gyro.
    These inefficiencies translated into a 100% overhead. So, once they were removed, the data latency was reduced to about 50%.
    These changes have already been implemented in the forthcoming 2.5 beta.

    2) Since any device which exists in the robot configuration has the effect of adding to the overall data latency, there are gains to be realized by "disabling" any devices not currently being used. (thnaks to other posters on this forum for this idea) In the case of this year's game, this definitely includes the MR color sensors that aren't needed for normal driving, only for beacon identification.

    The dev. team sees a real advantage to providing an API to enable and disable sensors to improve performance, but this requires further coding and testing, so it's also unlikely to be included in the 2.5 beta, but it is possible for teams to selectively implement this same process in their own opmodes. (see end of post)

    3) There may be some small gains in I2C efficiency that can be gained from a tweak of the general I2C interface to utilize block data transfers.
    These changes are still under review, and will probably not be implemented until after the 2.5 Beta.

    To give you an idea what can be expected...

    The following Gyro latency times indicate the improvements that have be obtained from methods 1 and 2.
    These times indicate how many mSec between changing Gyro values.

    Configuration: 1 Gyro
    (27 mSec) SDK 2.3 and 2.4
    (13 mSec) SDK 2.5

    Configuration: 1 Gyro, 2 color sensors
    (80 mSec) SDK 2.3 and 2.4
    (25 mSec) SDK 2.5
    (14 mSec) SDK 2.5 with Color sensors disabled while driving.


    Configuration: 1 Gyro, 2 color sensors, 4 Motor Controllers, 1 Servo Controller.
    (170 mSec) SDK 2.3 and 2.4
    (65 mSec) SDK 2.5
    (25 mSec) SDK 2.5 with Color sensors disabled while driving.

    So your specific improvments will depend on what you have on your robot and whether you can disable other sensors while driving.

    The following code shows how the color sensors can be initailized, and then subsequently enabled and disabled.


    Code:
    // Declare Color Sensor objects (and other items required to enable/disable)
    ModernRoboticsI2cColorSensor  leftColor = null;
    ModernRoboticsI2cColorSensor  rightColor = null;
    
    I2cAddr leftColorAddress  = I2cAddr.create8bit(0x3c);
    I2cAddr rightColorAddress = I2cAddr.create8bit(0x4c);
    
    I2cController   leftColorController;
    I2cController   rightColorController;
    
    I2cController.I2cPortReadyCallback leftColorCallback;
    I2cController.I2cPortReadyCallback rightColorCallback;
    
    boolean colorSensorsDisabled = false;
    Code:
    /***
    * Initialize two color sensors, and take a copy of callback handlers for later use.
    */
    public void colorInit() {
    	leftColor   = myOpMode.hardwareMap.get(ModernRoboticsI2cColorSensor.class, "left color");
    	leftColor.setI2cAddress(leftColorAddress);
    	leftColorController = leftColor.getI2cController();
    	leftColorCallback =  leftColorController.getI2cPortReadyCallback(leftColor.getPort());
    
    	rightColor  = myOpMode.hardwareMap.get(ModernRoboticsI2cColorSensor.class, "right color");
    	rightColor.setI2cAddress(rightColorAddress);
    	rightColorController = rightColor.getI2cController();
    	rightColorCallback =  rightColorController.getI2cPortReadyCallback(rightColor.getPort());
    
    	colorSensorsDisabled = false;
    }
    Code:
    /***
    *   enable color sensors by re-registering callbacks
    */
    public void colorEnable() {
    	if(colorSensorsDisabled) {
    	    if (leftColorCallback != null)
    		leftColorController.registerForI2cPortReadyCallback(leftColorCallback, leftColor.getPort());
    	    if (rightColorCallback != null)
    		rightColorController.registerForI2cPortReadyCallback(rightColorCallback, rightColor.getPort());
    	}
    	colorSensorsDisabled = false;
    }
    Code:
    /***
    *   disable color sensors by de-registering callbacks
    */
    public void colorDisable()
    {
    	if (!colorSensorsDisabled) {
    	    leftColorController.deregisterForPortReadyCallback(leftColor.getPort());
    	    rightColorController.deregisterForPortReadyCallback(rightColor.getPort());
    	}
    	colorSensorsDisabled = true;
    }

  • #2
    Phil,
    Thank you, thank you!!!
    We will be looking forward to the 2.5 beta release. In the meantime, I assume the mechanism to "disable/enable I2C sensors" will work with SDK 2.3, 2.4?

    Comment


    • #3
      To make things easier, here is a class that wraps around the logic Phil has provided and can be used with different I2c sensors.
      Code:
      /*
       * Copyright (c) 2015 Titan Robotics Club (http://www.titanrobotics.com)
       *
       * Permission is hereby granted, free of charge, to any person obtaining a copy
       * of this software and associated documentation files (the "Software"), to deal
       * in the Software without restriction, including without limitation the rights
       * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
       * copies of the Software, and to permit persons to whom the Software is
       * furnished to do so, subject to the following conditions:
       *
       * The above copyright notice and this permission notice shall be included in all
       * copies or substantial portions of the Software.
       *
       * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
       * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
       * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
       * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
       * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
       * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
       * SOFTWARE.
       */
      import com.qualcomm.robotcore.hardware.I2cController;
      import com.qualcomm.robotcore.hardware.I2cDevice;
      
      public class FtcI2cDeviceState
      {
          private I2cDevice device;
          private I2cController i2cController;
          private I2cController.I2cPortReadyCallback deviceCallback;
          private boolean deviceEnabled;
      
          public FtcI2cDeviceState(I2cDevice device)
          {
              this.device = device;
              i2cController = device.getI2cController();
              deviceCallback = i2cController.getI2cPortReadyCallback(device.getPort());
              deviceEnabled = true;
          }   //FtcI2cDeviceState
      
      public boolean isEnabled()
          {
              return deviceEnabled;
          }   //isEnabled
      
      public void setEnabled(boolean enabled)
          {
              if (deviceEnabled != enabled)
              {
                  if (enabled)
                  {
                      i2cController.registerForI2cPortReadyCallback(deviceCallback, device.getPort());
                  }
                  else
      {
                      i2cController.deregisterForPortReadyCallback(device.getPort());
                  }
              }
          }   //setEnabled
      
      }   //class FtcI2cDeviceState
      Here is an example on how to use this class.
      Code:
      public ModernRoboticsI2cColorSensor beaconColorSensor;
      public FtcI2cDeviceState beaconColorSensorState;public ModernRoboticsI2cRangeSensor rangeSensor;public FtcI2cDeviceState rangeSensorState;...// In your init method....beaconColorSensor = hardwareMap.get(ModernRoboticsI2cColorSensor.class, "colorSensor");beaconColorSensorState = new FtcI2cDeviceState((I2cDevice)beaconColorSensor);beaconColorSensorState.setEnabled(false);rangeSensor = hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "rangeSensor");rangeSensorState = new FtcI2cDeviceState((I2cDevice)rangeSensor);rangeSensorState.setEnabled(false);...// Somewhere in your code when you need the color sensor.beaconColorSensorState.setEnabled(true);// When you are done with the color sensor.beaconColorSensorState.setEnabled(false);...// Somewhere in your code when you need the range sensor.rangeSensorState.setEnabled(true);// When you are done with the range sensor.rangeSensorState.setEnabled(false);
      Caveat: I don't have access to the robot, so the code is not tested yet.

      Comment


      • #4
        Hmm, cut and paste in the forum window sometimes did funny things to spacing! Here is the example code with proper spacing.
        Code:
        public ModernRoboticsI2cColorSensor beaconColorSensor;
        public FtcI2cDeviceState beaconColorSensorState;
        public ModernRoboticsI2cRangeSensor rangeSensor;
        public FtcI2cDeviceState rangeSensorState;
        ...
        // In your init method.
        beaconColorSensor = hardwareMap.get(ModernRoboticsI2cColorSensor.class, "colorSensor");
        beaconColorSensorState = new FtcI2cDeviceState((I2cDevice)beaconColorSensor);
        beaconColorSensorState.setEnabled(false);
        rangeSensor = hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "rangeSensor");
        rangeSensorState = new FtcI2cDeviceState((I2cDevice)rangeSensor);
        rangeSensorState.setEnabled(false);
        ...
        // Somewhere in your code when you need the color sensor.
        beaconColorSensorState.setEnabled(true);
        // When you are done with the color sensor.
        beaconColorSensorState.setEnabled(false);
        ...
        // Somewhere in your code when you need the range sensor.
        rangeSensorState.setEnabled(true);
        // When you are done with the range sensor.
        rangeSensorState.setEnabled(false);

        Comment


        • #5
          Phil,
          Got a question: when PortReadyCallback is de-registered and re-registered later on, would the very first data point right after re-registering valid? Knowing I2C device takes time to get data from the hardware, I was afraid that when the callback is de-registered, the cache will stop refreshing. So when you re-register the callback, would I get a very stale data point from the cache if I call getData immediately after re-registering?
          We need to know the exact behavior because we may need to "turn it on" a little earlier before we need the data.

          Comment


          • #6
            Sorry, found a minor bug in the wrapper class. Correction below:
            Code:
            import com.qualcomm.robotcore.hardware.I2cController;
            import com.qualcomm.robotcore.hardware.I2cDevice;
            
            public class FtcI2cDeviceState
            {
                private I2cDevice device;
                private I2cController.I2cPortReadyCallback deviceCallback;
                private boolean deviceEnabled;
            
                public FtcI2cDeviceState(I2cDevice device)
                {
                    this.device = device;
                    deviceCallback = device.getI2cPortReadyCallback();
                    deviceEnabled = true;
                    if (device.getI2cController().getI2cPortReadyCallback(device.getPort()) != deviceCallback)
                        throw new IllegalStateException("PortReadyCallback don't match!");
                }   //FtcI2cDeviceState
            
            public boolean isEnabled()
                {
                    return deviceEnabled;
                }   //isEnabled
            
            public void setEnabled(boolean enabled)
                {
                    if (deviceEnabled != enabled)
                    {
                        if (enabled)
                        {
                            device.registerForI2cPortReadyCallback(deviceCallback);
                        }
                        else
            {
                            device.deregisterForPortReadyCallback();
                        }
                        deviceEnabled = enabled;
                    }
                }   //setEnabled
            
            }   //class FtcI2cDeviceState
            

            Comment


            • #7
              Originally posted by mikets View Post
              Phil,
              Got a question: when PortReadyCallback is de-registered and re-registered later on, would the very first data point right after re-registering valid? Knowing I2C device takes time to get data from the hardware, I was afraid that when the callback is de-registered, the cache will stop refreshing. So when you re-register the callback, would I get a very stale data point from the cache if I call getData immediately after re-registering?
              We need to know the exact behavior because we may need to "turn it on" a little earlier before we need the data.
              I would definitely turn it on a little early. You will definitely get very stale data until the next fresh read (10's of mSec later).
              Or you could wrap the read with some code that blocks till it gets new data. Could be tricky to know for sure though.

              Comment


              • #8
                Originally posted by mikets View Post
                Phil,
                Thank you, thank you!!!
                We will be looking forward to the 2.5 beta release. In the meantime, I assume the mechanism to "disable/enable I2C sensors" will work with SDK 2.3, 2.4?
                Yes. That is correct.

                Comment


                • #9
                  Phil, how about this question?
                  Originally posted by mikets View Post
                  Phil,
                  Got a question: when PortReadyCallback is de-registered and re-registered later on, would the very first data point right after re-registering valid? Knowing I2C device takes time to get data from the hardware, I was afraid that when the callback is de-registered, the cache will stop refreshing. So when you re-register the callback, would I get a very stale data point from the cache if I call getData immediately after re-registering?
                  We need to know the exact behavior because we may need to "turn it on" a little earlier before we need the data.

                  Comment


                  • #10
                    Sorry, please ignore the above post, I didn't realized you had two posts.

                    Comment


                    • #11
                      Originally posted by Philbot View Post
                      I would definitely turn it on a little early. You will definitely get very stale data until the next fresh read (10's of mSec later).
                      Or you could wrap the read with some code that blocks till it gets new data. Could be tricky to know for sure though.
                      That usually isn't a problem. Assuming autonomous is implemented using a state machine or just linear execution of "steps", you will just turn on the I2C sensor the state/step before unless the step before was a gyro turn that requires the other I2C sensors to be off. In that case, you can probably do a 50-100 msec delay before going into the step/state that requires the I2C sensor data.

                      Comment


                      • #12
                        Originally posted by Philbot View Post
                        Hi All....

                        1) There were some inefficiencies in the MR Gyro I2C handling code which were slowing down the transfer of fresh heading information from the Gyro.
                        These inefficiencies translated into a 100% overhead. So, once they were removed, the data latency was reduced to about 50%.
                        These changes have already been implemented in the forthcoming 2.5 beta.
                        To clarify here, the inefficiency is proportional to the number of devices also wanting access to the i2c bus through the usb bus (which is the big bottleneck). The result of the inefficiency was to starve the gyro of opportunities to exercise reads. Which is to say, you don't really see it if nothing is configured except for a single gyro. The gyro now has equal opportunity to exercise reads, which implies that the more devices you have the more latency you have, but all devices will have roughly equal latency as they all now get an equal opportunity to share the scarce resource of a single usb bus.

                        Comment


                        • #13
                          Are these improvements only for the MR gyro, or do they carry over to the BNO055 IMU as well?

                          Comment


                          • #14
                            Originally posted by 4634 Programmer View Post
                            Are these improvements only for the MR gyro, or do they carry over to the BNO055 IMU as well?
                            The changes in the 2.5 Beta only effect the MR Gyro (That's where the inefficiency was).
                            However, the action of turning off un-needed I2C devices will probably improve the latency of all remaining I2C devices.

                            Possible improvements to all I2C devices will come out of the more complete review of the I2C handlers.

                            Comment


                            • #15
                              Outstanding news. Thanks to the team. I can confirm the relative improvement gained by disabling the color sensor via deregistering the callback.
                              This is the setup we used at states yesterday. I am excited about getting the 2.5 beta MR I2C gyro changes to chop the sample update time further.
                              From the description, it sounds like the update rate of any component is still slowed down by the addition of other components to the USB bus - whether they are I2C devices connected to the CDIM, motor controllers, or servo controllers. Is this correct? If so, is it USB bus contention that is the next hurdle?

                              Comment

                              Working...
                              X