cp2012 Robotell USB-CAN Adapter userspace implementation

index.js

const usb = require('usb')
const util = require('util')
const assert = require('assert')

const USB_DIR_OUT = 0
const USB_TYPE_VENDOR = (0x02 << 5)
const USB_RECIP_DEVICE = 0x00

const _PACKET_ESC = 0xA5
const _PACKET_HEAD = 0xAA
const _PACKET_TAIL = 0x55

const calculateChecksum = (input) => {
  let checksum = 0
  for (let i = 0; i < 16; ++i) {
     checksum = (checksum + input[i]) & 0xFF
  }
  return checksum
}

const escapeInput = (input) => {
  const output = []
  for (let i = 0; i < input.length; ++i) {
    if (input[i] === _PACKET_ESC || input[i] === _PACKET_HEAD || input[i] === _PACKET_TAIL) {
      output.push(_PACKET_ESC)
    }
    output.push(input[i])
  }
  return output
}

const unescapeInput = (input) => {
  const output = []
  for (let i = 0; i < input.length; ++i) {
    if (input[i] === _PACKET_ESC) {
      continue
    }
    output.push(input[i])
  }
  return output
}

const buildFrame = (payload, dataLen, msgChan, msgFormat, msgType) => {
  const checksumInput = [
     payload[0],
     payload[1],
     payload[2],
     payload[3],
     payload[4],
     payload[5],
     payload[6],
     payload[7],
     payload[8],
     payload[9],
     payload[10],
     payload[11],
     dataLen,
     msgChan,
     msgFormat,
     msgType
  ]
  const checksum = calculateChecksum(checksumInput)
  return Buffer.from([].concat(
     [_PACKET_HEAD, _PACKET_HEAD],
     escapeInput(checksumInput),
     [checksum],
     [_PACKET_TAIL, _PACKET_TAIL]
  ))
}

const reset = () => {
  const _CAN_RESET_ID = 0x01FFFEC0
  const payload = Buffer.alloc(12)
  payload.writeUInt32LE(_CAN_RESET_ID, 0)
  const dataLen = 0x04
  const msgChan = 0xFF
  const msgFormat = 0x01
  const msgType = 0x01
  return buildFrame(payload, dataLen, msgChan, msgFormat, msgType)
}


const setSerialRate = (serialRate) => {
  const _CAN_SERIALBPS_ID = 0x01FFFE90
  const payload = Buffer.alloc(12)
  payload.writeUInt32LE(_CAN_SERIALBPS_ID, 0)
  payload.writeUInt32LE(serialRate, 4)
  const dataLen = 0x04
  const msgChan = 0xFF
  const msgFormat = 0x01
  const msgType = 0x01
  return buildFrame(payload, dataLen, msgChan, msgFormat, msgType)
}

const setBitRate = (bitRate) => {
  const _CAN_BAUD_ID = 0x01FFFED0
  const payload = Buffer.alloc(12)
  payload.writeUInt32LE(_CAN_BAUD_ID, 0)
  payload.writeUInt32LE(bitRate, 4)
  const dataLen = 0x04
  const msgChan = 0xFF
  const msgFormat = 0x01
  const msgType = 0x01
  return buildFrame(payload, dataLen, msgChan, msgFormat, msgType)
}

const sendCanFrame = (arbitrationId, data) => {
  const _CAN_BAUD_ID = 0x01FFFED0
  const payload = Buffer.alloc(12)
  payload.writeUInt32LE(arbitrationId, 0)
  payload.writeUInt8(data[0], 4)
  payload.writeUInt8(data[1], 5)
  payload.writeUInt8(data[2], 6)
  payload.writeUInt8(data[3], 7)
  payload.writeUInt8(data[4], 8)
  payload.writeUInt8(data[5], 9)
  payload.writeUInt8(data[6], 10)
  payload.writeUInt8(data[7], 11)
  const dataLen = 0x08
  const msgChan = 0x00
  const msgFormat = 0x00
  const msgType = 0x00
  return buildFrame(payload, dataLen, msgChan, msgFormat, msgType)
}

const controlTransfer = ({device, requestType, request, value, index, data}) => {
  return device.controlTransfer(
    requestType,
    request,
    value,
    index,
    data
  )
}

