|
0
|
1 ;=============================================================================
|
|
|
2 ;
|
|
|
3 ; File tesmode.asm
|
|
|
4 ;
|
|
|
5 ; Test Mode for Hardware check
|
|
|
6 ;
|
|
|
7 ; Copyright (c) 2011, JD Gascuel, HeinrichsWeikamp, all right reserved.
|
|
|
8 ;=============================================================================
|
|
|
9 ; HISTORY
|
|
|
10 ; 2012-10-31 : [mH] Creation
|
|
|
11
|
|
|
12 #include "ostc3.inc" ; Mandatory header
|
|
|
13 #include "shared_definitions.h" ; Mailbox from/to p2_deco.c
|
|
|
14 #include "start.inc"
|
|
|
15 #include "tft.inc"
|
|
|
16 #include "tft_outputs.inc"
|
|
|
17 #include "isr.inc"
|
|
|
18 #include "adc_lightsensor.inc"
|
|
|
19 #include "strings.inc"
|
|
|
20 #include "sleepmode.inc"
|
|
|
21 #include "wait.inc" ; speed_*
|
|
|
22 #include "mcp.inc" ; RX
|
|
|
23 #include "i2c.inc"
|
|
|
24 #include "surfmode.inc"
|
|
|
25 #include "math.inc"
|
|
|
26 #include "eeprom_rs232.inc"
|
|
67
|
27 #include "ms5541.inc"
|
|
0
|
28
|
|
|
29 extern compass
|
|
|
30 extern compass_filter
|
|
|
31 extern compass_filter_init
|
|
|
32 extern compass_reset_calibration
|
|
|
33 extern compass_add_calibration
|
|
|
34 extern compass_solve_calibration
|
|
|
35
|
|
|
36 testmode CODE
|
|
|
37
|
|
|
38 ;=============================================================================
|
|
|
39 ; Boot tasks for all modes
|
|
|
40 global testloop
|
|
67
|
41 testloop: ; Page1
|
|
0
|
42 call speed_normal
|
|
|
43 bcf no_sensor_int ; Sensor ISR
|
|
|
44
|
|
|
45 clrf CCP1CON ; stop PWM
|
|
|
46 bcf PORTC,2 ; Pull PWM output to GND
|
|
|
47 call TFT_boot ; Initialize TFT (includes clear screen)
|
|
|
48 call enable_ir ; Enable IR-Port
|
|
|
49 WIN_TOP .0
|
|
|
50 WIN_LEFT .0
|
|
|
51 WIN_FONT FT_SMALL
|
|
|
52 WIN_INVERT .0 ; Init new Wordprocessor
|
|
|
53 call TFT_standard_color
|
|
|
54 call TFT_Display_FadeIn
|
|
|
55
|
|
|
56 call I2C_init_compass
|
|
|
57 call I2C_init_accelerometer
|
|
|
58
|
|
|
59 ; Init compass/accel filtering value.
|
|
|
60 call I2C_RX_compass
|
|
|
61 call I2C_RX_accelerometer
|
|
|
62 call compass_filter_init
|
|
|
63
|
|
|
64 clrf timeout_counter2
|
|
|
65 clrf timeout_counter3
|
|
|
66 bcf premenu ; clear premenu flag
|
|
|
67 bcf menubit ; clear menu flag
|
|
|
68
|
|
|
69 bcf switch_left
|
|
|
70 bcf switch_right
|
|
|
71
|
|
|
72 testloop_loop:
|
|
|
73 btfss onesecupdate ; do every second tasks?
|
|
|
74 bra testloop_loop2 ; no, loop
|
|
|
75
|
|
|
76 ; One Second tasks
|
|
|
77 call speed_normal
|
|
|
78 call I2C_RX_compass ; Test Compass
|
|
|
79 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
80 call compass_filter ; Filter Raw compass + accel readings.
|
|
|
81 banksel common
|
|
|
82
|
|
|
83 ; Make a second measure and filter, to show faster reactions.
|
|
|
84 call I2C_RX_compass ; Test Compass
|
|
|
85 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
86 call compass_filter ; Filter Raw compass + accel readings.
|
|
|
87 call compass ; Do compass corrections.
|
|
|
88 banksel common
|
|
|
89
|
|
|
90 call TFT_update_raw_data
|
|
|
91 movlw .240
|
|
|
92 call timeout_testmode ; check timeout
|
|
|
93
|
|
|
94 bcf onesecupdate ; every second tasks done
|
|
|
95
|
|
|
96 testloop_loop2:
|
|
|
97 ; Tasks approx. every 50ms for all modes
|
|
|
98 bcf LEDg
|
|
|
99 btfsc vusb_in
|
|
|
100 bsf LEDg
|
|
|
101
|
|
|
102 ; Mode tasks
|
|
|
103 extern surfloop
|
|
|
104 btfsc switch_left
|
|
67
|
105 bra testloop2 ; Page 2
|
|
0
|
106
|
|
|
107 btfsc switch_right
|
|
|
108 goto compass_calibration_loop
|
|
|
109
|
|
|
110 btfsc sleepmode ; Sleepmode active?
|
|
67
|
111 bra testloop2 ; Page 2
|
|
|
112
|
|
|
113 bra testloop_loop ; loop testmode
|
|
|
114
|
|
|
115 testloop2: ; Page2
|
|
|
116 call speed_normal
|
|
|
117 bsf no_sensor_int ; disable sensor interrupt
|
|
|
118 call get_calibration_data ; Get calibration data from pressure sensor
|
|
|
119 banksel common ; get_calibration_data uses isr_backup
|
|
|
120 bcf no_sensor_int ; normal sensor interrupt mode
|
|
|
121
|
|
|
122 clrf CCP1CON ; stop PWM
|
|
|
123 bcf PORTC,2 ; Pull PWM output to GND
|
|
|
124 call TFT_boot ; Initialize TFT (includes clear screen)
|
|
|
125 WIN_TOP .0
|
|
|
126 WIN_LEFT .0
|
|
|
127 WIN_FONT FT_SMALL
|
|
|
128 WIN_INVERT .0 ; Init new Wordprocessor
|
|
|
129 call TFT_standard_color
|
|
|
130 call TFT_Display_FadeIn
|
|
|
131
|
|
|
132 clrf timeout_counter2
|
|
|
133 clrf timeout_counter3
|
|
|
134 bcf premenu ; clear premenu flag
|
|
|
135 bcf menubit ; clear menu flag
|
|
|
136
|
|
|
137 bcf switch_left
|
|
|
138 bcf switch_right
|
|
|
139
|
|
|
140 testloop2_loop:
|
|
|
141 btfss onesecupdate ; do every second tasks?
|
|
|
142 bra testloop2_loop2 ; no, loop
|
|
|
143
|
|
|
144 ; One Second tasks
|
|
|
145 call TFT_update_raw_data2
|
|
|
146 movlw .240
|
|
|
147 call timeout_testmode ; check timeout
|
|
|
148
|
|
|
149 bcf onesecupdate ; every second tasks done
|
|
|
150
|
|
|
151 testloop2_loop2:
|
|
|
152 ; Tasks approx. every 50ms for all modes
|
|
|
153 bcf LEDg
|
|
|
154 btfsc vusb_in
|
|
|
155 bsf LEDg
|
|
|
156
|
|
|
157 ; Mode tasks
|
|
|
158 btfsc switch_left
|
|
|
159 goto surfloop ; Exit
|
|
|
160
|
|
|
161 btfsc sleepmode ; Sleepmode active?
|
|
0
|
162 goto sleeploop ; Yes, switch into sleepmode!
|
|
|
163
|
|
67
|
164 bra testloop2_loop ; loop testmode
|
|
0
|
165
|
|
|
166
|
|
|
167 global compass_calibration_loop
|
|
|
168 compass_calibration_loop: ; Compass calibration
|
|
|
169 bsf no_sensor_int ; No Sensor ISR
|
|
|
170 call I2C_init_accelerometer
|
|
|
171 call I2C_init_compass_fast
|
|
|
172 call TFT_ClearScreen
|
|
|
173
|
|
|
174 ; Mask
|
|
|
175 WIN_COLOR color_greenish
|
|
|
176 WIN_SMALL .16,.0
|
|
|
177 STRCPY_TEXT_PRINT tCompassMenu
|
|
|
178 call TFT_standard_color
|
|
|
179 WIN_SMALL .0,.215
|
|
|
180 STRCPY_TEXT_PRINT tExit
|
|
|
181
|
|
|
182 clrf timeout_counter2
|
|
|
183 clrf timeout_counter3
|
|
|
184
|
|
|
185 call speed_fastest
|
|
|
186 call I2C_RX_compass ; read compass
|
|
|
187 call I2C_RX_accelerometer ; read Accelerometer
|
|
|
188 call compass_filter_init ; set DX_f values
|
|
|
189 call compass_reset_calibration ; Reset CX_f values
|
|
|
190 banksel common
|
|
|
191
|
|
|
192 compass_calibration_loop2:
|
|
|
193 call I2C_RX_compass ; read compass
|
|
|
194 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
195 call compass_filter ; Filter compass raw data
|
|
|
196 banksel common
|
|
|
197
|
|
|
198 ; Twice
|
|
|
199 call I2C_RX_compass ; read compass
|
|
|
200 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
201 call compass_filter ; Filter compass raw data
|
|
|
202 banksel common
|
|
|
203 ;
|
|
|
204 ; ; Three
|
|
|
205 ; call I2C_RX_compass ; read compass
|
|
|
206 ; call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
207 ; call compass_filter ; Filter compass raw data
|
|
|
208 ; banksel common
|
|
|
209 ;
|
|
|
210 ; ; Four times to get cleaner values
|
|
|
211 ; call I2C_RX_compass ; read compass
|
|
|
212 ; call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
213 ; call compass_filter ; Filter compass raw data
|
|
|
214
|
|
|
215 ; And register only one value out of four:
|
|
|
216 call compass_add_calibration ; check and store new max/min values
|
|
|
217 banksel common
|
|
|
218
|
|
|
219 call TFT_compass_fast ; show values
|
|
|
220 ; btg LEDg ; Show read-out interval
|
|
|
221
|
|
|
222 btfsc sleepmode ; Sleepmode active?
|
|
|
223 bra compass_calibration_exit ; Yes, exit
|
|
|
224
|
|
|
225 btfsc switch_left ; Button pressed?
|
|
|
226 bra compass_calibration_exit ; Yes, exit
|
|
|
227
|
|
|
228 btfss onesecupdate ; do every second tasks?
|
|
|
229 bra compass_calibration_loop2 ; no, loop here
|
|
|
230
|
|
|
231 movlw .240
|
|
|
232 call timeout_testmode ; check timeout
|
|
|
233 bcf onesecupdate ; clear flag
|
|
|
234
|
|
|
235 bra compass_calibration_loop2 ; loop here
|
|
|
236
|
|
|
237 compass_calibration_exit:
|
|
|
238 call compass_solve_calibration
|
|
|
239 banksel common
|
|
|
240 extern option_save_all
|
|
|
241 call option_save_all ; save all settings into EEPROM
|
|
|
242 bcf sleepmode ; Clear the flag before exiting to surfacemode
|
|
|
243 movlw .6
|
|
|
244 movwf menupos3 ; Set to compass view...
|
|
|
245 goto surfloop ; ...and exit
|
|
|
246
|
|
|
247 END |
