66,7 → 66,7 |
#define SERVO_LEN 8 |
#define SERVO_STOP 1 |
|
#define TICK_LEN 1600 /* us */ |
#define TICK_LEN 1600 /* ns */ |
|
struct servo_data { |
int min_angle_sec; |
273,13 → 273,13 |
diff = abs(Baud20MhzData[i+1] - baud); |
if (diff < mindiff) { |
mindiff = diff; |
spbrg = (unsigned char)(Baud20MhzData[i+1]); |
spbrg = (unsigned char)(Baud20MhzData[i]); |
w = 0; |
} |
diff = abs(Baud20MhzData[i+2] - baud); |
if (diff < mindiff) { |
mindiff = diff; |
spbrg = (unsigned char)(Baud20MhzData[i+2]); |
spbrg = (unsigned char)(Baud20MhzData[i]); |
w = 1; |
} |
i += 3; |
386,7 → 386,7 |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = period/TICK_LEN/8 & 0xFF; |
b = (period*1000)/TICK_LEN/8 & 0xFF; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
421,7 → 421,7 |
if (timeout_event != NIL) kern_event_delete(timeout_event); |
|
if (!timer_expired) |
return (res*TICK_LEN*8); |
return (res*TICK_LEN/1000*8); |
else |
return -1; |
|
634,8 → 634,44 |
|
} |
|
/* 1000.0000:0011.0000:mmmm.mmmm */ |
int servo_set_levels(int mask) |
{ |
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 = 0x80; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x30; |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = (unsigned char)(mask & 0xFF); |
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:0100.0000 */ |
int servo_get_servo_levels(void) |
int servo_get_levels(void) |
{ |
struct timespec current_time; |
unsigned char b; |