File content as of revision 17:3ffb2e3e3bec:

/**
 *  @file   XBusServo.cpp
 *  @brief  this header file will contain all required
 *          definitions and basic utilities functions.
 *          for controling XBus servo.
 *  @author Zak Sawa
 *  @note   Copyright (c) 2014-2014 JR PROPO
 *  @note   Released under the MIT License: http://mbed.org/license/mit
 */

#include "XBusServo.h"
#include "pinmap.h"
#include "gpio_api.h"



#define kXBusBaudrate           250000          // bps

#define kPacketHeaderFooterSize 3
#define kPacketPreDataSize      3
#define kStartOffsetOfCHData    4

#define kCHDataSize             4
#define kCHDataPacketCommand    0
#define kCHDataPacketLength     1
#define kCHDataPacketKey        2
#define kCHDataPacketType       3

#define kCmdDataPacketSize      8
#define kCmdDataPacketCommand   0
#define kCmdDataPacketLength    1
#define kCmdDataPacketKey       2
#define kCmdDataPacketCH_ID     3
#define kCmdDataPacketOrder     4
#define kCmdDataPacketData1     5
#define kCmdDataPacketData2     6
#define kCmdDataPacketCRC       7


// XBus Command
typedef enum {
    kXBusCmd_Set =              0x20,
    kXBusCmd_Get =              0x21,
    kXBusCmd_Status =           0x22,
    kXBusCmd_ModeA =            0xa4
} XBusCmd;


// XBus device mode
typedef enum {
    kXBusMode_Operate =         0x01,
    kXBusMode_IDSet =           0x02
} XBusMode;


#define DEBUG

#ifdef DEBUG
#define DBG(fmt) printf(fmt)
#define DBGF(fmt, ...) printf(fmt, __VA_ARGS__)
#else
#define DBG(...)
#define DBGF(...)
#endif


/****************************************************************************
    XBusServo::XBusServo

    @param tx           pin name for tx
    @param rx           pin name for rx
    @param maxServoNum  max number of servo that you want to connect.
                        (limit 50)
                        this does just to reserve the buffer.  you need to
                        add XBus servo at the beginning of your sketch

    @bref               Constructor

    @note               2014/09/02 : move from Arduino lib by Sawa
*****************************************************************************/
XBusServo::XBusServo(PinName tx, PinName rx, PinName sw, uint8_t maxServoNum)
    : XBusPort(tx, rx), TxSwitch(sw, 0)
{
    DBG("XBusServo::XBusServo\n");

    // initialize serial
    txPin = tx;
    txOnly = false;
    if (rx == NC)
        txOnly = true;
    maxServo = maxServoNum;

    // initialise vars
    numOfServo = 0;
    if (maxServo > kXBusMaxServoNum)
        maxServo = kXBusMaxServoNum;
    else if (maxServo == 0)
        maxServo = 1;
    dirty = false;
    serialCommandBusy = false;
    recieveBufferPointer = 0;
    modifyServosNow = false;
    sendLength = 0;
    need2ReadData = false;
    chPacketBuffer = NULL;
    sendBuffer = NULL;
}


//****************************************************************************
//  XBusServo::start
//    return :    error code
//    parameter : none
//
//    start to use XBus
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusError XBusServo::start()
{
    int        bufferSize;            // channel data packet buffer size

    DBG("XBusServo::start\n");

    // allocate my buffer
    bufferSize = kStartOffsetOfCHData + maxServo * kCHDataSize + 1;     // add 1 for CRC
    if (bufferSize < kCmdDataPacketSize)
        bufferSize = kCmdDataPacketSize;

    chPacketBuffer = new uint8_t[bufferSize];
    if (chPacketBuffer == NULL) {
        stop();
        return kXBusError_MemoryFull;
    }

    sendBuffer = new uint8_t[bufferSize];
    if (sendBuffer == NULL) {
        stop();
        return kXBusError_MemoryFull;
    }

    // initialize channel packer buffer
    chPacketBuffer[kCHDataPacketCommand]    = kXBusCmd_ModeA;
    chPacketBuffer[kCHDataPacketLength]     = 0x00;
    chPacketBuffer[kCHDataPacketKey]        = 0x00;
    chPacketBuffer[kCHDataPacketType]       = 0x00;

    // initialize serial
    XBusPort.baud(kXBusBaudrate);
    XBusPort.format(8, Serial::None, 1);
#if DEVICE_SERIAL_FC
    XBusPort.set_flow_control(RawSerial::Disabled, NC, NC);
#endif

#ifdef TARGET_KL25Z
    // do nothing here
#else
    XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq);
