获取温度

背景：SDK将 温度传到底层，底层得到温度后用于温度补偿的算法运算

 

1、定义一个结构体，用于存储温度信息

typedef struct __attribute__((__packed__)) OBTemp_Compesation
{
    char data_laser[4];
    char data_ir[4];
}OBTemp_Compesation;

定义一个结构体，用于存储raw_cmd内容

typedef struct __attribute__((__packed__)) reg_raw_cmd
{
    uint8_t start;
    uint8_t error;
    uint8_t len;
    uint16_t cmd;
    uint8_t data[REG_RAW_CMD_DATA_LEN];
} reg_raw_cmd_t;

 

2、实习获取温度的功能

int raw_cmd_set_temperature()
{
    uint16_t len, off, size;
    float temp_ir, temp_laser;
    len = raw_cmd->len + 1;         //raw_data
    off = raw_cmd->data[0] | raw_cmd->data[1] << 8;//store the offset
    size =  sizeof(OBTemp_Compesation);

    memcpy((char *)&temp_string + off, raw_cmd->data[2], len - 2);

    if(len < 3 || len > 14)
    {
        return -1;
    }

    if((off + len - 2) > size)
    {
        return -2;
    }

    if(off == 0)
    {
        memcpy((char *)&temp_string, 0, size);
    }

    if((off + len - 2) == size)
    {
        memcpy((char *)&temp_ir, &temp_string.data_ir, 4);
        memcpy((char *)&temp_laser, &temp_string.data_laser, 4);
        Temperature_deflection_compensation(temp_ir, temp_laser);
    }

    return 0;
}