modularization/src/driver/driver_lidar_bk_16z/lidar_16zdriver.cpp

#include "lidar_16zdriver.h"

static uint32_t s_CrcTab32[] = {
    0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005,
    0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD,
    0x4C11DB70, 0x48D0C6C7, 0x4593E01E, 0x4152FDA9, 0x5F15ADAC, 0x5BD4B01B, 0x569796C2, 0x52568B75,
    0x6A1936C8, 0x6ED82B7F, 0x639B0DA6, 0x675A1011, 0x791D4014, 0x7DDC5DA3, 0x709F7B7A, 0x745E66CD,
    0x9823B6E0, 0x9CE2AB57, 0x91A18D8E, 0x95609039, 0x8B27C03C, 0x8FE6DD8B, 0x82A5FB52, 0x8664E6E5,
    0xBE2B5B58, 0xBAEA46EF, 0xB7A96036, 0xB3687D81, 0xAD2F2D84, 0xA9EE3033, 0xA4AD16EA, 0xA06C0B5D,
    0xD4326D90, 0xD0F37027, 0xDDB056FE, 0xD9714B49, 0xC7361B4C, 0xC3F706FB, 0xCEB42022, 0xCA753D95,
    0xF23A8028, 0xF6FB9D9F, 0xFBB8BB46, 0xFF79A6F1, 0xE13EF6F4, 0xE5FFEB43, 0xE8BCCD9A, 0xEC7DD02D,
    0x34867077, 0x30476DC0, 0x3D044B19, 0x39C556AE, 0x278206AB, 0x23431B1C, 0x2E003DC5, 0x2AC12072,
    0x128E9DCF, 0x164F8078, 0x1B0CA6A1, 0x1FCDBB16, 0x018AEB13, 0x054BF6A4, 0x0808D07D, 0x0CC9CDCA,
    0x7897AB07, 0x7C56B6B0, 0x71159069, 0x75D48DDE, 0x6B93DDDB, 0x6F52C06C, 0x6211E6B5, 0x66D0FB02,
    0x5E9F46BF, 0x5A5E5B08, 0x571D7DD1, 0x53DC6066, 0x4D9B3063, 0x495A2DD4, 0x44190B0D, 0x40D816BA,
    0xACA5C697, 0xA864DB20, 0xA527FDF9, 0xA1E6E04E, 0xBFA1B04B, 0xBB60ADFC, 0xB6238B25, 0xB2E29692,
    0x8AAD2B2F, 0x8E6C3698, 0x832F1041, 0x87EE0DF6, 0x99A95DF3, 0x9D684044, 0x902B669D, 0x94EA7B2A,
    0xE0B41DE7, 0xE4750050, 0xE9362689, 0xEDF73B3E, 0xF3B06B3B, 0xF771768C, 0xFA325055, 0xFEF34DE2,
    0xC6BCF05F, 0xC27DEDE8, 0xCF3ECB31, 0xCBFFD686, 0xD5B88683, 0xD1799B34, 0xDC3ABDED, 0xD8FBA05A,
    0x690CE0EE, 0x6DCDFD59, 0x608EDB80, 0x644FC637, 0x7A089632, 0x7EC98B85, 0x738AAD5C, 0x774BB0EB,
    0x4F040D56, 0x4BC510E1, 0x46863638, 0x42472B8F, 0x5C007B8A, 0x58C1663D, 0x558240E4, 0x51435D53,
    0x251D3B9E, 0x21DC2629, 0x2C9F00F0, 0x285E1D47, 0x36194D42, 0x32D850F5, 0x3F9B762C, 0x3B5A6B9B,
    0x0315D626, 0x07D4CB91, 0x0A97ED48, 0x0E56F0FF, 0x1011A0FA, 0x14D0BD4D, 0x19939B94, 0x1D528623,
    0xF12F560E, 0xF5EE4BB9, 0xF8AD6D60, 0xFC6C70D7, 0xE22B20D2, 0xE6EA3D65, 0xEBA91BBC, 0xEF68060B,
    0xD727BBB6, 0xD3E6A601, 0xDEA580D8, 0xDA649D6F, 0xC423CD6A, 0xC0E2D0DD, 0xCDA1F604, 0xC960EBB3,
    0xBD3E8D7E, 0xB9FF90C9, 0xB4BCB610, 0xB07DABA7, 0xAE3AFBA2, 0xAAFBE615, 0xA7B8C0CC, 0xA379DD7B,
    0x9B3660C6, 0x9FF77D71, 0x92B45BA8, 0x9675461F, 0x8832161A, 0x8CF30BAD, 0x81B02D74, 0x857130C3,
    0x5D8A9099, 0x594B8D2E, 0x5408ABF7, 0x50C9B640, 0x4E8EE645, 0x4A4FFBF2, 0x470CDD2B, 0x43CDC09C,
    0x7B827D21, 0x7F436096, 0x7200464F, 0x76C15BF8, 0x68860BFD, 0x6C47164A, 0x61043093, 0x65C52D24,
    0x119B4BE9, 0x155A565E, 0x18197087, 0x1CD86D30, 0x029F3D35, 0x065E2082, 0x0B1D065B, 0x0FDC1BEC,
    0x3793A651, 0x3352BBE6, 0x3E119D3F, 0x3AD08088, 0x2497D08D, 0x2056CD3A, 0x2D15EBE3, 0x29D4F654,
    0xC5A92679, 0xC1683BCE, 0xCC2B1D17, 0xC8EA00A0, 0xD6AD50A5, 0xD26C4D12, 0xDF2F6BCB, 0xDBEE767C,
    0xE3A1CBC1, 0xE760D676, 0xEA23F0AF, 0xEEE2ED18, 0xF0A5BD1D, 0xF464A0AA, 0xF9278673, 0xFDE69BC4,
    0x89B8FD09, 0x8D79E0BE, 0x803AC667, 0x84FBDBD0, 0x9ABC8BD5, 0x9E7D9662, 0x933EB0BB, 0x97FFAD0C,
    0xAFB010B1, 0xAB710D06, 0xA6322BDF, 0xA2F33668, 0xBCB4666D, 0xB8757BDA, 0xB5365D03, 0xB1F740B4};
