/*****************************************************************
 * Copyright:2016-2021 www.corvin.cn ROS小课堂
 * Description:使用串口方式读取IMU模块信息.
 * Author: corvin
 * History:
 *   20200401:init this file.
 *   20200702:将读取到的各数据的高低自己合成后要转换为short类型,
 *     这样才能进行/32768.0*16.0运算.
******************************************************************/
#include<ros/ros.h>
#include<stdio.h>
#include<stdlib.h>
#include<fcntl.h>
#include<unistd.h>
#include<termios.h>
#include<string.h>
#include<sys/time.h>
#include<sys/types.h>


unsigned char r_buf[88];  //一次从串口中读取的数据存储缓冲区
static int port_fd = -1; //串口打开时的文件描述符
float acce[3],angular[3],angle[3],quater[4];

/****************************************************
 * Description:打开IMU模块串口,输入两个参数即可连接.
 *   open()打开失败时,返回-1,成功打开时返回文件描述符.
 *   各参数意义如下:
 *   const char *path:IMU设备的挂载地址;
 *   const int baud:串口通讯的波特率;
 ***************************************************/
int openSerialPort(const char *path, const int baud)
{
    int fd = 0;
    struct termios terminfo;
    bzero(&terminfo, sizeof(terminfo));

    fd = open(path, O_RDWR|O_NOCTTY);
    if (-1 == fd)
    {
        ROS_ERROR("Open imu dev error:%s, baud:%d", path, baud);
        return -1;
    }

    //判断当前连接的设备是否为终端设备
    if (isatty(STDIN_FILENO) == 0)
    {
        ROS_ERROR("IMU dev isatty error !");
        return -2;
    }

    /*设置串口通信波特率-bps*/
    switch(baud)
    {
        case 9600:
           cfsetispeed(&terminfo, B9600); //设置输入速度
           cfsetospeed(&terminfo, B9600); //设置输出速度
           break;

        case 57600:
           cfsetispeed(&terminfo, B57600);
           cfsetospeed(&terminfo, B57600);
           break;

       case 115200:
           cfsetispeed(&terminfo, B115200);
           cfsetospeed(&terminfo, B115200);
           break;

       default://设置默认波特率9600
           cfsetispeed(&terminfo, B9600);
           cfsetospeed(&terminfo, B9600);
           break;
    }

    //设置数据位 - 8 bit
    terminfo.c_cflag |= CLOCAL|CREAD;
    terminfo.c_cflag &= ~CSIZE;
    terminfo.c_cflag |= CS8;

    //不设置奇偶校验位 - N
    terminfo.c_cflag &= ~PARENB;

    //设置停止位 - 1
    terminfo.c_cflag &= ~CSTOPB;

    //设置等待时间和最小接收字符
    terminfo.c_cc[VTIME] = 0;
    terminfo.c_cc[VMIN]  = 1;

    //清除串口缓存的数据
    tcflush(fd, TCIFLUSH);

    if((tcsetattr(fd, TCSANOW, &terminfo)) != 0)
    {
        ROS_ERROR("set imu serial port attr error !");
        return -3;
    }

    return fd;
}

/**************************************
 * Description:关闭串口文件描述符.
 *************************************/
int closeSerialPort()
{
    int ret = close(port_fd);
    return ret;
}

/*****************************************************
 * Description:向串口中发送数据.
 ****************************************************/
int send_data(int fd, char *send_buffer, int length)
{
	length = write(fd, send_buffer, length*sizeof(unsigned char));
	return length;
}

/*******************************************************
 * Description:从串口中接收数据.
 *******************************************************/
int recv_data(int fd, unsigned char* recv_buffer, int len)
{
    int length = read(fd, recv_buffer, len);
    return length;
}

/************************************************************
 * Description:根据串口数据协议来解析有效数据,这里共有4种数据.
 *   解析读取其中的44个字节,正好是4帧数据,每一帧数据是11个字节.
 *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
 *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
 ***********************************************************/
