Measure/include/message/lidar_rs_helios.cpp

#include "lidar_rs_helios.h"

bool LidarMessageRsHelios::packet(void *data, int size) {
    unsigned char *cloud = (unsigned char *) data;
    // security checks
    if (cloud == nullptr || size < 0) {
        printf("---Debug error %s %d\n", __func__, __LINE__);
        return false;
    }

    // clear m_data_
    if (m_data_ != nullptr) {
        free(m_data_);
        m_data_ = nullptr;
    }

    // pack data
    bool ret = false;
    ret = packHelios16P(cloud, size);

    return true;
}

bool LidarMessageRsHelios::checkFrameHeader(unsigned char *data) {
    // header ID
    if (!(data[0] == 0X55 && data[1] == 0XAA && data[2] == 0X05 && data[3] == 0X5A)) {
        printf("---Debug error %s %d\n", __func__, __LINE__);
        printf("---Debug Head: %#X %#X %#X %#X \n", data[0], data[1], data[2], data[3]);
        return false;
    }

    // 协议版本号
    if (!(data[4] == 0X00 && data[5] == 0X01)) {
        printf("---Debug error %s %d\n", __func__, __LINE__);
        return false;
    }

    // Lidar Type && Lidar Model
    if (!(data[31] == 0X06 && data[32] > 0X00 && data[32] < 0X05)) {
        printf("---Debug error %s %d\n", __func__, __LINE__);
        return false;
    }

    // Tail
    if (!(data[1246] == 0X00 && data[1247] == 0XFF)) {
        printf("---Debug error %s %d\n", __func__, __LINE__);
        return false;
    }

    return true;
}

bool LidarMessageRsHelios::packHelios16P(unsigned char *data, int size) {
    m_length_ = size + ZX::PACKET_HEAD_SIZE + ZX::PACKET_END_SIZE;
    m_data_ = (unsigned char *) malloc(m_length_);
    memset(m_data_, 0, m_length_);
    packInfo(ZX::LIDAR_RS_HELIOS_16P, size / 7, 0X01);

    for (int i = 0; i < size; i++) {
        m_data_[ZX::PACKET_HEAD_SIZE + i] = data[i];
    }

    return true;
}

void LidarMessageRsHelios::parseDualDataBlock(unsigned char *data) {

}

void LidarMessageRsHelios::parseSingleDataBlock(unsigned char *data) {
    unsigned short distance, roll = 0, yaw = 0, yaw_delta = 0;
    int coor_data = 42, coor_m_data = ZX::PACKET_HEAD_SIZE;

    for (int i = 0; i < 11; i++) {
        unsigned int angle_a = (data[44 + 100 * i] << 8 | data[45 + 100 * i]);
        unsigned int angle_b = (data[144 + 100 * i] << 8 | data[145 + 100 * i]);
        // printf("---Debug %d %#X \n", i, angle_a);
        if (angle_b < angle_a) {
            angle_b += 36000;
        }

        yaw_delta += (angle_b - angle_a);
    }

    yaw_delta = yaw_delta / 22;

    for (int i = 0; i < 12; i++) {
        // get yaw
        yaw = data[coor_data + 2] << 8 | data[coor_data + 3];
        coor_data += 4;

        for (int j = 0; j < 32; j++) {
            // prase distance
            m_data_[coor_m_data + 0] = data[coor_data + 0];
            m_data_[coor_m_data + 1] = data[coor_data + 1];
            // prase roll
            m_data_[coor_m_data + 2] = roll_angle[j % 16] >> 8;
            m_data_[coor_m_data + 3] = roll_angle[j % 16];

            // prase yaw
            if (j > 15) {
                m_data_[coor_m_data + 4] = (yaw + yaw_delta) >> 8;
                m_data_[coor_m_data + 5] = (yaw + yaw_delta);
            } else {
                m_data_[coor_m_data + 4] = (yaw) >> 8;
                m_data_[coor_m_data + 5] = (yaw);
            }

            // prase i
            m_data_[coor_m_data + 6] = data[coor_data + 2];

            coor_m_data += 7;
            coor_data += 3;
        }
    }

}