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@2:8bff2a7afea1, 2016-06-14 (annotated)

Committer:
Cotzo
Date:
Tue Jun 14 19:50:08 2016 +0000
Revision:
2:8bff2a7afea1
Parent:
1:9110a3da84c9 
Added: Documenting functions

Who changed what in which revision?

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