controller

src/controller/controller_Geely_xingyueL/include/car_control.h

@@ -5,6 +5,13 @@
 #include "kx_11_can_frame/asdm_sim_canfd_frame.h"
 #include <thread>
 
+enum class AppCtrlSm   //cxw,20220622
+{
+  kStandby = 0,
+  kStartup = 1,
+  kActive = 2,
+  kShutDown = 3
+};
 
 enum class StsMach
 {

src/controller/controller_Geely_xingyueL/main.cpp

@@ -15,6 +15,7 @@
 #include "include/car_control.h"
 
 CarControl car_control_module;
+AppCtrlSm app_ctrl_car_sm_ = AppCtrlSm::kStandby;
 
 using namespace std;
 
@@ -40,6 +41,20 @@ double lastEpsSetVal = 0;
 
 GearPrkgAssistReq lastgearSetVal= GearPrkgAssistReq::kNoRequest;
 
+const int gCommunctionNum = 10;
+int communicate_cnt=0; //判断decition中是否有决策，超过设定的数值以后认为没有决策退出控制车辆
+bool communicate_state=0;
+
+
+void quit_ctrl()  //退出车辆控制，cxw,20220622
+{
+    car_control_module.set_lat_lgt_ctrl_req(false);//握手请求, true:请求握手， false:退出握手
+    car_control_module.set_target_gear( GearPrkgAssistReq::kNoRequest);// 目标档位请求
+    car_control_module.set_target_pinion_ag_in_deg(car_control_module.get_current_steer_ang_in_deg());//设置目标方向盘角度请求， -450°至+450°（车辆实测范围值），方向盘左正，右负
+    car_control_module.set_target_acc_mps2(0);//// 目标加减速度值
+    car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);// 转向灯控制请求
+}
+
 static void ShareChassis(void * pa,iv::chassis  * pchassis)
 {
     char * str;
@@ -118,7 +133,7 @@ void executeDecition(const iv::brain::decition decition)
     }
     // gearSetVal=GearPrkgAssistReq::kTargetGearD;
 
-    if(abs(EpsSetVal-Steer_current_value)<90)
+    if(abs(EpsSetVal-Steer_current_value)<90)//限制车辆方向盘转角，cxw,20220622
     {
         ;
     }
@@ -129,6 +144,8 @@ void executeDecition(const iv::brain::decition decition)
         else
             EpsSetVal=Steer_current_value-90;
     }
+
+
     car_control_module.set_target_gear(gearSetVal);
     car_control_module.set_target_acc_mps2(speedSetVal);
     car_control_module.set_target_pinion_ag_in_deg(EpsSetVal);
@@ -184,20 +201,21 @@ void sendthread()
     iv::brain::decition xdecition;
 //    car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
 //    car_control_module.sm_task_20ms();  // 线控状态机函数
