57,13 → 57,13 |
#define SERVO_PORT 1 |
|
int timer_expired = 0; |
int timeout_event; |
unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
|
void set_timer_expired(void *arg) |
{ |
|
timeout_event = NIL; |
timer_expired = 1; |
|
} |
|
unsigned com_read(unsigned port,unsigned reg) |
146,17 → 146,55 |
|
} |
|
/* 1000.1100 */ |
int servo_get_setup_switch(void) |
{ |
struct timespec current_time; |
unsigned char b; |
int err,res; |
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
return 0; |
b = 0x8C; |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) timer_expired = 1; |
res = com_receive(SERVO_PORT); |
|
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return res; |
else |
return -1; |
|
} |
|
/* 1000.111s */ |
int servo_set_RC5_switch(int data) |
{ |
struct timespec current_time; |
unsigned char b; |
int err; |
|
timer_expired = 0; |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
|
b = 0x8E | (data & 0x01); |
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; |
|
} |
|
166,7 → 204,7 |
|
struct timespec current_time; |
unsigned char b; |
int err,timeout_event; |
int err; |
|
if (servo > 7) return -1; |
|
178,21 → 216,24 |
b = 0x00 | (servo & 0x07); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) return -1; |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | ((angle >> 8) & 0x0F); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) return -1; |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | (angle & 0xFF); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) return -1; |
if (err != (int)(b)) timer_expired = 1; |
|
kern_event_delete(timeout_event); |
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return 0; |
else |
return -1; |
|
} |
|
199,10 → 240,9 |
/* 0000.1ppp */ |
int servo_get_angle(int servo) |
{ |
|
struct timespec current_time; |
unsigned char b; |
int err,res,timeout_event; |
int err,res; |
|
if (servo > 7) return -1; |
|
214,10 → 254,11 |
b = 0x08 | (servo & 0x07); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) return -1; |
res = com_receive(SERVO_PORT); |
if (err != (int)(b)) timer_expired = 1; |
res = com_receive(SERVO_PORT) << 8; |
res |= com_receive(SERVO_PORT); |
|
kern_event_delete(timeout_event); |
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return res; |
232,7 → 273,7 |
|
struct timespec current_time; |
unsigned char b; |
int err,res,timeout_event; |
int err,res; |
|
if (port > 4) return -1; |
|
244,11 → 285,11 |
b = 0x40 | (port & 7); |
err = com_send(SERVO_PORT, b); |
err = com_receive(SERVO_PORT); |
if (err != (int)(b)) return -1; |
if (err != (int)(b)) timer_expired = 1; |
res = com_receive(SERVO_PORT) << 8; |
res |= com_receive(SERVO_PORT); |
|
kern_event_delete(timeout_event); |
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return res; |