acos asin atan2 for teensy 3.1

Status
Not open for further replies.
S

spanner

New member
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
 
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
 
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.
 
I get a "ram overflowed" error when compiling if I try to use the atan2 function in libfixmath. is there something i am missing?
 
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
 
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.
 
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_from_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_from_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_from_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_from_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_from_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_from_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_from_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_from_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_from_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_from_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_from_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_from_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_from_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=fix16_from_float(((2*i+1)/12)*pi);
legxh=fix16_mul(fix16_from_float(6),fix16_sin(leggammah));
legyh=fix16_mul(fix16_from_float(6),fix16_cos(leggammah));
legxc=fix16_mul(fix16_from_float(11),fix16_sin(leggammah));
legyc=fix16_mul(fix16_from_float(11),fix16_cos(leggammah));
legx=legxc;
legy=legyc;
legz=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,legxh);
y=fix16_sub(legy,legyh);
x2=fix16_mul(legx,legx);
y2=fix16_mul(legy,legy);

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

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

legalpha= fix16_add(fix16_acos(fix16_div(legz,legL)),fix16_acos(fix16_div(fix16_sub(t2mf2,Lsq),fix16_mul(m2f,legL))));

legbeta= fix16_acos(fix16_div(fix16_sub(Lsq,t2pf2),m2tf));
 
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.
 
Status
Not open for further replies.
Back
Top