How to get Yaw angle or Heading on Robot Inventor Hub?

[johnscary-ev3]

I am using imu.tilt() to get pitch and roll angles on Robot Inventor Hub.
This is working great, but I am wondering if we can also get yaw (z axis rotation) angle?
From documentation it mentions that a heading function is under development.
That would be great, but a yaw angle could serve this purpose as long as pitch and roll angles were small, I think.
Is there a way to get yaw angle while heading function in being developed?
Thanks

[laurensvalk]

Yaw and heading are mostly the same. The sensor doesn’t measure them directly.

To get them, we need to add a background process that continuously monitors the IMU to keep track of yaw motion. We haven’t gotten around to doing this yet.

[johnscary-ev3]

Thanks much for the reply.
Since we have ability to get angular velocity in z-axis (yaw rate), I will try to do my own routine which integrates this over time to get the yaw angle. I imagine that your system level code would do essentially the same thing.
It will be interesting to see how accurate this will be.

[johnscary-ev3]

I have started working on a yaw_angle routine but I noticed some problems with the yaw_rate readings that may make it hard to do.
When I ran your yaw_rate demo (slightly modified for Blast robot) I see that the yaw_rate takes a while to settle down after you move the robot. So, if you integrate this, the yaw_angle tends to drift after you stop moving the robot.
In this data example, I started the program and then moved the robot ~90dg in yaw at ~ 40-60 on time axis. After I stopped moving, the yaw_rate takes quite a while to settle back down to zero.
Any ideas on this?

from pybricks.hubs import PrimeHub, InventorHub
from pybricks.tools import wait
from pybricks.geometry import Axis

# Initialize the hub.
#hub = PrimeHub()
# Initialize the hub. In this case, specify that the hub is mounted with the
# top side facing forward and the front side facing to the right.
# For example, this is how the hub is mounted in BLAST in the 51515 set.
hub = InventorHub(top_side=Axis.X, front_side=-Axis.Y)

# Get the acceleration or angular_velocity along a single axis.
# If you need only one value, this is more memory efficient.
while True:

    # Read the forward acceleration.
    # forward_acceleration = hub.imu.acceleration(Axis.X)

    # Read the yaw rate.
    yaw_rate = hub.imu.angular_velocity(Axis.Z)

    # Print the yaw rate.
    print(yaw_rate)
    wait(100)

[laurensvalk]

Thanks sharing!

What version of the firmware did you try this on?

I thought this would have been fixed by pybricks/pybricks-micropython@2e02c27.

[johnscary-ev3]

I have:
MicroPython v3.1.0-110-gfedf09ed on 2022-01-17; SPIKE Prime Hub with STM32F413VG

Looks like I need to update and try this again. Thanks.

[laurensvalk]

By the way, here's a tip for data logging if you use Pybricksdev.

Try running a variation of this script. This example will create a file called "my_data.txt" on your computer where you can easily process it as you like.

Anything in between these two special print commands goes into the file.

print("_file_begin_", "my_data.txt")
# This is example data, but you can print anything you want.
print("4,8037,2,0,2,1021,2,0,4500,38498")
print("9,8037,2,0,2,1622,2,3,27000,41289")
print("14,8051,2,0,2,1357,2,13,13500,43731")
print("19,8051,2,0,2,1210,2,23,4500,46521")
print("24,8044,2,0,2,2551,2,18,58500,48963")
print("29,8044,2,0,2,1015,2,41,-9000,51753")
print("34,8030,2,0,2,2573,2,34,54000,54195")
print("39,8030,2,0,2,1842,2,55,20500,56986")
print("44,8028,2,0,2,2008,3,61,25000,59427")
print("49,8028,2,0,2,2396,3,66,38500,62218")
print("54,8016,2,0,2,3375,3,70,77000,64660")
print("59,8016,2,0,2,2369,4,88,32000,67450")
print("64,8010,2,0,2,2965,4,90,54500,69892")
print("69,8010,2,0,2,2253,5,106,48000,46500")
print("74,8036,2,0,2,2245,5,106,48000,46500")
# This will close the file and save it on your computer
print("_file_end_")

[johnscary-ev3]

Thanks for the tip!

[dlech]

@cmyers734

[johnscary-ev3]

I upgraded to latest firmware 3.2 to try the fix for yaw angle rate drifting as in above chart.
The new firmware does seem to work better but I did notice a small, fixed offset in yaw rate of about 1 dg/s.
This causes a slight linear drift after I stop moving the robot.
So, I took the first ten readings and averaged them and subtracted it out.
In the chart below, we now get a fairly constant yaw angle after the move.
Again, in this test I rotated Robot ~90 deg in the 10 to 70 time range using same program as above.
I think this could be useful now to get, and hold, a heading angle as long as the offset stays fixed.
Any comments?

[laurensvalk]

This is exactly how a basic yaw determination algorithm works indeed. 🎉

I did notice a small, fixed offset

This is known as gyro drift. Your solution to subtract the stationary average is perfectly fine depending on the application.

In some applications, an additional sensor can be used to provide this correction. In the 3D case, we'll be able to use the acceleration sensor to ensure tilt and roll won't drift. Since we don't have a compass, we can't correct for long term yaw errors, but we can still make it stationary with a method that is conceptually similar to yours.

I'm actually really looking forward to working on this when the time comes. 3D rotation is fun :)

[cmyers734]

I have been interested in adding some firmware-level calibration routines to the IMU class in pybricks-micropython to get good heading readings, so this discussion is a good kick. In the EV3 days, our FLL team would use the gyro, but ever since the SPIKE Prime, we haven't been able to be more accurate with the gyro than estimating with motor encoder counts.

So where our heads are at now is that we need to address three main problems to get a usable heading (we are still experimenting, so this is all subject to change):

• Bias: you will see this as a non-zero velocity reading when the unit is not moving. The simplest approach here is to do what you are doing. You take a number of samples, compute the average, and this is your bias. You then just remove it from all subsequent readings. Note: we have found that this will change with temperature. So far, we have found that it's best to take a number of samples after the unit has been on for a second or so. Some gyro's ask you to throw out the first N samples.
• Bias instability: This is where even though you've computed the bias, it will still drift after moving around. We have found that with the SPIKE Prime gyro, one needs to re-calibrate periodically when we know the unit is not moving.
• Scale-factor error: the gyro will have a fairly repeatable error at any given rate of change. Basically, if you swing it around at a known rate of X degrees per second, the gyro might tell you (X * 0.9) instead. It may be different than 0.9 if you swing it faster or slower. The basic calibration process for this is to create a calibration fixture such that one can swing the unit around at a known rate and angle. Then measure the average scale factor error, then compensate. This is why you're ending up at 80 degrees in a 90 degree swing.

All of the above change with temperature and age, so if you are using this in competition, you'd need to re-calibrate often.

As a fun observation, we have noticed that the scale-factor error is different when you swing clockwise vs counterclockwise (by a LOT).

Here are some of the better (more understandable) papers/sites that we have been collecting:

• https://www.analog.com/media/en/technical-documentation/technical-articles/gyrocalibration_edn_eu_7_2010.pdf
• https://mwrona.com/posts/gyro-noise-analysis/
• https://www.ncbi.nlm.nih.gov/pmc/articles/PMC3787445/

[johnscary-ev3]

Wow, thanks for all the insights and details on using the new Gyro sensor for heading.
I saw the Bias but did not realize we also need to deal with Scale-factors.
I did wonder why I got 80 dg instead of 90 dg when I rotated the robot.
My experimental setup was just grabbing the robot and turning it on my desk.
Will need something better for calibration. Maybe do a controlled tank turn while taking gyro data.

I read the first paper on your list and have a couple questions from that.
With respect to noise, which I also noticed in my data, it seems like it mostly drops out when integrating vs. time so not a big deal, I think?
The article also mentioned to calculate the Scale-factor in both directions but then just take an average.
You mentioned that they differ by a lot.
So, I guess your plan would be to apply different factors depending on which way we are turning? That seems reasonable.
I am looking forward to seeing what you all come up for the gyro-based heading.

[cmyers734]

We've tried two different approaches so far as a calibration rig. The most recent was to buy a cheap photo turntable like the kind that photographers use to take 360 shots of their products. It has three rotation speeds and can do continuous, 90 deg, and 180 back-and-forth. The previous was to build one using the kit itself. A small/large motor simply turns a platform (as you likely know this means +/- 3 degrees at the motor because of slack in gear box). I suspect that the homemade one out of legos is better because it's more programmable.

I like your idea, too, for a rough order of magnitude, but eventually, you will need something more accurate than tank steer (otherwise, you can just stick to tank steer instead of heading-based control).

So far this particular gyro seems to be "extra bad". I found questions on ST's forum asking about the difference in left/right scale and the ST staff basically said: "dunno".

While I might accidentally sound authoritative on this subject, I assure you that I'm not an expert (so please share what you learn/see as well). We just tried everything we could last year to get the gyro to be useful for FLL competition and simply gave up on it. It was hard to explain to the kids what was going on much less fix it. In the process, we learned a lot, though. Since then, pybricks support for the SPIKE gave us new hope that maybe we could fix these things at a low enough level to make it viable.

As for running it left/right then taking an average, this is where we're at with our gyro journey: computing this scale factor to see if it's consistent enough. I just need to get some free time from work to complete the rig and see. My plan was to compute them separately and use them separately. My worry is that around 0 degrees-per-second, I will asymmetrically magnify the error and effectively re-introduce drift. You can imagine all the things that one might try to fix this.

