F030C8T6_Kbus_MIX.git

#include "OrdLidar.h"
 
#include "KMachine.h"
 
int nPosX;
int nPosY;
int nPosZ;
 
int nPosZ1,nPosZ2;
 
int pCount1=0;
int pCount2=0;
 
int eCount1;
int eCount2;
 
int dCount1=0;
int dCount2=0;
 
int vCount1=0;
int vCount2=0;
 
int results[32];
 
 
void Uart3SendPacket(char * str, int len);
void Uart5SendPacket(char * str, int len);
 
//ÕýÏÒ±í£¬0 - 360 ´ú±í 0 - 2PI,½á¹û0 - 1000 ±íÊ¾0 - 1£¬±íÖÐÖ»ÓÐ0 - PI/2¼´1/4ÖÜÆÚµÄÊý¾Ý£¬ÆäËûµÄÊý¾ÝÓÃ¶Ô³ÆºÍ¾µÏñµÃµ½¡£
const int SinTable[] =  
{
0,17,35, 52, 70, 87, 105, 122, 139, 156, 174, 191, 208, 225, 242, 259, 276, 292,
309, 326, 342, 358, 375, 391, 407, 423, 438, 454, 469, 485, 500, 515, 530, 545,
559, 574, 588, 602, 616, 629, 643, 656, 669, 682, 695, 707, 719, 731, 743, 755,
766, 777, 788, 799, 809, 819, 829, 839, 848, 857, 866, 875, 883, 891, 899, 906,
914, 921, 927, 934, 940, 946, 951, 956, 961, 966, 970, 974, 978, 982, 985, 988,
990, 993, 995, 996, 998, 999, 999, 1000, 1000,
1000, 999, 999, 998, 996, 995,
993, 990, 988, 985, 982, 978, 974, 970, 966, 961, 956, 951, 946, 940, 934, 927,
921, 914, 906, 899, 891, 883, 875, 866, 857, 848, 839, 829, 819, 809, 799, 788,
777, 766, 755, 743, 731, 719, 707, 695, 682, 669, 656, 643, 629, 616, 602, 588,
574, 559, 545, 530, 515, 500, 485, 469, 454, 438, 423, 407, 391, 375, 358, 342,
326, 309, 292, 276, 259, 242, 225, 208, 191, 174, 156, 139, 122, 105, 87, 70,
52, 35, 17, 0, -17, -35, -52, -70, -87, -105, -122, -139, -156, -174, -191,
-208, -225, -242, -259, -276, -292, -309, -326, -342, -358, -375, -391, -407,
-423, -438, -454, -469, -485, -500, -515, -530, -545, -559, -574, -588, -602,
-616, -629, -643, -656, -669, -682, -695, -707, -719, -731, -743, -755, -766,
-777, -788, -799, -809, -819, -829, -839, -848, -857, -866, -875, -883, -891,
-899, -906, -914, -921, -927, -934, -940, -946, -951, -956, -961, -966, -970,
-974, -978, -982, -985, -988, -990, -993, -995, -996, -998, -999, -999, -1000,
-1000, -1000, -999, -999, -998, -996, -995, -993, -990, -988, -985, -982, -978,
-974, -970, -966, -961, -956, -951, -946, -940, -934, -927, -921, -914, -906,
-899, -891, -883, -875, -866, -857, -848, -839, -829, -819, -809, -799, -788,
-777, -766, -755, -743, -731, -719, -707, -695, -682, -669, -656, -643, -629,
-616, -602, -588, -574, -559, -545, -530, -515, -500, -485, -469, -454, -438,
-423, -407, -391, -375, -358, -342, -326, -309, -292, -276, -259, -242, -225,
-208, -191, -174, -156, -139, -122, -105, -87, -70, -52, -35, -17
};
 
 
 
char StartCMD[2] = { 0xA5,0x60 };
char StopCMD[2] = { 0xA5,0x65 };
 
 
int StartAngle;
int EndAngle;
 
int OrdLidarStart(int nIdx)
{
    if (nIdx == 0 || nIdx == -1 ) {
        Uart3SendPacket(StartCMD,sizeof(StartCMD));
    }
    if (nIdx == 1 || nIdx == -1) {
        Uart5SendPacket(StartCMD,sizeof(StartCMD));
    }
        return 0;
};
 
 
int OrdLidarStop(int nIdx){
    if (nIdx == 0 || nIdx == -1 ) {
        Uart3SendPacket(StopCMD,sizeof(StopCMD));
    }
    if (nIdx == 1 || nIdx == -1) {
        Uart5SendPacket(StopCMD,sizeof(StopCMD));
    }
    return 0;
};
 
