开源|带你快速入门和理解并模拟实现GPS户外机器人的定位与导航

while(GLOBAL_SWITCH)
    {   
          if(onceread ==0)
           {
            onceread =1;
              if(is_file_exist("waypoints.csv")!=0)//判断航点的文件是否存在
                {
                    GLOBAL_STATUS=STOP_STATUS;
                    GLOBAL_SWITCH =0 ;
                    onceread =0;
                    DEBUG(LOG_ERR,"waypoint file is not exist \n");
                    break;  
                }
        }
        if((latitude ==0.0)&&(longitude == 0.0))//是否定位成功
        {
              DEBUG(LOG_ERR,"GPS CANNOT LOCATION PLEASE CHECK \n");
                  GLOBAL_STATUS = STOP_STATUS;
                  GLOBAL_SWITCH = 0;
                  break;

        }
          if ((getmillis() - lastSubMillis > SUBSUMPTION_INTERVAL))//定时处理
            {
                switch(GLOBAL_STATUS)
                {
                case STANDBY_STATUS://启动后的初始状态
                    DEBUG(LOG_DEBUG,"STANDBY STATUS \n");
                    lastGPSMillis =0 ;
                    ReadWaypointsFile();//读取航点到队列
                    CalculateHeadingToWaypoint();//计算当前角度和航点的方向
                    CalculateDistanceToWaypoint();//计算当前和航点的距离
                    GLOBAL_STATUS = ROTATE_STATUS ;
                break;
                case CACULATE_STATUS://计算状态
                    DEBUG(LOG_DEBUG,"CACULATE STATUS \n");
                    CalculateHeadingToWaypoint();
                    CalculateDistanceToWaypoint();
                    GLOBAL_STATUS = ROTATE_STATUS ;
                break;
                case ROTATE_STATUS ://旋转状态
                    DEBUG(LOG_DEBUG,"ROTATE_STATUS   \n");
                    if (abs((int)car_heading_simulation - (int)targetHeading) > 10)
                    {

                        RotateDegrees(targetHeading);//这里需要根据求出的角度进行转动，需要旋转的角度位 当前航向角-两点之间的角度，需要做-180~180之间的归一化

                    }else {
                            GLOBAL_STATUS = MOVE_STATUS ;
                    }

                break;
                case MOVE_STATUS ://前进状态  
                    DEBUG(LOG_DEBUG,"MOVE_STATUS   \n");
                    if((waypointRange > 200))//大于100m 认为不合法 所以规划路径时需要注意
                    {  
                        DEBUG(LOG_ERR,"distance > 200m \n");
                         break;
                    }  
                    MoveDistance(waypointRange);

                    break;    

                case WAYPOINTARRIVE_STATUS://读取下一个航点
                    DEBUG(LOG_DEBUG,"WAYPOINTARRIVE_STATUS,currentWaypoint:%d,waypointCount:%d\n",currentWaypoint,waypointCount);
                    if(currentWaypoint < waypointCount-1 )
                    {
                        currentWaypoint ++;
                        DEBUG(LOG_DEBUG,"currentWaypoint ++\n");
                        GLOBAL_STATUS = CACULATE_STATUS ;
                    }
                    else  if(currentWaypoint >= waypointCount-1 ){
                        GLOBAL_STATUS = STOP_STATUS ;
                        DEBUG(LOG_ERR,"currentWaypoint >= waypointCount stop status\n");
                    }
                break;
                case STOP_STATUS :

                break;
                case MANUAL_STATUS :
                break;
                default :
                break;

                }//end switch
                // 必须先运行一次 standby 状态 只有当在目的地附近3米内才会转换状态
                ret =  isInRange(3, latitude , longitude, waypoints[currentWaypoint].latitude, waypoints[currentWaypoint].longitude);
                if (ret == 1) //点在圆圈内
                {   
                    DEBUG(LOG_DEBUG,"arrive into circle scale ,will read next point  \n");
                    GLOBAL_STATUS = WAYPOINTARRIVE_STATUS ;
                }
                waypointlongitude =  waypoints[currentWaypoint].longitude;
                waypointlatitude = waypoints[currentWaypoint].latitude;
                CalculateHeadingToWaypoint();//计算两点之间的角度
                int b_heading = abs((int)car_heading_simulation - (int)targetHeading);//获取当前航向和两点之间的差值
                DEBUG(LOG_DEBUG,"caculate b-heading:%d  ，ret=%d \n",b_heading,ret);
                if ((b_heading > 10 )&&(ret != 1)  )
                {
                    GLOBAL_STATUS = ROTATE_STATUS ;
                }
                lastSubMillis = getmillis();
            }//end  sub loop

      }//end while switch on
}