modularization/src/driver/driver_lidar_bk_16z/common/ssFrameLib.cpp

/* -*- mode: C++ -*-
 *  All right reserved, Sure_star Coop.
 *  @Technic Support: <sdk@isurestar.com>
 *  $Id$
 */

#include "ssFrameLib.h"

int checkSum(unsigned char *_dataBuf, int count_)
{
  int rtn = 0;
  for (int i = 0; i < count_; i++)
  {
    rtn += _dataBuf[i];
  }
  rtn = rtn & 0xFF;
  return rtn;
}

lidarAPi::DEB_FRAME_S packDEBV3Frame(lidarAPi::SCD_FRAME_TYPE_E flag, int regAddress, int regData)
{
  lidarAPi::DEB_FRAME_S tmpFrame_;
  memset(&tmpFrame_, 0, sizeof(lidarAPi::DEB_FRAME_S));

  switch (flag)
  {
  case lidarAPi::eCmdWrite:
    tmpFrame_.msgHead = DEB_FRAME_WRITE;
    break;
  case lidarAPi::eCmdRead:
    tmpFrame_.msgHead = DEB_FRAME_READ;
    break;
  case lidarAPi::eCmdQuery:
    tmpFrame_.msgHead = DEB_FRAME_READ;
    break;
  default:
    tmpFrame_.msgHead = DEB_FRAME_WRITE;
    break;
  }
  unsigned short tmpSData = 0;
  tmpSData = regAddress;

  tmpFrame_.regAddress = tmpSData;

  tmpFrame_.regData = regData;

  unsigned char *tmpBase = (unsigned char *)&tmpFrame_;
  tmpFrame_.msgCheckSum = checkSum(tmpBase + 2, sizeof(lidarAPi::DEB_FRAME_S) - 2);
  return tmpFrame_;
}