char startFlag0 = 0;
char startFlag1 = 0;
 
#define VALID_DATA_MAX 400
stLidarDot validData0[400];
stLidarDot validData1[400];
 
int nValidCount0 = 0;
int nValidCount1 = 0;
 
#define ORG_SUB_DEGREE 100
#define SUB_DEGREE 64        //64
 
#define START_DEGREE 110
#define END_DEGREE 250
 
#define MAX_DISTANCE 500                //mm
 
//ÕûÊýÇóSINÖµ£¬´øÏßÐÔ²åÖµ¹¦ÄÜ£¬ ÊäÈë0 - 360*64 Îª 0 - 2PI,Ò»¸öÖÜÆÚ;Êä³ö-1000 ÖÁ +1000,´ú±í-1µ½+1;
int sini(int a)
{
    if (a <0) {a += 360 * SUB_DEGREE;}
    a = a % (360 * SUB_DEGREE);
    int b;
    b=a / SUB_DEGREE;
    int xx = a % SUB_DEGREE;
    if (b< 90 )
    {
        int d1= SinTable[b ];
        int d2 =SinTable[b+1 ];
        int d;
        d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
        
        return (d);
    }
    else if (b<180 )
    {    
        int c;
        c=180-b;
        
        int d1= SinTable[c ];
        int d2 =SinTable[c-1 ];
        int d;
        d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
        
        return (d);
    }
    
    else if (b<270)
    {
        int c;
        c=b-180;
        int d1= SinTable[c ];
        int d2 =SinTable[c+1 ];
        int d;
        d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
        
        return (-d);
    }
    else 
    {
        int c;
        c=360 -b;
        int d1= SinTable[c ];
        int d2 =SinTable[c-1 ];
        int d;
        d = ((SUB_DEGREE-xx) * d1 + xx * d2 ) / SUB_DEGREE;
        
        return (-d);
    }
    
//    return a;
}
 
int cosi(int a)
{
    return sini(a+90*SUB_DEGREE);
}
 
