128,7 → 128,7 |
{-324000, 324000, 1200, 1600}, |
{-324000, 324000, 1200, 1600}}}; |
|
#define SERVO_TIMEOUT 10000 |
//#define SERVO_TIMEOUT 10000 |
|
#define RXTX_BUFF_MAX 100 |
volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX]; |
137,12 → 137,10 |
volatile unsigned int TX_position[4] = {0,0,0,0}; |
volatile unsigned int RX_cycle[4] = {0,0,0,0}; |
volatile unsigned int TX_cycle[4] = {0,0,0,0}; |
volatile unsigned int servo_skip[4] = {0,0,0,0}; |
volatile unsigned int lock_write[4] = {0,0,0,0}; |
volatile int servo_timer[4] = {NIL,NIL,NIL,NIL}; |
|
#define SERVO_LOCK 0 |
#define SERVO_UNLOCK 1 |
#define SERVO_SKIP (BYTE *)(0xFFFFFFFF) |
|
const int BaudTable[] = { |
1200, |
2400, |
167,8 → 165,6 |
if (data != RXTX_buff[port][RX_position[port]]) |
servo_rx_error(port,1); |
|
RXTX_addr[port][RX_position[port]] = SERVO_SKIP; |
|
RX_position[port]++; |
|
if (RX_position[port] >= RXTX_BUFF_MAX) { |
180,39 → 176,18 |
servo_rx_error(port,2); |
|
} else { |
*RXTX_addr[port][RX_position[port]] = data; |
RXTX_buff[port][RX_position[port]] = 1; //Unlock the data |
|
if (RXTX_addr[port][RX_position[port]] != SERVO_SKIP) { |
RX_position[port]++; |
|
*RXTX_addr[port][RX_position[port]] = data; |
RXTX_buff[port][RX_position[port]] = SERVO_UNLOCK; //Unlock the data |
|
RXTX_addr[port][RX_position[port]] = SERVO_SKIP; |
|
RX_position[port]++; |
|
if (RX_position[port] >= RXTX_BUFF_MAX) { |
if (RX_position[port] >= RXTX_BUFF_MAX) { |
RX_cycle[port]++; |
RX_position[port] = 0; |
} |
if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
(RX_cycle[port] > TX_cycle[port])) |
servo_rx_error(port,3); |
|
} else { |
|
RX_position[port]++; |
|
if (RX_position[port] >= RXTX_BUFF_MAX) { |
RX_cycle[port]++; |
RX_position[port] = 0; |
} |
if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
(RX_cycle[port] > TX_cycle[port])) |
servo_rx_error(port,3); |
|
servo_indication(port, data); |
|
} |
if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
(RX_cycle[port] > TX_cycle[port])) |
servo_rx_error(port,3); |
|
} |
|
233,17 → 208,9 |
int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) { |
|
int i = 0, old, oldcycle, RX = 0; |
volatile int start = kern_gettime(NULL), timeout = 0; |
SYS_FLAGS f; |
|
while(lock_write[port] == 1 && timeout == 0) |
if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1; |
|
if (timeout == 1) { |
lock_write[port] = 0; |
return -1; |
} |
|
while(lock_write[port] == 1); |
lock_write[port] = 1; |
|
f = kern_fsave(); |
263,9 → 230,6 |
TX_position[port] = old; |
TX_cycle[port] = oldcycle; |
lock_write[port] = 0; |
|
kern_printf("(SERVO: BUFFER_FULL !!)"); |
|
kern_frestore(f); |
return -1; |
} |
274,7 → 238,7 |
|
i = 0; |
while(i < len_res) { |
RXTX_buff[port][TX_position[port]] = SERVO_LOCK; //Lock the data |
RXTX_buff[port][TX_position[port]] = 0; //Lock the data |
RXTX_addr[port][TX_position[port]] = res+i; |
RX = TX_position[port]; |
TX_position[port]++; |
286,9 → 250,6 |
TX_position[port] = old; |
TX_cycle[port] = oldcycle; |
lock_write[port] = 0; |
|
kern_printf("(SERVO: BUFFER_FULL !!)"); |
|
kern_frestore(f); |
return -1; |
} |
303,31 → 264,39 |
|
} |
|
int servo_wait(unsigned port, int RX) { |
void servo_timeout(void *arg) { |
|
volatile int start = kern_gettime(NULL), timeout = 0, temp; |
unsigned port = (unsigned)(arg); |
|
/* Active wait until number bytes received */ |
while(RXTX_buff[port][RX] == 0 && timeout == 0) |
if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1; |
servo_timer[port] = NIL; |
servo_skip[port] = 1; |
|
if (timeout == 1) { |
SYS_FLAGS f; |
kern_printf("(SERVO: TIMEOUT PORT:%d)",port); |
|
} |
|
f = kern_fsave(); |
int servo_wait(unsigned port, int RX) { |
|
temp = RX; |
#ifdef SERVO_TIMEOUT |
struct timespec acttime; |
#endif |
servo_skip[port] = 0; |
|
while(RXTX_addr[port][temp] != SERVO_SKIP) { |
RXTX_addr[port][temp] = SERVO_SKIP; |
temp--; |
} |
#ifdef SERVO_TIMEOUT |
kern_gettime(&acttime); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&acttime); |
servo_timer[port] = kern_event_post(&acttime,servo_timeout,(void *)(port)); |
#endif |
|
kern_frestore(f); |
/* Active wait until number bytes received */ |
while (RXTX_buff[port][RX] == 0 && servo_skip[port] == 0); |
|
} |
#ifdef SERVO_TIMEOUT |
if (servo_timer[port] != NIL) |
kern_event_delete(servo_timer[port]); |
#endif |
|
return timeout; |
return 0; |
|
} |
|
401,7 → 370,7 |
int servo_get_RS232_baudrate(int port) |
{ |
unsigned char b[1],r[2]; |
int res, res_w, res_b, t; |
int res, res_w, res_b; |
int servo_port = (unsigned)(port); |
int RX; |
|
410,9 → 379,9 |
|
if (RX != -1) { |
|
t = servo_wait(servo_port, RX); |
servo_wait(servo_port, RX); |
|
if (t == 0) { |
if (servo_skip[port] == 0) { |
|
res_w = r[0]; /* bit W */ |
res_b = r[1]; /* byte SPBRG */ |
465,7 → 434,7 |
int servo_get_period(int port) |
{ |
unsigned char b[1],r[1]; |
int res, RX, t; |
int res, RX; |
int servo_port = (unsigned)(port); |
|
b[0] = 0x89; |
473,9 → 442,9 |
|
if (RX != -1) { |
|
t = servo_wait(servo_port, RX); |
servo_wait(servo_port, RX); |
|
if (t == 0) { |
if (servo_skip[port] == 0) { |
|
res = r[0]; |
|
504,7 → 473,7 |
int servo_get_setup_switch(int port) |
{ |
unsigned char b[1],r[1]; |
int res, RX, t; |
int res, RX; |
int servo_port = (unsigned)(port); |
|
b[0] = 0x8C; |
512,9 → 481,9 |
|
if (RX != -1) { |
|
t = servo_wait(servo_port, RX); |
servo_wait(servo_port, RX); |
|
if (t == 0) { |
if (servo_skip[port] == 0) { |
|
res = r[0]; |
|
618,7 → 587,7 |
int servo_get_levels(int port, int bank) |
{ |
unsigned char b[2],r[1]; |
int res, RX, t; |
int res, RX; |
int servo_port = (unsigned)(port); |
|
b[0] = 0x80; |
627,9 → 596,9 |
|
if (RX != -1) { |
|
t = servo_wait(servo_port, RX); |
servo_wait(servo_port, RX); |
|
if (t == 0) { |
if (servo_skip[port] == 0) { |
|
res = r[0]; |
|
722,7 → 691,7 |
int servo_get_angle_sec(int port, int servo) |
{ |
unsigned char b[1],r[2]; |
int res,data,RX,t; |
int res,data,RX; |
int servo_port = (unsigned)(port); |
|
if (servo > 15) return -1; |
732,9 → 701,9 |
|
if (RX != -1) { |
|
t = servo_wait(servo_port, RX); |
servo_wait(servo_port, RX); |
|
if (t == 0) { |
if (servo_skip[port] == 0) { |
|
res = (int)(r[0]) << 8; |
res |= r[1]; |
755,7 → 724,7 |
int servo_get_analog(int port, int adport) |
{ |
unsigned char b[1],r[2]; |
int res, RX, t; |
int res, RX; |
int servo_port = (unsigned)(port); |
|
if (adport > 7) return -1; |
765,9 → 734,9 |
|
if (RX != -1) { |
|
t = servo_wait(servo_port, RX); |
servo_wait(servo_port, RX); |
|
if (t == 0) { |
if (servo_skip[port] == 0) { |
|
res = (int)r[0] << 8; |
res |= r[1]; |