Federico Luis Pinna Gonzalez / FastAtan2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   FastECompass

FastAtan2.cpp@0:88345d07348b, 2016-06-14 (annotated)

Committer:
Cotzo
Date:
Tue Jun 14 17:02:30 2016 +0000
Revision:
0:88345d07348b
Child:
1:9110a3da84c9
Initial commit library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Cotzo 0:88345d07348b 1 /*
Cotzo 0:88345d07348b 2 * MathFunc.cpp
Cotzo 0:88345d07348b 3 *
Cotzo 0:88345d07348b 4 * Created on: 22 de may. de 2016
Cotzo 0:88345d07348b 5 * Author: Fede Pinna
Cotzo 0:88345d07348b 6 */
Cotzo 0:88345d07348b 7 #include "FastAtan2.h"
Cotzo 0:88345d07348b 8
Cotzo 0:88345d07348b 9
Cotzo 0:88345d07348b 10 int32_t atan_q15(uint32_t x, uint32_t y){
Cotzo 0:88345d07348b 11
Cotzo 0:88345d07348b 12 int ang;
Cotzo 0:88345d07348b 13 int rdiv;
Cotzo 0:88345d07348b 14
Cotzo 0:88345d07348b 15 if(y>x){
Cotzo 0:88345d07348b 16 rdiv=(x<<QFORMAT)/y;
Cotzo 0:88345d07348b 17 ang= PI2_Q15 - FMUL(PI4_Q15,rdiv,QFORMAT)-FMUL(rdiv,FMUL(FSUBI(rdiv,1,QFORMAT),C1+FMUL(C2,rdiv,QFORMAT),QFORMAT),QFORMAT);
Cotzo 0:88345d07348b 18
Cotzo 0:88345d07348b 19 }else{
Cotzo 0:88345d07348b 20 rdiv=(y<<QFORMAT)/x;
Cotzo 0:88345d07348b 21 ang=FMUL(PI4_Q15,rdiv,QFORMAT)-FMUL(rdiv,FMUL(FSUBI(rdiv,1,QFORMAT),C1+FMUL(C2,rdiv,QFORMAT),QFORMAT),QFORMAT);
Cotzo 0:88345d07348b 22 }
Cotzo 0:88345d07348b 23
Cotzo 0:88345d07348b 24 return ang;
Cotzo 0:88345d07348b 25 }
Cotzo 0:88345d07348b 26
Cotzo 0:88345d07348b 27 int32_t atan2_q15(int32_t x, int32_t y){
Cotzo 0:88345d07348b 28
Cotzo 0:88345d07348b 29 int ang;
Cotzo 0:88345d07348b 30
Cotzo 0:88345d07348b 31 if((x>0)&&(y>0)){ //I Quadrant
Cotzo 0:88345d07348b 32 ang=atan_q15(x,y);
Cotzo 0:88345d07348b 33 }else if((x<0)&&(y>0)){ //II Quadrant
Cotzo 0:88345d07348b 34 x=-x;
Cotzo 0:88345d07348b 35 ang=PI_Q15-atan_q15(x,y);
Cotzo 0:88345d07348b 36
Cotzo 0:88345d07348b 37 }else if((x<0)&&(y<0)){ //III Quadrant
Cotzo 0:88345d07348b 38 x=-x;
Cotzo 0:88345d07348b 39 y=-y;
Cotzo 0:88345d07348b 40 ang=-(PI_Q15-atan_q15(x,y));
Cotzo 0:88345d07348b 41
Cotzo 0:88345d07348b 42 }else{ //IV Quadrant
Cotzo 0:88345d07348b 43 y=-y;
Cotzo 0:88345d07348b 44 ang=-atan_q15(x,y);
Cotzo 0:88345d07348b 45 }
Cotzo 0:88345d07348b 46
Cotzo 0:88345d07348b 47 return ang;
Cotzo 0:88345d07348b 48 }