#endif

    serial_pinout_tx(txPin);

    return kXBusError_NoError;
}


//****************************************************************************
//  XBusServo::stop
//    return :    none
//    parameter : none
//
//    stop to use XBus
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
void XBusServo::stop()
{
    DBG("XBusServo::stop\n");

    delete[] chPacketBuffer;
    chPacketBuffer = NULL;

    delete[] sendBuffer;
    sendBuffer = NULL;
}


//****************************************************************************
//  XBusServo::write
//    return :    none
//    parameter : buffer      data buffer to send
//                length      data length on the buffer
//
//    start to send all packet data
//    2014/09/30 : first write by Sawa
//****************************************************************************
void XBusServo::write(uint8_t* buffer, uint8_t length)
{
//  DBG("XBusServo::write\n");

    if (serialCommandBusy)
        return;

    serialCommandBusy = true;
    XBusServo::sendLength = length;
    sendBufferPointer = buffer;

    if (XBusPort.putc(*sendBufferPointer++) < 0) {
        serialCommandBusy = false;
        XBusServo::sendLength = 0;
    } else {
        XBusServo::sendLength--;
    }

#ifdef TARGET_KL25Z
    XBusPort.attach(this, &XBusServo::TxIrqHandler, RawSerial::TxIrq);
#endif

}


//****************************************************************************
//  XBusServo::flush
//    return :    none
//    parameter : none
//
//    wait to send all packet data.  never call from interrupt.
//    2014/09/30 : first write by Sawa
//****************************************************************************
void XBusServo::flush(void)
{
//    DBG("XBusServo::flush\n");

    while(serialCommandBusy)
        ;
}


//****************************************************************************
//  XBusServo::TxSwitchHandler
//    return :    none
//    parameter : none
//
//    handler for Tx switch
//    2014/10/29 : first write by Sawa
//****************************************************************************
void XBusServo::TxSwitchHandler(void)
{
    TxSwitch.write(1);
}


//****************************************************************************
//  XBusServo::TxIrqHandler
//    return :    none
//    parameter : none
//
//    handler for Tx buffer empty
//    2014/09/30 : first write by Sawa
//    2014/10/24 : add to setup reading for command data packet
//****************************************************************************
void XBusServo::TxIrqHandler(void)
//void XBusServo::TxIrqHandler(MODSERIAL_IRQ_INFO *q)
{
    int             result = 0;
//    DBG("XBusServo::TxIrqHandler\n");

    if (! serialCommandBusy)
        return;                                       // Is it noise ?

    if (XBusServo::sendLength > 0) {
        result = XBusPort.putc(*sendBufferPointer++);
        if (result < 0) {
            // error occured
            XBusServo::sendLength = 0;
            serialCommandBusy = false;
        } else {
            XBusServo::sendLength--;
            return;
        }
    }

#ifdef TARGET_KL25Z
    XBusPort.attach(NULL, RawSerial::TxIrq);
#endif

    if (result < 0)
        return;

    if (need2ReadData) {
#ifdef TARGET_KL25Z
        TxSwitchTimer.attach_us(this, &XBusServo::TxSwitchHandler, 27);
#endif
    }

    serialCommandBusy = false;
}


