Mercurial > public > hwos_code
annotate src/ms5541.asm @ 40:1e2d2b0bca5b
Bugfix: Simulator in CC mode
| author | mh@mh-THINK.fritz.box |
|---|---|
| date | Thu, 15 Aug 2013 14:34:50 +0200 |
| parents | 50c3e2c7ba7a |
| children | 7ca1105751c7 |
| rev | line source |
|---|---|
| 0 | 1 ;============================================================================= |
| 2 ; | |
| 3 ; File ms5541.asm | |
| 4 ; | |
| 5 ; Sensor subroutines | |
| 6 ; | |
| 7 ; Copyright (c) 2011, JD Gascuel, HeinrichsWeikamp, all right reserved. | |
| 8 ;============================================================================= | |
| 9 ; HISTORY | |
| 10 ; 2011-08-03 : [mH] moving from OSTC code | |
| 11 | |
| 12 #include "ostc3.inc" ; Mandatory header | |
| 13 #include "math.inc" ; Math routines | |
| 14 | |
| 15 sensors CODE | |
| 16 | |
| 17 ;============================================================================= | |
| 18 ; Expose internal variables, to ease debug: | |
| 19 global D1, D2 | |
| 20 global C1, C2, C3, C4, C5, C6 | |
| 21 global xdT, xdT2, OFF, SENS, amb_pressure_avg, temperature_avg | |
| 22 | |
| 23 ;============================================================================= | |
| 24 global calculate_compensation | |
| 25 calculate_compensation: | |
| 26 ; xdT = D2 - C5 (s16 range -11.400 .. +12.350) | |
| 27 movf C5+0,W ; Get Value to be subtracted | |
| 28 subwf D2+0,W ; Do the Low Byte | |
| 29 movwf xdT+0 | |
| 30 movf C5+1,W ; Then the high byte. | |
| 31 subwfb D2+1,W | |
| 32 movwf xdT+1 | |
| 33 | |
| 34 ; Second order temperature calculation | |
| 35 ; xdT/128 is in range -89..+96, hence signed 8bit. dT/128 = (2*dT)/256 | |
| 36 rlcf xdT+0,W ; put hit bit in carry. | |
| 37 rlcf xdT+1,W ; inject in high byte. | |
| 38 movwf isr_xA+0 ; and put result in low byte. | |
| 39 clrf isr_xA+1 | |
| 40 btfsc xdT+1,7 ; If dT < 0 | |
| 41 setf isr_xA+1 ; then signextend to -1 | |
| 42 movff isr_xA+0,isr_xB+0 ; copy A to B | |
| 43 movff isr_xA+1,isr_xB+1 | |
| 44 call isr_signed_mult16x16 ; dT*dT --> xC (32 bits) | |
| 45 | |
| 46 ; dT >= 0: divide by 8, ie. 3 shifts rights. | |
| 47 ; dT < 0: divide by 2, ie. 1 shifts rights. | |
| 48 movlw .3 | |
| 49 btfss xdT+1,7 ; Was dT negative ? | |
| 50 movlw .1 | |
| 51 calc_loop_1: | |
| 52 bcf STATUS,C ;dT^2 is positive, so injected zeros. | |
| 53 rrcf isr_xC+1,F | |
| 54 rrcf isr_xC+0,F | |
| 55 decfsz WREG | |
| 56 bra calc_loop_1 | |
| 57 | |
| 58 movf isr_xC+0,W ; dT2 = dT - (dT/128)*(dT/128)/(2 ...or... 8) | |
| 59 subwf xdT+0,W | |
| 60 movwf xdT2+0 | |
| 61 movf isr_xC+1,W | |
| 62 subwfb xdT+1,W | |
| 63 movwf xdT2+1 | |
| 64 | |
| 65 ; Calculate OFF = C2 + ((C4-250)*dT2)/2^12 + 10000 | |
| 66 ; (range +9.246 .. +18.887) | |
| 67 movff C4+0,isr_xA+0 | |
| 68 movff C4+1,isr_xA+1 | |
| 69 ; movlw LOW(-.250) ; C4 - 250 --> A | |
| 70 ; addwf C4+0,W | |
| 71 ; movwf isr_xA+0 | |
| 72 ; movlw -1 ; HIGH(- .250) is not understood... | |
| 73 ; addwfc C4+1,W | |
| 74 ; movwf isr_xA+1 | |
| 75 | |
| 76 movff xdT2+0,isr_xB+0 ; dT2 --> B | |
| 77 movff xdT2+1,isr_xB+1 | |
| 78 call isr_signed_mult16x16 | |
| 79 movlw .12-.8 ; A 12bit shift = 1 byte + 4 bits. | |
| 80 call isr_shift_C31 | |
| 81 | |
| 82 movlw LOW(.10000) ; Add 10000 | |
| 83 addwf isr_xC+1,F | |
| 84 movlw HIGH(.10000) | |
| 85 addwfc isr_xC+2,F | |
| 86 | |
| 87 movf C2+0,W ; Add C2, and save into OFF | |
| 88 addwf isr_xC+1,W | |
| 89 movwf OFF+0 | |
| 90 movf C2+1,W | |
| 91 addwfc isr_xC+2,W | |
| 92 movwf OFF+1 | |
| 93 | |
| 94 ; Calculate SENS = C1/2 + ((C3+200)*dT)/2^13 + 3000 | |
| 95 movlw LOW(.200) ; C3+200 --> A | |
| 96 addwf C3+0,W | |
| 97 movwf isr_xA+0 | |
| 98 movlw HIGH(.200) | |
| 99 addwfc C3+1,W | |
| 100 movwf isr_xA+1 | |
| 101 ; B still contains dT2 | |
| 102 call isr_signed_mult16x16 ; A*B --> C | |
| 103 movlw .13-.8 ; A 13bit shift = 1 byte + 5 bits. | |
| 104 call isr_shift_C31 | |
| 105 | |
| 106 bcf STATUS,C ; SENS = C1 / 2 | |
| 107 rrcf C1+1,W | |
| 108 movwf SENS+1 | |
| 109 rrcf C1+0,W | |
| 110 movwf SENS+0 | |
| 111 | |
| 112 movlw LOW(.3000) ; Add 3000 | |
| 113 addwf isr_xC+1,F | |
| 114 movlw HIGH(.3000) | |
| 115 addwfc isr_xC+2,F | |
| 116 | |
| 117 movf isr_xC+1,W ; And sum into SENS | |
| 118 addwf SENS+0,F | |
| 119 movf isr_xC+2,W | |
| 120 addwfc SENS+1,F | |
| 121 | |
| 122 ; calculate amb_pressure = (sens * (d1-off))/2^12 + 1000 | |
| 123 movf OFF+0,W ; d1-off --> a | |
| 124 subwf D1+0,W | |
| 125 movwf isr_xA+0 | |
| 126 movf OFF+1,W | |
| 127 subwfb D1+1,W | |
| 128 movwf isr_xA+1 | |
| 129 | |
| 130 movff SENS+0,isr_xB+0 ; sens --> b | |
| 131 movff SENS+1,isr_xB+1 | |
| 132 call isr_signed_mult16x16 | |
| 133 movlw .12-.8 ; a 12bit shift = 1 byte + 4 bits. | |
| 134 call isr_shift_C31 | |
| 135 | |
| 136 movlw LOW(.1000) ; add 1000 | |
| 137 addwf isr_xC+1,F | |
| 138 movlw HIGH(.1000) | |
| 139 addwfc isr_xC+2,F | |
| 140 | |
| 141 banksel common | |
| 142 btfss simulatormode_active ; are we in simulator mode? | |
| 143 bra calc_compensation_2 ; no | |
| 144 banksel isr_backup | |
| 145 | |
| 146 movff sim_pressure+0,isr_xC+1 ; override readings with simulator values | |
| 147 movff sim_pressure+1,isr_xC+2 | |
| 148 | |
| 149 calc_compensation_2: | |
| 150 banksel isr_backup | |
| 151 movf isr_xC+1,W ; Then sum_up to pressure averaging buffer. | |
| 152 addwf amb_pressure_avg+0,F | |
| 153 movf isr_xC+2,W | |
| 154 addwfc amb_pressure_avg+1,F | |
| 155 | |
| 156 ; calculate temp = 200 + dT*(C6+100)/2^11 | |
| 157 movlw LOW(.100) ; C6 + 100 --> A | |
| 158 addwf C6+0,W | |
| 159 movwf isr_xA+0 | |
| 160 movlw HIGH(.100) | |
| 161 addwfc C6+1,W | |
| 162 movwf isr_xA+1 | |
| 163 | |
| 164 movff xdT2+0,isr_xB+0 ; dT2 --> B | |
| 165 movff xdT2+1,isr_xB+1 | |
| 166 call isr_signed_mult16x16 ; A*B | |
| 167 movlw .11-.8 ; A 12bit shift = 1 byte + 3 bits. | |
| 168 call isr_shift_C31 | |
| 169 | |
| 170 movlw LOW(.200) ; Add 200 | |
| 171 addwf isr_xC+1,F | |
| 172 movlw HIGH(.200) | |
| 173 addwfc isr_xC+2,F | |
| 174 | |
| 175 movf isr_xC+1,W | |
| 176 addwf temperature_avg+0,F | |
| 177 movf isr_xC+2,W | |
| 178 addwfc temperature_avg+1,F | |
| 179 | |
| 180 return ; Done. | |
| 181 | |
| 182 ;============================================================================= | |
| 183 global get_pressure_start | |
| 184 get_pressure_start: | |
| 185 rcall reset_MS5541 | |
| 186 movlw b'10100000' ;+3*high as start and 1+low as stop! | |
| 187 get_pressure_start2: | |
| 188 movwf isr1_temp | |
| 189 movlw d'12' | |
| 190 rcall send_data_MS5541 | |
| 191 return | |
| 192 | |
| 193 global get_pressure_value | |
| 194 get_pressure_value: | |
| 195 btfsc MS5541_miso ; Conversion done? | |
| 196 return ; No, Return | |
| 197 rcall get_2bytes_MS5541 | |
| 198 movff dMSB,D1+1 | |
| 199 movff dLSB,D1+0 | |
| 200 return | |
| 201 | |
| 202 ;============================================================================= | |
| 203 global get_temperature_start | |
| 204 get_temperature_start: | |
| 205 rcall reset_MS5541 | |
| 206 movlw b'10010000' ;+3*high as start and 1+low as stop! | |
| 207 bra get_pressure_start2 ; continue in "get_pressure" | |
| 208 | |
| 209 global get_temperature_value | |
| 210 get_temperature_value: | |
| 211 btfsc MS5541_miso ; Conversion done? | |
| 212 return ; No, Return | |
| 213 rcall get_2bytes_MS5541 | |
| 214 movff dMSB,D2+1 | |
| 215 movff dLSB,D2+0 | |
| 216 return | |
| 217 | |
| 218 ;============================================================================= | |
| 219 global get_calibration_data | |
| 220 get_calibration_data: | |
| 221 banksel common | |
| 222 bsf no_sensor_int ; disable sensor interrupts | |
| 223 banksel isr_backup ; Back to Bank0 ISR data | |
| 224 | |
| 225 rcall reset_MS5541 | |
| 226 movlw b'01010100' ;+3*high as start and 1+low as stop! | |
| 227 movwf isr1_temp | |
| 228 movlw d'13' | |
| 229 rcall send_data_MS5541 | |
| 230 rcall get_2bytes_MS5541 | |
| 231 movff dMSB,W1+1 | |
| 232 movff dLSB,W1+0 | |
| 233 | |
| 234 movlw b'01011000' ;+3*high as start and 1+low as stop! | |
| 235 movwf isr1_temp | |
| 236 movlw d'13' | |
| 237 rcall send_data_MS5541 | |
| 238 rcall get_2bytes_MS5541 | |
| 239 movff dMSB,W2+1 | |
| 240 movff dLSB,W2+0 | |
| 241 | |
| 242 movlw b'01100100' ;+3*high as start and 1+low as stop! | |
| 243 movwf isr1_temp | |
| 244 movlw d'13' | |
| 245 rcall send_data_MS5541 | |
| 246 rcall get_2bytes_MS5541 | |
| 247 movff dMSB,W3+1 | |
| 248 movff dLSB,W3+0 | |
| 249 | |
| 250 movlw b'01101000' ;+3*high as start and 1+low as stop! | |
| 251 movwf isr1_temp | |
| 252 movlw d'13' | |
| 253 rcall send_data_MS5541 | |
| 254 rcall get_2bytes_MS5541 | |
| 255 movff dMSB,W4+1 | |
| 256 movff dLSB,W4+0 | |
| 257 | |
| 258 ; calculate C1 (16Bit) | |
| 259 movff W1+1, C1+1 | |
| 260 bcf STATUS,C | |
| 261 rrcf C1+1 | |
| 262 bcf STATUS,C | |
| 263 rrcf C1+1 | |
| 264 bcf STATUS,C | |
| 265 rrcf C1+1 | |
| 266 movff W1+0, C1+0 | |
| 267 bsf STATUS,C | |
| 268 btfss W1+1,0 | |
| 269 bcf STATUS,C | |
| 270 rrcf C1+0 | |
| 271 bsf STATUS,C | |
| 272 btfss W1+1,1 | |
| 273 bcf STATUS,C | |
| 274 rrcf C1+0 | |
| 275 bsf STATUS,C | |
| 276 btfss W1+1,2 | |
| 277 bcf STATUS,C | |
| 278 rrcf C1+0 | |
| 279 | |
| 280 ; calculate C2 (16Bit) | |
| 281 movff W2+0, C2+0 | |
| 282 bsf STATUS,C | |
| 283 btfss W2+1,0 | |
| 284 bcf STATUS,C | |
| 285 rrcf C2+0 | |
| 286 bsf STATUS,C | |
| 287 btfss W2+1,1 | |
| 288 bcf STATUS,C | |
| 289 rrcf C2+0 | |
| 290 bsf STATUS,C | |
| 291 btfss W2+1,2 | |
| 292 bcf STATUS,C | |
| 293 rrcf C2+0 | |
| 294 bsf STATUS,C | |
| 295 btfss W2+1,3 | |
| 296 bcf STATUS,C | |
| 297 rrcf C2+0 | |
| 298 bsf STATUS,C | |
| 299 btfss W2+1,4 | |
| 300 bcf STATUS,C | |
| 301 rrcf C2+0 | |
| 302 bsf STATUS,C | |
| 303 btfss W2+1,5 | |
| 304 bcf STATUS,C | |
| 305 rrcf C2+0 | |
| 306 | |
| 307 movff W2+1, C2+1 | |
| 308 bsf STATUS,C | |
| 309 btfss W1+0,0 | |
| 310 bcf STATUS,C | |
| 311 rrcf C2+1 | |
| 312 bsf STATUS,C | |
| 313 btfss W1+0,1 | |
| 314 bcf STATUS,C | |
| 315 rrcf C2+1 | |
| 316 bsf STATUS,C | |
| 317 btfss W1+0,2 | |
| 318 bcf STATUS,C | |
| 319 rrcf C2+1 | |
| 320 bcf STATUS,C | |
| 321 rrcf C2+1 | |
| 322 bcf STATUS,C | |
| 323 rrcf C2+1 | |
| 324 bcf STATUS,C | |
| 325 rrcf C2+1 | |
| 326 | |
| 327 ; calculate C3 (16Bit) | |
| 328 movff W3+1,C3+0 | |
| 329 bsf STATUS,C | |
| 330 btfss W3+0,7 | |
| 331 bcf STATUS,C | |
| 332 rlcf C3+0 | |
| 333 bsf STATUS,C | |
| 334 btfss W3+0,6 | |
| 335 bcf STATUS,C | |
| 336 rlcf C3+0 | |
| 337 clrf C3+1 | |
| 338 btfsc W3+1,7 | |
| 339 bsf C3+1,1 | |
| 340 btfsc W3+1,6 | |
| 341 bsf C3+1,0 | |
| 342 | |
| 343 ; calculate C4 (16Bit) | |
| 344 movff W4+1,C4+0 | |
| 345 bsf STATUS,C | |
| 346 btfss W4+0,7 | |
| 347 bcf STATUS,C | |
| 348 rlcf C4+0 | |
| 349 clrf C4+1 | |
| 350 btfsc W4+1,7 | |
| 351 bsf C4+1,0 | |
| 352 | |
| 353 ; C4=C4-250 | |
| 354 movlw LOW(-.250) ; C4 - 250 --> C4 | |
| 355 addwf C4+0,W | |
| 356 movwf C4+0 | |
| 357 movlw -1 ; HIGH(- .250) is not understood... | |
| 358 addwfc C4+1,W | |
| 359 movwf C4+1 | |
| 360 | |
| 361 ; calculate C5 (16Bit) | |
| 362 movff W3+0,C5+0 | |
| 363 bcf C5+0,6 | |
| 364 btfsc W2+0,0 | |
| 365 bsf C5+0,6 | |
| 366 bcf C5+0,7 | |
| 367 btfsc W2+0,1 | |
| 368 bsf C5+0,7 | |
| 369 clrf C5+1 | |
| 370 btfsc W2+0,2 | |
| 371 bsf C5+1,0 | |
| 372 btfsc W2+0,3 | |
| 373 bsf C5+1,1 | |
| 374 btfsc W2+0,4 | |
| 375 bsf C5+1,2 | |
| 376 btfsc W2+0,5 | |
| 377 bsf C5+1,3 | |
| 378 | |
| 379 ; calculate C5 = UT1 | |
| 380 ; C5 = 8*C5 + 10000 (u16 range 10.000 .. +42.760) | |
| 381 clrf isr_xA+1 | |
| 382 movlw d'8' | |
| 383 movwf isr_xA+0 | |
| 384 movff C5+0,isr_xB+0 | |
| 385 movff C5+1,isr_xB+1 | |
| 386 call isr_unsigned_mult16x16 ;isr_xA*isr_xB=isr_xC | |
| 387 movff isr_xC+0,C5+0 | |
| 388 movff isr_xC+1,C5+1 | |
| 389 movlw LOW d'10000' | |
| 390 addwf C5+0,F | |
| 391 movlw HIGH d'10000' | |
| 392 addwfc C5+1,F ; = 8*C5 + 10000 | |
| 393 | |
| 394 ; calculate C6 (16Bit) | |
| 395 clrf C6+1 | |
| 396 movff W4+0,C6+0 | |
| 397 bcf C6+0,7 | |
| 398 | |
| 399 banksel common | |
| 400 bcf no_sensor_int ; enable sensor interrupts | |
| 401 bcf pressure_refresh ; Clear flag | |
| 402 banksel isr_backup ; Back to Bank0 ISR data | |
| 403 | |
| 404 clrf sensor_state_counter ; Then reset State counter | |
| 405 | |
| 406 return | |
| 407 | |
| 408 ;============================================================================= | |
| 409 reset_MS5541_one: | |
| 410 bsf MS5541_mosi | |
| 411 bra send_clk_pulse ; Send one high-low sequence on MS5541_clk -> and return | |
| 412 | |
| 413 reset_MS5541_zero: | |
| 414 bcf MS5541_mosi | |
| 415 bra send_clk_pulse ; Send one high-low sequence on MS5541_clk -> and return | |
| 416 | |
| 417 reset_MS5541: | |
| 418 rcall reset_MS5541_one ;0 | |
| 419 rcall reset_MS5541_zero | |
| 420 rcall reset_MS5541_one | |
| 421 rcall reset_MS5541_zero | |
| 422 rcall reset_MS5541_one | |
| 423 rcall reset_MS5541_zero | |
| 424 rcall reset_MS5541_one | |
| 425 rcall reset_MS5541_zero | |
| 426 rcall reset_MS5541_one | |
| 427 rcall reset_MS5541_zero | |
| 428 rcall reset_MS5541_one | |
| 429 rcall reset_MS5541_zero | |
| 430 rcall reset_MS5541_one | |
| 431 rcall reset_MS5541_zero | |
| 432 rcall reset_MS5541_one | |
| 433 rcall reset_MS5541_zero ;15 | |
| 434 rcall reset_MS5541_zero | |
| 435 rcall reset_MS5541_zero | |
| 436 rcall reset_MS5541_zero | |
| 437 rcall reset_MS5541_zero | |
| 438 rcall reset_MS5541_zero ;20 | |
| 439 return | |
| 440 | |
| 441 get_2bytes_MS5541: | |
| 442 movlw d'8' | |
| 443 movwf clock_count | |
| 444 rcall recieve_loop | |
| 445 movff isr1_temp,dMSB | |
| 446 | |
| 447 movlw d'8' | |
| 448 movwf clock_count | |
| 449 rcall recieve_loop | |
| 450 movff isr1_temp,dLSB | |
| 451 bra send_clk_pulse ; Send one high-low sequence on MS5541_clk -> and return | |
| 452 ;return | |
| 453 | |
| 454 recieve_loop: | |
| 455 rcall send_clk_pulse ; Send one high-low sequence on MS5541_clk | |
| 456 btfss MS5541_miso ;MSB first | |
| 457 bcf STATUS,C | |
| 458 btfsc MS5541_miso ;MSB first | |
| 459 bsf STATUS,C | |
| 460 rlcf isr1_temp,F | |
| 461 decfsz clock_count,F | |
| 462 bra recieve_loop | |
| 463 return | |
| 464 | |
| 465 send_clk_pulse: | |
| 466 bsf MS5541_clk | |
| 467 nop | |
| 468 nop | |
| 469 nop | |
| 470 nop | |
|
29
50c3e2c7ba7a
adding cardinal (and ordinal) directions for the compass
heinrichsweikamp
parents:
0
diff
changeset
|
471 nop |
|
50c3e2c7ba7a
adding cardinal (and ordinal) directions for the compass
heinrichsweikamp
parents:
0
diff
changeset
|
472 nop |
| 0 | 473 bcf MS5541_clk |
| 474 nop | |
| 475 nop | |
|
29
50c3e2c7ba7a
adding cardinal (and ordinal) directions for the compass
heinrichsweikamp
parents:
0
diff
changeset
|
476 nop |
|
50c3e2c7ba7a
adding cardinal (and ordinal) directions for the compass
heinrichsweikamp
parents:
0
diff
changeset
|
477 nop |
| 0 | 478 return |
| 479 | |
| 480 send_data_MS5541: | |
| 481 movwf clock_count ; From WREG | |
| 482 ; send three startbits first | |
| 483 bcf MS5541_clk | |
|
29
50c3e2c7ba7a
adding cardinal (and ordinal) directions for the compass
heinrichsweikamp
parents:
0
diff
changeset
|
484 nop |
|
50c3e2c7ba7a
adding cardinal (and ordinal) directions for the compass
heinrichsweikamp
parents:
0
diff
changeset
|
485 nop |
| 0 | 486 bsf MS5541_mosi |
| 487 movlw d'3' | |
| 488 subwf clock_count,F ; total bit counter | |
| 489 rcall send_clk_pulse ; Send one high-low sequence on MS5541_clk | |
| 490 rcall send_clk_pulse ; Send one high-low sequence on MS5541_clk | |
| 491 rcall send_clk_pulse ; Send one high-low sequence on MS5541_clk | |
| 492 ; now send 8 bytes from isr_temp1 and fill-up with zeros | |
| 493 send_data_MS5541_2: | |
| 494 bcf MS5541_clk | |
|
29
50c3e2c7ba7a
adding cardinal (and ordinal) directions for the compass
heinrichsweikamp
parents:
0
diff
changeset
|
495 nop |
|
50c3e2c7ba7a
adding cardinal (and ordinal) directions for the compass
heinrichsweikamp
parents:
0
diff
changeset
|
496 nop |
| 0 | 497 |
| 498 btfss isr1_temp,7 ;MSB first | |
| 499 bcf MS5541_mosi | |
| 500 btfsc isr1_temp,7 ;MSB first | |
| 501 bsf MS5541_mosi | |
| 502 | |
| 503 bsf MS5541_clk | |
| 504 | |
| 505 bcf STATUS,C | |
| 506 rlcf isr1_temp,F | |
|
29
50c3e2c7ba7a
adding cardinal (and ordinal) directions for the compass
heinrichsweikamp
parents:
0
diff
changeset
|
507 nop |
|
50c3e2c7ba7a
adding cardinal (and ordinal) directions for the compass
heinrichsweikamp
parents:
0
diff
changeset
|
508 nop |
| 0 | 509 ; nop |
| 510 ; nop | |
| 511 ; nop | |
| 512 ; nop | |
| 513 ; bcf MS5541_clk | |
| 514 | |
| 515 decfsz clock_count,F | |
| 516 bra send_data_MS5541_2 | |
| 517 bcf MS5541_clk | |
| 518 return | |
| 519 | |
| 520 END |