Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 737 → Rev 738

/shark/trunk/ports/servo/servo.c
128,7 → 128,7
{-324000, 324000, 1200, 1600},
{-324000, 324000, 1200, 1600}}};
 
#define SERVO_TIMEOUT 20000
#define SERVO_TIMEOUT 1000000
 
#define RXTX_BUFF_MAX 100
volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
166,9 → 166,23
void servo_indication(unsigned port, BYTE data) {
 
if (RXTX_addr[port][RX_position[port]] == NULL) {
if (data != 0)
if (data != 0) {
servo_rx_error(port,1,data);
while(RXTX_addr[port][RX_position[port]] != SERVO_SKIP) {
RXTX_addr[port][RX_position[port]] = SERVO_SKIP;
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,2,data);
 
}
}
 
RXTX_addr[port][RX_position[port]] = SERVO_SKIP;
 
RX_position[port]++;
307,6 → 321,26
i++;
}
 
/* SERVO_SKIP close the command sequence */
RXTX_addr[port][TX_position[port]] = SERVO_SKIP;
TX_position[port]++;
if (TX_position[port] >= RXTX_BUFF_MAX) {
TX_cycle[port]++;
TX_position[port] = 0;
}
if (TX_position[port] == RX_position[port]) {
TX_position[port] = old;
TX_cycle[port] = oldcycle;
lock_write[port] = 0;
#ifdef SERVO_DEBUG
kern_printf("(SERVO: BUFFER FULL)");
#endif
kern_frestore(f);
return -1;
}
 
kern_frestore(f);
 
com_irq_send(port, len_msg, msg);
320,25 → 354,27
volatile int start = kern_gettime(NULL), timeout = 0, temp;
 
/* Active wait until number bytes received */
while(RXTX_buff[port][RX] == 0 && timeout == 0)
while(RXTX_buff[port][RX] == SERVO_LOCK && timeout == 0)
if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1;
 
if (timeout == 1) {
SYS_FLAGS f;
 
f = kern_fsave();
 
#ifdef SERVO_DEBUG
kern_printf("(SERVO: TIMEOUT GET)");
#endif
 
f = kern_fsave();
 
temp = RX;
 
while(RXTX_addr[port][temp] != SERVO_SKIP) {
RXTX_addr[port][temp] = SERVO_SKIP;
RXTX_buff[port][temp] = SERVO_UNLOCK;
temp--;
if (temp < 0) temp = RXTX_BUFF_MAX-1;
}
 
kern_frestore(f);
 
}