#define IS_CRC_START_32 0xFFFFFFFF
#define PACKAGELEN 60000

lidar_16zdriver::lidar_16zdriver() {
    setparam();
    socketInit();
    configDeviceParams();

    mpThreadRaw = new std::thread(&lidar_16zdriver::RecvRawThread,this);
}

void lidar_16zdriver::socketInit()
{
    m_ctl_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.ctrlport, m_input_para.ctrlport);
    m_data_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.dataport, m_input_para.dataport);
    m_heart_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.heart_port, m_input_para.heart_port);
}

void lidar_16zdriver::setparam()
{
    m_input_para.device_name ="R-Fans-16";
    m_input_para.display_mode = "overlay";

    m_input_para.device_ip = "192.168.0.10";
    m_input_para.dataport = 2014;
    m_input_para.ctrlport = 2013;
    m_input_para.heart_port = 2030;

    m_input_para.frame_id =  "world";

    m_input_para.save_xyz = false;
    m_input_para.scnSpeed = 10;
    m_input_para.simu_filepath = "";
    m_input_para.cfg_path = "";
    m_input_para.dual_echo = false;
    m_input_para.export_path = "";
    m_input_para.read_once = false;
    m_input_para.save_isf = false;
    m_input_para.isf_path = "";
    m_input_para.data_format = 0;
    m_input_para.decrangetype = 1;
    m_input_para.leveltype = 0;
    m_input_para.levelvec = "1,0,0,0,0,0,0,0";
    m_input_para.chn_corr_table_path = "";
    m_input_para.window_pulsewidth_table_path = "";
    m_input_para.isFaceToPS = false;
    m_input_para.lsrFreq = 1000;
    m_input_para.is_trans = false;
    m_input_para.is_ntp = false;

    m_input_para.anlign_angle_x = 0;
    m_input_para.anlign_angle_y = 0;
    m_input_para.anlign_angle_z = 0;
    m_input_para.groud_distance = -1.9;
    m_input_para.road_type = 0 ;

    m_input_para.use_algorithm = false;
    m_input_para.algorithmType = 0;

    m_input_para.Level0C = false;
    m_input_para.Level0G = false;
    m_input_para.Level0F = false;

    m_input_para.Level1A = false;
    m_input_para.Level1B = false;
    m_input_para.Level1F = false;
    m_input_para.Level1G = false;
    m_input_para.Level1L = false;
    m_input_para.Level1U = false;
    m_input_para.Level1Q = false;

    m_input_para.Level2A = false;
    m_input_para.Level2D = false;

    m_input_para.LevelAB = false;
    m_input_para.LevelAC = false;
    m_input_para.LevelAG = false;
    m_input_para.LevelAH = false;
    m_input_para.LevelAI = false;
    m_input_para.LevelAJ = false;
    m_input_para.LevelAM = false;
}

