| 128,7 → 128,7 |
|---|
| {-324000, 324000, 1200, 1600}, |
| {-324000, 324000, 1200, 1600}}}; |
| |
| #define SERVO_TIMEOUT 10000 |
| #define SERVO_TIMEOUT 20000 |
| |
| #define RXTX_BUFF_MAX 100 |
| volatile static BYTE RXTX_buff[4][RXTX_BUFF_MAX]; |
| 157,7 → 157,9 |
|---|
| |
| void servo_rx_error(unsigned port, unsigned type) { |
| |
| kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d)",port,type); |
| #ifdef SERVO_DEBUG |
| kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d)",port,type); |
| #endif |
| |
| } |
| |
| 222,9 → 224,11 |
|---|
| |
| void servo_confirm(unsigned port, BYTE msg_status) { |
| |
| if (msg_status == COM_ERROR) { |
| kern_printf("(SERVO: PORT:%d ERROR:%d)",port,SCom_Error[port]); |
| } |
| #ifdef SERVO_DEBUG |
| if (msg_status == COM_ERROR) { |
| kern_printf("(SCOM: PORT:%d ERROR:%d)",port,SCom_Error[port]); |
| } |
| #endif |
| |
| lock_write[port] = 0; |
| |
| 240,6 → 244,11 |
|---|
| if ((start + SERVO_TIMEOUT) < kern_gettime(NULL)) timeout = 1; |
| |
| if (timeout == 1) { |
| |
| #ifdef SERVO_DEBUG |
| kern_printf("(SERVO: TIMEOUT SEND)"); |
| #endif |
| |
| lock_write[port] = 0; |
| return -1; |
| } |
| 264,7 → 273,9 |
|---|
| TX_cycle[port] = oldcycle; |
| lock_write[port] = 0; |
| |
| kern_printf("(SERVO: BUFFER_FULL !!)"); |
| #ifdef SERVO_DEBUG |
| kern_printf("(SERVO: BUFFER FULL)"); |
| #endif |
| |
| kern_frestore(f); |
| return -1; |
| 287,7 → 298,9 |
|---|
| TX_cycle[port] = oldcycle; |
| lock_write[port] = 0; |
| |
| kern_printf("(SERVO: BUFFER_FULL !!)"); |
| #ifdef SERVO_DEBUG |
| kern_printf("(SERVO: BUFFER FULL)"); |
| #endif |
| |
| kern_frestore(f); |
| return -1; |
| 316,6 → 329,10 |
|---|
| |
| f = kern_fsave(); |
| |
| #ifdef SERVO_DEBUG |
| kern_printf("(SERVO: TIMEOUT GET)"); |
| #endif |
| |
| temp = RX; |
| |
| while(RXTX_addr[port][temp] != SERVO_SKIP) { |