50,72 → 50,16 |
#define MSR 6 |
#define SPad 7 |
|
/* Parity value */ |
#define NONE 0 |
#define ODD 1 |
#define EVEN 3 |
|
#define barrier() __asm__ __volatile__("" ::: "memory"); |
|
#define SERVO_DEBUG |
|
#define SERVO_TIMEOUT 100000 /* us */ |
|
#define SERVO_PORT 2 |
#define SERVO_SPEED 38400 |
#define SERVO_PARITY NONE |
#define SERVO_LEN 8 |
#define SERVO_STOP 1 |
#define SERVO_PORT 1 |
|
int timer_expired = 0; |
int timeout_event; |
unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
|
const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
|
const int BaudTable[] = { |
1200, |
2400, |
4800, |
9600, |
14400, |
19200, |
38400, |
57600, |
115200, |
230400, |
-1}; |
|
/* 20MHz: SPBRG, BAUD BRGH=0, BAUD BRGH=1 */ |
const int Baud20MhzData[] = { |
255, 1221, 4882, |
254, 1225, 4902, |
253, 1230, 4921, |
252, 1235, 4941, |
133, 2332, 9328, |
132, 2350, 9398, |
131, 2367, 9470, |
130, 2385, 9542, |
129, 2404, 9615, |
128, 2422, 9690, |
127, 2441, 9766, |
126, 2461, 9842, |
88, 0, 14045, |
87, 0, 14204, |
86, 0, 14368, |
85, 0, 14535, |
84, 0, 14706, |
66, 4664, 18657, |
65, 4735, 18939, |
64, 4808, 19231, |
63, 4883, 19531, |
32, 9470, 37879, |
31, 9766, 39062, |
21, 14205, 56818, |
15, 19531, 0, |
10, 0, 113636, |
7, 39062, 0, |
-1, -1, -1}; |
|
void set_timer_expired(void *arg) |
{ |
timeout_event = NIL; |
159,121 → 103,30 |
} |
} |
|
int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop) |
{ |
unsigned long div,b_mask; |
|
/* Now set up the serial link */ |
b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3); |
div = 115200L / speed; |
/* Clear serial interrupt enable register */ |
com_write(port,IER,0); |
/* Empty input buffer */ |
com_read(port,RBR); |
/* Activate DLAB bit for speed setting */ |
com_write(port,LCR,0x80); |
/* Load baud divisor */ |
com_write(port,0,div & 0x00FF); |
div >>= 8; |
com_write(port,1,div & 0x00FF); |
/* Load control word (parity,stop bit,bit len) */ |
com_write(port,LCR,b_mask); |
/* Attiva OUT1 & OUT2 */ |
com_write(port,MCR,0x0C); |
|
return 0; |
} |
|
int com_close(unsigned port) |
{ |
|
com_write(port,IER,0); |
com_read(port,IIR); |
com_read(port,RBR); |
|
return 0; |
|
} |
|
int servo_open(void) |
{ |
int err; |
|
err = com_open(SERVO_PORT, SERVO_SPEED, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
|
return err; |
|
} |
|
int servo_close(void) |
{ |
int err; |
|
err = com_close(SERVO_PORT); |
|
return err; |
|
} |
|
/* 1000.011w:bbbb.bbbb */ |
/* 1000.0100:bbbb.bbbb */ |
int servo_set_RS232_baudrate(int baud) |
{ |
struct timespec current_time; |
unsigned char b; |
int err,diff,mindiff,i; |
unsigned char spbrg, w; |
|
i = 0; |
while(BaudTable[i] != baud && BaudTable[i] != -1) i++; |
if (BaudTable[i] == -1) { |
kern_printf("SERVO:Error wrong baud rate\n"); |
return -1; |
} |
|
i = 0; |
mindiff = 0xFFFFFF; |
spbrg = 0; |
w = 0; |
while(Baud20MhzData[i] != -1) { |
diff = abs(Baud20MhzData[i+1] - baud); |
if (diff < mindiff) { |
mindiff = diff; |
spbrg = (unsigned char)(Baud20MhzData[i+1]); |
w = 0; |
} |
diff = abs(Baud20MhzData[i+2] - baud); |
if (diff < mindiff) { |
mindiff = diff; |
spbrg = (unsigned char)(Baud20MhzData[i+2]); |
w = 1; |
} |
i += 3; |
} |
|
#ifdef SERVO_DEBUG |
kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w); |
#endif |
|
int err; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x86 | (w & 0x01); |
b = 0x84; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) timer_expired = 1; |
|
b = spbrg; |
b = baud & 0xFF; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) timer_expired = 1; |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
com_close(SERVO_PORT); |
com_open(SERVO_PORT, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
|
|
if (!timer_expired) |
return 0; |
else |
308,7 → 161,7 |
|
} |
|
/* 1000.0100 */ |
/* 1000.0110 */ |
int servo_store_RS232_buadrate(void) |
{ |
struct timespec current_time; |
320,7 → 173,7 |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x84; |
b = 0x86; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) timer_expired = 1; |