| 43,10 → 43,10 |
|---|
| |
| #include "servo.h" |
| |
| //#define SERVO_DEBUG |
| #define SERVO_DEBUG |
| |
| #define SERVO_SPEED 19200 |
| #define SERVO_PARITY NONE |
| #define SERVO_PARITY EVEN |
| #define SERVO_LEN 8 |
| #define SERVO_STOP 1 |
| |
| 155,10 → 155,10 |
|---|
| 115200, |
| -1}; |
| |
| void servo_rx_error(unsigned port, unsigned type) { |
| void servo_rx_error(unsigned port, unsigned type, unsigned data) { |
| |
| #ifdef SERVO_DEBUG |
| kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d)",port,type); |
| kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d DATA:%02x)",port,type,data); |
| #endif |
| |
| } |
| 166,8 → 166,8 |
|---|
| void servo_indication(unsigned port, BYTE data) { |
| |
| if (RXTX_addr[port][RX_position[port]] == NULL) { |
| if (data != RXTX_buff[port][RX_position[port]]) |
| servo_rx_error(port,1); |
| if (data != 0) |
| servo_rx_error(port,1,data); |
| |
| RXTX_addr[port][RX_position[port]] = SERVO_SKIP; |
| |
| 179,7 → 179,7 |
|---|
| } |
| if ((RX_cycle[port] == TX_cycle[port] && RX_position[port] > TX_position[port]) || |
| (RX_cycle[port] > TX_cycle[port])) |
| servo_rx_error(port,2); |
| servo_rx_error(port,2,data); |
| |
| } else { |
| |
| 198,7 → 198,7 |
|---|
| } |
| 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_rx_error(port,3,data); |
| |
| } else { |
| |
| 210,7 → 210,7 |
|---|
| } |
| 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_rx_error(port,3,data); |
| |
| servo_indication(port, data); |
| |
| 249,7 → 249,6 |
|---|
| kern_printf("(SERVO: TIMEOUT SEND)"); |
| #endif |
| |
| lock_write[port] = 0; |
| return -1; |
| } |
| |