//****************************************************************************
//  XBusServo::RxIrqHandler
//    return :    none
//    parameter : none
//
//    handler for Rx buffer full
//    2014/09/30 : first write by Sawa
//    2014/10/21 : add to ignore the data myself
//    2014/10/24 : modify to get the data from 1st byte of buffer
//****************************************************************************
void XBusServo::RxIrqHandler(void)
//void XBusServo::RxIrqHandler(MODSERIAL_IRQ_INFO *q)
{
//    DBG("XBusServo::RxIrqHandler\n");

    while (XBusPort.readable()) {
        recieveBuffer[recieveBufferPointer++] = XBusPort.getc();
        if (recieveBufferPointer >= kRecieveBufferSize)
            recieveBufferPointer = 0;
    }
}


//****************************************************************************
//  XBusServo::sendChannelDataPacket
//    return :    none
//    parameter : none
//
//    This should be called on the timer handler like MsTimer2 when you
//    use the XBus servo.
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
void XBusServo::sendChannelDataPacket(void)
{
//    DBG("XBusServo::sendChannelDataPacket\n");

    if (modifyServosNow)
        return;

    if (numOfServo > 0) {
        if (dirty) {
            memcpy(sendBuffer, chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + kPacketHeaderFooterSize);
            dirty = false;
        }

        need2ReadData = false;
        write(sendBuffer, sendBuffer[kCHDataPacketLength] + kPacketHeaderFooterSize);
    }
}


//****************************************************************************
//  XBusServo::sendCommandDataPacket
//    return :    error code
//    parameter : command     The commnad that you want to send
//                channelID   The channel ID of the XBus servo that you want to set up
//                order       The order that you want to set up
//                value       The value that you want to set / get
//                valueSize   The value size.  1 byte(char) or 2 byte(int)
//
//    This should NOT be called on interrupt handler when you
//    setup the XBus servo.
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusError XBusServo::sendCommandDataPacket(uint8_t command, uint8_t channelID, uint8_t order, int16_t* value, uint8_t valueSize)
{
    int                 sendSize;
    int                 prevData = -1;
    uint8_t             dummy;

    DBG("XBusServo::sendCommandDataPacket\n");

    // setup command
    sendBuffer[kCmdDataPacketCommand] = command;
    sendBuffer[kCmdDataPacketLength] = valueSize + kPacketPreDataSize;
    sendBuffer[kCmdDataPacketKey] = 0x00;
    sendBuffer[kCmdDataPacketCH_ID] = channelID;
    sendBuffer[kCmdDataPacketOrder] = order;
    if (valueSize == 1) {                   // 1 byte value
        sendBuffer[kCmdDataPacketData1] = *value & 0x00FF;
        sendBuffer[kCmdDataPacketData2] = crc8(sendBuffer, sendBuffer[kCHDataPacketLength] + 2);
    } else {
        sendBuffer[kCmdDataPacketData1] = (*value >> 8) & 0x00FF;
        sendBuffer[kCmdDataPacketData2] = *value & 0x00FF;
        sendBuffer[kCmdDataPacketCRC] = crc8(sendBuffer, sendBuffer[kCHDataPacketLength] + 2);
    }

    // to recover channel data packet when you call sendChannelDataPacket after this
    dirty = true;

    // prepair recieve data
    XBusPort.attach(this, &XBusServo::RxIrqHandler, RawSerial::RxIrq);
    while (XBusPort.readable())
        dummy = XBusPort.getc();
    recieveBufferPointer = 0;                                   // reset recieve buffer

    // send command
    sendSize = sendBuffer[kCmdDataPacketLength] + kPacketHeaderFooterSize;
    need2ReadData = ((! txOnly) && (channelID != 0));
    recieveBuffer[sendSize + kCmdDataPacketLength] = 5;            // dummy to get real packet size
    write(sendBuffer, sendSize);
//    DBG("XBusServo::sendCommandDataPacket 2\n");

    // if it's tx only mode or ID=0, it done
    if (! need2ReadData) {
        flush();
        return kXBusError_NoError;
    }

// wait to read all packet
    while(recieveBufferPointer < (recieveBuffer[sendSize + kCmdDataPacketLength] + kPacketHeaderFooterSize + sendSize)) {
        if (prevData != recieveBufferPointer) {
            prevData = recieveBufferPointer;
//           DBGF("XBusServo::sendCommandDataPacket 3 %02d %02X \n", recieveBufferPointer, recieveBuffer[recieveBufferPointer - 1]);
        }
        //                                           need to add time out
    }

    TxSwitch.write(0);
//    DBG("XBusServo::sendCommandDataPacket 4\n");
    // change bus direction to Tx mode
    XBusPort.attach(NULL, RawSerial::RxIrq);
    need2ReadData = false;
    serialCommandBusy = false;

    // check CRC
    if (crc8(recieveBuffer, recieveBuffer[kCHDataPacketLength] + 3) != 0) {
        DBG("XBusServo::sendCommandDataPacket kXBusError_CRCError\n");
        return kXBusError_CRCError;
    }

    // check unsupported
    if (recieveBuffer[kCmdDataPacketOrder] == kXBusOrder_1_Unsupported) {
        DBG("XBusServo::sendCommandDataPacket kXBusError_Unsupported\n");
        return kXBusError_Unsupported;
    }

    // send back the value
    if (valueSize == 1) {                   // 1 byte value
        *value = recieveBuffer[sendSize + kCmdDataPacketData1];
        if (*value & 0x0080)
            *value |= 0xFF00;
    } else {
        *value = recieveBuffer[sendSize + kCmdDataPacketData1];
        *value <<= 8;
        *value |= recieveBuffer[sendSize + kCmdDataPacketData2];
    }

    return kXBusError_NoError;
}


