Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 733 → Rev 732

/shark/trunk/ports/servo/servo.c
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];