|
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 mcp_reset_rssi ; Setup RX
|
|
|
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
|
|
|
105 goto surfloop ; Exit
|
|
|
106
|
|
|
107 btfsc switch_right
|
|
|
108 goto compass_calibration_loop
|
|
|
109
|
|
|
110 btfsc sleepmode ; Sleepmode active?
|
|
|
111 goto sleeploop ; Yes, switch into sleepmode!
|
|
|
112
|
|
|
113 bra testloop_loop ; loop testmode
|
|
|
114
|
|
|
115
|
|
|
116 global compass_calibration_loop
|
|
|
117 compass_calibration_loop: ; Compass calibration
|
|
|
118 bsf no_sensor_int ; No Sensor ISR
|
|
|
119 call I2C_init_accelerometer
|
|
|
120 call I2C_init_compass_fast
|
|
|
121 call TFT_ClearScreen
|
|
|
122
|
|
|
123 ; Mask
|
|
|
124 WIN_COLOR color_greenish
|
|
|
125 WIN_SMALL .16,.0
|
|
|
126 STRCPY_TEXT_PRINT tCompassMenu
|
|
|
127 call TFT_standard_color
|
|
|
128 WIN_SMALL .0,.215
|
|
|
129 STRCPY_TEXT_PRINT tExit
|
|
|
130
|
|
|
131 clrf timeout_counter2
|
|
|
132 clrf timeout_counter3
|
|
|
133
|
|
|
134 call speed_fastest
|
|
|
135 call I2C_RX_compass ; read compass
|
|
|
136 call I2C_RX_accelerometer ; read Accelerometer
|
|
|
137 call compass_filter_init ; set DX_f values
|
|
|
138 call compass_reset_calibration ; Reset CX_f values
|
|
|
139 banksel common
|
|
|
140
|
|
|
141 compass_calibration_loop2:
|
|
|
142 call I2C_RX_compass ; read compass
|
|
|
143 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
144 call compass_filter ; Filter compass raw data
|
|
|
145 banksel common
|
|
|
146
|
|
|
147 ; Twice
|
|
|
148 call I2C_RX_compass ; read compass
|
|
|
149 call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
150 call compass_filter ; Filter compass raw data
|
|
|
151 banksel common
|
|
|
152 ;
|
|
|
153 ; ; Three
|
|
|
154 ; call I2C_RX_compass ; read compass
|
|
|
155 ; call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
156 ; call compass_filter ; Filter compass raw data
|
|
|
157 ; banksel common
|
|
|
158 ;
|
|
|
159 ; ; Four times to get cleaner values
|
|
|
160 ; call I2C_RX_compass ; read compass
|
|
|
161 ; call I2C_RX_accelerometer ; Test Accelerometer
|
|
|
162 ; call compass_filter ; Filter compass raw data
|
|
|
163
|
|
|
164 ; And register only one value out of four:
|
|
|
165 call compass_add_calibration ; check and store new max/min values
|
|
|
166 banksel common
|
|
|
167
|
|
|
168 call TFT_compass_fast ; show values
|
|
|
169 ; btg LEDg ; Show read-out interval
|
|
|
170
|
|
|
171 btfsc sleepmode ; Sleepmode active?
|
|
|
172 bra compass_calibration_exit ; Yes, exit
|
|
|
173
|
|
|
174 btfsc switch_left ; Button pressed?
|
|
|
175 bra compass_calibration_exit ; Yes, exit
|
|
|
176
|
|
|
177 btfss onesecupdate ; do every second tasks?
|
|
|
178 bra compass_calibration_loop2 ; no, loop here
|
|
|
179
|
|
|
180 movlw .240
|
|
|
181 call timeout_testmode ; check timeout
|
|
|
182 bcf onesecupdate ; clear flag
|
|
|
183
|
|
|
184 bra compass_calibration_loop2 ; loop here
|
|
|
185
|
|
|
186 compass_calibration_exit:
|
|
|
187 call compass_solve_calibration
|
|
|
188 banksel common
|
|
|
189 extern option_save_all
|
|
|
190 call option_save_all ; save all settings into EEPROM
|
|
|
191 bcf sleepmode ; Clear the flag before exiting to surfacemode
|
|
|
192 movlw .6
|
|
|
193 movwf menupos3 ; Set to compass view...
|
|
|
194 goto surfloop ; ...and exit
|
|
|
195
|
|
|
196 END |
