38,26 → 38,12 |
|
#include <ll/sys/ll/ll-instr.h> |
#include "kernel/kern.h" |
#include "servo.h" |
|
#define THR 0 |
#define RBR 0 |
#define IER 1 |
#define FCR 2 |
#define IIR 2 |
#define LCR 3 |
#define MCR 4 |
#define LSR 5 |
#define MSR 6 |
#define SPad 7 |
#include "drivers/scom.h" |
#include "drivers/scomirq.h" |
|
/* Parity value */ |
#define NONE 0 |
#define ODD 1 |
#define EVEN 3 |
#include "servo.h" |
|
#define barrier() __asm__ __volatile__("" ::: "memory"); |
|
//#define SERVO_DEBUG |
//#define SERVO_TIMEOUT_EVENT |
|
149,7 → 135,11 |
int timer_expired = 0; |
int timeout_event; |
|
const unsigned com_base[] = {0x03F8,0x02F8,0x03E8,0x02E8}; |
#define RXTX_BUFF_MAX 100 |
static BYTE RXTX_buff[4][RXTX_BUFF_MAX]; |
static BYTE *RXTX_addr[4][RXTX_BUFF_MAX]; |
unsigned int RX_position[4] = {0,0,0,0}; |
unsigned int TX_position[4] = {0,0,0,0}; |
|
const int BaudTable[] = { |
1200, |
169,91 → 159,81 |
timer_expired = 1; |
} |
|
unsigned com_read(unsigned port,unsigned reg) |
{ |
unsigned b; |
if (port > 3 || reg > 7) return(0); |
b = ll_in(com_base[port]+reg); |
return(b); |
} |
void servo_rx_error(unsigned port) { |
|
void com_write(unsigned port,unsigned reg,unsigned value) |
{ |
if (port > 3 || reg > 7) return; |
ll_out(com_base[port]+reg,value); |
kern_printf("SERVO: RX ERROR COM%d\n",port); |
|
} |
|
int com_send(unsigned port,BYTE b) |
{ |
while ((com_read(port,LSR) & 32) == 0 && !timer_expired) |
barrier(); |
if (!timer_expired) { |
#ifdef SERVO_DEBUG |
kern_printf("(SERVO WRITE p = %d b = %02x)",port,b); |
#endif |
com_write(port,THR,b); |
return 0; |
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); |
RX_position[port]++; |
if (RX_position[port] >= RXTX_BUFF_MAX) RX_position[port] = 0; |
if (TX_position[port] == RX_position[port]) |
servo_rx_error(port); |
} else { |
#ifdef SERVO_DEBUG |
kern_printf("(WRITE TIMEOUT)"); |
#endif |
return -1; |
*RXTX_addr[port][RX_position[port]] = data; |
RX_position[port]++; |
if (RX_position[port] >= RXTX_BUFF_MAX) RX_position[port] = 0; |
if (TX_position[port] == RX_position[port]) |
servo_rx_error(port); |
} |
|
cprintf("(TXRX:%d:%d)",TX_position[port],RX_position[port]); |
|
} |
|
int com_receive(unsigned port) |
{ |
int b; |
void servo_confirm(unsigned port, BYTE msg_status) { |
|
while ((com_read(port,LSR) & 1) == 0 && !timer_expired) |
barrier(); |
if (!timer_expired) { |
b = (int)(com_read(port,RBR)); |
#ifdef SERVO_DEBUG |
kern_printf("(SERVO READ p = %d b = %02x)",port,b); |
if (msg_status == COM_ERROR) |
kern_printf("(SERVO:COM%d ERROR)",port); |
#endif |
return b; |
} else { |
#ifdef SERVO_DEBUG |
kern_printf("(READ TIMEOUT)"); |
#endif |
return -1; |
|
} |
} |
|
int com_open(unsigned port,DWORD speed,BYTE parity,BYTE len,BYTE stop) |
{ |
unsigned long div,b_mask; |
int servo_send_msg(unsigned port, BYTE *msg, unsigned len_msg, BYTE *res, unsigned len_res) { |
|
/* Now set up the serial link */ |
b_mask = (parity & 3) * 8 + (stop & 1) * 4 + ((len - 5) & 3); |
div = 115200L / speed; |
/* Clear serial interrupt enable register */ |
com_write(port,IER,0); |
/* Empty input buffer */ |
com_read(port,RBR); |
/* Activate DLAB bit for speed setting */ |
com_write(port,LCR,0x80); |
/* Load baud divisor */ |
com_write(port,0,div & 0x00FF); |
div >>= 8; |
com_write(port,1,div & 0x00FF); |
/* Load control word (parity,stop bit,bit len) */ |
com_write(port,LCR,b_mask); |
/* Attiva OUT1 & OUT2 */ |
com_write(port,MCR,0x0C); |
int i = 0, old; |
|
return 0; |
old = TX_position[port]; |
|
cprintf("(SEND:%d:%d)",len_msg,len_res); |
|
while(i < len_msg) { |
RXTX_buff[port][TX_position[port]] = msg[i]; |
RXTX_addr[port][TX_position[port]] = NULL; |
TX_position[port]++; |
if (TX_position[port] >= RXTX_BUFF_MAX) TX_position[port] = 0; |
if (TX_position[port] == RX_position[port]) { |
TX_position[port] = old; |
cprintf("Error\n"); |
return -1; |
} |
i++; |
} |
|
int com_close(unsigned port) |
{ |
i = 0; |
while(i < len_res) { |
RXTX_buff[port][TX_position[port]] = 0; |
RXTX_addr[port][TX_position[port]] = res+i; |
TX_position[port]++; |
if (TX_position[port] >= RXTX_BUFF_MAX) TX_position[port] = 0; |
if (TX_position[port] == RX_position[port]) { |
TX_position[port] = old; |
cprintf("Error\n"); |
return -1; |
} |
i++; |
} |
|
com_write(port,IER,0); |
com_read(port,IIR); |
com_read(port,RBR); |
com_irq_send(port, len_msg, msg); |
|
cprintf("(TXRX:%d:%d)",TX_position[port],RX_position[port]); |
|
return 0; |
|
} |
264,6 → 244,10 |
|
err = com_open((unsigned)(port), (DWORD)speed, SERVO_PARITY, SERVO_LEN, SERVO_STOP); |
|
com_init_irq((unsigned)(port)); |
|
com_set_functions(servo_confirm,servo_indication); |
|
return err; |
|
} |
272,6 → 256,8 |
{ |
int err; |
|
com_close_irq((unsigned)(port)); |
|
err = com_close((unsigned)(port)); |
|
return err; |
909,11 → 895,8 |
/* 0000.Pppp:0000.vvvv:vvvv.vvvv */ |
int servo_set_angle_sec(int port, int servo, int angle_sec) |
{ |
#ifdef SERVO_TIMEOUT_EVENT |
struct timespec current_time; |
#endif |
unsigned char b; |
int err, angle_tick; |
unsigned char b[3]; |
int angle_tick; |
int servo_port = (unsigned)(port); |
|
if (servo > 15) return -1; |
922,37 → 905,12 |
servo_table[port][servo].delta_tick / |
(servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec)) * 1000 / TICK_LEN; |
|
timer_expired = 0; |
b[0] = 0x00 | (servo & 0x0F); |
b[1] = 0x00 | ((angle_tick >> 8) & 0x0F); |
b[2] = 0x00 | (angle_tick & 0xFF); |
servo_send_msg(servo_port, b, 3, NULL, 0); |
|
#ifdef SERVO_TIMEOUT_EVENT |
kern_gettime(¤t_time); |
ADDUSEC2TIMESPEC(SERVO_TIMEOUT,¤t_time); |
timeout_event = kern_event_post(¤t_time, set_timer_expired, NULL); |
#else |
timeout_event = NIL; |
#endif |
|
b = 0x00 | (servo & 0x0F); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | ((angle_tick >> 8) & 0x0F); |
err = com_send(servo_port, b); |
err = com_receive(servo_port); |
if (err != (int)(b)) timer_expired = 1; |
|
b = 0x00 | (angle_tick & 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; |
|
} |
|