Revision 1603 trunk/code/projects/fp_math/fp_math.c

View differences:

fp_math.c
86 86

  
87 87
	//Move theta into [0, pi/2] w/ appropriate sign.
88 88
	theta = ABS(theta) % FP_TWO_PI; 
89

  
89
	
90
	//Reflecting into [0, pi] doesn't change sign.
90 91
	if(theta > FP_PI) 
91 92
		theta = FP_TWO_PI - theta;
92 93
	
94
	//Reflecting into [0, pi/2] does.
93 95
	if(theta > FP_PI_OVER_TWO) {
94 96
		theta = FP_PI - theta;
95 97
		negative = 1;
96 98
	}
97

  
98
	//Find the nearest table values. FIXME
99
	
100
	//Find n such that theta is between x_{n} and x_{n+1}
99 101
	n = theta / TABLE_STEP;
100 102
	while( n < TABLE_LENGTH - 1 
101
		&& (x_np1 = pgm_read_dword(&linspace[n+1]) < theta)) 
103
		&& pgm_read_dword(&linspace[n+1]) < theta) 
102 104
		n++;
103 105
	
104
	//theta is between x_{n} and x_{n+1}
105

  
106
	//Edge case
106 107
	if(n == TABLE_LENGTH - 1) { 
107 108
		//Perform linear interpolation, since we're close to zero anyway.
108 109
		x_n = pgm_read_dword(&linspace[TABLE_LENGTH - 1]);
......
114 115
		return negative ? -result : result;
115 116
	}
116 117

  
117
	if(n == TABLE_LENGTH) { 
118
		//We didn't find a value! Oh no!
119
		usb_puts("fp_math: Fatal! We couldn't find surrounding table values! \n\r");
120
				  
121
		return 0;
122
	}
123

  
124
	//Address the general case. Quadratic interpolation.
118
	//Perform quadratic interpolation.
119
	
125 120
	//Load in the necessary values.
126 121
	x_n = pgm_read_dword(&linspace[n]);
127 122
	x_np1 = pgm_read_dword(&linspace[n + 1]);

Also available in: Unified diff