void lidar_16zdriver::configDeviceParams()
{

    lidarAPi::DEB_PROGRM_S params;
    params.cmdstat = lidarAPi::eDevCmdWork;
    params.lsrFreq = m_input_para.lsrFreq;
    params.scnSpeed = m_input_para.scnSpeed;
    setState(params);

    return ;


    bool dual_echo = m_input_para.dual_echo;

    params.cmdstat = lidarAPi::eDevCmdWork;
    params.dataFormat = lidarAPi::eFormatCalcData;

    // set start rps
    params.scnSpeed = m_input_para.scnSpeed;
    progSet(params);

    unsigned int regData = 0;
    regData = 0;
    if (dual_echo)
    {
        regData = CFANS_DUAL;
    }
    else
    {
        regData = CFANS_ECHO;
    }

    m_ctl_socket->HW_WRREG(0, REG_DATA_LEVEL, regData);
    m_ctl_socket->HW_WRREG(0, REG_DATA_LEVEL_OLD, regData);
}


int lidar_16zdriver::progSet(lidarAPi::DEB_PROGRM_S &program)
{
    unsigned int tmpData = 0;
    if ((m_input_para.device_name == "R-Fans-32") || (m_input_para.device_name == "R-Fans-16"))
    {
        switch (program.scnSpeed)
        {
        case ANGLE_SPEED_0HZ:
            tmpData |= CMD_SCAN_DISABLE;
            tmpData |= CMD_SCAN_SPEED_10HZ;
        case ANGLE_SPEED_10HZ:
            tmpData |= CMD_SCAN_ENABLE;
            tmpData |= CMD_SCAN_SPEED_10HZ;
            break;
        case ANGLE_SPEED_20HZ:
            tmpData |= CMD_SCAN_ENABLE;
            tmpData |= CMD_SCAN_SPEED_20HZ;
            break;
        case ANGLE_SPEED_5HZ:
            tmpData |= CMD_SCAN_ENABLE;
            tmpData |= CMD_SCAN_SPEED_5HZ;
            break;
        default:
            tmpData |= CMD_SCAN_ENABLE;
            tmpData |= CMD_SCAN_SPEED_10HZ;
            break;
        }
    }
    else if ((m_input_para.device_name == "C-Fans-128") || (m_input_para.device_name == "C-Fans-32") || (m_input_para.device_name == "C-Fans-256"))
    {
        switch (program.scnSpeed)
        {
        case ANGLE_SPEED_0HZ_cfan:
            tmpData |= CMD_SCAN_DISABLE;
            tmpData |= CMD_SCAN_SPEED_10HZ_C;
            break;
        case ANGLE_SPEED_10HZ_cfan:
            tmpData |= CMD_SCAN_ENABLE;
            tmpData |= CMD_SCAN_SPEED_10HZ_C;
            break;
        case ANGLE_SPEED_20HZ_cfans:
            tmpData |= CMD_SCAN_ENABLE;
            tmpData |= CMD_SCAN_SPEED_20HZ_C;
            break;
        case ANGLE_SPEED_40HZ_cfans:
            tmpData |= CMD_SCAN_ENABLE;
            tmpData |= CMD_SCAN_SPEED_40HZ_C;
            break;
        case ANGLE_SPEED_60HZ_cfans:
            tmpData |= CMD_SCAN_ENABLE;
            tmpData |= CMD_SCAN_SPEED_60HZ_C;
            break;
        case ANGLE_SPEED_80HZ_cfans:
            tmpData |= CMD_SCAN_ENABLE;
            tmpData |= CMD_SCAN_SPEED_80HZ_C;
            break;
        default:
            tmpData |= CMD_SCAN_ENABLE;
            tmpData |= CMD_SCAN_SPEED_40HZ_C;
            break;
        }
    }

    switch (program.cmdstat)
    {
    case lidarAPi::eDevCmdWork: // 默认为工作状态，此值在QT中设为常量
        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL_OLD, tmpData);
        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL, tmpData);
        break;
    case lidarAPi::eDevCmdIdle: // 待机状态
        tmpData = CMD_RCV_CLOSE;
        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL_OLD, tmpData);
        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL, tmpData);
        break;
    case lidarAPi::eDevCmdAsk:
        break;
    default:
        break;
    }

    return 0;
}

