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:
- 0:88345d07348b
- Child:
- 1:9110a3da84c9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/FastAtan2.cpp Tue Jun 14 17:02:30 2016 +0000
@@ -0,0 +1,48 @@
+/*
+ * MathFunc.cpp
+ *
+ * Created on: 22 de may. de 2016
+ * Author: Fede Pinna
+ */
+#include "FastAtan2.h"
+
+
+int32_t atan_q15(uint32_t x, uint32_t y){
+
+ 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);
+
+ }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;
+}
+
+int32_t atan2_q15(int32_t x, int32_t y){
+
+ 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);
+
+ }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);
+ }
+
+ return ang;
+}