File content as of revision 1:bd80d3e8f3a3:

/* XBusServo.cpp file
 *
 * for mbed
 *
 * Copyright (c) 2014-2014 JR PROPO
 * Released under the MIT License: http://mbed.org/license/mit
 *
 * by Zak Sawa
 */

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



#define kXBusBaudrate           250000          // bps

#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
//    return :    none
//    parameter : tx           pin name for tx
//                rx           pin name for rx
//                maxServoNum  max number of servo that you want to connect.
//                             (limit 50)
//                             this does just to resetve the buffer.  you need to
//                             add XBus servo at the beginning of your sketch
//
//    Constructor
//    2014/09/02 : move from Arduino lib by Sawa
//****************************************************************************
XBusServo::XBusServo(PinName tx, PinName rx, uint8_t maxServoNum)
    : XBusPort(tx, rx)
{
    DBG("XBusServo::XBusServo\n");

    // initialize serial
    txPin = tx;
    txOnly = (rx == NC);
    maxServo = maxServoNum;
}


//****************************************************************************
//  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");

    // initialise vars
    numOfServo = 0;
    if (maxServo > kXBusMaxServoNum)
        maxServo = kXBusMaxServoNum;
    else if (maxServo == 0)
        maxServo = 1;
    dirty = 0;
    serialWriteBusy = 0;
    recieveBufferPointer = 0;
    modifyServosNow = 0;
    sendLength = 0;
    bufferSize = kStartOffsetOfCHData + maxServo * kCHDataSize + 1;     // add 1 for CRC
    if (bufferSize < kCmdDataPacketSize)
        bufferSize = kCmdDataPacketSize;

    chPacketBuffer = (uint8_t*)malloc(bufferSize);
    sendBuffer = (uint8_t*)malloc(bufferSize);

    // 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, RawSerial::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
    XBusPort.attach(this, &XBusServo::RxIrqHandler, RawSerial::RxIrq);

    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");

    free(chPacketBuffer);
    free(sendBuffer);
}

//****************************************************************************
//  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 (serialWriteBusy)
        return;

    serialWriteBusy = 1;
    XBusServo::sendLength = length;
    sendBufferPointer = buffer;

    if (XBusPort.putc(*sendBufferPointer++) < 0) {
        serialWriteBusy = 0;
        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
//    2014/09/30 : first write by Sawa
//****************************************************************************
void XBusServo::flush(void)
{
//    DBG("XBusServo::flush\n");

    while(serialWriteBusy)
        ;
}


//****************************************************************************
//  XBusServo::TxIrqHandler
//    return :    none
//    parameter : none
//
//    handler for Tx buffer empty
//    2014/09/30 : first write by Sawa
//****************************************************************************
void XBusServo::TxIrqHandler(void)
{
//    DBG("XBusServo::TxIrqHandler\n");

    if (! serialWriteBusy)
        return;

    if (XBusServo::sendLength <= 0) {
        serialWriteBusy = 0;
#ifdef TARGET_KL25Z
        XBusPort.attach(NULL, RawSerial::TxIrq);
#endif
    } else {
        if (XBusPort.putc(*sendBufferPointer++) < 0) {
            serialWriteBusy = 0;
            XBusServo::sendLength = 0;
#ifdef TARGET_KL25Z
            XBusPort.attach(NULL, RawSerial::TxIrq);
#endif
        } else
            XBusServo::sendLength--;
    }
}


//****************************************************************************
//  XBusServo::RxIrqHandler
//    return :    none
//    parameter : none
//
//    handler for Rx buffer full
//    2014/09/30 : first write by Sawa
//****************************************************************************
void XBusServo::RxIrqHandler(void)
{
//    DBG("XBusServo::RxIrqHandler\n");

    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] + 3);
            dirty = 0;
        }

        write(sendBuffer, sendBuffer[kCHDataPacketLength] + 3);
    }
}


//****************************************************************************
//  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 the timer handler like MsTimer2 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;
    uint8_t*            recievedPacket;

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

    // setup command
    sendBuffer[kCmdDataPacketCommand] = command;
    sendBuffer[kCmdDataPacketLength] = valueSize + 3;
    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 = 1;

    // send command
    recieveBufferPointer = 0;                                   // reset recieve buffer
    sendSize = sendBuffer[kCmdDataPacketLength] + 3;
    write(sendBuffer, sendSize);
    flush();                                                    // wait to send all bytes

    // if it's tx only mode, it done
    if (txOnly)
        return kXBusError_NoError;

    if (channelID != 0) {
        // change bus direction to Rx mode
//       DigitalInOut::input();

        recievedPacket = &(recieveBuffer[sendSize]);

        // wait to read all packet
        while(recieveBufferPointer < (sendSize + kCmdDataPacketSize))
            ;

        // change bus direction to Tx mode
        serial_pinout_tx(txPin);

        // check CRC
        if (crc8(recievedPacket, recievedPacket[kCHDataPacketLength] + 3) != 0)
            return kXBusError_CRCError;

        // check unsupported
        if (recievedPacket[kCmdDataPacketOrder] == kXBusOrder_1_Unsupported)
            return kXBusError_Unsupported;

        // send back the value
        if (valueSize == 1) {                   // 1 byte value
            *value = recievedPacket[kCmdDataPacketData1];
            if (*value & 0x0080)
                *value |= 0xFF00;
        } else {
            *value = recievedPacket[kCmdDataPacketData1];
            *value <<= 8;
            *value |= recievedPacket[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 = 1;

    // 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 = 1;

    // atomic flag off
    modifyServosNow = 0;

    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 = 1;

                // 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 = 1;

                // atomic flag off
                modifyServosNow = 0;

                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
//
//    remove the servo from the buffer on this library
//    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 = 1;

                // 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 = 0;

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