61 #define DS_ALL_ACTIVE 0x07
62 #define DS_ALL_PASSIVE (~DS_ALL_ACTIVE)
70 volatile unsigned char ds_channel;
74 #ifdef CONF_DSENSOR_ROTATION
79 static signed char rotation_state[3];
80 static signed char rotation_new_state[3];
81 static unsigned int state_duration[3];
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];
96 static const signed char ad2state[16]={
98 -1,-1,-1,-1,-1, 2, 2, 2, 3, 3, 3, 3, 1, 1, 1, 0
112 static const signed char diff2change[7]={
130 if(sensor>=&
AD_A && sensor<=&
AD_C) {
131 unsigned channel=(unsigned) (sensor-&
AD_A);
132 signed char state=ad2state[(*sensor)>>12];
137 rotation_state[channel]=state;
138 rotation_new_state[channel] = -1;
139 state_duration[channel]=0;
148 void ds_rotation_handler() {
149 unsigned channel =ds_channel;
150 unsigned raw =(*((&
AD_A)+channel));
151 signed char newstate=ad2state[raw>>12];
156 if (newstate == rotation_new_state[channel]) {
157 if (++state_duration[channel] == 2) {
158 signed char change = diff2change[newstate - rotation_state[channel] + 3];
162 #ifdef CONF_DSENSOR_VELOCITY
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;
174 if (time == last_rotation[channel])
175 ds_velocities[channel] = 1000 * change;
177 unsigned int time_diff = (time - last_rotation[channel]);
178 if (time_diff > 1000) {
179 rotation_dir[channel] = 0;
180 ds_velocities[channel] = 0;
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;
192 rotation_state[channel] = newstate;
193 rotation_new_state[channel] = -1;
195 }
else if (newstate != rotation_state[channel]) {
196 rotation_new_state[channel] = newstate;
197 state_duration[channel] = 1;
198 #ifdef CONF_DSENSOR_VELOCITY
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;
209 int speed = 1000 / time_diff;
210 ds_velocities[channel] = rotation_dir[channel] > 0 ? speed : -speed;
211 next_rotation[channel] = time + time_diff / 2;
218 #endif // CONF_DSENSOR_ROTATION
220 #ifdef CONF_DSENSOR_MUX
221 unsigned char ds_mux;
223 volatile int ds_muxs[3][3];
227 #define DS_MUX_PULSE_TM_MS 10
232 unsigned long nextTm;
235 unsigned int attached[3];
239 enum {ds_mux_prepRead,
242 ds_mux_pulse_high} action;
245 ds_mux_data_t ds_mux_data[3];
247 #endif //CONF_DSENSOR_MUX
251 static inline void ds_power_on(
unsigned channel) {
268 static inline void ds_power_off(
unsigned channel) {
285 #ifdef CONF_DSENSOR_MUX
289 void ds_mux_on(
volatile unsigned *sensor,
318 ds_mux_data[i].attached[0]=ch1;
319 ds_mux_data[i].attached[1]=ch2;
320 ds_mux_data[i].attached[2]=ch3;
330 ds_mux_data[i].attached[0]+=160;
332 ds_mux_data[i].attached[1]+=135;
334 ds_mux_data[i].attached[2]+=25;
355 for(j=0;j<3 && ds_mux_data[i].attached[j]==0;j++);
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;
377 void ds_mux_handler() {
378 unsigned sen=ds_channel;
381 if(ds_mux_data[sen].nextTm <=
sys_time) {
384 switch(ds_mux_data[sen].action) {
385 case ds_mux_prepRead:
387 ds_mux_data[sen].action=ds_mux_read;
394 ds_muxs[sen][(int)ds_mux_data[sen].channel]=
SENSOR_3;
397 ds_muxs[sen][(int)ds_mux_data[sen].channel]=
SENSOR_2;
400 ds_muxs[sen][(int)ds_mux_data[sen].channel]=
SENSOR_1;
409 ds_mux_data[sen].channel++;
410 if(ds_mux_data[sen].channel>2) {
411 ds_mux_data[sen].channel=0;
417 (ds_mux_data[sen].attached
418 [(
int)ds_mux_data[sen].channel])==0);
423 ds_mux_data[sen].remainingEdges=
424 ((ds_mux_data[sen].channel+1)*2)-1;
427 ds_mux_data[sen].action=ds_mux_pulse_high;
428 ds_mux_data[sen].nextTm=
sys_time+DS_MUX_PULSE_TM_MS;
430 case ds_mux_pulse_low:
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;
438 case ds_mux_pulse_high:
441 ds_mux_data[sen].remainingEdges--;
443 if(ds_mux_data[sen].remainingEdges==0) {
446 ds_mux_data[sen].action=ds_mux_prepRead;
457 ds_mux_data[sen].attached[(int)ds_mux_data[sen].channel];
463 ds_mux_data[sen].action=ds_mux_pulse_low;
464 ds_mux_data[sen].nextTm=
sys_time+DS_MUX_PULSE_TM_MS;
474 #endif //CONF_DSENSOR_MUX
481 extern void ds_handler(
void);
482 #ifndef DOXYGEN_SHOULD_SKIP_THIS
489 mov.b @_ds_channel,r6l ; r6l = current channel\n\
491 mov.b @_ds_activation,r6h ; r6h = activation bitmask\n\
492 btst r6l,r6h ; activate output?\n\
494 bset r6l,@_PORT6:8 ; activate output of last port scanned\n\
499 mov.b @_ds_rotation,r6h ; r6h = rotation bitmask\n\
500 btst r6l,r6h ; process rotation sensor?\n\
503 push r0 ; save r0..r3\n\
506 push r3 ; r4..r6 saved by gcc if necessary\n\
508 jsr _ds_rotation_handler ; process rotation sensor\n\
518 #ifdef CONF_DSENSOR_MUX
520 mov.b @_ds_mux,r6h ; r6h = mux bitmask\n\
521 btst r6l,r6h ; process mux sensor?\n\
524 push r0 ; save r0..r3\n\
527 push r3 ; r4..r6 saved by gcc if necessary\n\
529 jsr _ds_mux_handler ; process mux sensor\n\
539 inc r6l ; next channel\n\
540 and #0x03,r6l ; limit to 0-3\n\
542 mov.b @_ds_activation,r6h ; r6h = activation bitmask\n\
543 btst r6l,r6h ; activate output?\n\
545 bclr r6l,@_PORT6:8 ; set output inactive for reading\n\
548 ; The settle time for reading the value from active sensor start here\n\
550 ; moved here for helping timing problems\n\
551 mov.b r6l,@_ds_channel ; store next channel\n\
553 ; Added a delay loop for sensor settle time\n\
555 mov.b #0x04, r6h ; delay loop\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\
561 ; Total loop delay 32 states (?)\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\
568 ; The settle time for reading the value from active sensor finish here\n\
570 bset #0x5,@_AD_CSR:8 ; go!\n\
574 #endif // DOXYGEN_SHOULD_SKIP_THIS
588 #ifdef CONF_DSENSOR_ROTATION
592 #ifdef CONF_DSENSOR_MUX
604 # warning "Rotation initialization might fail."
615 PORT6 &=DS_ALL_PASSIVE;
620 #endif // CONF_DSENSOR