int OrdLidarParsePkt(int nLidarIdx, OradarLidarFrame * pLindarPkt, int len1)
{
    int iRet = 0;
    if (pLindarPkt->header != 0x54) return 0;        // check for start sign
    int nDotNum = pLindarPkt->ver_len &0x1f;
    if (len1 != nDotNum*3 +11) return -1;
    if (nDotNum<2) return 0;
    
    int startAngle = (pLindarPkt->start_angle) * SUB_DEGREE / ORG_SUB_DEGREE;    // degree * 100;
    int endAngle = (pLindarPkt->end_angle ) * SUB_DEGREE / ORG_SUB_DEGREE;         // degree * 100;
    if (startAngle >= endAngle)    {eCount1++; return 0;}
    int diffAngle = (endAngle - startAngle);
    int eachAngle = diffAngle / (nDotNum - 1);    
    
    if (nLidarIdx == 0) 
    {
        pCount1++;
        for (int i = 0;i < nDotNum && i < 40 ;i++)
        {
            unsigned char confidence = pLindarPkt->point[i].confidence;
            int angle = eachAngle * i + startAngle;    // degree * 64
            dCount1++;
            
            if (angle < START_DEGREE * SUB_DEGREE )    startFlag0 = 1;
            if (angle >= START_DEGREE * SUB_DEGREE && angle < END_DEGREE * SUB_DEGREE) // &&x>100000&&x<250000)
            {
                if (pLindarPkt->point[i].confidence < 30) continue;
                int value = pLindarPkt->point[i].distance; // distance
                
                int x = value * sini(angle  )/1000;            //     mm
                int z = -value * cosi(angle )/1000;            //  mm
                
                //vectorX.push_back(x);
                //vectorY.push_back(y);
                validData0[nValidCount0].distance = value;
                validData0[nValidCount0].x = x;
                validData0[nValidCount0].y = z;
                if (nValidCount0 < VALID_DATA_MAX - 1 ) nValidCount0 ++;
                //vCount1++;
            }
 
            if (angle > END_DEGREE * SUB_DEGREE)
            {
                if (startFlag0 == 1 && nValidCount0 > 0)
                {
                        vCount1 = nValidCount0;
                    ProcessPos(0,validData0,nValidCount0);
                    
 
                }
                startFlag0 = 0;
                nValidCount0 = 0;
            }
        }
    }
 
    if (nLidarIdx == 1) 
    {
        pCount2++;
        for (int i = 0;i < nDotNum && i < 40 ;i++)
        {
            unsigned char confidence = pLindarPkt->point[i].confidence;
            int angle = eachAngle * i + startAngle;
 
            dCount2++;
            
            if (angle < START_DEGREE * SUB_DEGREE)    startFlag1 = 1;
            if (angle >= START_DEGREE * SUB_DEGREE && angle < END_DEGREE * SUB_DEGREE ) // &&x>100000&&x<250000)
            {
                if (confidence < 30) continue;                
                int value = pLindarPkt->point[i].distance; // distance
 
                int x = value * sini(angle - 180 * SUB_DEGREE )/1000;            //    mm
                int y = value * cosi(angle - 180 * SUB_DEGREE )/1000;            //    mm //vectorX.push_back(x);
                //vectorY.push_back(y);
                validData1[nValidCount1].distance = value; 
                validData1[nValidCount1].x = x;
                validData1[nValidCount1].y = y;
                if (nValidCount1 < VALID_DATA_MAX - 1 ) nValidCount1 ++;                
                //vCount2 ++;
            }
 
            if (angle > END_DEGREE * SUB_DEGREE)
            {
                if (startFlag1 == 1 && nValidCount1 > 0 )
                {
                        vCount2 = nValidCount1;
 
                        int minDisIndex = 0;    int minDistance = 55555;
                        int minZ = 9999; int minZIndex= -1;
                        int planeCount =0;
                    int firstmin =1000, firstmax = -1000;
                        int secondmin = 1000, secondmax = -1000;
                    
                        int firstCount=0; int secondCount =0;    int midCount =0;
                    
                        // Çó minZ ºÍ minDistance;
                        for (int j = 0;j < nValidCount1;j++){
                            if (validData1[j].distance < 50) continue;        // skip too small points;
                            if (validData1[j].y < 40) continue;        // skip too small points;                                                    
                            if (validData1[j].x < -200 || validData1[j].x > 500) continue;                                    
                            if (validData1[j].y < minZ) {    minZIndex = j; minZ = validData1[j].y;    }
                            if (validData1[j].distance < minDistance) {
                                minDisIndex = j; minDistance = validData1[j].distance;
                            }    
                        }
                        
                        for (int j = 0;j < nValidCount1 && minZ<500;j++)
                        {
                            int x = validData1[j].x;
                            int y = validData1[j].y;
                            int d = validData1[j].distance;        
 
                            if (d < 50) continue;        // skip too small points;
                            if (y < 40) continue;        // skip too small points;                                                
                            if (x < -200 || x > 500) continue;                        
                            
                            // Ö»²éÕÒ µ×²¿ 50mm Êý¾Ý
                
                            if (y > minZ + 50) {
                                if ((firstCount >0  && secondCount ==0) 
                                    || (secondCount >0 && firstCount ==0)) {
                                    midCount++;
                                }
                                continue;
                            }
                            planeCount++;
                            if (x > -200 && x < 500) 
                                {
                                if (x < firstmin)  firstmin = x;
                                if (x > secondmax) secondmax = x;
                            }                            
                            
                                // ºóÑØ
                            if (x > -200 && x < 100)
                            {
                                if (validData1[j].x > firstmax)  firstmax =x;
                                firstCount++;
                            }
                                // Ç°ÑØ
                            if (x > 200 && x < 500 )
                            {
                                if (x < secondmin)  secondmin = x;
                                secondCount++;
                            }
 
                        }
                        int avg=0;
                        if (firstCount > 0 && secondCount > 0 && midCount > 0 ){
                            avg = (secondmin + firstmax) / 2;
                        }else if (planeCount>10) {
                            avg = (firstmin + secondmax) /2;
                        }
                    
                        nPosY = (nPosY *3 + avg) /4;
                        if (minZ < 9999) {
                        nPosZ2 = (minZ + nPosZ2 *3)/4;
                        } else  {
                    //        results[4]= nValidCount1;
                    //        results[5]= minDistance;
                            
                        }
//                        nPosY = validData1[minDisIndex].x /1000;
//                        nPosZ2 = validData1[minDisIndex].x /1000;
//                        nPosZ2 = midcount;
                }
                startFlag1 = 0;
                nValidCount1 = 0;
            }
        }
    }    
    nPosZ = ((nPosZ1 + nPosZ2) + nPosZ *2)/4;
    
//    nPosX = pCount1;
//    nPosY = pCount2;
    return iRet;
}