[FIXED] Porting to Teensy 3.0 (32 bit ARM Cortex-M4)

21 posts / 0 new
Last post
PaulStoffregen
[FIXED] Porting to Teensy 3.0 (32 bit ARM Cortex-M4)

I've started looking at FreeIMU on Teensy 3.0. This work will probably pave the way for compatibility with most other 32 bit platforms.

How should I submit patches? Do you want each individual change (perhaps with explanation), or just a big patch file, or some other format?

For example, one of the first things I discovered is a private memory variable "_C" in MS561101BA.h conflicts with the _C constant defined in newlib's ctype.h header. Adding these 3 lines fixes that specific issue:

#ifdef _C
#undef _C
#endif

It's likely there will be a lot of these very minor issues.... I'm just getting started and will look at this more tomorrow. If you can give me any guidance on how you'd like patches submitted, I'll try to use that format.

admin1
admin1's picture

Hi Paul, once again thanks for your help on this!

Don't bother sending small patches, it shouldn't be major code changes and I should be able to understand them, when you think the code is not obvious, please comment it .. I hope you'll follow this thread in case I'll need clarifications. Please start working from the version on the repository. More info here.

Regarding the _C issue, it's probably better to rename the private variable rather than undefining stuff from other libs.. right?

PaulStoffregen

I've been working with the code today. Here's the stuff I've had to change so far.

Probably the most interesting issue was in invSqrt(). It doesn't work on ARM. Here's a rewrite that should be much more portable.

float invSqrt(float number) {
  union {
    float f;
    int32_t i;
  } y;

  y.f = number;
  y.i = 0x5f375a86 - (y.i >> 1);
  y.f = y.f * ( 1.5f - ( number * 0.5f * y.f * y.f ) );
  return y.f;
}

Another issue was where FreeIMU::getRawValues() calls accgyro.getMotion6(). On AVR, the int * array is the same as int16_t * that getMotion6() expects, but on 32 bit ARM int * points to twice as much memory as 32 bit integers. This may not be the most elegant solution....

    #ifdef __AVR__
      accgyro.getMotion6(&raw_values[0], &raw_values[1], &raw_values[2], &raw_values[3], &raw_values[4], &raw_values[5]);
    #else
      int16_t ax, ay, az, gx, gy, gz;
      accgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
      raw_values[0] = ax;
      raw_values[1] = ay;
      raw_values[2] = az;
      raw_values[3] = gx;
      raw_values[4] = gy;
      raw_values[5] = gz;
    #endif

There were a couple small AVR assumptions in the init() function. I just used #ifdef checks for __AVR__ to remove them when the process isn't AVR.

  // disable internal pullups of the ATMEGA which Wire enable by default
  #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
    // deactivate internal pull-ups for twi
    // as per note from atmega8 manual pg167
    cbi(PORTC, 4);
    cbi(PORTC, 5);
  #elif defined(__AVR__)
    // deactivate internal pull-ups for twi
    // as per note from atmega128 manual pg204
    cbi(PORTD, 0);
    cbi(PORTD, 1);
  #endif

  #if defined(__AVR__)
    if(fastmode) { // switch to 400KHz I2C - eheheh
      TWBR = ((F_CPU / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c
    }
  #endif
admin1
admin1's picture

Thanks Paul for your help.

Regarding the invSqrt .. I think we have to be cautious on messing that one since it's kind of voodoo programming.. have you tried your implementation and compared with the original one in terms of speed and results?

PaulStoffregen

I tested invSqrt() by adding Serial.print and looking at the numbers in both the original (when running on AVR) and the new code. I printed the input number, and the 1.0 divided by the output number. Both produce results are close very close, but not exact the ideal answer (as told by the square root button on my calculator).

So far, I've only looked at a few actual numbers from incoming data, not a rigorous test with a large set of identical data. I could do more testing, if you feel it's necessary. Please let me know what you need from me, to move this process forward?

One thing is certain... the existing code does not work at all on ARM. Maybe a cautious initial approach might be to add the new code, with a #ifdef __AVR__ (current code) #else (new ARM version). That could at least let FreeIMU work on Teensy 3.0. It's probably also make it compatible with Arduino Due.

I did not look at the speed. I suspect the new code might be slightly faster, since it avoids pointers and the compiler probably keeps the 32 bit data in a register rather than having to store it out to memory, only to read it back. Other than the method of converting the binary data from float to integer and back to float, they implement exactly the same math. Both end with a single iteration of Newton-Raphson approximation, which requires a floating point operations. With software-implemented floating point, the time spent in the float library functions is probably far more that either way of moving the 32 bits.

admin1
admin1's picture

Code committed in r42. Keep on the good work. Thanks.

PaulStoffregen

Great. :-)

Any chance you could add these 3 lines in MS561101BA.h? Anywhere after #include Arduino.h and Wire.h will work.

#ifdef _C
#undef _C
#endif

I'll retest the trunk on both Teensy 2.0 and 3.0 in a couple days... but without the _C workaround, FreeIMU definitely won't compile for Teensy 3.0 (or Arduino Due, or any other ARM board using newlib).

admin1
admin1's picture

Right, I forgot about that one. I don't think undefining defs from other libs is the best approach here.. so I renamed _C to _Cal which also make it more straightforward to understand. See r44.

Thanks again.

PaulStoffregen

I tried R45 on both Teensy 3.0 and 2.0. Compiles without error and seems to work well on both.

I tested with the yaw_pitch_roll example, tilting my breadboard at different angles that seemed to correspond pretty well to the numbers in the serial monitor.

Renaming to _Cal is a better and cleaner solution.

admin1
admin1's picture

Awesome! Looking forward testing it myself on a 32bit micro ;-)

