brickOS Kernel Developer v0.9.0
dsensor.c
Go to the documentation of this file.
1
6/*
7 * The contents of this file are subject to the Mozilla Public License
8 * Version 1.0 (the "License"); you may not use this file except in
9 * compliance with the License. You may obtain a copy of the License at
10 * http://www.mozilla.org/MPL/
11 *
12 * Software distributed under the License is distributed on an "AS IS"
13 * basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
14 * License for the specific language governing rights and limitations
15 * under the License.
16 *
17 * The Original Code is legOS code, released October 17, 1999.
18 *
19 * The Initial Developer of the Original Code is Markus L. Noga.
20 * Portions created by Markus L. Noga are Copyright (C) 1999
21 * Markus L. Noga. All Rights Reserved.
22 *
23 * Contributor(s): Markus L. Noga <markus@noga.de>
24 * Eric Habnerfeller <ehaberfe@atitech.ca>
25 * Lou Sortman <lou@sunsite.unc.edu>
26 */
27
28/*
29 * 2000.03.11 - Paolo Masetti <paolo.masetti@itlug.org>
30 *
31 * - Included a fix for rotation sensor posted by "Ben Jackson"
32 * on lugnet.robotics.rcx.legos
33 *
34 * 2000.04.30 - Paolo Masetti <paolo.masetti@itlug.org>
35 *
36 * - ISR Reading routine fix to make read values stable.
37 * - Fixed rotation sensor status table values to avoid offset problems.
38 *
39 * 2000.09.06 - Jochen Hoenicke <jochen@gnu.org>
40 *
41 * - Added velocity calculation for rotation sensor.
42 */
43
44#include <dsensor.h>
45
46#ifdef CONF_DSENSOR
47
48#include <sys/h8.h>
49#include <sys/irq.h>
50#include <sys/bitops.h>
51#include <rom/registers.h>
52#include <unistd.h>
53#include <conio.h>
54
56//
57// Definitions
58//
60
61#define DS_ALL_ACTIVE 0x07
62#define DS_ALL_PASSIVE (~DS_ALL_ACTIVE)
63
65//
66// Variables
67//
69
70volatile unsigned char ds_channel;
71
72unsigned char ds_activation;
73
74#ifdef CONF_DSENSOR_ROTATION
75unsigned char ds_rotation;
76
77volatile int ds_rotations[3];
78
79static signed char rotation_state[3];
80static signed char rotation_new_state[3];
81static unsigned int state_duration[3];
82
83#ifdef CONF_DSENSOR_VELOCITY
84volatile int ds_velocities[3];
85static unsigned int last_rotation[3];
86static unsigned int next_rotation[3];
87static signed char rotation_dir[3];
88#endif
89
90
91
93
96static const signed char ad2state[16]={
97 // 0 1 2 3 4 5 6 7 8 9 a b c d e f // (sensor value>>12)
98 -1,-1,-1,-1,-1, 2, 2, 2, 3, 3, 3, 3, 1, 1, 1, 0 // New values to be used
99 // with delayed read
100
101// -1,-1,-1,-1,-1,-1, 2, 2, 2, 3, 3, 3, 1, 1, 1, 0 // Old values: biased for
102 // non-delayed read
103
104};
105
107
112static const signed char diff2change[7]={
113 //-3 -2 -1 0 1 2 3 // newstate-state
114 1, 0,-1, 0, 1, 0,-1
115};
116
118//
119// Functions
120//
122
124
129void ds_rotation_set(volatile unsigned *sensor,int pos) {
130 if(sensor>=&AD_A && sensor<=&AD_C) { // catch range violations
131 unsigned channel=(unsigned) (sensor-&AD_A);
132 signed char state=ad2state[(*sensor)>>12];
133
134 if(state<0)
135 state=0;
136
137 rotation_state[channel]=state;
138 rotation_new_state[channel] = -1;
139 state_duration[channel]=0;
140 ds_rotations[channel]=pos; // reset counter
141
142 }
143}
144
146
148void ds_rotation_handler() {
149 unsigned channel =ds_channel;
150 unsigned raw =(*((&AD_A)+channel));
151 signed char newstate=ad2state[raw>>12];
152
153 if (newstate < 0)
154 return;
155
156 if (newstate == rotation_new_state[channel]) {
157 if (++state_duration[channel] == 2) {
158 signed char change = diff2change[newstate - rotation_state[channel] + 3];
159
160 ds_rotations[channel] += change;
161
162#ifdef CONF_DSENSOR_VELOCITY
163 {
164 /* We only take the lowest 16 bits of sys_time. We have to be
165 * a bit careful with wraparounds, but this is handled here.
166 */
167 unsigned int time = (unsigned int) sys_time;
168 if (change != rotation_dir[channel]) {
169 rotation_dir[channel] = change;
170 ds_velocities[channel] = 0;
171 last_rotation[channel] = time;
172 next_rotation[channel] = time + 1000;
173 } else {
174 if (time == last_rotation[channel])
175 ds_velocities[channel] = 1000 * change;
176 else {
177 unsigned int time_diff = (time - last_rotation[channel]);
178 if (time_diff > 1000) {
179 rotation_dir[channel] = 0;
180 ds_velocities[channel] = 0;
181 } else {
182 int speed = 1000 / time_diff;
183 ds_velocities[channel] = change > 0 ? speed : -speed;
184 last_rotation[channel] = time;
185 next_rotation[channel] = time + time_diff * 3 / 2;
186 }
187 }
188 }
189 }
190#endif
191
192 rotation_state[channel] = newstate;
193 rotation_new_state[channel] = -1;
194 }
195 } else if (newstate != rotation_state[channel]) {
196 rotation_new_state[channel] = newstate;
197 state_duration[channel] = 1;
198#ifdef CONF_DSENSOR_VELOCITY
199 } else {
200 /* No rotation change, check if velocity measure timeouts. */
201 unsigned int time = (unsigned int) sys_time;
202 if (rotation_dir[channel] &&
203 ((signed int) (time - next_rotation[channel])) >= 0) {
204 unsigned int time_diff = (time - last_rotation[channel]);
205 if (time_diff > 1000) {
206 rotation_dir[channel] = 0;
207 ds_velocities[channel] = 0;
208 } else {
209 int speed = 1000 / time_diff;
210 ds_velocities[channel] = rotation_dir[channel] > 0 ? speed : -speed;
211 next_rotation[channel] = time + time_diff / 2;
212 }
213 }
214#endif
215 }
216
217}
218#endif // CONF_DSENSOR_ROTATION
219
220#ifdef CONF_DSENSOR_MUX
221unsigned char ds_mux;
222
223volatile int ds_muxs[3][3];
224
225
226//width of each mux pulse
227#define DS_MUX_PULSE_TM_MS 10
228
229
230
231typedef struct {
232 unsigned long nextTm; //timestamp for next pulse
233 char remainingEdges; //edges left in pulse train
234 char channel; //current mux sub channel (0,1,2)
235 unsigned int attached[3];//what channels are sensors attached to
236 //this also defines the number of ms
237 //to wait before reading the value
238
239 enum {ds_mux_prepRead,
240 ds_mux_read,
241 ds_mux_pulse_low,
242 ds_mux_pulse_high} action; //specify next action
243} ds_mux_data_t;
244
245ds_mux_data_t ds_mux_data[3]; //data on mux
246
247#endif //CONF_DSENSOR_MUX
248
249
250
251static inline void ds_power_on(unsigned channel) {
252 switch(channel) {
253 case 0:
254 bit_set(&PORT6,0);
255 break;
256 case 1:
257 bit_set(&PORT6,1);
258 break;
259 case 2:
260 bit_set(&PORT6,2);
261 break;
262 default:
263 //bad
264 break;
265 }
266}//endof ds_power_on
267
268static inline void ds_power_off(unsigned channel) {
269 switch(channel) {
270 case 0:
271 bit_clear(&PORT6,0);
272 break;
273 case 1:
274 bit_clear(&PORT6,1);
275 break;
276 case 2:
277 bit_clear(&PORT6,2);
278 break;
279 default:
280 //bad
281 break;
282 }
283}//endof ds_power_off
284
285#ifdef CONF_DSENSOR_MUX
286
287
289void ds_mux_on(volatile unsigned *sensor,
290 unsigned int ch1,
291 unsigned int ch2,
292 unsigned int ch3) {
293 unsigned char i,j;
294 ds_passive(sensor);//powered, but not active in legOS sense
295
296
297 if(ch1==0 &&
298 ch2==0 &&
299 ch3==0) {
300 //umm this is useless
301 //avoid endless cycling
302 ds_mux_off(sensor);
303 return;
304 }
305
306 if (sensor == &SENSOR_3) {
307 i=0;
308 } else if (sensor == &SENSOR_2) {
309 i=1;
310 } else if (sensor == &SENSOR_1) {
311 i=2;
312 } else {
313 //bad
314 return;
315 }
316
317
318 ds_mux_data[i].attached[0]=ch1;
319 ds_mux_data[i].attached[1]=ch2;
320 ds_mux_data[i].attached[2]=ch3;
321
322 //add extended time based on the channel
323 //this is required by the mux
324 //the user supplies extra time based on the
325 //type of sensor they hook up
326 //these defaults give enough time to read
327 //a light sensor and should be ok for most
328 //sensors
329 if(ch1)
330 ds_mux_data[i].attached[0]+=160;
331 if(ch2)
332 ds_mux_data[i].attached[1]+=135;
333 if(ch3)
334 ds_mux_data[i].attached[2]+=25;
335
336
337
338
339 //check if we're just adjusting the ports
340 //if so we can return here
341 if(i==0 && ds_mux&1)
342 return;
343 if(i==1 && ds_mux&2)
344 return;
345 if(i==2 && ds_mux&4)
346 return;
347
348 //starting up mux
349
350 //power up
351 ds_power_on(i);
352
353 //schedule first event
354 //find first attached sensor
355 for(j=0;j<3 && ds_mux_data[i].attached[j]==0;j++);
356
357 ds_mux_data[i].channel=j;
358 ds_mux_data[i].remainingEdges=((j+1)*2);
359 ds_mux_data[i].action=ds_mux_pulse_low;
360 ds_mux_data[i].nextTm=sys_time+DS_MUX_PULSE_TM_MS;
361
362 if (sensor == &SENSOR_3) {
363 bit_set(&ds_mux, 0);
364 } else if (sensor == &SENSOR_2) {
365 bit_set(&ds_mux, 1);
366 } else if (sensor == &SENSOR_1) {
367 bit_set(&ds_mux, 2);
368 } else {
369 //bad
370 return;
371 }
372
373}//endof ds_mux_on
374
375
376
377void ds_mux_handler() {
378 unsigned sen=ds_channel;
379
380
381 if(ds_mux_data[sen].nextTm <= sys_time) {
382 //we've reached our next scheduled step
383 //lcd_int(sys_time-ds_mux_data[sen].nextTm);
384 switch(ds_mux_data[sen].action) {
385 case ds_mux_prepRead:
386 ds_power_off(sen);//power down for read
387 ds_mux_data[sen].action=ds_mux_read;
388 ds_mux_data[sen].nextTm=sys_time;//do it ASAP, but not now
389 break;
390 case ds_mux_read:
391 //read data
392 switch(sen) {
393 case 0:
394 ds_muxs[sen][(int)ds_mux_data[sen].channel]=SENSOR_3;
395 break;
396 case 1:
397 ds_muxs[sen][(int)ds_mux_data[sen].channel]=SENSOR_2;
398 break;
399 case 2:
400 ds_muxs[sen][(int)ds_mux_data[sen].channel]=SENSOR_1;
401 break;
402 default:
403 //bad
404 }
405
406
407 //change channel
408 do {
409 ds_mux_data[sen].channel++;
410 if(ds_mux_data[sen].channel>2/*max chan*/) {
411 ds_mux_data[sen].channel=0;
412 }
413 //make sure selected channel is marked attached
414 //don't worry about an endless loop ds_mux_on makes
415 //sure at least one channel is attached
416 } while(
417 (ds_mux_data[sen].attached
418 [(int)ds_mux_data[sen].channel])==0);
419
420
421 //use this low pulse as the first low pulse of next train
422
423 ds_mux_data[sen].remainingEdges=
424 ((ds_mux_data[sen].channel+1)*2)-1;
425
426 //schedule next high pulse
427 ds_mux_data[sen].action=ds_mux_pulse_high;
428 ds_mux_data[sen].nextTm=sys_time+DS_MUX_PULSE_TM_MS;
429 break;
430 case ds_mux_pulse_low:
431 //go low
432 ds_power_off(sen);
433 //schedule next high pulse
434 ds_mux_data[sen].nextTm=sys_time+DS_MUX_PULSE_TM_MS;
435 ds_mux_data[sen].remainingEdges--;
436 ds_mux_data[sen].action=ds_mux_pulse_high;
437 break;
438 case ds_mux_pulse_high:
439 //go high
440 ds_power_on(sen);
441 ds_mux_data[sen].remainingEdges--;
442
443 if(ds_mux_data[sen].remainingEdges==0) {
444 //done with train
445 //schedule prepRead
446 ds_mux_data[sen].action=ds_mux_prepRead;
447
448 //schedule enough time for the mux to make the switch
449 //this is scaled because the timeout the mux uses starts
450 //when the first pulse comes in, it is around 70ms, so
451 //when switching to sensor 1 we must want an additional
452 //amount of time before it mux reacts, we wait less for 2
453 //and not at all for 3
454 //then we wait a little bit before reading the sensor
455 //this give the sensor time to power up
456 ds_mux_data[sen].nextTm=sys_time+
457 ds_mux_data[sen].attached[(int)ds_mux_data[sen].channel];
458 //lcd_int(ds_mux_data[sen].channel+1);
459
460 break;
461 } else {
462 //schedule next low pulse
463 ds_mux_data[sen].action=ds_mux_pulse_low;
464 ds_mux_data[sen].nextTm=sys_time+DS_MUX_PULSE_TM_MS;
465 }
466 break;
467 default:
468 //bad
469 }
470
471 }
472}//endof ds_mux_handler
473
474#endif //CONF_DSENSOR_MUX
475
476
477
478
480//
481extern void ds_handler(void);
482#ifndef DOXYGEN_SHOULD_SKIP_THIS
483__asm__("\n\
484.text\n\
485.align 1\n\
486_ds_handler:\n\
487 ; r6 saved by ROM\n\
488\n\
489 mov.b @_ds_channel,r6l ; r6l = current channel\n\
490\n\
491 mov.b @_ds_activation,r6h ; r6h = activation bitmask\n\
492 btst r6l,r6h ; activate output?\n\
493 beq ds_noset\n\
494 bset r6l,@_PORT6:8 ; activate output of last port scanned\n\
495 ds_noset:\n\
496 "
498 "\n\
499 mov.b @_ds_rotation,r6h ; r6h = rotation bitmask\n\
500 btst r6l,r6h ; process rotation sensor?\n\
501 beq ds_norot\n\
502\n\
503 push r0 ; save r0..r3\n\
504 push r1\n\
505 push r2\n\
506 push r3 ; r4..r6 saved by gcc if necessary\n\
507\n\
508 jsr _ds_rotation_handler ; process rotation sensor\n\
509\n\
510 pop r3\n\
511 pop r2\n\
512 pop r1\n\
513 pop r0\n\
514 ds_norot:\n\
515 "
516#endif
517
518#ifdef CONF_DSENSOR_MUX
519 "\n\
520 mov.b @_ds_mux,r6h ; r6h = mux bitmask\n\
521 btst r6l,r6h ; process mux sensor?\n\
522 beq ds_nomux\n\
523\n\
524 push r0 ; save r0..r3\n\
525 push r1\n\
526 push r2\n\
527 push r3 ; r4..r6 saved by gcc if necessary\n\
528\n\
529 jsr _ds_mux_handler ; process mux sensor\n\
530\n\
531 pop r3\n\
532 pop r2\n\
533 pop r1\n\
534 pop r0\n\
535 ds_nomux:\n\
536 "
537#endif
538 "\n\
539 inc r6l ; next channel\n\
540 and #0x03,r6l ; limit to 0-3\n\
541\n\
542 mov.b @_ds_activation,r6h ; r6h = activation bitmask\n\
543 btst r6l,r6h ; activate output?\n\
544 beq ds_nounset\n\
545 bclr r6l,@_PORT6:8 ; set output inactive for reading\n\
546 ds_nounset:\n\
547\n\
548 ; The settle time for reading the value from active sensor start here\n\
549\n\
550 ; moved here for helping timing problems\n\
551 mov.b r6l,@_ds_channel ; store next channel\n\
552\n\
553 ; Added a delay loop for sensor settle time\n\
554\n\
555 mov.b #0x04, r6h ; delay loop\n\
556settle:\n\
557 nop ; each nop is a 2 state clock delay\n\
558 dec.b r6h ; 2 states ?\n\
559 bne settle ; 4 states\n\
560\n\
561 ; Total loop delay 32 states (?)\n\
562\n\
563 mov.b @_AD_CSR:8,r6h ; r6h = A/D CSR\n\
564 and.b #0x7c,r6h ; reset scanmode and channel num\n\
565 or.b r6l,r6h ; scan next channel\n\
566 mov.b r6h,@_AD_CSR:8 ; put r6h back on A/D CSR\n\
567\n\
568 ; The settle time for reading the value from active sensor finish here\n\
569\n\
570 bset #0x5,@_AD_CSR:8 ; go!\n\
571\n\
572 rts\n\
573");
574#endif // DOXYGEN_SHOULD_SKIP_THIS
575
576
578
581void ds_init(void) {
582 rom_port6_ddr|=DS_ALL_ACTIVE; // notify ROM we are using
583 PORT6_DDR =rom_port6_ddr; // PORT6 bit 0..2 as outputs
584
585 ds_activation=0; // all sensors passive
586 ds_channel =0; // start on channel 0
587
588#ifdef CONF_DSENSOR_ROTATION
589 ds_rotation =0; // rotation tracking disabled
590#endif
591
592#ifdef CONF_DSENSOR_MUX
593 ds_mux=0; // muxing disabled
594#endif
595
596 ad_vector=&ds_handler; // setup IRQ handler
597 AD_CR &=~ADCR_EXTERN;
600
601#ifdef CONF_CONIO
602 delay(10); // wait for initial A/D
603#else
604# warning "Rotation initialization might fail."
605#endif
606}
607
608
610
612void ds_shutdown(void) {
613
614 AD_CSR=0x00;
615 PORT6 &=DS_ALL_PASSIVE;
616 rom_port6_ddr&=DS_ALL_PASSIVE;
618}
619
620#endif // CONF_DSENSOR
__asm__("\n\ .text\n\ .globl _atomic_inc\n\ _atomic_inc:\n\ stc ccr, r1h ; save flags\n\ orc #0x80, ccr ; disable all but NMI\n\ mov.b @r0, r1l\n\ inc r1l\n\ mov.b r1l, @r0\n\ ldc r1h, ccr ; restore flags\n\ rts\n\ ")
Internal Interface: H8/300 bit operations.
#define bit_set(byte, bit)
set a single bit in memory
Definition bitops.h:59
#define bit_clear(byte, bit)
clear a single bit in memory
Definition bitops.h:65
#define CONF_DSENSOR_ROTATION
rotation sensor
Definition config.h:78
void delay(unsigned ms)
uncalibrated delay loop
Definition conio.c:204
Interface: console input / output.
#define SENSOR_2
Sensor on input pad 2.
Definition dsensor.h:62
void ds_passive(volatile unsigned *sensor)
set sensor mode to passive (light sensor detects ambient light)
Definition dsensor.h:180
void ds_rotation_set(volatile unsigned *sensor, int pos)
set rotation to an absolute value
#define SENSOR_1
< the raw sensors
Definition dsensor.h:61
#define SENSOR_3
Sensor on input pad 3.
Definition dsensor.h:63
unsigned char ds_activation
activation bitmask
unsigned char ds_rotation
rotation bitmask
volatile int ds_rotations[3]
rotational position
Internal Interface: H8/3297 processor registers.
#define ADCSR_ENABLE_IRQ
Definition h8.h:333
unsigned char PORT6_DDR
port 6 data direction register
#define ADCSR_GROUP_0
Definition h8.h:339
#define ADCSR_TIME_266
Definition h8.h:336
#define ADCSR_START
Definition h8.h:334
#define ADCSR_AN_0
Definition h8.h:342
volatile unsigned AD_A
A/D converter data register A.
volatile unsigned char AD_CSR
A/D converter control / status register.
unsigned char AD_CR
A/D converter control register.
volatile unsigned char PORT6
port 6 I/O register
volatile unsigned AD_C
A/D converter data register C.
Internal LNP Interface: RCX redirected IRQ vectors.
void * ad_vector
A/D interrupt vector.
ROM Interface: RCX registers cached by ROM functions.
unsigned char rom_port6_ddr
ROM shadow of port 6 DDR.
void ds_shutdown(void)
shutdown sensor a/d conversion
void ds_init(void)
initialize sensor a/d conversion
volatile time_t sys_time
current system time in ms
Definition systime.c:63
Interface: reduced UNIX standard library.