Code

Sine and cosine functions through MacLaurin expansion

November 18, 2010 Coded in Mixed C and ASM for the Freescale DSP56F8xx

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