As for the noise, you will see it as noise that is normally distributed around the static bias. So yes... over time, this will/should cancel out. For best results, you want to sample "fast enough". I'm interested in using the buffering/FIFO capabilities of the chip to get more samples under the hood to see if this bias is more stable, but I'm not sure about the implications for all the other things going on in the firmware.

[johnscary-ev3]

Hi again,
I have been playing around to see if I can get a useful Yaw_angle integration and Heading from the Gyro readings.
This is a quick update of my learnings which I hope may help your efforts to add to pybricks.

Basically, I seem to be fighting the noise in the Yaw_angle readings even though I thought it might cancel out.
There is some data below for a no motion case trying to get the offset readings.
Below is some code I did for this. For Offset I read 10 times and discard and then average 50 more readings.
This is still not really stable, but I settled on that.
For Yaw angle readings I have the routine in a ~50msec loop where I take 5 Gyro readings and average them.
I can still see drift in Yaw Angle when not moving due to noise spikes, so I put in a filter for minimum useful reading of 0.3 which seems to suppress drift.
Hopefully you can reduce yaw rate noise with different Gyro read settings or averaging.

Also, I am doing this on Blast robot in which the Hub is the head and can move around when you move the arms or head.
So, I put in an Enable flag to not integrate Yaw Angle when moving the head/arms and not the body. Probably best to not have a Hub that is not part of the body but thought I would try. This works ok but not perfect.

I suspect that I also need a higher sample rate but have not optimized on that yet.
I think doing this a lower level would allow more samples as mentioned above and give better accuracy by not missing any movements.
My code does ok on slow smooth movements, but jerky ones tend to mess up the yaw_angle.

# Get Yaw Angle using timer to get time interval
yaw_timer = StopWatch()
yaw_timer.reset()
yaw_angle_integration = 0
yaw_angle_running = False
yaw_angle_integration_enabled = True
# yaw rate calibration factors; offset in degrees/sec
# yaw_rate_offset will be measured during robot setup or when requested
yaw_rate_offset = 1.0
yaw_rate_factor = 1.0
yaw_rate_offset_max =-10
yaw_rate_offset_min = 10
yaw_rate_min_use = 0.3
yaw_rate_num_samples = 5
def get_yaw_angle():
    global yaw_timer, yaw_angle_running, yaw_angle_integration
    if yaw_angle_integration_enabled:
        # Read the yaw rate value with Averaging. Calculate Yaw_angle.
        if yaw_angle_running:
            yaw_rate_raw =0
            for i in range(yaw_rate_num_samples):
                yaw_rate_raw += hub.imu.angular_velocity(Axis.Z)
            yaw_rate_raw = yaw_rate_raw/yaw_rate_num_samples
            yaw_rate = (yaw_rate_raw-yaw_rate_offset)*yaw_rate_factor
            delta_time = yaw_timer.time()
            delta_time = delta_time/1000
            # Check for low yaw rate which means we are not moving
            # So don't integrate to reduce drift
            if abs(yaw_rate) > yaw_rate_min_use:
                yaw_angle_integration = yaw_angle_integration + yaw_rate*delta_time
        # First time, start the timer, set yaw_angle_running   
        yaw_angle_running = True
    # Reset Timer and return Yaw Angle
    yaw_timer.reset()
    return(yaw_angle_integration)

# Get Yaw Rate Offset ; Call during robot setup while Not moving
def get_yaw_rate_offset():
    global yaw_rate_offset
    global yaw_rate_offset_max, yaw_rate_offset_min
    yaw_rate_offset = 0
    print('Yaw_Rate Offset Setup:')
    # Take First group of readings of yaw rate and Discard them
    num_samples = 10
    for i in range(num_samples):
        wait(100)
        yaw_rate = hub.imu.angular_velocity(Axis.Z)
    # Take Second group of readings of yaw rate and Average them
    num_samples = 50
    for i in range(num_samples):
        wait(100)
        yaw_rate = hub.imu.angular_velocity(Axis.Z)
        yaw_rate_offset = yaw_rate_offset + yaw_rate
        print('Yaw Rate Reading #', i+1, '=', yaw_rate)
    yaw_rate_offset = yaw_rate_offset/num_samples
    if(yaw_rate_offset > yaw_rate_offset_max):
        yaw_rate_offset_max = yaw_rate_offset
    if(yaw_rate_offset < yaw_rate_offset_min):
        yaw_rate_offset_min = yaw_rate_offset
    print("Yaw Rate Offset Average=", yaw_rate_offset, 'Min=', yaw_rate_offset_min, 'Max=', yaw_rate_offset_max)
  

# Reset Yaw Angle Integration
def reset_yaw_angle():
    global  yaw_timer, yaw_angle_running, yaw_angle_integration
    yaw_angle_integration = 0
    yaw_angle_running = False
    yaw_timer.reset()

# Get Heading from Yaw Angle Integration
def get_heading_angle():
    global  yaw_angle_integration
    if yaw_angle_integration <= 0:
        heading_angle = (-yaw_angle_integration % 360) 
    else:
        heading_angle = 360-(yaw_angle_integration % 360)
    return(heading_angle)

