Sine and cosine functions through MacLaurin expansion
This library implements C-callable sine and cosine functions implemented through MacLaurin expansion for the Freescale DSP56F800E family of digital signal controllers.
Source includes the header file for inclusion in C applications as well as the code itself and an usage example in C.
/* -------------- SinCos.h begins -------------- */
#ifndef __SINCOS_H__
#define __SINCOS_H__
#include "types.h"
/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = sine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle);
/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = cosine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle);
#endif //ifndef __SINCOS_H__
/* -------------- SinCos.h ends -------------- */
/* -------------- SinCos.c begins -------------- */
/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = sine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle){
CLR.W Y1 //Clear Y1
MOVE.W Y0,B //Compute absolute value of angle
ABS B
CMP.W #$4000,B //|angle| > pi/2?
BLT SinAngleOK //No, proceed
ADD.W #$8000,Y0 //Yes, add pi and set flag for
BFSET #$0001,Y1 //final negation of result
SinAngleOK: MPYR Y0,Y0,A //Compute angle squared
MOVE.W A,X0 //X0 = angle squared
MOVE.W #$0002,B //Compute Maclaurin expansion
MOVE.W #$FFE2,A
MACR X0,B1,A //A1 = first partial result
MOVE.W #$0150,B
MACR X0,A1,B //B1 = second partial result
MOVE.W #$F669,A
MACR X0,B1,A //A1 = third partial result
MOVE.W #$28CD,B
MACR X0,A1,B //B1 = fourth partial result
MOVE.W #$AD52,A
MACR X0,B1,A //A1 = fifth partial result
MOVE.W #$3244,B
MACR X0,A1,B //B1 = sixth partial result
MOVE.W #$0003,X0 //Required shift amount
MPYR B1,Y0,A //A = result / 8
CMP #$1000,A //If magnitude is maximum
BNE SinCheckSatL //saturate the result
MOVE.W #$7FFF,A
BRA SinValOK
SinCheckSatL: CMP.W #$F000,A
BNE SinNoSat
MOVE.W #$8001,A
BRA SinValOK
SinNoSat: ASLL.L X0,A //Shift to get correct result
//(Maclaurin coefficients are
//divided by 8 to avoid overflow)
SinValOK: BRCLR #$0001,Y1,Update_Sin //Negate result if |angle| was
NEG A //greater than pi/2
Update_Sin: MOVE.W A,Y0 //Save result
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in ;
; subsequent calculations. ;
; ;
; Input: Y0 = angle (signed fractional; -1 = -pi, +1 = +pi) ;
; ;
; Output: Y0 = cosine (signed fractional) ;
; ;
; Registers modified: A, B, X0, Y ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle){
ADD.W #$4000,Y0 //Add pi/2 to angle
JSR Sin //Call sine function
RTS //Return from subroutine
}
/* -------------- SinCos.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific includes */
#include "SinCos.h"
/* global variables */
Frac16 Angle,cosphi,sinphi;
/* Function calls */
cosphi=Cos(Angle); //Compute sin and cos of angle
sinphi=Sin(Angle); //(for use in Park transforms)
/* -------------- Usage example ends ------------- */
