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; |
} |
|