m8ta
You are not authenticated, login. |
|
{646} | ||
We approximated sine and cosine, needed to vector the stepper motor phase currents, in fixed-point arithmetic first in C on linux - where the results could be plotted in matlab - before converting to MSP430 code. Since the trigonometric functions are repeating, we only need a polynomial approximation of sine from 0 to pi/2. The taylor series for sine is sin(x) = x - x^3/3! + x^5/5! - x^7/7! ...; a quick check in matlab showed that the first three terms are enough to get an accurate approximation in the domain of interest. The MSP430 does not have division, however, so we approximated 1/3! = 1/6 as (1/8 + 1/32 + 1/128) and 1/5! = 1/120 as 1/128; division by powers of two is possible with right bit-shift operations. We chose base 11 (5 bits whole, 11 bits fractional) representation to avoid overflow: if 2^11 -> 1, we need to represent (pi/2)^5 -> 9.5 ; ceil(log_2(9.5)) = 4 (plus one bit for safety). The C program below shows this test. #include <stdio.h> char qsin(short i){ //i goes from 0 pi/2 base 11 or... // 0 to 3217 unsigned int cube, fifth, result; cube = (i*i) >> 11; cube = (cube*i) >> 11; //max = 7937 fifth = (cube*i) >> 11; fifth = (fifth*i) >> 11; // max = 19585 //our approximation to sine based on taylor series: //original: sin(x) = x - x^3/3! + x^5/5! //sin(x) = x - x^3*(1/8+1/32+1/128) + x^5*(1/128) result = (unsigned int)i - ((cube >> 3) + (cube >> 5) + (cube >> 7)) + (fifth >> 7); //result is base 11. need it to be base 7. result = result >> 4; if(result > 127) result = 127; return (char)result; } //this is tricky, as it involves shifts, x-inversions, and y-inversions. //but it all makes sense if you plot out the respective functions. char isin(short i){ // i is base 2^11 //but we accept 0 to 2*pi or 12867 if(i >= 0 && i < 3217) return qsin(i); else if(i >= 3217 && i < 6434) return qsin(6434 - i); else if(i >= 6434 && i < 9651) return -1*qsin(i - 6434); else if(i >= 9651 && i < 12867) return -1*qsin(12867 - i); else return 0; } char icos(short i){ // i is base 2^11 //but we accept 0 to 2*pi or 12867 if(i >= 0 && i < 3217) return qsin(3217 - i); else if(i >= 3217 && i < 6434) return -1*qsin(i - 3217); else if(i >= 6434 && i < 9651) return -1*qsin(9651 - i); else if(i >= 9651 && i < 12867) return qsin(i - 9651); else return 0; } int main(void){ short i; for(i=0; i<12867; i++){ printf("%d\t%f\t%d\t%d\n", i, ((float)i)/2048.0, (int)(isin(i)), (int)(icos(i))); } return 0; } We compiled and ran this program on the command line: gcc integer_trig.c ./a.out > test.txt Then we imported the data into matlab and plotted (Actual double floating-point sine and cosine are plotted on the same axis as thin black and magenta lines, respectively) Later, we had to change the naive standard implementation of multiply to assembly to properly implement the fixed-point arithmetic - the MSP430's standard library did not implement the 16x16 multiply followed by shift correctly (it only keeps the bottom 16 bits). Note: this assembly function is for use with Code-Composer Studio, available from the Texas Instruments website. It seems that the IAR compiler uses different assembly syntax. ;******************************************************************************* .cdecls C,LIST,"msp430x54x.h" ; Include device header file ;------------------------------------------------------------------------------- .text ; Progam Start ;------------------------------------------------------------------------------- ;;.sect "mply_11" ;;.asmfunc "mply_11" .global mply_11 ;; this MUST BE PRECEDED BY TABS !!!!! mply_11: PUSH SR ; DINT ; turn off interrupts here. NOP ; required after DINT MOV.W R12, &MPY ; load the first operand. MOV.W R13, &OP2 ; load the second operand & start multiplication. MOV.W &RESLO, R12 ; low to R12 (this is the return value) MOV.W &RESHI, R13 ; high to R13 RRA.W R12 ; 1 RRA.W R12 ; 2 RRA.W R12 ; 3 RRA.W R12 ; 4 RRA.W R12 ; 5 RRA.W R12 ; 6 RRA.W R12 ; 7 RRA.W R12 ; 8 RRA.W R12 ; 9 RRA.W R12 ; 10 RRA.W R12 ; 11 RLA.W R13 ; 1 RLA.W R13 ; 2 RLA.W R13 ; 3 RLA.W R13 ; 4 RLA.W R13 ; 5 ;; r14 can be clobbered across a function call, according to the msp430 ABI MOV.W #0x001f, R14 AND.W R14, R12 ; mask off all but the bottom 5 bits from RESLO ADD.W R13, R12 ; add (logical OR) the results. R12 is the 11-bit fixed point result. POP.W SR ; RETA ; return from subroutine. .end Note the MSP430 does not have an opcode for multiple arithmetic shifts, nor does it have code for logical shifts - hence the need for repeated shifts and bitmasks! |