void parse_serialData(unsigned char chr)
{
    static unsigned char chrBuf[100];
    static unsigned char chrCnt = 0;

    signed short sData[4]; //save 8 Byte valid info
    unsigned char i = 0;
    unsigned char frameSum = 0;

    chrBuf[chrCnt++] = chr; //保存当前字节,字节计数加1

    //判断是否满一个完整数据帧11个字节
    if(chrCnt < 11)
        return;

    //计算数据帧的前十个字节的校验和
    for(i=0; i<10; i++)
    {
        frameSum += chrBuf[i];
    }

    //找到数据帧第一个字节是0x55,校验和是否正确,若两者有一个不正确,
    //都需要移动到最开始的字节,再读取新的字节进行判断数据帧完整性
    if ((chrBuf[0] != 0x55)||(frameSum != chrBuf[10]))
    {
        memcpy(&chrBuf[0], &chrBuf[1], 10); //将有效数据往前移动1字节位置
        chrCnt--; //字节计数减1,需要再多读取一个字节进来,重新判断数据帧
        return;
    }

    //for(i=0;i<11; i++)
    //  printf("0x%x ",chrBuf[i]);
    //printf("\n");

    memcpy(&sData[0], &chrBuf[2], 8);
    switch(chrBuf[1])
    {
        case 0x51: //x,y,z轴 加速度输出
            acce[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0*16.0;
            acce[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0*16.0;
            acce[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0*16.0;
            break;

        case 0x52: //角速度输出
            angular[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0*2000.0;
            angular[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0*2000.0;
            angular[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0*2000.0;
            break;

        case 0x53: //欧拉角度输出, roll, pitch, yaw
            angle[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0*180.0;
            angle[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0*180.0;
            angle[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0*180.0;
            break;

        case 0x59: //四元素输出
            quater[0] = ((short)(((short)chrBuf[3]<<8)|chrBuf[2]))/32768.0;
            quater[1] = ((short)(((short)chrBuf[5]<<8)|chrBuf[4]))/32768.0;
            quater[2] = ((short)(((short)chrBuf[7]<<8)|chrBuf[6]))/32768.0;
            quater[3] = ((short)(((short)chrBuf[9]<<8)|chrBuf[8]))/32768.0;
            break;
    }
    chrCnt = 0;
}

/*****************************************************************
 * Description:根据串口配置信息来连接IMU串口,准备进行数据通信,
 *   两个输入参数的意义如下:
 *   const char* path:IMU设备的挂载地址;
 *   const int baud:IMU模块的串口通信波特率;
 ****************************************************************/
int initSerialPort(const char* path, const int baud)
{
    port_fd = openSerialPort(path, baud);
    if(port_fd < 0)
    {
        ROS_ERROR("openSerialPort error !");
        return -1;
    }

    return 0;
}

float getAccX()
{
    return acce[0];
}
float getAccY()
{
    return acce[1];
}
float getAccZ()
{
    return acce[2];
}

float getAngularX()
{
    return angular[0];
}

float getAngularY()
{
    return angular[1];
}

float getAngularZ()
{
    return angular[2];
}

float getAngleX()
{
    return angle[0];
}
float getAngleY()
{
    return angle[1];
}
float getAngleZ()
{
    return angle[2];
}

/********************************************************
 * Description:从串口读取数据,然后解析出各有效数据段,这里
 *   一次性从串口中读取100个字节,然后需要从这些字节中进行
 *   解析读取44个字节,正好是4帧数据,每一帧数据是11个字节.
 *   有效数据包括加速度输出(0x55 0x51),角速度输出(0x55 0x52)
 *   角度输出(0x55 0x53),四元素输出(0x55 0x59).
 ********************************************************/
int getImuData()
{
    int data_len = 0;

    bzero(r_buf, sizeof(r_buf)); //存储新数据前,将缓冲区清零

    //开始从串口中读取数据,并将其保存到r_buf缓冲区中
    data_len = recv_data(port_fd, r_buf, sizeof(r_buf));
    if(data_len == -1)
    {
        ROS_ERROR("read serial port data failed !\n");
        closeSerialPort();
        return -1;
    }

    //printf("recv data len:%d\n", data_len);
    for (int i=0; i<data_len; i++)
    {
        //printf("0x%02x ", r_buf[i]);
        parse_serialData(r_buf[i]);
    }
    //printf("\n\n");

    return 0;
}