[johnscary-ev3]

Great question on the sample timing.
I was wondering about the same thing after I wrote my routines.
So I added some capability to get the Average time interval and the Max and Min.
That code is below. I have another command routine that I use to periodically request a printout of the info.
What I found is that the interval between the samples is fairly stable, but the length was longer than I expected.
I first had a 50msec wait time and then interrogated the sensors, check motor move status, and got remote commands in a loop.
I was seeing times greater than 100msec for this loop. After some checking I found that the LEGO Remote button checking was taking 70 to 100msec and other items took only about 6 to 7 msec. First, I tried to speed this up by reducing the wait from 50 to 1 msec.
This sort of worked but I found that the system became unstable and would occasionally "hang". If I put 10 msec wait in the loop then it was ok. I guess that is subject to another issue but by removing LEGO Remote and using wait of 10msec I was able to get the sample times down to 16 to 17 msec range. This makes it work somewhat better.

I have not really calibrated the scale factors yet so just using 1.0 which actually seem to be accurate to about 1 to 2 % most of the time when I do a 360 dg tank turn. Will try to do that calibration next.
Sudden, fast moves seem to give problems as you mention.
Doing an Automatic re-zero of static bias sound interesting. I can do it manually now with a keyboard request command but was not really sure why the bias tends to bounce around. I would need a faster bias offset sample routine with no waits or prints to do this.
Will try it.
Sampling in the background will definitely be better long term ;0)

# Get Yaw Angle using timer to get time invertal
yaw_timer = StopWatch()
yaw_timer.reset()
yaw_angle_integration = 0
yaw_angle_running = False
yaw_angle_integration_enabled = True
# for Timer Interval Statistics
yaw_angle_time_interval_avg = 0
yaw_angle_time_interval_avg_sum =0
yaw_angle_time_interval_avg_count = 0
yaw_angle_time_interval_avg_max = 0
yaw_angle_time_interval_avg_min = 100
# yaw rate calibration factors; offset in degrees/sec
# yaw_rate_offset will be measured during robot setup or when requested
yaw_rate_offset = 1.0
yaw_rate_factor = 1.0
yaw_rate_offset_max =-10
yaw_rate_offset_min = 10
yaw_rate_min_use = 0.3
yaw_rate_num_samples = 5
def calculate_yaw_angle():
    global yaw_timer, yaw_angle_running, yaw_angle_integration
    global yaw_angle_time_interval_avg 
    global yaw_angle_time_interval_avg_sum
    global yaw_angle_time_interval_avg_count
    global yaw_angle_time_interval_avg_max 
    global yaw_angle_time_interval_avg_min 
    if yaw_angle_integration_enabled:
        # Read the yaw rate value with Averaging. Calculate Yaw_angle.
        if yaw_angle_running:
            yaw_rate_raw =0
            for i in range(yaw_rate_num_samples):
                yaw_rate_raw += hub.imu.angular_velocity(Axis.Z)
            yaw_rate_raw = yaw_rate_raw/yaw_rate_num_samples
            yaw_rate = (yaw_rate_raw-yaw_rate_offset)*yaw_rate_factor
            delta_time = yaw_timer.time()
            delta_time = delta_time/1000
            # Collect Time Interval Statistics Avg Min Max
            yaw_angle_time_interval_avg_sum += delta_time
            yaw_angle_time_interval_avg_count += 1
            yaw_angle_time_interval_avg = yaw_angle_time_interval_avg_sum/yaw_angle_time_interval_avg_count
            if yaw_angle_time_interval_avg > yaw_angle_time_interval_avg_max:
                yaw_angle_time_interval_avg_max = yaw_angle_time_interval_avg
            if yaw_angle_time_interval_avg < yaw_angle_time_interval_avg_min:
                yaw_angle_time_interval_avg_min = yaw_angle_time_interval_avg

            # Check for low yaw rate which means we are not moving
            # So don't integrate to reduce drift
            if abs(yaw_rate) > yaw_rate_min_use:
                yaw_angle_integration = yaw_angle_integration + yaw_rate*delta_time
        # First time, start the timer, set yaw_angle_running   
        yaw_angle_running = True
    # Reset Timer and return Yaw Angle
    yaw_timer.reset()

def get_yaw_angle():
    yaw_angle = yaw_angle_integration
    return(yaw_angle)

