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 
64 //
66 // Variables
67 //
69 
70 volatile unsigned char ds_channel;
71 
72 unsigned char ds_activation;
73 
74 #ifdef CONF_DSENSOR_ROTATION
75 unsigned char ds_rotation;
76 
77 volatile int ds_rotations[3];
78 
79 static signed char rotation_state[3];
80 static signed char rotation_new_state[3];
81 static unsigned int state_duration[3];
82 
83 #ifdef CONF_DSENSOR_VELOCITY
84 volatile int ds_velocities[3];
85 static unsigned int last_rotation[3];
86 static unsigned int next_rotation[3];
87 static signed char rotation_dir[3];
88 #endif
89 
90 
91 
93 
96 static 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 
112 static 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 
129 void 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 
148 void 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
221 unsigned char ds_mux;
222 
223 volatile int ds_muxs[3][3];
224 
225 
226 //width of each mux pulse
227 #define DS_MUX_PULSE_TM_MS 10
228 
229 
230 
231 typedef 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 
245 ds_mux_data_t ds_mux_data[3]; //data on mux
246 
247 #endif //CONF_DSENSOR_MUX
248 
249 
250 
251 static 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 
268 static 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 
289 void 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 
377 void 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 //
481 extern 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\
556 settle:\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 
581 void 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 
612 void 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
#define bit_clear(byte, bit)
clear a single bit in memory
Definition: bitops.h:65
#define ADCSR_AN_0
Definition: h8.h:342
Internal Interface: H8/3297 processor registers.
volatile int ds_rotations[3]
rotational position
#define ADCSR_TIME_266
Definition: h8.h:336
void ds_passive(volatile unsigned *sensor)
set sensor mode to passive (light sensor detects ambient light)
Definition: dsensor.h:180
unsigned char PORT6_DDR
port 6 data direction register
#define CONF_DSENSOR_ROTATION
rotation sensor
Definition: config.h:78
#define ADCSR_START
Definition: h8.h:334
Interface: console input / output.
void * ad_vector
A/D interrupt vector.
#define SENSOR_3
Sensor on input pad 3.
Definition: dsensor.h:63
unsigned char rom_port6_ddr
ROM shadow of port 6 DDR.
volatile unsigned char AD_CSR
A/D converter control / status register.
ROM Interface: RCX registers cached by ROM functions.
unsigned char ds_activation
activation bitmask
Internal Interface: H8/300 bit operations.
__asm__("\.text\.globl _atomic_inc\ _atomic_inc:\ stc ccr, r1h ; save flags\ orc #0x80, ccr ; disable all but NMI\ mov.b @r0, r1l\ inc r1l\ mov.b r1l, @r0\ ldc r1h, ccr ; restore flags\ rts\")
#define ADCSR_GROUP_0
Definition: h8.h:339
Interface: reduced UNIX standard library.
volatile unsigned AD_A
A/D converter data register A.
#define SENSOR_1
< the raw sensors
Definition: dsensor.h:61
void delay(unsigned ms)
uncalibrated delay loop
Definition: conio.c:204
unsigned char AD_CR
A/D converter control register.
volatile time_t sys_time
current system time in ms
Definition: systime.c:63
void ds_rotation_set(volatile unsigned *sensor, int pos)
set rotation to an absolute value
volatile unsigned AD_C
A/D converter data register C.
void ds_init(void)
initialize sensor a/d conversion
volatile unsigned char PORT6
port 6 I/O register
#define ADCR_EXTERN
Definition: h8.h:351
#define SENSOR_2
Sensor on input pad 2.
Definition: dsensor.h:62
unsigned char ds_rotation
rotation bitmask
Internal LNP Interface: RCX redirected IRQ vectors.
#define bit_set(byte, bit)
set a single bit in memory
Definition: bitops.h:59
void ds_shutdown(void)
shutdown sensor a/d conversion
#define ADCSR_ENABLE_IRQ
Definition: h8.h:333

brickOS is released under the Mozilla Public License.
Original code copyright 1998-2005 by the authors.

Generated for brickOS Kernel Developer by doxygen 1.8.12