//****************************************************************************
//  XBusServo::addServo
//    return :    error code
//    parameter : channelID   channel ID of the XBus servo that you want to use
//                initValue   initial value of this XBus servo
//                            use kXbusServoNeutral for center of the XBus servo
//
//    add new servo to the buffer on this library
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusError XBusServo::addServo(uint8_t channelID, uint16_t initValue)
{
    uint16_t         dataOffset;

    DBG("XBusServo::addServo\n");

    // max check
    if (numOfServo >= maxServo)
        return kXBusError_ServoNumOverflow;

    // convert to servo ID
    channelID &= 0x3F;

    // scan servo ID that is already added
    if (numOfServo > 0) {
        uint16_t     servoNo;

        for (servoNo = 0; servoNo < numOfServo; servoNo++)
            if (chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo] == channelID)
                return kXBusError_AddWithSameID;                            // found same servo ID
    }

    // atomic flag on
    modifyServosNow = true;

    // add new servo
    dataOffset = kStartOffsetOfCHData + kCHDataSize * numOfServo;
    numOfServo++;
    chPacketBuffer[kCHDataPacketLength] = numOfServo * kCHDataSize + 2;     // add 2 for key and type

    chPacketBuffer[dataOffset] = channelID;
    chPacketBuffer[dataOffset + 1] = 0x00;
    chPacketBuffer[dataOffset + 2] = (initValue >> 8) & 0x00FF;
    chPacketBuffer[dataOffset + 3] = initValue & 0x00FF;

    // calc CRC
    chPacketBuffer[dataOffset + 4] = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
    dirty = true;

    // atomic flag off
    modifyServosNow = false;

    return kXBusError_NoError;
}