admin1
admin1's picture

A question: how did you managed the EEPROM stuff? Is EEPROM.h available on Teensy 3.0? I seem to recall that it is not available on the Arduino DUE...

One of the things still missing but already on the TODO, would be making the library usable on different I2C busses.. ideally people should be able to connect multiple IMUs on different I2C busses.. I'll be working on this as soon as I get my hands on a 32bit board! ;-)

admin1
admin1's picture

Also, it would be cool to know how fast is a Teensy 3.0 compared to a regular Arduino UNO/Leonardo/Teensy 2.0..
There is a FreeIMU_speedtest program in the library.. it would be cool to know your numbers.

PaulStoffregen

I ran the speed test. Teensy 3.0's faster CPU with slow 100 kbit/sec I2C is no match for Teensy 2.0's fast 400 kbit/sec I2C.

Would you be willing to add a tiny bit of #ifdef __MK20DX128__ code to support Teensy 3.0's equivenant to the AVR TWBR register?

Here's the results:

Teensy 2.0 at 400 kbit/sec I2C


Testing raw reading speed (average on 1024 samples):
--> result: 1248 microseconds .... 1 milliseconds
Testing calibrated reading speed (average on 1024 samples):
--> result: 1518 microseconds .... 1 milliseconds
Testing sensor fusion speed (average on 1024 samples):
--> result: 3325 microseconds .... 3 milliseconds

Teensy 2.0 at 100 kbit/sec I2C

Testing raw reading speed (average on 1024 samples):
--> result: 3294 microseconds .... 3 milliseconds
Testing calibrated reading speed (average on 1024 samples):
--> result: 3493 microseconds .... 3 milliseconds
Testing sensor fusion speed (average on 1024 samples):
--> result: 5290 microseconds .... 5 milliseconds

Teensy 3.0 at 100 kbit/sec I2C

Testing raw reading speed (average on 1024 samples):
--> result: 2120 microseconds .... 2 milliseconds
Testing calibrated reading speed (average on 1024 samples):
--> result: 2074 microseconds .... 2 milliseconds
Testing sensor fusion speed (average on 1024 samples):
--> result: 2476 microseconds .... 2 milliseconds
PaulStoffregen

Here's Teensy 3.0 at 400 kbit/sec I2C:

Testing raw reading speed (average on 1024 samples):
--> result: 588 microseconds .... 0 milliseconds
Testing calibrated reading speed (average on 1024 samples):
--> result: 603 microseconds .... 0 milliseconds
Testing sensor fusion speed (average on 1024 samples):
--> result: 1022 microseconds .... 1 milliseconds

Here's the code I added to enable 400 kbit/sec speed:

  #if defined(__AVR__) // only valid on AVR, not on 32bit platforms (eg: Arduino 2, Teensy 3.0)
    if(fastmode) { // switch to 400KHz I2C - eheheh
      TWBR = ((F_CPU / 400000L) - 16) / 2; // see twi_init in Wire/utility/twi.c
    }
  #elif defined(__arm__)
    if(fastmode) {
      #if defined(CORE_TEENSY) && F_BUS == 48000000
        I2C0_F = 0x85;  // Teensy 3.0 at 48 or 96 MHz
        I2C0_FLT = 2;
      #elif defined(CORE_TEENSY) && F_BUS == 24000000
        I2C0_F = 0x45;  // Teensy 3.0 at 24 MHz
        I2C0_FLT = 1;
      #endif
    }
  #endif
PaulStoffregen

I'm seeing a strange lock up problem occasionally while running at the higher speed. It's very mysterious, where uploading and restarting the Teensy does not recover, but power cycling the entire board does.

Is it possible the MPU6050 can get into a strange state where FreeIMU can not recover?

I'll investigate further tomorrow....

admin1
admin1's picture

Wow, that is really fast! 1KHz of orientation sensing is really fast! I just been able to put my hands on a Arduino Due, I'll let you know how fast that goes ;-)

I saw something similar to that when using the ITG3200 gyroscope on older versions of the FreeIMU library, probably related to noisy I2C communication.. I was seeing this a lot when I was doing I2C overclocking tests..

I haven't seen this anymore with the MPU6050, but yeah it doesn't sound new..

admin1
admin1's picture

I added your I2C switching speed.. however, I think that Wire should be responsible of doing this, not the FreeIMU library..
There was some work for a setSpeed function for Wire but it seems that it didn't go anywhere. (new issue tracker, old one)

PaulStoffregen

I agree, the Wire library really should provide something like Wire.setSpeed(). It's easy to implement, but difficult to get the Arduino Team to accept. When they do (and enough Arduino users have upgraded), this ugly TWBR & I2C0_F stuff will not be necessary.

I've been testing more on Teensy 3.0, and I believe I've discovered the problem I was seeing at 400 kbit/sec. The good news is FreeIMU seems to be very solid. However, the 0x85 constant I gave you is not so good. It really ought to be 0x1A.

In other words, please change this:

    I2C0_F = 0x85;

to this:

    I2C0_F = 0x1A;

Would you like a Teensy 3.0 board for testing FreeIMU? Please email me directly (paul at pjrc dot com) with your shipping address, and I'll get one in the mail to you!

admin1
admin1's picture

Thanks Paul! Wrong I2C bus speed is a common error when working on this kind of stuff! Fixed in r47.

A Teensy 3.0 would be great! And.. never say no to free hardware!
This is my address:
Fabio Varesano
c/o Dipartimento di Informatica, Universita' degli Studi di Torino
via Pessinetto 12
10149 Torino
Italy

PaulStoffregen

A Teensy 3.0 is going in the mail to you today.

admin1
admin1's picture

Thanks Paul, I appreciate it.

Log in or register to post comments