Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 720 → Rev 721

/shark/trunk/ports/servo/servo.c
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(&current_time);
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
timeout_event = kern_event_post(&current_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 */