uint32_t isCrc32(uint8_t const *pIn, uint16_t len)
{
    if (pIn == NULL)
    {
        printf("%s:%d pIn is NULL\n", __FILE__, __LINE__);
        return 0;
    }

    uint32_t crc;
    uint32_t tmp;
    uint32_t longC;
    uint8_t const *ptr;
    uint16_t a;

    crc = IS_CRC_START_32;
    ptr = pIn;

    if (ptr != NULL)
        for (a = 0; a < len; a++)
        {

            longC = 0x000000FFL & (uint32_t)*ptr;
            tmp = crc ^ longC;
            crc = (crc >> 8) ^ s_CrcTab32[tmp & 0xff];

            ptr++;
        }

    crc ^= 0xffffffffL;

    return crc & 0xffffffffL;
}

int lidar_16zdriver::lidar_run(SCDEV_CMD_E me_cmd, float mt_scnspd, int mt_lsrfreq, int mt_lsrpwr)
{
    LIDAR_COMMAND_S cmd;
    memset(&cmd.commander, 0, sizeof(LIDAR_CMDRPT_U));
    cmd.commander.cmdId = eMsgProgam;
    cmd.commander.isAsk = 0;
    cmd.commander.prgCtrl.cmdID = me_cmd;
    cmd.commander.prgCtrl.echo_etype = 8;
    cmd.commander.prgCtrl.laserFreqKHz = mt_lsrfreq;
    cmd.commander.prgCtrl.laserFirePwr = mt_lsrpwr;
    cmd.commander.prgCtrl.scanAngleSpeed = mt_scnspd;
    cmd.check_sum = isCrc32((uint8_t *)&cmd.commander, sizeof(LIDAR_CMDRPT_U));
    return m_ctl_socket->write((unsigned char *)&cmd, sizeof(LIDAR_COMMAND_S));
}

int lidar_16zdriver::setState(lidarAPi::DEB_PROGRM_S &program)
{
    switch (program.cmdstat)
    {
    case lidarAPi::eDevCmdWork:
        lidar_run(SCDEV_CMD_E::eDevCmdWork, program.scnSpeed, program.lsrFreq);
        break;
    case lidarAPi::eDevCmdIdle:
        lidar_run(SCDEV_CMD_E::eDevCmdIdle, program.scnSpeed, program.lsrFreq);
        break;
    case lidarAPi::eDevCmdAsk:
        break;
    default:
        break;
    }
    return 0;
}


int lidar_16zdriver::RecvRawThread()
{
    const int MAXPACLEN = 60000;
    while(mbRun)
    {
        std::shared_ptr<unsigned char> pread = std::shared_ptr<unsigned char>(new unsigned  char[MAXPACLEN]);
        int ret = m_data_socket->read(pread.get(), MAXPACLEN);
        // cout << ret << " " << ros::Time::now() << endl;
        if (ret <= 0)
        {
            std::cout<<" configure. "<<std::endl;
            usleep(500);
            configDeviceParams();
            continue;
        }

        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" recv "<<ret<<std::endl;
        std::cout<<" mod: "<<ret%1248<<std::endl;

    }
}