//****************************************************************************
//  XBusServo::removeServo
//    return :    error code
//    parameter : channelID   channel ID of the XBus servo that you want to remove
//
//    remove the servo from the buffer on this library
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusError XBusServo::removeServo(uint8_t channelID)
{
    DBG("XBusServo::removeServo\n");

    // min check
    if (numOfServo == 0)
        return kXBusError_ServoNumIsZero;

    // convert to servo ID
    channelID &= 0x3F;

    // scan servo ID that is already added
    if (numOfServo > 0) {
        uint16_t     servoNo;

        for (servoNo = 0; servoNo < numOfServo; servoNo++)
            if (chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo] == channelID) {
                // atomic flag on
                modifyServosNow = true;

                // copy data after that
                if (servoNo < (numOfServo - 1))
                    memcpy(&(chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * servoNo]),
                           &(chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * (servoNo + 1)]),
                           kCHDataSize * (numOfServo - servoNo - 1));

                // update packet size
                numOfServo--;
                chPacketBuffer[kCHDataPacketLength] = numOfServo * kCHDataSize + 2;     // add 2 for key and type

                // calc CRC
                chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * numOfServo]
                = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
                dirty = true;

                // atomic flag off
                modifyServosNow = false;

                return kXBusError_NoError;
            }
    }

    return kXBusError_IDNotFound;
}


//****************************************************************************
//  XBusServo::setServo
//    return :    error code
//    parameter : channelID   channel ID of the XBus servo that you want to set
//                value       value of this XBus servo
//                            use kXbusServoNeutral for center of the XBus servo
//
//    set new angle to the servo
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusError XBusServo::setServo(uint8_t channelID, uint16_t value)
{
    uint16_t         dataOffset;

//   DBG("XBusServo::setServo\n");

    // convert to servo ID
    channelID &= 0x3F;

    // scan servo ID that is already added
    if (numOfServo > 0) {
        int     servoNo;

        for (servoNo = 0; servoNo < numOfServo; servoNo++) {
            dataOffset = kStartOffsetOfCHData + kCHDataSize * servoNo;

            if (chPacketBuffer[dataOffset] == channelID) {
                // atomic flag on
                modifyServosNow = true;

                // set value
                chPacketBuffer[dataOffset + 2] = (value >> 8) & 0x00FF;
                chPacketBuffer[dataOffset + 3] = value & 0x00FF;

                // calc CRC
                chPacketBuffer[kStartOffsetOfCHData + kCHDataSize * numOfServo]
                = crc8(chPacketBuffer, chPacketBuffer[kCHDataPacketLength] + 2);
                dirty = 1;

                // atomic flag off
                modifyServosNow = false;

                return kXBusError_NoError;
            }
        }
    }

    return kXBusError_IDNotFound;
}


//****************************************************************************
//  XBusServo::setChannelID
//    return :    error code
//    parameter : oldChannelID    channel IDof the XBus servo to change the ID
//                newChannelID    new channel ID for the XBus servo
//
//    set new channel ID to the XBus servo
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusError XBusServo::setChannelID(uint8_t oldChannelID, uint8_t newChannelID)
{
    XBusError       result;
    int16_t         value;

    DBG("XBusServo::setChannelID\n");

    value = kXBusMode_IDSet;
    result = sendCommandDataPacket(kXBusCmd_Set, oldChannelID, kXBusOrder_1_Mode, &value, 1);
    if (result != kXBusError_NoError)
        return result;

    value = newChannelID;
    result = sendCommandDataPacket(kXBusCmd_Set, oldChannelID, kXBusOrder_1_ID, &value, 1);

    return result;
}


//****************************************************************************
//  XBusServo::setChannelID
//    return :    error code
//    parameter : newChannelID    new channel ID for the XBus servo
//
//    set new channel ID to the XBus servo
//    this is only for TX only mode
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusError XBusServo::setChannelID(uint8_t newChannelID)
{
    XBusError       result;
    int16_t         value;

    DBG("XBusServo::setChannelID\n");

    if (txOnly)
        return kXBusError_OnlyForTxOnlyMode;

    value = kXBusMode_IDSet;
    result = sendCommandDataPacket(kXBusCmd_Set, 0, kXBusOrder_1_Mode, &value, 1);
    if (result != kXBusError_NoError)
        return result;

    value = newChannelID;
    result = sendCommandDataPacket(kXBusCmd_Set, 0, kXBusOrder_1_ID, &value, 1);

    return result;
}


