Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 725 → Rev 726

/shark/trunk/ports/servo/servo.c
129,8 → 129,8
{-324000, 324000, 1200, 1600}}};
 
#define RXTX_BUFF_MAX 100
static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
static BYTE *RXTX_addr[4][RXTX_BUFF_MAX];
volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
volatile static BYTE *RXTX_addr[4][RXTX_BUFF_MAX];
volatile unsigned int RX_position[4] = {0,0,0,0};
volatile unsigned int TX_position[4] = {0,0,0,0};
volatile unsigned int RX_cycle = 0;
159,6 → 159,7
if (RXTX_addr[port][RX_position[port]] == NULL) {
if (data != RXTX_buff[port][RX_position[port]])
servo_rx_error(port,1);
 
RX_position[port]++;
 
if (RX_position[port] >= RXTX_BUFF_MAX) {
171,6 → 172,8
 
} else {
*RXTX_addr[port][RX_position[port]] = data;
RXTX_buff[port][RX_position[port]] = 1; //Unlock the data
 
RX_position[port]++;
 
if (RX_position[port] >= RXTX_BUFF_MAX) {
180,6 → 183,7
if ((RX_cycle == TX_cycle && RX_position[port] > TX_position[port]) ||
(RX_cycle > TX_cycle))
servo_rx_error(port,2);
 
}
 
}
193,7 → 197,7
 
int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) {
 
int i = 0, old, oldcycle, RX;
int i = 0, old, oldcycle, RX = 0;
SYS_FLAGS f;
 
f = kern_fsave();
220,8 → 224,9
 
i = 0;
while(i < len_res) {
RXTX_buff[port][TX_position[port]] = 0;
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]++;
if (TX_position[port] >= RXTX_BUFF_MAX) {
TX_cycle++;
236,8 → 241,6
i++;
}
 
RX = (int)RX_position;
 
kern_frestore(f);
 
com_irq_send(port, len_msg, msg);
246,18 → 249,10
 
}
 
int servo_wait(unsigned port, int RX, int number) {
int servo_wait(unsigned port, int RX) {
 
int f = RX, i = 0;
 
while (i < number) {
f++;
if (f >= RXTX_BUFF_MAX) f = 0;
i++;
}
 
/* Active wait until number bytes received */
while (RX_position[port] < f);
while (RXTX_buff[port][RX] == 0);
 
return 0;
 
340,7 → 335,7
b[0] = 0x85;
RX = servo_send_msg(servo_port, b, 1, r, 2);
 
servo_wait(servo_port, RX, 2);
servo_wait(servo_port, RX);
 
res_w = r[0]; /* bit W */
res_b = r[1]; /* byte SPBRG */
395,7 → 390,7
b[0] = 0x89;
RX = servo_send_msg(servo_port, b, 1, r, 1);
 
servo_wait(servo_port, RX, 1);
servo_wait(servo_port, RX);
 
res = r[0];
 
426,7 → 421,7
b[0] = 0x8C;
RX = servo_send_msg(servo_port, b, 1, r, 1);
 
servo_wait(servo_port, RX, 1);
servo_wait(servo_port, RX);
 
res = r[0];
 
533,7 → 528,7
b[1] = 0x40 | (0x01 & bank);
RX = servo_send_msg(servo_port, b, 2, r, 1);
servo_wait(servo_port, RX, 1);
servo_wait(servo_port, RX);
 
res = r[0];
 
629,9 → 624,9
b[0] = 0x10 | (servo & 0x0F);
RX = servo_send_msg(servo_port, b, 1, r, 2);
 
servo_wait(servo_port, RX, 2);
servo_wait(servo_port, RX);
 
res = r[0] << 8;
res = (int)(r[0]) << 8;
res |= r[1];
 
data = ((res*TICK_LEN/1000) - servo_table[port][servo].zero_tick) *
654,7 → 649,7
b[0] = 0x40 | (adport & 0x07);
RX = servo_send_msg(servo_port, b, 1, r, 2);
 
servo_wait(servo_port, RX, 2);
servo_wait(servo_port, RX);
 
res = r[0] << 8;
res |= r[1];