const transferDataOut = (outEndpoint, frame) => {
  return new Promise((resolve, reject) => {
    //console.debug(`transferDataOut: ${frame.toString('hex')}`)
    outEndpoint.transfer(frame, (err) => {
      if (err) {
        return reject(err)
      }
      resolve()
    })
  })
}

const transferDataIn = (inEndpoint, length) => {
  return new Promise((resolve, reject) => {
    inEndpoint.transfer(length, (err, frame) => {
      if (err) {
        return reject(err)
      }
      //console.debug(`transferDataIn: ${frame.toString('hex')}`)
      resolve(frame)
    })
  })
}

const processBuffer = (buffer, cb) => {
  let state = 'EXPECTING_HEADER'
  let i = 0
  let bytesProcessed = 0
  let payload = []
  while (i < buffer.length) {
    if (state === 'EXPECTING_HEADER') {
      if (buffer[i] !== _PACKET_HEAD || buffer[i + 1] !== _PACKET_HEAD) {
        break
      }
      i += 2
      state = 'READING_PAYLOAD'
    } else if (state === 'READING_PAYLOAD') {
      if (buffer[i] === _PACKET_TAIL && buffer[i + 1] === _PACKET_TAIL) {
        cb(Buffer.from(payload))
        payload = []
        i += 2
        bytesProcessed = i
        state = 'EXPECTING_HEADER'
      } else {
        payload.push(buffer[i])
        i += 1
      }
    }
  }
  return bytesProcessed
}

const run = async () => {
  const deviceList = usb.getDeviceList()
  const device = deviceList.find(device => {
    return device.deviceDescriptor.idProduct === 0xea60 &&
      device.deviceDescriptor.idVendor === 0x10c4
  })
  if (!device) {
    throw new Error('Device not found')
  }
  device.open()
  device.interfaces[0].claim()
  device.controlTransfer = util.promisify(device.controlTransfer)
  const inEndpoint = device.interfaces[0].endpoints.find(endpoint => endpoint.constructor.name === 'InEndpoint')
  const outEndpoint = device.interfaces[0].endpoints.find(endpoint => endpoint.constructor.name === 'OutEndpoint')
  // init device
  const serialRate = 115200
  const bitRate = 1000000
  await controlTransfer({
    device,
    requestType: USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
    request: 0x00,
    index: 0x00,
    value: 0x01,
    data: Buffer.from([])
  })
  await controlTransfer({
    device,
    requestType: USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
    request: 0x07,
    index: 0x00,
    value: 0x03 | 0x0100 | 0x0200,
    data: Buffer.from([])
  })
  await controlTransfer({
    device,
    requestType: USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
    request: 0x01,
    index: 0x00,
    value: 0x384000 / serialRate,
    data: Buffer.from([])
  })
  await transferDataOut(outEndpoint, reset())
  await transferDataOut(outEndpoint, setSerialRate(serialRate))
  await transferDataOut(outEndpoint, setBitRate(bitRate))
  // fill recv buffer
  setTimeout(async () => {
    let buffer = Buffer.from([])
    for (;;) {
      const frame = await transferDataIn(inEndpoint, 64)
      buffer = Buffer.concat([buffer, frame])
      const bytesProcessed = processBuffer(buffer, (frame) => {
        const checksum = frame[frame.length - 1]
        const msgType = frame[frame.length - 2]
        const msgFormat = frame[frame.length - 3]
        const msgChan = frame[frame.length - 4]
        const dataLen = frame[frame.length - 5]
        if (dataLen === 0x08 && msgChan === 0x00 && msgFormat === 0x00 && msgType === 0x00) {
          const unescapedFrame = Buffer.from(unescapeInput(frame))
          const arbitrationId = unescapedFrame.readUInt32LE(0)
          const payload = unescapedFrame.slice(4, 4 + dataLen)
          console.log(`> ${arbitrationId.toString(16)} ${payload.toString('hex')}`)
        }
      })
      buffer = buffer.slice(bytesProcessed)
    }
  }, 0)
  // send messsages to device
  for (;;) {
    const arbitrationId = 0x7E5
    const payload = Buffer.from([0x02, 0x3E, 0x00, 0x55, 0x55, 0x55, 0x55, 0x55])
    await transferDataOut(outEndpoint, sendCanFrame(arbitrationId, payload))
    console.log(`< ${arbitrationId.toString(16)} ${payload.toString('hex')}`)
    await new Promise(resolve => setTimeout(resolve, 50))
  }
}

run()