|
|
@@ -44,6 +44,7 @@ GearPrkgAssistReq lastgearSetVal= GearPrkgAssistReq::kNoRequest;
|
|
|
const int gCommunctionNum = 10;
|
|
|
int communicate_cnt=0; //判断decition中是否有决策，超过设定的数值以后认为没有决策退出控制车辆
|
|
|
bool communicate_state=0;
|
|
|
+int case_state=0;//记录switch case的状态，调试用
|
|
|
|
|
|
|
|
|
void quit_ctrl() //退出车辆控制，cxw,20220622
|
|
|
@@ -240,24 +241,29 @@ void sendthread()
|
|
|
if (communicate_state!=0)
|
|
|
{
|
|
|
app_ctrl_car_sm_ = AppCtrlSm::kStartup;
|
|
|
+ case_state=0;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
quit_ctrl();
|
|
|
std::cout<<" no decition "<<std::endl;
|
|
|
+ case_state=-1;
|
|
|
}
|
|
|
break;
|
|
|
case AppCtrlSm::kStartup:
|
|
|
if (car_control_module.is_lat_lgt_ctrl_active()) //底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
|
|
|
{
|
|
|
app_ctrl_car_sm_ = AppCtrlSm::kActive;
|
|
|
+ case_state=10;
|
|
|
}
|
|
|
else {
|
|
|
if (!communicate_state){//通讯失联
|
|
|
app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
|
|
|
+ case_state=11;
|
|
|
}
|
|
|
else {
|
|
|
car_control_module.set_lat_lgt_ctrl_req(true);
|
|
|
+ case_state=12;
|
|
|
}
|
|
|
// app_ctrl_car_sm_ = AppCtrlSm::kStandby;
|
|
|
// quit_ctrl();
|
|
|
@@ -267,32 +273,37 @@ void sendthread()
|
|
|
if (!(communicate_state && car_control_module.is_lat_lgt_ctrl_active()))//通讯失联或底盘不可控
|
|
|
{
|
|
|
app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
|
|
|
+ case_state=30;
|
|
|
if(!car_control_module.is_lat_lgt_ctrl_active())
|
|
|
{
|
|
|
//ctrl_active_state_ =false;
|
|
|
quit_ctrl();
|
|
|
std::cout<<"quit ADAS "<<std::endl;
|
|
|
- //ROS_INFO("control exit ");
|
|
|
+ case_state=31;
|
|
|
}
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
executeDecition(xdecition);
|
|
|
+ case_state=32;
|
|
|
}
|
|
|
break;
|
|
|
case AppCtrlSm::kShutDown:
|
|
|
if (car_control_module.get_chassis_statemachine_sts() != StsMach::kApaActive &&
|
|
|
car_control_module.get_chassis_statemachine_sts() != StsMach::kHwaActive)
|
|
|
{
|
|
|
+ case_state=20;
|
|
|
app_ctrl_car_sm_ = AppCtrlSm::kStandby;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
car_control_module.set_lat_lgt_ctrl_req(false);
|
|
|
+ case_state=21;
|
|
|
}
|
|
|
break;
|
|
|
default:
|
|
|
app_ctrl_car_sm_ = AppCtrlSm::kShutDown;
|
|
|
+ case_state=60;
|
|
|
break;
|
|
|
}
|
|
|
|
|
|
@@ -327,16 +338,16 @@ void sendthread()
|
|
|
outfile <<dt2.time().hour()<<":"<<dt2.time().minute()<<":"<<dt2.time().second()<<":"<<dt2.time().msec()<<","
|
|
|
<<"time_cycle"<<","<< msec <<","
|
|
|
<<"chasis_status" << "," <<(int)car_control_module.get_chassis_err_state()<< ","
|
|
|
+ <<"machine_state"<< ","<<(int)car_control_module.get_chassis_statemachine_sts()<< ","
|
|
|
<<"Decide_ACC" << "," <<lastspeedSetVal << ","
|
|
|
// <<"Decide_gear"<< "," <<lastgearSetVal << ","
|
|
|
// <<"Vehicle_RealSpd"<< "," <<setprecision(2)<<now_gps_ins.<< ","
|
|
|
<<"Decide_Angle"<< "," <<lastEpsSetVal << ","
|
|
|
-// <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
|
|
|
-// <<"Vehicle_GPS_X"<< ","<< setprecision(10)<<now_gps_ins.gps_lat<< ","
|
|
|
-// <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
|
|
|
+ <<"communicate_cnt"<< ","<<communicate_cnt<< ","
|
|
|
+ <<"communicate_state"<< ","<<communicate_state<< ","
|
|
|
+ <<"case_state"<< ","<< case_state<< ","
|
|
|
// <<"Trace_Point"<< ","<<PathPoint<< ","
|
|
|
-// <<"OBS_Distance"<< ","<<obsDistance<< ","
|
|
|
- <<"machine_state"<< ","<<(int)car_control_module.get_chassis_statemachine_sts()<< ","
|
|
|
+// <<"OBS_Distance"<< ","<<obsDistance<< ","
|
|
|
<<endl;
|
|
|
outfile.close();
|
|
|
|
chenxiaowei