File content as of revision 0:7d81654c5ce8:

typedef struct{
    int front, rear, count, size;
    char *buff;
}Queue;

typedef union{
    short i;
    char b[2];
}int_b;

class SerialServo {
public:
    SerialServo(PinName tx, PinName rx);
    int start(int,int);
    int readShort(char*, short*);
    void sendShort(char, short);
    
private:  
    Queue *make_queue(int);
    bool is_full(Queue *);
    int get_count(Queue *);
    bool enqueue(Queue *, char);
    bool is_empty(Queue *);
    char dequeue(Queue *, bool *);
    void enqueueBuff();
    Queue *_q;
    RawSerial _servo;
    Ticker _timer;
};

SerialServo::SerialServo(PinName tx, PinName rx)
        :_servo(tx,rx){
}

int SerialServo::start(int baud_rate, int buff_size)
{
    if((_q = make_queue(buff_size)) == NULL)  return -1;
    _servo.baud(baud_rate);
    _servo.attach(this, &SerialServo::enqueueBuff, Serial::RxIrq);
    return 0;
}

void SerialServo::sendShort(char c, short data)
{
    int_b s_data;
    s_data.i = data;
    _servo.putc(c);
    _servo.putc(s_data.b[0]);
    _servo.putc(s_data.b[1]);
}

int SerialServo::readShort(char *c, short *data)
{
    bool err; 
    if(get_count(_q) >= sizeof('A') + sizeof(short)){
        *c = dequeue(_q, &err);
        if(*c >= 'A' && *c <= 'Z'){
        int_b x;
        x.b[0] = dequeue(_q, &err);
        x.b[1] = dequeue(_q, &err);
        *data = x.i;
        return 0;
        }else readShort(c, data);
    }
    return -1;
}

void SerialServo::enqueueBuff()
{
    if(_servo.readable() == 1) SerialServo::enqueue(_q,_servo.getc());
}
    
    
//********************
//Bufferに関する関数
//********************

Queue *SerialServo::make_queue(int n)
{
  Queue *que;
  que = (Queue *)malloc(sizeof(Queue));
  if (que != NULL) {
    que->front = 0;
    que->rear = 0;
    que->count = 0;
    que->size = n;
    que->buff = (char *)malloc(sizeof(char) * n);
    if (que->buff == NULL) {
      free(que);
      return NULL;
    }
  }
  return que;
}


// キューは満杯か
bool SerialServo::is_full(Queue *que)
{
  return que->count == que->size;
}

// データの挿入
bool SerialServo::enqueue(Queue *que, char x)
{
  if (is_full(que)) return false;
  que->buff[que->rear++] = x;
  que->count++;
  if (que->rear == que->size)
    que->rear = 0;
  return true;
}

// キューは空か
bool SerialServo::is_empty(Queue *que)
{
  return que->count == 0;
}

int SerialServo::get_count(Queue *que)
{
    return que->count;
}

// データを取り出す
char SerialServo::dequeue(Queue *que, bool *err)
{
  if (is_empty(que)) {
    *err = false;
    return 0;
  }
  char x = que->buff[que->front++];
  que->count--;
  *err = true;
  if (que->front == que->size)
    que->front = 0;
  return x;
}