User avatar
LetHopeItsSnowing
Posts: 357
Joined: Sat May 26, 2012 6:40 am
Location: UK
Contact: Website

Reading orientation data in a loop

Fri May 15, 2015 2:26 pm

Hi,

I seem to be experiencing an odd issue.

If I read orientation data in loop say once every 2 seconds after only a few cycles it goes out of sync, if however I read it once every 0.1 seconds it doesnt go out of sync.

To show what I mean I recorded it and uploaded it as an 'unlisted' video to youtube https://youtu.be/MS1Tfv5CifM

This is my code which reads the data every 0.1 seconds and this works perfectly:

Code: Select all

from astro_pi import AstroPi
from time import sleep

ap = AstroPi()

ap.set_imu_config(True, True, True)

while(True):
    orientation = ap.get_orientation_degrees()
    print("yaw = {}; pitch = {}; roll = {}".format(orientation["yaw"], orientation["pitch"], orientation["roll"]))

    sleep(0.1)
The same program but with a 2 second sleep, stops working after only a few reads and I get very weird results.

Code: Select all

from astro_pi import AstroPi
from time import sleep

ap = AstroPi()

ap.set_imu_config(True, True, True)

while(True):
    orientation = ap.get_orientation_degrees()
    print("yaw = {}; pitch = {}; roll = {}".format(orientation["yaw"], orientation["pitch"], orientation["roll"]))

    sleep(2)
Any ideas?

Mart
"am I getting slower, or is stuff more complicated; either way I now have to write it down - stuffaboutcode.com"

Johnny5C
Posts: 82
Joined: Thu Apr 18, 2013 3:32 pm

Re: Reading orientation data in a loop

Fri May 15, 2015 4:54 pm

I have run your code on my pi, and it look OK on first glance. Watching your video were you only changing the yaw to test it?

User avatar
Davespice
Forum Moderator
Forum Moderator
Posts: 1662
Joined: Fri Oct 14, 2011 8:06 pm
Location: The Netherlands
Contact: Twitter

Re: Reading orientation data in a loop

Fri May 15, 2015 4:57 pm

I've confirmed the issue. It's almost as if there is a fifo queue inside RTIMUlib and if you read it slowly you slowly work through movements that have occurred in the past. I'll have a look into this but if the problem is with RTIMUlib then I'll need to get ShiftPlusOne to help.

I have a workaround for you in the meantime.

Code: Select all

from astro_pi import AstroPi
from time import sleep
try:
    import thread
except ImportError:
    import _thread as thread

ap = AstroPi()

ap.set_imu_config(True, True, True)

orientation = None

def orientation_thread():
    global orientation
    global ap
    while(True):
        orientation = ap.get_orientation_degrees()

thread.start_new_thread(orientation_thread, ())

sleep(1)

while(True):
    print("yaw = {0:.2f}; pitch = {1:.2f}; roll = {2:.2f}".format(orientation["yaw"], orientation["pitch"], orientation["roll"]))
    sleep(2)

Johnny5C
Posts: 82
Joined: Thu Apr 18, 2013 3:32 pm

Re: Reading orientation data in a loop

Fri May 15, 2015 5:51 pm

Yeah, just tinkering with yaw, I can see that is plays catchup after you stop moving.

But, pitch and roll take big jumps to the new position.

User avatar
LetHopeItsSnowing
Posts: 357
Joined: Sat May 26, 2012 6:40 am
Location: UK
Contact: Website

Re: Reading orientation data in a loop

Fri May 15, 2015 6:14 pm

Thanks Dave.

That describes the behaviour I'm seeing perfectly.
"am I getting slower, or is stuff more complicated; either way I now have to write it down - stuffaboutcode.com"

ShiftPlusOne
Raspberry Pi Engineer & Forum Moderator
Raspberry Pi Engineer & Forum Moderator
Posts: 5885
Joined: Fri Jul 29, 2011 5:36 pm
Location: The unfashionable end of the western spiral arm of the Galaxy

