-
Notifications
You must be signed in to change notification settings - Fork 1
/
trigonometry.asm
197 lines (138 loc) · 3.42 KB
/
trigonometry.asm
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
sin: b832mov Sine, Theta ;Small-angle approximation of Sine
ret
cos: b832mul TempA, Theta, Theta ;Small-angle approximation of Cosine
b832fdiv TempA, 1
b832ldi TempB, 1
b832sub Cosine, TempB, TempA
ret
;---
thetascale:
b832store Theta ;store in 8.32
b832mul Theta, Theta, MagicNumber
ret
;--- transfer between 16.8 <--> 8.24 , right align, sign extend ---
transfer168840:
clr zl
transfer168832:
clr zh
transfer168824:
transfer824168:
transfer832168:
transfer840168:
mov yl, yh ;right align
mov yh, xl
mov xl, xh
tst xh ;sign extend
brpl tra1
ser xh
ret
tra1: clr xh
ret
;--- Rotate vector[VectorA ,VectorB] with angle Theta
RotateVector:
rcall sin
rcall cos
b832mul VectorNewA, VectorA, Cosine ;VectorNewA = VectorA * cos(Theta) - VectorB * sin(Theta)
b832mul TempA, VectorB, Sine
b832sub VectorNewA, VectorNewA, TempA
b832mul VectorNewB, VectorA, Sine ;VectorNewB = VectorA * sin(Theta) + VectorB * cos(Theta)
b832mul TempA, VectorB, Cosine
b832add VectorNewB, VectorNewB, TempA
ret
;--- Rotate 3D vector vector[VectorX, VectorY, VectorZ]
Rotate3dVector:
; Get the time since we were last here. X = 6250 for 1/400sec = 2.5msecs
cli
lds xl, tcnt1l ;X = TCNT1 - TrigTimer, TrigTimer = TCNT1
lds xh, tcnt1h
sei
lds zl, TrigTimerL
lds zh, TrigTimerH
sts TrigTimerL, xl
sts TrigTimerH, xh
sub xl, zl
sbc xh, zh
brpl trig1 ;X = ABS(X)
ldz 0
sub zl, xl
sbc zh, xh
movw x, z
trig1:
clr yh ; X=X/16
lsr xh
ror xl
ror yh
lsr xh
ror xl
ror yh
lsr xh
ror xl
ror yh
lsr xh
ror xl
ror yh
clr yl
clr zh ; Now we have the delta time in an 8.32 format (Original Timer Value X/4096)
b832store MagicNumberTime
b832mul MagicNumber, MagicNumberTime, MagicNumberMult ; Calculate the Magic Number based on the delta time
b16load GyroPitchVC ;rotate around X axis (pitch)
rcall transfer168832
rcall thetascale
b832mov VectorA, VectorY
b832mov VectorB, VectorZ
rcall RotateVector
b832mov VectorY, VectorNewA
b832mov VectorZ, VectorNewB
b16load GyroRollVC ;rotate around Y axis (roll)
rcall transfer168832
rcall thetascale
b832mov VectorA, VectorX
b832mov VectorB, VectorZ
rcall RotateVector
b832mov VectorX, VectorNewA
b832mov VectorZ, VectorNewB
b16load GyroYaw ;rotate around Z axis (yaw)
rcall transfer168832
rcall thetascale
b832mov VectorA, VectorX
b832mov VectorB, VectorY
rcall RotateVector
b832mov VectorX, VectorNewA
b832mov VectorY, VectorNewB
ret
;--- Get length of 3D vector vector[VectorX, VectorY, VectorZ] ---
Lenght3dVector:
b832mul TempA, VectorX, VectorX
b832mul TempB, VectorY, VectorY
b832add TempA, TempA, TempB
b832mul TempB, VectorZ, VectorZ
b832add LengthVector, TempA, TempB
ret
;--- extraxt Euler angles roll/pitch from 3D vector vector[VectorX, VectorY, VectorZ] ---
ExtractEulerAngles:
b832mov TempD, VectorX
rcall ext2
b16mov EulerAngleRoll, Angle
b832mov TempD, VectorY
rcall ext2
b16mov EulerAnglePitch, Angle
ret
ext2: ;b824mul TempA, TempD, TempD ;approximation of a quarter circle (lol :-)
b832ldi TempB, 90 ;convert to degrees (0 to 90)
b832mul TempA, TempD, TempB
b832load TempA
rcall transfer832168
b16store Angle
; b824load VectorZ ;mirror on X/Y plane (90 to 180)
; tst xh
; brpl ext3
; b16ldi Temp, 180
; b16sub Angle, Temp, Angle
;ext3:
; b824load TempD ;mirror on the Z axis (0 to -180)
; tst xh
; brpl ext1
; b16ldi Temp, -1
; b16mul Angle, Angle, Temp
;ext1:
ret