Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 285 → Rev 286

/shark/trunk/ports/servo/servo.c
50,16 → 50,72
#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 1
#define SERVO_PORT 2
#define SERVO_SPEED 38400
#define SERVO_PARITY NONE
#define SERVO_LEN 8
#define SERVO_STOP 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;
103,30 → 159,121
}
}
 
/* 1000.0100:bbbb.bbbb */
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 */
int servo_set_RS232_baudrate(int baud)
{
struct timespec current_time;
unsigned char b;
int err;
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
timer_expired = 0;
kern_gettime(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x84;
b = 0x86 | (w & 0x01);
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
 
b = baud & 0xFF;
b = spbrg;
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
161,7 → 308,7
 
}
 
/* 1000.0110 */
/* 1000.0100 */
int servo_store_RS232_buadrate(void)
{
struct timespec current_time;
173,7 → 320,7
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
b = 0x86;
b = 0x84;
err = com_send(SERVO_PORT, b);
err = com_receive(SERVO_PORT);
if (err != (int)(b)) timer_expired = 1;
/shark/trunk/ports/servo/include/servo.h
16,7 → 16,7
 
/* Servo control */
 
#define DEG180 0xFFFF
#define DEG180 0xFFFFFF
#define SERVO_ANGLE(deg,min,sec) (((deg)*3600 + (min)*60 + (sec)) * DEG180 / 648000)
 
int servo_turn_off(int servo);