# Get Yaw Rate Offset ; Call during robot setup while Not moving
def get_yaw_rate_offset():
    global yaw_rate_offset
    global yaw_rate_offset_max, yaw_rate_offset_min
    yaw_rate_offset = 0
    print('Yaw_Rate Offset Setup:')
    # Take First group of readings of yaw rate and Discard them
    num_samples = 10
    for i in range(num_samples):
        wait(100)
        yaw_rate = hub.imu.angular_velocity(Axis.Z)
    # Take Second group of readings of yaw rate and Average them
    num_samples = 50
    for i in range(num_samples):
        wait(100)
        yaw_rate = hub.imu.angular_velocity(Axis.Z)
        yaw_rate_offset = yaw_rate_offset + yaw_rate
        print('Yaw Rate Reading #', i+1, '=', yaw_rate)
    yaw_rate_offset = yaw_rate_offset/num_samples
    if(yaw_rate_offset > yaw_rate_offset_max):
        yaw_rate_offset_max = yaw_rate_offset
    if(yaw_rate_offset < yaw_rate_offset_min):
        yaw_rate_offset_min = yaw_rate_offset
    print("Yaw Rate Offset Average=", yaw_rate_offset, 'Min=', yaw_rate_offset_min, 'Max=', yaw_rate_offset_max)
  

# Reset Yaw Angle Integration
def reset_yaw_angle():
    global yaw_timer, yaw_angle_running, yaw_angle_integration
    global yaw_angle_time_interval_avg 
    global yaw_angle_time_interval_avg_sum
    global yaw_angle_time_interval_avg_count
    global yaw_angle_time_interval_avg_max 
    global yaw_angle_time_interval_avg_min 
    yaw_angle_integration = 0
    yaw_angle_running = False
    # Reset Timer Interval Statistics
    yaw_angle_time_interval_avg = 0
    yaw_angle_time_interval_avg_sum =0
    yaw_angle_time_interval_avg_count = 0
    yaw_angle_time_interval_avg_max = 0
    yaw_angle_time_interval_avg_min = 100

# Get Heading from Yaw Angle Integration
def get_heading_angle():
    global  yaw_angle_integration
    if yaw_angle_integration <= 0:
        heading_angle = (-yaw_angle_integration % 360) 
    else:
        heading_angle = 360-(yaw_angle_integration % 360)
    return(heading_angle)

[laurensvalk]

Lots of great discussion here!

First, I tried to speed this up by reducing the wait from 50 to 1 msec. This sort of worked but I found that the system became unstable and would occasionally "hang". If I put 10 msec wait in the loop then it was ok.

That sounds like this issue: #232. We'll need to write a proper I2C driver that correctly handles all errors.

[cmyers734]

On top of that / once any I2C errors are handled, do we have any schematics of how that chip is wired to the MCU from an interrupt perspective? It could be that we could have an ISR triggered when there is a new reading that can be read at high speed.

[johnscary-ev3]

Thanks for info on possible I2C errors with imu. Good to know others have seem similar behavior.
Interestingly my routine above "calculate_yaw_angle()", that reads 5 times in a row with no wait, seems to be ok as long as I do the 10msec wait in my main loop.
Maybe some fast reading is ok if it gets a chance to rest between bursts ;0)

[johnscary-ev3]

Thanks for suggestion about doing Auto re-zero of static bias.
I tried to implement that by modifying my routine that calculates the yaw angle. Code below.
When that routine sees a small yaw rate it uses that data to calculate a new zero offset.
This assumes that low yaw rate means the robot is not moving.
My routine takes 5 reading in a row as before but collects 200 of these low-rate readings and produces a new offset average.
So, it is averaging 1000 offset readings for the new offset, Because of the loop timing, it is doing that about every 5 seconds.
Below is a graph I made from data collected this way, each point is average of 1000 readings at ~ 5 sec intervals and robot not moving.
Two observations are that there is still some noise in these data points, even with 1000 readings per point, and we see a downward drift.
I will study this some more to see if I can determine cause of drift but just seems to be how long program is running to me.

Any comments?

# Get Yaw Angle using timer to get time interval
yaw_timer = StopWatch()
yaw_timer.reset()
yaw_angle_integration = 0
yaw_angle_running = False
yaw_angle_integration_enabled = True
# for Timer Interval Statistics
yaw_angle_time_interval_avg = 0
yaw_angle_time_interval_avg_sum =0
yaw_angle_time_interval_avg_count = 0
yaw_angle_time_interval_avg_max = 0
yaw_angle_time_interval_avg_min = 100
# yaw rate calibration factors; offset in degrees/sec
# yaw_rate_offset will be measured during robot setup or when requested
yaw_rate_offset = 1.0
yaw_rate_factor = 1.0
yaw_rate_offset_max =-10
yaw_rate_offset_min = 10
yaw_rate_min_use = 0.3
yaw_rate_num_samples = 5
# Auto Offset Calculation
yaw_rate_offset_auto_max_use = 0.2
yaw_rate_offset_auto_num_samples =200
yaw_rate_offset_auto = 0
yaw_rate_offset_auto_count = 0