-QTime time;
-time.start();
+//QTime time;
+//time.start();
 
     while(1)
     {
-        int t = time.elapsed();
-        if(t > 21)
-            qDebug("%d",t);
-        time.restart();
+//        int t = time.elapsed();
+//        if(t > 21)
+//            qDebug("%d",t);
+//        time.restart();
         if(gnDecitionNum <= 0)
         {
             gdecition_def.set_wheelangle(car_control_module.get_current_steer_ang_in_deg());
             xdecition.CopyFrom(gdecition_def);
             std::cout<<"copy from gdecition_def@@@@@@@@@@@@"<<std::endl;
+            communicate_cnt++;
         }
         else
         {
@@ -206,23 +224,152 @@ time.start();
             std::cout<<"copy from ADAS gdecition"<<std::endl;
             gMutex.unlock();
             gnDecitionNum--;
+            communicate_cnt=0;
+            communicate_state=1;
         }
-
-        bool bstatus = car_control_module.is_lat_lgt_ctrl_active();
-
-        if(bstatus == true)
+        if(communicate_cnt>gCommunctionNum)
         {
-            //            std::cout<<"di pan ke kong "<<std::endl;
-            executeDecition(xdecition);
+            communicate_state=0;   //说明decition无决策
+            communicate_cnt=gCommunctionNum+1;
         }
-        else
+
+        switch (app_ctrl_car_sm_)
         {
-            std::cout<<" lat lgt req. status: "<<(int)car_control_module.get_chassis_err_state()
-                    <<"  machine state: "<<(int)car_control_module.get_chassis_statemachine_sts()<< std::endl;
-            car_control_module.set_lat_lgt_ctrl_req(true);
+            case AppCtrlSm::kStandby://ctrl_car节点有消息输入，否则退出与汽车的控制
+                if (communicate_state!=0)
+                {
+                    app_ctrl_car_sm_ = AppCtrlSm::kStartup;
+                }
+                else
+                {
+                    quit_ctrl();
+                }
+                break;
+            case AppCtrlSm::kStartup:
+                if (car_control_module.is_lat_lgt_ctrl_active()) //底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
+                {
+                    app_ctrl_car_sm_ = AppCtrlSm::kActive;
+                }
+                else {
+                    if (!communicate_state){//通讯失联
+                        app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
+                    }
+                    else {
+                        car_control_module.set_lat_lgt_ctrl_req(true);
+                    }
+                    // app_ctrl_car_sm_ = AppCtrlSm::kStandby;
+                    // quit_ctrl();
+                }
+                break;
+            case AppCtrlSm::kActive:
+                if (!(communicate_state && car_control_module.is_lat_lgt_ctrl_active()))//通讯失联或底盘不可控
+                {
+                    app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
+                    if(!car_control_module.is_lat_lgt_ctrl_active())
+                    {
+                        //ctrl_active_state_ =false;
+                        quit_ctrl();
+                        //ROS_INFO("control exit ");
+                    }
+                }
+                else
+                {
+                    executeDecition(xdecition);
+//                    if(gear != (int)receive_data.twist.linear.z)
+//                    {
+//                        if(car_control_obj_.get_current_vehicle_spd_in_ms()!=0)
+//                        {
+//                            if(car_control_obj_.get_current_vehicle_spd_in_ms() > 10) car_control_obj_.set_target_acc_mps2(-3);
+//                            else if(car_control_obj_.get_current_vehicle_spd_in_ms() > 5) car_control_obj_.set_target_acc_mps2(-2);
+//                            else if(car_control_obj_.get_current_vehicle_spd_in_ms() > 2) car_control_obj_.set_target_acc_mps2(-1);
+//                            else car_control_obj_.set_target_acc_mps2(-0.5);
+//                        }
+//                        else{
+//                            gear = (int)receive_data.twist.linear.z;
+//                        }
+//                    }
+//                    else
+//                    {
+//                        switch ((int)receive_data.twist.linear.z)//设置挡位
+//                        {
+//                            case 1: // P Gear
+//                                car_control_obj_.set_target_gear( GearPrkgAssistReq::kTargetGearP);
+//                                break;
+//                            case 2: // R Gear
+//                                car_control_obj_.set_target_gear( GearPrkgAssistReq::kTargetGearR);
+//                                break;
+//                            case 3: // N Gear
+//                                car_control_obj_.set_target_gear( GearPrkgAssistReq::kNoRequest);
+//                                break;
+//                            case 4: // D Gear
+//                                car_control_obj_.set_target_gear( GearPrkgAssistReq::kTargetGearD);
+//                                break;
+//                            default:
+//                                car_control_obj_.set_target_gear( GearPrkgAssistReq::kTargetGearP);
+//                                break;
+//                        }
+
+//                        // pinion angle request  设置目标方向盘角度请求
+//                        car_control_obj_.set_target_pinion_ag_in_deg((int)receive_data.twist.angular.z);//
+//                        if(receive_data.twist.angular.z > 5)
+//                        {
+//                            car_control_obj_.set_turn_light_status(TurnLightIndicReq::kLeft);
+//                        }
+//                        else if (receive_data.twist.angular.z < -5)
+//                        {
+//                            car_control_obj_.set_turn_light_status(TurnLightIndicReq::kRight);
+//                        }
+//                        else
+//                        {
+//                            car_control_obj_.set_turn_light_status(TurnLightIndicReq::kOff);
+//                        }
+
+//                        //设置加减速
+//                        var_ = keep_speed(fabs(receive_data.twist.linear.x));
+//                        // ROS_INFO("speed x %f , y %f , z %f angle %f\n",receive_data.twist.linear.x,var_,car_control_obj_.get_current_vehicle_spd_in_ms(),receive_data.twist.linear.z);
+//                        car_control_obj_.set_target_acc_mps2(var_);
+//                    }
+
+                }
+                break;
+            case AppCtrlSm::kShutDown:
+                if (car_control_module.get_chassis_statemachine_sts() != StsMach::kApaActive &&
+                    car_control_module.get_chassis_statemachine_sts() != StsMach::kHwaActive)
+                {
+                    app_ctrl_car_sm_ = AppCtrlSm::kStandby;
+                }
+                else
+                {
+                    car_control_module.set_lat_lgt_ctrl_req(false);
+                }
+                break;
+            default:
+                app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
+                break;
         }
 
 
+
+
+
+
+
+
+//        bool bstatus = car_control_module.is_lat_lgt_ctrl_active();
+
+//        if(bstatus == true)
+//        {
+//            //            std::cout<<"di pan ke kong "<<std::endl;
+//            executeDecition(xdecition);
+//        }
+//        else
+//        {
+//            std::cout<<" lat lgt req. status: "<<(int)car_control_module.get_chassis_err_state()
+//                    <<"  machine state: "<<(int)car_control_module.get_chassis_statemachine_sts()<< std::endl;
+//            car_control_module.set_lat_lgt_ctrl_req(true);
+//        }
+
+
                        string filename="log.txt";
                        ofstream outfile;
                        outfile.open(filename, ostream::app);