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

Committer:
Cotzo
Date:
2016-06-14
Revision:
0:88345d07348b
Child:
1:9110a3da84c9

File content as of revision 0:88345d07348b:

/*
 * 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;
}