def calculate_yaw_angle():
    global yaw_timer, yaw_angle_running, yaw_angle_integration
    global yaw_angle_time_interval_avg 
    global yaw_angle_time_interval_avg_sum
    global yaw_angle_time_interval_avg_count
    global yaw_angle_time_interval_avg_max 
    global yaw_angle_time_interval_avg_min
    global yaw_rate_offset 
    global yaw_rate_offset_auto_max_use 
    global yaw_rate_offset_auto_num_samples
    global yaw_rate_offset_auto 
    global yaw_rate_offset_auto_count 
    if yaw_angle_integration_enabled:
        # Read the yaw rate value with Averaging. Calculate Yaw_angle.
        if yaw_angle_running:
            yaw_rate_raw =0
            for i in range(yaw_rate_num_samples):
                yaw_rate_raw += hub.imu.angular_velocity(Axis.Z)
            yaw_rate_raw = yaw_rate_raw/yaw_rate_num_samples
            yaw_rate = (yaw_rate_raw-yaw_rate_offset)*yaw_rate_factor
            delta_time = yaw_timer.time()
            delta_time = delta_time/1000
            # Collect Time Interval Statistics Avg Min Max
            yaw_angle_time_interval_avg_sum += delta_time
            yaw_angle_time_interval_avg_count += 1
            yaw_angle_time_interval_avg = yaw_angle_time_interval_avg_sum/yaw_angle_time_interval_avg_count
            if yaw_angle_time_interval_avg > yaw_angle_time_interval_avg_max:
                yaw_angle_time_interval_avg_max = yaw_angle_time_interval_avg
            if yaw_angle_time_interval_avg < yaw_angle_time_interval_avg_min:
                yaw_angle_time_interval_avg_min = yaw_angle_time_interval_avg

            # Check for low yaw rate which means we are not moving
            # So don't integrate to reduce drift
            if abs(yaw_rate) > yaw_rate_min_use:
                yaw_angle_integration = yaw_angle_integration + yaw_rate*delta_time

            # Do Auto Offset Calculations if small yaw_rate is detected
            elif abs(yaw_rate) < yaw_rate_offset_auto_max_use:
                yaw_rate_offset_auto += yaw_rate_raw
                yaw_rate_offset_auto_count += 1
                if(yaw_rate_offset_auto_count == yaw_rate_offset_auto_num_samples):
                    yaw_rate_offset = yaw_rate_offset_auto/yaw_rate_offset_auto_count
                    yaw_rate_offset_auto = 0
                    yaw_rate_offset_auto_count = 0
                    print("New Auto yaw_rate_offset=", yaw_rate_offset)

        # First time, start the timer, set yaw_angle_running   
        yaw_angle_running = True
    # Reset Timer and return Yaw Angle
    yaw_timer.reset()

[dlech]

I'm interested in using the buffering/FIFO capabilities of the chip to get more samples under the hood to see if this bias is more stable,

Reading the datasheet, I don't see how the FIFO could get us a higher sample rate compared to not using it. Since the FIFO can only be read 2 bytes at a time, using it introduces quite a bit of overhead (32 bits of overhead for every 2 bytes read, i.e 24 cycles per byte read) . There are 6-bytes per data set and 4 data sets, so 24 bytes to read per sample. And since the chips are using I2C at 400kHz best case scenario reading the FIFI is 400k cycles/sec / 24 cycles/byte / 24 byte/sample = 400 sample/sec.

If we just read the regular value registers instead of the FIFO register, we can read everything (temp/accel/gyro) in a single read of 14 bytes of data. This comes with 44 bits of overhead per each read, so 11.14 cycles per byte read. Best case scenario here would be 400k cycles/sec / 11.14 cycles/byte / 14 bytes/sample = 2564 samples/sec.

[johnscary-ev3]

@dlech Your comment above implies we can read temperature from the gyro sensor.
If so, that would enable some temperature corrections, assuming we could get the temperature coefficients.
My data, and other comments above, indicate correcting for temperature could make this more accurate and stable.
Is that part of your plans?
Thanks,

[laurensvalk]

the FIFO can only be read 2 bytes at a time

Having a controllable built-in buffer with no way to read it would seem rather surprising. The snippets below appear to suggest otherwise?

It would seem ideal to do something like this, where we can rely on the sensor to give us super constant sampling intervals, and read out a burst of data whenever the sensor contiki process is given some time after the buffer is, say, half way full.

When the data is in, we can do the integration from the last known time up to the time of the last sample, and discard the data to make space for the next burst read.

---

Feature overview:

Burst-mode read from application note:

Snippet from the library we are using:

[dlech]

Thanks, I missed the application note. So updating the calculation (ignoring the overhead of the 32-bits to start the read), we get absolute max of 400k cycles/sec / 9 cycles/byte / 24 byte/sample = 1851 sample/sec.

What kind of sample burst period would we want, realistically? Every 10ms?