//****************************************************************************
//  XBusServo::getDataSize
//    return :    data size for this order
//    parameter : order       the order that you want to know
//
//    get the data size of this order
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
uint8_t XBusServo::getDataSize(uint8_t order)
{
    uint8_t        dataSize = 1;

    DBG("XBusServo::getDataSize\n");

    switch(order) {
        case kXBusOrder_2_Version:      // only for get
        case kXBusOrder_2_Product:      // only for get
        case kXBusOrder_2_Reset:        // only for set
        case kXBusOrder_2_ParamWrite:   // only for set
        case kXBusOrder_2_Reverse:
        case kXBusOrder_2_Neutral:
        case kXBusOrder_2_H_Travel:
        case kXBusOrder_2_L_Travel:
        case kXBusOrder_2_H_Limit:
        case kXBusOrder_2_L_Limit:
        case kXBusOrder_2_PowerOffset:
        case kXBusOrder_2_AlarmDelay:
        case kXBusOrder_2_CurrentPos:   // only for get
        case kXBusOrder_2_MaxInteger:
            dataSize = 2;
    }

    return dataSize;
}


//****************************************************************************
//  XBusServo::setCommand
//    return :    error code
//    parameter : channelID   channel ID of the XBus servo that you want to set to
//                order       the order that you want
//                value       the value that you want to set and return current value
//
//    send set command to the XBus servo
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusError XBusServo::setCommand(uint8_t channelID, uint8_t order, int16_t* value)
{
    DBG("XBusServo::setCommand\n");

    return sendCommandDataPacket(kXBusCmd_Set, channelID, order, value, getDataSize(order));
}


//****************************************************************************
//  XBusServo::getCommand
//    return :    error code
//    parameter : channelID   channel ID of the XBus servo that you want to get from
//                order       the order that you want
//                value       the value that you want to get from
//
//    send get command to the XBus servo
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusError XBusServo::getCommand(uint8_t channelID, uint8_t order, int16_t* value)
{
    DBG("XBusServo::getCommand\n");

    return sendCommandDataPacket(kXBusCmd_Get, channelID, order, value, getDataSize(order));
}


//****************************************************************************
//  XBusServo::setCommand
//    return :    error code
//    parameter : order       the order that you want
//                value       the value that you want to set current value
//
//    send set command to the XBus servo
//    this is only for TX only mode
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusError XBusServo::setCommand(uint8_t order, int16_t* value)
{
    DBG("XBusServo::setCommand\n");

    if (txOnly)
        return kXBusError_OnlyForTxOnlyMode;

    return sendCommandDataPacket(kXBusCmd_Set, 0, order, value, getDataSize(order));
}






//****************************************************************************
// for CRC
static uint8_t s_crc_array[256] = {
    0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83,
    0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41,
    0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
    0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc,
    0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, 0xfe, 0xa0,
    0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
    0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d,
    0x7c, 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff,
    0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
    0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07,
    0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58,
    0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
    0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6,
    0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24,
    0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
    0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9,
    0x8c, 0xd2, 0x30, 0x6e, 0xed, 0xb3, 0x51, 0x0f,
    0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
    0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92,
    0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50,
    0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
    0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee,
    0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1,
    0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
    0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49,
    0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b,
    0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
    0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16,
    0xe9, 0xb7, 0x55, 0x0b, 0x88, 0xd6, 0x34, 0x6a,
    0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
    0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7,
    0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35,
};


uint8_t XBusServo::crc_table(uint8_t  data, uint8_t  crc)
{
    uint16_t  index = (data ^ crc) & 0xff;

    crc = s_crc_array[index];
    return crc;
}


uint8_t XBusServo::crc8(uint8_t* buffer, uint8_t  length)
{
    uint8_t  crc = 0;

    while (length-- > 0)
        crc = crc_table(*buffer++, crc);
    return crc;
}