140,6 → 140,8 |
static BYTE *RXTX_addr[4][RXTX_BUFF_MAX]; |
unsigned int RX_position[4] = {0,0,0,0}; |
unsigned int TX_position[4] = {0,0,0,0}; |
unsigned int RX_cycle = 0; |
unsigned int TX_cycle = 0; |
|
const int BaudTable[] = { |
1200, |
159,58 → 161,71 |
timer_expired = 1; |
} |
|
void servo_rx_error(unsigned port) { |
void servo_rx_error(unsigned port, unsigned type) { |
|
kern_printf("SERVO: RX ERROR COM%d\n",port); |
kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d)",port,type); |
|
} |
|
void servo_indication(unsigned port, BYTE data) { |
|
|
if (RXTX_addr[port][RX_position[port]] == NULL) { |
if (data != RXTX_buff[port][RX_position[port]]) |
servo_rx_error(port); |
servo_rx_error(port,1); |
RX_position[port]++; |
if (RX_position[port] >= RXTX_BUFF_MAX) RX_position[port] = 0; |
if (TX_position[port] == RX_position[port]) |
servo_rx_error(port); |
|
if (RX_position[port] >= RXTX_BUFF_MAX) { |
RX_cycle++; |
RX_position[port] = 0; |
} |
if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) || |
(RX_cycle > TX_cycle)) |
servo_rx_error(port,2); |
|
} else { |
*RXTX_addr[port][RX_position[port]] = data; |
RX_position[port]++; |
if (RX_position[port] >= RXTX_BUFF_MAX) RX_position[port] = 0; |
if (TX_position[port] == RX_position[port]) |
servo_rx_error(port); |
|
if (RX_position[port] >= RXTX_BUFF_MAX) { |
RX_cycle++; |
RX_position[port] = 0; |
} |
if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) || |
(RX_cycle > TX_cycle)) |
servo_rx_error(port,2); |
} |
|
cprintf("(TXRX:%d:%d)",TX_position[port],RX_position[port]); |
|
} |
|
void servo_confirm(unsigned port, BYTE msg_status) { |
|
#ifdef SERVO_DEBUG |
if (msg_status == COM_ERROR) |
kern_printf("(SERVO:COM%d ERROR)",port); |
#endif |
kern_printf("(SERVO: PORT:%d ERROR)",port); |
|
} |
|
int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) { |
|
int i = 0, old; |
|
int i = 0, old, oldcycle; |
SYS_FLAGS f; |
|
f = kern_fsave(); |
|
old = TX_position[port]; |
oldcycle = TX_cycle; |
|
cprintf("(SEND:%d:%d)",len_msg,len_res); |
|
while(i < len_msg) { |
RXTX_buff[port][TX_position[port]] = msg[i]; |
RXTX_addr[port][TX_position[port]] = NULL; |
TX_position[port]++; |
if (TX_position[port] >= RXTX_BUFF_MAX) TX_position[port] = 0; |
if (TX_position[port] >= RXTX_BUFF_MAX) { |
TX_cycle++; |
TX_position[port] = 0; |
} |
if (TX_position[port] == RX_position[port]) { |
TX_position[port] = old; |
cprintf("Error\n"); |
TX_cycle = oldcycle; |
kern_frestore(f); |
return -1; |
} |
i++; |
221,19 → 236,23 |
RXTX_buff[port][TX_position[port]] = 0; |
RXTX_addr[port][TX_position[port]] = res+i; |
TX_position[port]++; |
if (TX_position[port] >= RXTX_BUFF_MAX) TX_position[port] = 0; |
if (TX_position[port] >= RXTX_BUFF_MAX) { |
TX_cycle++; |
TX_position[port] = 0; |
} |
if (TX_position[port] == RX_position[port]) { |
TX_position[port] = old; |
cprintf("Error\n"); |
TX_cycle = oldcycle; |
kern_frestore(f); |
return -1; |
} |
i++; |
} |
|
kern_frestore(f); |
|
com_irq_send(port, len_msg, msg); |
|
cprintf("(TXRX:%d:%d)",TX_position[port],RX_position[port]); |
|
return 0; |
|
} |
248,6 → 267,8 |
|
com_set_functions(servo_confirm,servo_indication); |
|
com_irq_enable((unsigned)(port),ALL_IRQ); |
|
return err; |
|
} |
628,42 → 649,17 |
int servo_turn_on(int port, int servo) |
{ |
|
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err; |
unsigned char b[2]; |
int servo_port = (unsigned)(port); |
|
if (servo > 15) return -1; |
|
timer_expired = 0; |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
b[0] = 0x80; |
b[1] = 0x10 | (servo & 0x0F); |
servo_send_msg(servo_port, b, 2, NULL, 0); |
|
b = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
return 0; |
|
b = 0x10 | (servo & 0x0F); |
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); |
|
if (!timer_expired) |
return 0; |
else |
return -1; |
|
} |
|
/* 1000.0000:0010.0000 */ |