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.
Diff: FastAtan2.cpp
- Revision:
- 2:8bff2a7afea1
- Parent:
- 1:9110a3da84c9
--- a/FastAtan2.cpp Tue Jun 14 18:00:06 2016 +0000
+++ b/FastAtan2.cpp Tue Jun 14 19:50:08 2016 +0000
@@ -1,48 +1,45 @@
-/*
- * FastAtan2.cpp
- *
- * Created on: 22 de may. de 2016
- * Author: Fede Pinna
- */
+
#include "FastAtan2.h"
-int32_t atan_q15(uint32_t x, uint32_t y){
+int32_t atan_q15(uint32_t x, uint32_t y)
+{
- int ang;
- int rdiv;
+ int ang;
+ int rdiv;
- if(y>x){
- rdiv=(x<<QFORMAT)/y;
- ang= PI2_Q15 - FMUL(PI4_Q15,rdiv,QFORMAT)-FMUL(rdiv,FMUL(FSUBI(rdiv,1,QFORMAT),C1+FMUL(C2,rdiv,QFORMAT),QFORMAT),QFORMAT);
+ if(y>x) {
+ rdiv=(x<<QFORMAT)/y;
+ ang= PI2_Q15 - FMUL(PI4_Q15,rdiv,QFORMAT)-FMUL(rdiv,FMUL(FSUBI(rdiv,1,QFORMAT),C1+FMUL(C2,rdiv,QFORMAT),QFORMAT),QFORMAT);
- }else{
- rdiv=(y<<QFORMAT)/x;
- ang=FMUL(PI4_Q15,rdiv,QFORMAT)-FMUL(rdiv,FMUL(FSUBI(rdiv,1,QFORMAT),C1+FMUL(C2,rdiv,QFORMAT),QFORMAT),QFORMAT);
- }
+ } else {
+ rdiv=(y<<QFORMAT)/x;
+ ang=FMUL(PI4_Q15,rdiv,QFORMAT)-FMUL(rdiv,FMUL(FSUBI(rdiv,1,QFORMAT),C1+FMUL(C2,rdiv,QFORMAT),QFORMAT),QFORMAT);
+ }
- return ang;
+ return ang;
}
-int32_t atan2_q15(int32_t x, int32_t y){
+int32_t atan2_q15(int32_t x, int32_t y)
+{
- int ang;
+ int ang;
- if((x>0)&&(y>0)){ //I Quadrant
- ang=atan_q15(x,y);
- }else if((x<0)&&(y>0)){ //II Quadrant
- x=-x;
- ang=PI_Q15-atan_q15(x,y);
+ if((x>0)&&(y>0)) { //I Quadrant
+ ang=atan_q15(x,y);
+ } else if((x<0)&&(y>0)) { //II Quadrant
+ x=-x;
+ ang=PI_Q15-atan_q15(x,y);
- }else if((x<0)&&(y<0)){ //III Quadrant
- x=-x;
- y=-y;
- ang=-(PI_Q15-atan_q15(x,y));
+ } else if((x<0)&&(y<0)) { //III Quadrant
+ x=-x;
+ y=-y;
+ ang=-(PI_Q15-atan_q15(x,y));
- }else{ //IV Quadrant
- y=-y;
- ang=-atan_q15(x,y);
- }
+ } else { //IV Quadrant
+ y=-y;
+ ang=-atan_q15(x,y);
+ }
- return ang;
+ return ang;
}