Re: Reading orientation data in a loop

Sat May 16, 2015 2:16 pm

I don't know the internals of RTIMULib's data fusion algorithm. However, it's an iterative process of predicting values and adjusting parameters based on what's measured.
Ideally you'd iterate at the set sample rate.

If I had to guess, that's what this is. The filter can cope with lots of data with relatively small deltas, but if you're only giving it a few iterations per second, the tuning parameters don't really know what to do. It's like trying to determine a trend with 2 data points vs 100.

User avatar
LetHopeItsSnowing
Posts: 357
Joined: Sat May 26, 2012 6:40 am
Location: UK
Contact: Website

Re: Reading orientation data in a loop

Sun May 17, 2015 2:11 pm

I have created a sub class of AstroPi to do the job of reading the orientation data continuously for me.

Code: Select all

from astro_pi import AstroPi
from time import sleep

try:
    import thread
except ImportError:
    import _thread as thread

class AstroPiContinuous(AstroPi):
    """
    A class which continuously reads orientation data from AstroPi as without
    it the orientatin data looses sync
    """
    def __init__(self,
            fb_device='/dev/fb1',
            imu_settings_file='RTIMULib',
            text_assets='astro_pi_text',
            sample_rate = 0.1):

        AstroPi.__init__(self, fb_device, imu_settings_file, text_assets)
       
        self.sample_rate = sample_rate
        self.stopped = True
        self.running = False
        
    def start(self):
        """
        starts the thread that continuously reads the astro pi orientation data
        """
        #initialise the IMU by getting the orientation
        self.get_orientation()
        #start the orientation thread
        thread.start_new_thread(self._get_orientation_threaded, ())
        
    def _get_orientation_threaded(self):
        """
        reads the orientation data every sample rate to ensure astro pi is kept in sync
        """
        self.stopped = False
        self.running = True

        #keep reading the orientation data, this keeps AstroPi in sync
        while(not self.stopped):
            self.get_orientation()
            sleep(self.sample_rate)
            
        self.running = False
    
    def stop(self):
        """
        stops the continous read thread
        """
        self.stopped = True
        #wait for the thread to stop
        while(self.running):
            sleep(0.01)
            
    def __enter__(self):
        self.start()
        return self

    def __exit__(self, type, value, traceback):
        self.stop() 
        
#test
if __name__ == "__main__":
    with AstroPiContinuous() as ap:
        while(True):
            print(ap.get_orientation())
            sleep(2)
It might be helpful to others.
"am I getting slower, or is stuff more complicated; either way I now have to write it down - stuffaboutcode.com"

User avatar
richards-tech
Posts: 1
Joined: Thu Jun 18, 2015 2:43 pm

Re: Reading orientation data in a loop

Thu Jun 18, 2015 3:03 pm

That's a good solution. To confirm, it is important that the IMU is polled at the gyro sample rate or greater. The code does a piecewise linear approximation at each time step based on gyro rates so it is important to keep the time steps small. A thread to take of care of that is the way to go.

butchec
Posts: 5
Joined: Sun Jun 21, 2015 9:15 am

Re: Reading orientation data in a loop

Thu Aug 06, 2015 10:52 am

Hi,

I have been using the above work around for recording the orientation every 30seconds and everything has been working well. We successfully ran it for a full week with a log and picture taken every 30 seconds.

My son was one of the competition winners and for the final submission of our code we were asked to swap to a B+ in place of the B2.

All of our code is working well except for the orientation work around. The python program crashes out every time I run the program and include the module with the workaround code.

If I move the code back across to the B2 all is good.

Can anyone help with this? We need to post our final submission before Monday.

Both the Raspberry Pi B+ and the B2 are fresh installs with all of the latest updates. I have no problem running any code or program I have created except when I add the workaround module.

I also tested recording the orientation every 0.2 seconds and all was well, but as soon as i swap back to every 30 seconds and use the workaround it crashes on the B+

Cheers.

Chris

Return to “Astro Pi”