|
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"
|
|
|
27
|
|
|
28 extern compass
|
|
|
29 extern compass_filter
|
|
|
30 extern compass_filter_init
|
|
|
31 extern compass_reset_calibration
|
|
|
32 extern compass_add_calibration
|
|
|
33 extern compass_solve_calibration
|
|
|
34
|
|
|
35 testmode CODE
|
|
|
36
|
|
|
37 ;=============================================================================
|
|
|
38 ; Boot tasks for all modes
|
|
|
39 global testloop
|
|
|
40 testloop:
|
|
|
41 call speed_normal
|
|
|
42 bcf no_sensor_int ; Sensor ISR
|
|
|
43
|
|
|
44 clrf CCP1CON ; stop PWM
|
|
|
45 bcf PORTC,2 ; Pull PWM output to GND
|
|
|
46 call TFT_boot ; Initialize TFT (includes clear screen)
|
|
|
47 call enable_ir ; Enable IR-Port
|
|
|
48 WIN_TOP .0
|
|
|
49 WIN_LEFT .0
|
|
|
50 WIN_FONT FT_SMALL
|
|
|
51 WIN_INVERT .0 ; Init new Wordprocessor
|
|
|
52 call TFT_standard_color
|
|
|
53 call TFT_Display_FadeIn
|
|
|
54
|
|
|
55 call I2C_init_compass
|
|
|
56 call I2C_init_accelerometer
|
|
|
57
|
|
|
58 ; Init compass/accel filtering value.
|
|
|
59 call I2C_RX_compass
|
|
|
60 call I2C_RX_accelerometer
|
|
|
61 call compass_filter_init
|
|
|
62
|
|
|
63 clrf timeout_counter2
|
|
|
64 clrf timeout_counter3
|
|
|
65 bcf premenu ; clear premenu flag
|
|
|
66 bcf menubit ; clear menu flag
|
|
|
67
|
|
|
68 bcf switch_left
|
|
|
69 bcf switch_right
|
|
|
70
|
|
|
71 testloop_loop:
|
|
|
72 btfss onesecupdate ; do every second tasks?
|
|
|
73 bra testloop_loop2 ; no, loop
|
|
|
74
|
|
|
75 ; One Second tasks
|
|
|
76 call speed_normal
|
|
|
77 call I2C_RX_compass ; Test Compass
|
|
|
78 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
79 call compass_filter ; Filter Raw compass + accel readings.
|
|
|
80 banksel common
|
|
|
81
|
|
|
82 ; Make a second measure and filter, to show faster reactions.
|
|
|
83 call I2C_RX_compass ; Test Compass
|
|
|
84 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
85 call compass_filter ; Filter Raw compass + accel readings.
|
|
|
86 call compass ; Do compass corrections.
|
|
|
87 banksel common
|
|
|
88
|
|
|
89 call TFT_update_raw_data
|
|
|
90 movlw .240
|
|
|
91 call timeout_testmode ; check timeout
|
|
|
92
|
|
|
93 bcf onesecupdate ; every second tasks done
|
|
|
94
|
|
|
95 testloop_loop2:
|
|
|
96 ; Tasks approx. every 50ms for all modes
|
|
|
97 bcf LEDg
|
|
|
98 btfsc vusb_in
|
|
|
99 bsf LEDg
|
|
|
100
|
|
|
101 ; Mode tasks
|
|
|
102 extern surfloop
|
|
|
103 btfsc switch_left
|
|
|
104 goto surfloop ; Exit
|
|
|
105
|
|
|
106 btfsc switch_right
|
|
|
107 goto compass_calibration_loop
|
|
|
108
|
|
|
109 btfsc sleepmode ; Sleepmode active?
|
|
|
110 goto sleeploop ; Yes, switch into sleepmode!
|
|
|
111
|
|
|
112 bra testloop_loop ; loop testmode
|
|
|
113
|
|
|
114
|
|
|
115 global compass_calibration_loop
|
|
|
116 compass_calibration_loop: ; Compass calibration
|
|
|
117 bsf no_sensor_int ; No Sensor ISR
|
|
|
118 call I2C_init_accelerometer
|
|
|
119 call I2C_init_compass_fast
|
|
|
120 call TFT_ClearScreen
|
|
|
121
|
|
|
122 ; Mask
|
|
|
123 WIN_COLOR color_greenish
|
|
|
124 WIN_SMALL .16,.0
|
|
|
125 STRCPY_TEXT_PRINT tCompassMenu
|
|
|
126 call TFT_standard_color
|
|
|
127 WIN_SMALL .0,.215
|
|
|
128 STRCPY_TEXT_PRINT tExit
|
|
|
129
|
|
|
130 clrf timeout_counter2
|
|
|
131 clrf timeout_counter3
|
|
|
132
|
|
|
133 call speed_fastest
|
|
|
134 call I2C_RX_compass ; read compass
|
|
|
135 call I2C_RX_accelerometer ; read Accelerometer
|
|
|
136 call compass_filter_init ; set DX_f values
|
|
|
137 call compass_reset_calibration ; Reset CX_f values
|
|
|
138 banksel common
|
|
|
139
|
|
|
140 compass_calibration_loop2:
|
|
|
141 call I2C_RX_compass ; read compass
|
|
|
142 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
143 call compass_filter ; Filter compass raw data
|
|
|
144 banksel common
|
|
|
145
|
|
|
146 ; Twice
|
|
|
147 call I2C_RX_compass ; read compass
|
|
|
148 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
149 call compass_filter ; Filter compass raw data
|
|
|
150 banksel common
|
|
|
151 ;
|
|
|
152 ; ; Three
|
|
|
153 ; call I2C_RX_compass ; read compass
|
|
|
154 ; call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
155 ; call compass_filter ; Filter compass raw data
|
|
|
156 ; banksel common
|
|
|
157 ;
|
|
|
158 ; ; Four times to get cleaner values
|
|
|
159 ; call I2C_RX_compass ; read compass
|
|
|
160 ; call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
161 ; call compass_filter ; Filter compass raw data
|
|
|
162
|
|
|
163 ; And register only one value out of four:
|
|
|
164 call compass_add_calibration ; check and store new max/min values
|
|
|
165 banksel common
|
|
|
166
|
|
|
167 call TFT_compass_fast ; show values
|
|
|
168 ; btg LEDg ; Show read-out interval
|
|
|
169
|
|
|
170 btfsc sleepmode ; Sleepmode active?
|
|
|
171 bra compass_calibration_exit ; Yes, exit
|
|
|
172
|
|
|
173 btfsc switch_left ; Button pressed?
|
|
|
174 bra compass_calibration_exit ; Yes, exit
|
|
|
175
|
|
|
176 btfss onesecupdate ; do every second tasks?
|
|
|
177 bra compass_calibration_loop2 ; no, loop here
|
|
|
178
|
|
|
179 movlw .240
|
|
|
180 call timeout_testmode ; check timeout
|
|
|
181 bcf onesecupdate ; clear flag
|
|
|
182
|
|
|
183 bra compass_calibration_loop2 ; loop here
|
|
|
184
|
|
|
185 compass_calibration_exit:
|
|
|
186 call compass_solve_calibration
|
|
|
187 banksel common
|
|
|
188 extern option_save_all
|
|
|
189 call option_save_all ; save all settings into EEPROM
|
|
|
190 bcf sleepmode ; Clear the flag before exiting to surfacemode
|
|
|
191 movlw .6
|
|
|
192 movwf menupos3 ; Set to compass view...
|
|
|
193 goto surfloop ; ...and exit
|
|
|
194
|
|
|
195 END |
