Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 9 of 9

Thread: acos asin atan2 for teensy 3.1

  1. #1
    Junior Member
    Join Date
    Mar 2014
    Posts
    4

    acos asin atan2 for teensy 3.1

    Has anybody implemented these functions using the arm math library for improved speed? I'm about to start a hexapod project that will make heavy use of these functions and I'm wondering if its worth the effort over the functions in math.h

  2. #2
    Senior Member
    Join Date
    Sep 2013
    Location
    Hamburg, Germany
    Posts
    894
    Consider using libfixmath. It's not floating, but fixed point, quite fast compared to floating point operations and easy to use. I use it in real time display of astronomical objects, so I think the cpu load for converting/calculating coordinates of our projects are in the same ballpark. It provides a C and a C++ interface, both with all the standard trigonometric functions (sin,cos, tan, asin, acos, atan, atan2).

    IIRC, it also worked with Eigen on a Teensy 3.0. "IIRC" because I wanted to output Eigen matrices over Serial, but couldn't get text output to work with libfixmath. Apart from that, it happily calculates a hard iron offset vector for a set of magnetometers from a bunch of readings, using least squares. I think all of these features might be relevant to you.

    Regards

    Christoph

  3. #3
    Senior Member
    Join Date
    Jun 2013
    Location
    So. Calif
    Posts
    2,828
    fixed point math is almost a lost art.
    On 32 bit CPUs, it is an ideal choice if you get the fixed point concept fixed in your brain... it's up to you, not the CPU, to handle the overflows and so on, and remember were the "virtual" binary decimal point is.
    Huge speed benefit.

  4. #4
    Junior Member
    Join Date
    Mar 2014
    Posts
    4
    thanks. libfixmath is exactly what i am looking for. will give it a go

  5. #5
    Junior Member
    Join Date
    Mar 2014
    Posts
    4
    I get a "ram overflowed" error when compiling if I try to use the atan2 function in libfixmath. is there something i am missing?

  6. #6
    Senior Member
    Join Date
    Nov 2012
    Posts
    1,545
    That library has a very large lookup table which is probably causing the error. I think it has a constant that you can #define which removes the table and then, presumably, it calculates the values on-the-fly.

    Pete

  7. #7
    Senior Member+ MichaelMeissner's Avatar
    Join Date
    Nov 2012
    Location
    Ayer Massachussetts
    Posts
    3,978
    If the tables are constant, you might try decorating each of the declarations with the const keyword. The program memory space is much larger than the SRAM case (256K of program memory vs. 64K of flash memory on the Teensy 3.1), and the ARM compiler should put constant data into the program memory. On AVR systems like the Teensy 2.0 and most Arduinos, it can't do this automatically because AVR has a so-called Harvard architecture, and accessing program memory is different from data memory. On those systems, you need to use PROGMEM, and copy the data from the program memory to SRAM before you can use it. The Arm has unified addressing, so it can address read-only program memory using the same instructions as data memory.

  8. #8
    Junior Member
    Join Date
    Mar 2014
    Posts
    4
    Im not having any luck even removing the LUT functionality. the acos function is misbehaving now. Ive included my initial code below

    #define FIXMATH_NO_CACHE
    #define FIXMATH_FAST_SIN
    #define FIXMATH_NO_ROUNDING
    #include <fixmath.h>
    #include <math.h>




    float pi=3.141592654;
    fix16_t coxa=fix16_from_float(4.4);
    fix16_t tibia=fix16_from_float(10.0);
    fix16_t femur=fix16_from_float(6.0);
    fix16_t tibia2=fix16_mul(tibia,tibia);
    fix16_t femur2=fix16_mul(tibia,tibia);
    fix16_t t2mf2=fix16_sub(tibia2,femur2);
    fix16_t t2pf2=fix16_add(tibia2,femur2);
    fix16_t m2f=fix16_mul(fix16_from_float(-2.0),femur);
    fix16_t m2tf=fix16_mul(m2f,tibia);

    fix16_t x;
    fix16_t y;
    fix16_t x2;
    fix16_t y2;

    fix16_t legxh[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};
    fix16_t legyh[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};
    fix16_t leggammah[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};
    fix16_t legxc[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};
    fix16_t legyc[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};

    fix16_t legx[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};
    fix16_t legy[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};
    fix16_t legz[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};

    fix16_t legL1[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};
    fix16_t legL[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};

    fix16_t leggamma[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};
    fix16_t legalpha[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};
    fix16_t legbeta[] = {fix16_from_float(6),fix16_from_float(6),fix16_fro m_float(6),fix16_from_float(6),fix16_from_float(6) ,fix16_from_float(6)};

    void setup() {
    // put your setup code here, to run once:
    for (int i=0; i <=5; i++){
    leggammah[i]=fix16_from_float(((2*i+1)/12)*pi);
    legxh[i]=fix16_mul(fix16_from_float(6),fix16_sin(leggammah[i]));
    legyh[i]=fix16_mul(fix16_from_float(6),fix16_cos(leggammah[i]));
    legxc[i]=fix16_mul(fix16_from_float(11),fix16_sin(leggamma h[i]));
    legyc[i]=fix16_mul(fix16_from_float(11),fix16_cos(leggamma h[i]));
    legx[i]=legxc[i];
    legy[i]=legyc[i];
    legz[i]=fix16_from_float(4.0);
    }
    }

    void loop() {
    // put your main code here, to run repeatedly:
    for (int i=0; i <=5; i++){
    x=fix16_sub(legx[i],legxh[i]);
    y=fix16_sub(legy[i],legyh[i]);
    x2=fix16_mul(legx[i],legx[i]);
    y2=fix16_mul(legy[i],legy[i]);

    legL1[i]=fix16_sqrt(fix16_add(x2,y2));
    leggamma[i]=fix16_sub(fix16_atan2(x,y),leggammah[i]);

    fix16_t L2= fix16_sub(legL1[i],coxa);
    fix16_t Lsq= fix16_add(fix16_mul(legz[i],legz[i]),fix16_mul(L2,L2));
    legL[i]=fix16_sqrt(Lsq);

    legalpha[i]= fix16_add(fix16_acos(fix16_div(legz[i],legL[i])),fix16_acos(fix16_div(fix16_sub(t2mf2,Lsq),fix16 _mul(m2f,legL[i]))));

    legbeta[i]= fix16_acos(fix16_div(fix16_sub(Lsq,t2pf2),m2tf));

  9. #9
    Senior Member+ KurtE's Avatar
    Join Date
    Jan 2014
    Posts
    8,507
    Not sure if you want to go this route, but I have the Phoenix (Hexapod/Quad) code running on Arduinos/Teensy now/Linux I have some/most of the versions using fixed point math. This came originally from the code base for the Basic Atom Pro to run the Lynxmotion hexapods. The original code was done by a member there Jeroen (Xan). The code is up in several of my archives on Github. The one that has some of the stuff I have done for Teensy with Robotis AX servos is in the archive: https://github.com/KurtE/Phantom_Phoenix

    The file phoenix_code.h - has the fixed point math functions, including tables. Note: on some platforms (those Linux boxes with floating point hardware support), I am experimenting going to floating point. Also not sure if I updated it here, but the standard c function for sqrt appears to be faster than the fixed point one that is or was in the code base.

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •