WebSVN - shark - Path Comparison - /shark/trunk/ports/servo Rev 721 and /shark/trunk/ports/servo/ Rev 724

Ignore whitespace Rev 721 → Rev 724

 /shark/trunk/ports/servo/servo.c
 ,7 → 36,6
  *
  */
-#include <ll/sys/ll/ll-instr.h>
 #include "kernel/kern.h"
 #include "drivers/scom.h"
 ,10 → 44,7
 #include "servo.h"
 //#define SERVO_DEBUG
-//#define SERVO_TIMEOUT_EVENT
-#define SERVO_TIMEOUT 200000 /* us */
 #define SERVO_SPEED 19200
 #define SERVO_PARITY NONE
 #define SERVO_LEN 8
 ,9 → 128,6
   {-324000, 324000, 1200, 1600},
   {-324000, 324000, 1200, 1600}}};
-int timer_expired = 0;
-int timeout_event;
 #define RXTX_BUFF_MAX 100
 static BYTE RXTX_buff[4][RXTX_BUFF_MAX];
 static BYTE *RXTX_addr[4][RXTX_BUFF_MAX];
 ,12 → 148,6
 ,
         -1};
-void set_timer_expired(void *arg)
-{
-  timeout_event = NIL;
-  timer_expired = 1;
-}
 void servo_rx_error(unsigned port, unsigned type) {
   kern_printf("(SERVO: BUFFER ERROR PORT:%d ERR:%d)",port,type);
 ,6 → 264,8
 {
   int err;
+  com_irq_disable((unsigned)(port),ALL_IRQ);
   com_close_irq((unsigned)(port));
   err = com_close((unsigned)(port));
 ,11 → 277,8
 /* 1000.011w:bbbb.bbbb */
 int servo_set_RS232_baudrate(int port, int baud)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err, spbrg_temp, i;
+  unsigned char b[2];
+  int spbrg_temp, i;
   unsigned char spbrg, w;
   int servo_port = (unsigned)(port);
 ,66 → 302,27
     kern_printf("(SERVO:SBPRG %d W %d)",spbrg,w);
   #endif
-  timer_expired = 0;
+  b[0] = 0x86 | (w & 0x01);
+  b[1] = spbrg;
+  servo_send_msg(servo_port, b, 2, NULL, 0);
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  return 0;
-  b = 0x86 | (w & 0x01);
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
-  b = spbrg;
-  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);
-  /*com_close(servo_port);
-  com_open(servo_port, baud, SERVO_PARITY, SERVO_LEN, SERVO_STOP);*/
-  if (!timer_expired)
-    return 0;
-  else
-    return -1;
 }
 /* 1000.0101 */
 int servo_get_RS232_baudrate(int port)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err, res, res_w, res_b;
+  unsigned char b[1],r[2];
+  int res, res_w, res_b;
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  b[0] = 0x85;
+  servo_send_msg(servo_port, b, 1, r, 2);
-  b = 0x85;
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
+  res_w = r[0]; /* bit W */
+  res_b = r[1]; /* byte SPBRG */
-  res_w = com_receive(servo_port); /* bit W */
-  res_b = com_receive(servo_port); /* byte SPBRG */
   if (res_w != -1 && res_b != -1) {
     if (res_w)
       res = SERVO_CLOCK / ( 16 * (res_b + 1) );
 ,270 → 332,114
     return -1;
   }
-  if (timeout_event != NIL) kern_event_delete(timeout_event);
+  return res;
-  if (!timer_expired)
-    return res;
-  else
-    return -1;
 }
 /* 1000.0100 */
 int servo_store_RS232_baudrate(int port)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[1];
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  b[0] = 0x84;
+  servo_send_msg(servo_port, b, 1, NULL, 0);
+  return 0;
-  b = 0x84;
-  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.1010:llll.llll */
 int servo_set_period(int port, int period)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[2];
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  b[0] = 0x8A;
+  b[1] = (period*1000)/TICK_LEN_PERIOD/8 & 0xFF;
+  servo_send_msg(servo_port, b, 2, NULL, 0);
-  b = 0x8A;
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
+  return 0;
-  b = (period*1000)/TICK_LEN_PERIOD/8 & 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.1001 */
 int servo_get_period(int port)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err,res;
+  unsigned char b[1],r[1];
+  int res;
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
+  b[0] = 0x89;
+  servo_send_msg(servo_port, b, 1, r, 1);
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  res = r[0];
-  b = 0x89;
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
-  res = com_receive(servo_port);
+  return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8);
-  if (timeout_event != NIL) kern_event_delete(timeout_event);
-  if (!timer_expired)
-    return (((unsigned char)(res))*TICK_LEN_PERIOD/1000*8);
-  else
-    return -1;
 }
 /* 1000.1000 */
 int servo_store_period(int port)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[1];
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
+  b[0] = 0x88;
+  servo_send_msg(servo_port, b, 1, NULL, 0);
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  return 0;
-  b = 0x88;
-  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.1100 */
 int servo_get_setup_switch(int port)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err,res;
+  unsigned char b[1],r[1];
+  int res;
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  b[0] = 0x8C;
+  servo_send_msg(servo_port, b, 1, r, 1);
-  b = 0x8C;
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
-  res = com_receive(servo_port);
+  res = r[0];
-  if (timeout_event != NIL) kern_event_delete(timeout_event);
+  return res;
-  if (!timer_expired)
-    return res;
-  else
-    return -1;
 }
 /* 1000.111s */
 int servo_set_RC5_switch(int port, int data)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[1];
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
+  b[0] = 0x8E | (data & 0x01);
+  servo_send_msg(servo_port, b, 1, NULL, 0);
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  return 0;
-  b = 0x8E | (data & 0x01);
-  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:0000.Mmmm */
 int servo_turn_off(int port, int servo)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[2];
   int servo_port = (unsigned)(port);
   if (servo > 15) return -1;
-  timer_expired = 0;
+  b[0] = 0x80;
+  b[1] = 0x00 | (servo & 0x0F);
+  servo_send_msg(servo_port, b, 2, NULL, 0);
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  return 0;
-  b = 0x80;
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
-  b = 0x00 | (servo & 0x0F);
-  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:0001.Mmmm */
 int servo_turn_on(int port, int servo)
 {
   unsigned char b[2];
   int servo_port = (unsigned)(port);
 ,202 → 456,75
 /* 1000.0000:0010.0000 */
 int servo_turn_off_all(int port)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[2];
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  b[0] = 0x80;
+  b[1] = 0x20;
+  servo_send_msg(servo_port, b, 2, NULL, 0);
-  b = 0x80;
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
+  return 0;
-  b = 0x20;
-  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:0010.0001 */
 int servo_turn_on_all(int port)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[2];
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
+  b[0] = 0x80;
+  b[1] = 0x21;
+  servo_send_msg(servo_port, b, 2, NULL, 0);
+  return 0;
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
-  b = 0x80;
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
-  b = 0x21;
-  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:0101.000M:mmmm.mmmm */
-int servo_set_levels(int port, int bank,int mask)
+int servo_set_levels(int port, int bank, int mask)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[3];
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  b[0] = 0x80;
+  b[1] = 0x50 | (0x01 & bank);
+  b[2] = (unsigned char)(mask & 0xFF);
+  servo_send_msg(servo_port, b, 3, NULL, 0);
-  b = 0x80;
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
+  return 0;
-  b = 0x50 | (0x01 & bank);
-  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.000M */
 int servo_get_levels(int port, int bank)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[2],r[1];
+  int res;
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
+  b[0] = 0x80;
+  b[1] = 0x40 | (0x01 & bank);
+  servo_send_msg(servo_port, b, 2, r, 1);
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  res = r[0];
-  b = 0x80;
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
+  return res;
-  b = 0x40 | (0x01 & bank);
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
-  err = com_receive(servo_port);
-  if (timeout_event != NIL) kern_event_delete(timeout_event);
-  if (!timer_expired)
-    return err;
-  else
-    return -1;
 }
 /* 1000.0000:1000.0000 */
 int servo_store_levels(int port)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[2];
   int servo_port = (unsigned)(port);
-  timer_expired = 0;
+  b[0] = 0x80;
+  b[1] = 0x80;
+  servo_send_msg(servo_port, b, 2, NULL, 0);
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  return 0;
-  b = 0x80;
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
-  b = 0x80;
-  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;
 }
 int servo_set_max_angle_sec(int port, int servo, int angle_sec)
 ,78 → 577,38
 /* 0010.Pppp */
 int servo_store_default_position(int port, int servo)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err;
+  unsigned char b[1];
   int servo_port = (unsigned)(port);
   if (servo > 15) return -1;
-  timer_expired = 0;
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
-  b = 0x20 | (servo & 0x0F);
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
+  b[0] = 0x20 | (servo & 0x0F);
+  servo_send_msg(servo_port, b, 1, NULL, 0);
-  if (timeout_event != NIL) kern_event_delete(timeout_event);
+  return 0;
-  if (!timer_expired)
-    return 0;
-  else
-    return -1;
 }
 /* 0001.Pppp */
 int servo_get_angle_sec(int port, int servo)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err,res,data;
+  unsigned char b[1],r[2];
+  int res,data;
   int servo_port = (unsigned)(port);
   if (servo > 15) return -1;
-  timer_expired = 0;
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  b[0] = 0x10 | (servo & 0x0F);
+  servo_send_msg(servo_port, b, 1, r, 2);
-  b = 0x10 | (servo & 0x0F);
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
-  res = com_receive(servo_port) << 8;
-  res |= com_receive(servo_port);
+  res = r[0] << 8;
+  res |= r[1];
-  if (timeout_event != NIL) kern_event_delete(timeout_event);
   data = ((res*TICK_LEN/1000) - servo_table[port][servo].zero_tick) *
           (servo_table[port][servo].max_angle_sec - servo_table[port][servo].min_angle_sec) /
            servo_table[port][servo].delta_tick;
-  if (!timer_expired)
-    return data;
-  else
-    return -1;
+  return data;
 }
 ,39 → 615,19
 /* 0100:0aaa */
 int servo_get_analog(int port, int adport)
 {
-  #ifdef SERVO_TIMEOUT_EVENT
-    struct timespec current_time;
-  #endif
-  unsigned char b;
-  int err,res;
+  unsigned char b[1],r[2];
+  int res;
   int servo_port = (unsigned)(port);
   if (adport > 7) return -1;
-  timer_expired = 0;
-  #ifdef SERVO_TIMEOUT_EVENT
-    kern_gettime(&current_time);
-    ADDUSEC2TIMESPEC(SERVO_TIMEOUT,&current_time);
-    timeout_event = kern_event_post(&current_time, set_timer_expired, NULL);
-  #else
-    timeout_event = NIL;
-  #endif
+  b[0] = 0x40 | (adport & 0x07);
+  servo_send_msg(servo_port, b, 1, r, 2);
-  b = 0x40 | (adport & 0x07);
-  err = com_send(servo_port, b);
-  err = com_receive(servo_port);
-  if (err != (int)(b)) timer_expired = 1;
-  res = com_receive(servo_port) << 8;
-  res |= com_receive(servo_port);
+  res = r[0] << 8;
+  res |= r[1];
-  if (timeout_event != NIL) kern_event_delete(timeout_event);
+  return res;
-  if (!timer_expired)
-    return res;
-  else
-    return -1;
 }

Subversion Repositories shark

Compare Revisions

Ignore whitespace Rev 721 → Rev 724