If LEGO connected the interrupt pin of the IMU to the MCU, that could be a way to get constant sampling intervals too instead of using the FIFO (and perhaps more efficient as well... if there aren't more mistakes in my math 😁). And I guess we would need the interrupt pin if we go with the FIFO too to get the 1/2 full signal.

[dlech]

Reading more, it looks like it is possible to disable datasets in the FIFO, so we could cut the read time in half that way by eliminating the 3rd and 4th data sets. 400k cycles/sec / 9 cycles/byte / 12 byte/sample = 3703 sample/sec.

If 8-bit data is good enough, it looks like the accelerometer and gyro can be combined in a single data set, so we could even get 400k cycles/sec / 9 cycles/byte / 6 byte/sample = 7407 sample/sec this way.

[laurensvalk]

Nice!

What kind of sample burst period would we want, realistically? Every 10ms?

Yeah, or maybe 5 ms or 20 ms, depending on the resource constraints. Perhaps the buffer could be 10 ms and then drain 5ms worth every time our contiki process lets us, and then schedule the math for updating when the data is in. It would combine the previous state with the data to get the new state matching the end of that burst.

If in FIFO mode, I wonder if we can we still do a regular read as well. If so, we could read the FIFO buffer and do the math a bit less frequently (say every 50 or 100 ms), but still offer responsive angular rates and linear accelerations.

And I guess we would need the interrupt pin if we go with the FIFO too to get the 1/2 full signal.

The LEGO API supports free-fall detection so that indicates that the interrupt pins are connected at least on Prime Hub.

[dlech]

I've just pushed some changes for testing. I haven't messed with any FIFO stuff yet, but I've made a proper driver for the IMU chip so that it reads in the background instead of when called from Python. Now calling the acceleration() and angular_velocity() methods from Python should return "instantly" and give the most recent values that were read in the background. This could potentially fix the lock up issues seen. I've also temporarily added a new temp() method to read the IMU chip temperature so that we can experiment with that. I've also changed the gyro setting from 250dps to 1000dps so that could help with the "turning too fast" issues that we have seen.

You can grab a firmware from this ci build and flash it with pybricksdev to test. @johnscary-ev3 can you confirm if the hang is fixed with a delay of 1ms that you mentioned in #675 (reply in thread)? It won't actually hang now, but if there is still a problem, the values will stop changing.

[dlech]

You can always find the latest firmware at https://github.com/pybricks/pybricks-micropython/actions/workflows/build.yml?query=branch%3Amaster+is%3Asuccess

[johnscary-ev3]

Ok thanks.
So, I should grab the latest "successful master branch" build result.
I am not too sure how your version numbering is working but I can see now that v3.2.0b3 does not fully define the version.
Got this version info from the .json file in .zip file I just grabbed.
"firmware-version": "v3.2.0b3-45-g6d06933d"
Would it be possible to add this version info to the .zip file names?
Like "primehib-firmware_v3.2.0b3-45-g6d06933d.zip" ?
That would help us keep track of what firmware version we have.

[dlech]

Adding the firmware version is not so easy, but I added the CI build number so you can at least see that bigger number == newer.

[johnscary-ev3]

Thanks much.
I can see the build numbers on the .zip file names now.
That should help us keep better track of what firmware we have.

[johnscary-ev3]

I tried your latest Firmware rev ('primehub', '3.2.0b3', 'v3.2.0b3-46-g37e69699 on 2022-08-24') Build 1984.
In this rev I do not see any hang ups after running with my fast loop for an hour. So, I think it is good now.
Thanks much.

[dlech]

I think I just saw the "lock up" issue where the I2C bus has an error for the first time. This happened even with my new changes. However, I've also left a hub running all night without hitting the issue. So if anyone has any suggestions on how to reliably reproduce the issue, that would be helpful.

[dlech]

Current hypothesis, the error only happens when motors are running?

[dlech]

That hypothesis has been proved false. Got the error after about 30 minutes with no motors running.

Next hypothesis, it only happens on certain hubs? The hub that is currently failing is not the same hub that successfully ran overnight.

[laurensvalk]

It could be worth continuing this in #232

As mentioned there, I noticed at the time that Technic Hub seemed less susceptible to this, if at all. Are you seeing the same --- on which hub is it failing?

[dlech]

FYI, the lockup issue should be resolved now.

[dlech]

@cmyers734 @johnscary-ev3 Can you explain how you came to the conclusion that temperature is a major contributing factor to the error?

I've done some experiments where I turn all of the lights on the hub on or off to vary the temperature of the IMU and graphed them. I'm not seeing any effect of temperature on the angular rotation rate measured by the gyro.

First, I did this when the hub was not moving to see if temperature has an effect on the static bias error.

Then I did it while the hub was turning in place to see if temperature had an effect on the scale factor error (note: spikes are when the hub hit obstacles and I had to move it - it wasn't staying exactly in place).

I don't see any measurable effect of temperature. Am I missing something? Surely temperature would not affect the angle random walk error of the integrated value?

[cmyers734]

Jumping back into this:

1. On temperature, the task was really to get a good setup and evaluate the effect of temperature on the results (like you did). If it made a difference, then we could evaluate whether it needed correction and whether that correction had any linear relationship with what we had done at the baseline temp.

From looking at the datasheet, it looks to be 0.007% per degree C, which is not significant (given the large inaccuracy from every other source). However! After some discussions from folks that know the MEMS space better than I do, I was given "try it and see" / "leave no stone unturned" advice.

2. The FIFO idea was less about throughput and more about just getting samples that were at a known, small, predictable sampling interval. The idea of relying on the ISR to get predictability was of interest, too, but we were unsure of how they had wired things up internally.

Honestly, I feel like my thinking and experimentation so far is largely "flailing around" under the guise of sensor characterization. We know it is not giving us usable results and we suspect that it may be possible to calibrate these out, but it was all about getting a good runtime setup to make the calibrating possible. E.g. a tighter, evenly spaced sampling interval might eliminate an entire class of error that would be too difficult to cal out.

If it's at all helpful to anyone, we bought Arduino Nano 33 IoT to get better, faster prototyping environment. The Arduino environment makes for faster rebuilds. Here's the Nano 33 mounted to a turntable that can do 90/180/360 at a variety of speeds:

My FLL co-coaches and I are likely going to have to rely on encoder odometry for this season as we've simply run out of time outside of work to dedicate. That being said... if anyone DOES get production quality results (good enough for FLL competition) we'll happily incorporate it into the kids' repertoire. I haven't given up on it and hope to get some time post-season.

Also.. I cannot say enough good things about both of you for the wonderful work that I see here. Everything is done well with thoughtfulness and craft. You've built a beautiful body of work and I look forward to being a part of it in any small way in the future.

[johnscary-ev3]

I got the impression that temp was affecting the yaw rate offset since when I first turn on the hub and run my program to read that offset it starts at .98 and then slowly drifts to about .94 over period of ~ 5 minutes. (Now using latest firmware V3.2.0b3)
I assumed it was temperature based on other comments.
However, I now see other behavior that makes me wonder.
If I stop my program and immediately re-run it, instead of the offset starting at .94 as you might expect, it goes back to .98 and stays there with about a +/- 0.01 variation,
So, I am not really sure what that initial drift is due to.

[laurensvalk]

How to get Yaw angle or Heading on Robot Inventor Hub?

It occured to me that there is another practical (but perhaps unexpected) answer to the original question.

You can use the DriveBase.angle() method if you are dealing with a simple drive base.

Naturally, this has its own challenges like slipping wheels, but it can work well for some applications as shown in the video below.

---

Original response to @cmyers734 below.

My FLL co-coaches and I are likely going to have to rely on encoder odometry for this season as we've simply run out of time outside of work to dedicate.

You might like the DriveBase class. We should probably do a better job talking about what it can do by now :)

Here's 99 turns of 90/180/270 degrees and it's still on track at the end, despite not using the gyro.

drivebase_turn2.mp4

We've done our best to make something that students can really use in practice. If you try it, we'd love to get your feedback, both in terms of ease-of-use and performance.

[cmyers734]

Absolutely, Laurens, we've built up around the DriveBase. This video shows even better accuracy than what we're seeing likely due to our use of the "big wheels" and +/- 3 deg slack (under review... perhaps the kids will choose small wheels this year). Plus, the track width needs a little more attention.

I'm able to build the firmware on M1 mac os (thanks, Docker). While my other experiments are not PR quality, perhaps I can contribute my mac os build support if you think that would be helpful.

Happy to keep feedback and PRs coming this season as we start building!

[laurensvalk]

Sounds good!

By the way, it should be possible now to install the latest beta firmware on SPIKE Prime without building it locally, via https://beta.pybricks.com/

[cmyers734]

For that video, did you use .turn() or some other code with .angle() as you were suggesting above. Can I see the code?
As good as what we're seeing is, you're showing something that's even more impressive and I'd like to see what the difference is.
It could be a better-tuned track width or more balanced weight, too.

[laurensvalk]

This was using .turn() indeed. I don't think I have the exact script anymore, but it was randomly picking 0, 90, 180, 270 as a heading target, and then getting the right multiple of +/- 90 for turn() to get there compared to the heading target in the previous move.

Balance can be a factor, but it depends on your priorities. This centered balance can be usesful for turning, but less so for driving straight.

Another thing to notice here is that on average it goes as much to the left as it does to the right, so that incorrectly tuned track/wheel size don't accumulate. But this can be used in practice and FLL as well. For example, if you have to drive in an L-shape to the target, you can do everything in reverse when you return to base. (This would be true even with a gyro).

[laurensvalk]

While we're on the topic, you may also want to try the (currently undocumented) ability to blend multiple moves together.

To do it, choose Stop.NONE as the then argument in all but the last movement.

drive-continuous.mp4

[laurensvalk]

Updated answer: we have a solution for this now! Check out #989