|
@@ -130,7 +130,6 @@ int Canbus::Start() {
|
|
|
LOG(INFO)<<Name()<<": can't open ."<<szDevNode;
|
|
|
}
|
|
|
}
|
|
|
-
|
|
|
ros::Rate loop_rate(1/rate);
|
|
|
// 5. set timer to triger publish info periodly
|
|
|
timer_ = CreateTimer(ros::Duration(duration), &Canbus::OnTimer, this);
|
|
@@ -1668,7 +1667,7 @@ int Canbus::read_txt(string &path,int flag)
|
|
|
}
|
|
|
else if(flag == 6)
|
|
|
{
|
|
|
- for (int i = 0; i < 1190; i++)
|
|
|
+ for (int i = 0; i < 1900; i++)
|
|
|
{
|
|
|
if(myfile.eof())
|
|
|
{
|
|
@@ -2126,8 +2125,23 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
|
|
|
return true;
|
|
|
}
|
|
|
rackTestState = true;
|
|
|
- nedc_speeds[1182] = {0};
|
|
|
- string str = "/home/madesheng/publish/wwwroot/Upload/File/NEDC Speed.txt";
|
|
|
+ nedc_speeds[1810] = {0};
|
|
|
+ string str = "/home/madesheng/publish/wwwroot/Upload/File/CLTC Speed.txt";
|
|
|
+ if(req.uiStandard == 1)
|
|
|
+ {
|
|
|
+ standard_name = "NEDC";
|
|
|
+ str = "/home/madesheng/publish/wwwroot/Upload/File/NEDC Speed.txt";
|
|
|
+ }
|
|
|
+ else if(req.uiStandard == 2)
|
|
|
+ {
|
|
|
+ standard_name = "WLTC";
|
|
|
+ str = "/home/madesheng/publish/wwwroot/Upload/File/WLTC Speed.txt";
|
|
|
+ }
|
|
|
+ else if(req.uiStandard == 3)
|
|
|
+ {
|
|
|
+ standard_name = "CLTC";
|
|
|
+ str = "/home/madesheng/publish/wwwroot/Upload/File/CLTC Speed.txt";
|
|
|
+ }
|
|
|
int flag = Canbus::read_txt(str,6);
|
|
|
if(flag != ROBOT_SUCCESS)
|
|
|
{
|
|
@@ -2213,6 +2227,7 @@ void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
|
|
|
|
|
|
float set_speed = 0;
|
|
|
float actual_speed = msg_RTK.Speed;
|
|
|
+ float set_speed_1s = 0;
|
|
|
if(!rackTestState)
|
|
|
{
|
|
|
if(actual_speed == 0)
|
|
@@ -2293,6 +2308,34 @@ void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
|
|
|
set_speed = 0;
|
|
|
}
|
|
|
|
|
|
+ float set_speed1_1s = 0;
|
|
|
+ float set_speed2_1s = 0;
|
|
|
+ if(nedc_speed_count > 0)
|
|
|
+ {
|
|
|
+ set_speed1_1s = nedc_speeds[nedc_speed_count-1];
|
|
|
+ }
|
|
|
+ if(nedc_speed_count-1 == nedc_speed_number - 2)
|
|
|
+ {
|
|
|
+ set_speed2_1s = nedc_speeds[nedc_speed_count-1];
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ set_speed2_1s = nedc_speeds[nedc_speed_count];
|
|
|
+ }
|
|
|
+
|
|
|
+ if(set_speed1_1s == 0 && set_speed2_1s ==0)
|
|
|
+ {
|
|
|
+ set_speed_1s = 0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ set_speed_1s = set_speed_last_1s + (set_speed2_1s - set_speed1_1s) * delta_time;
|
|
|
+ }
|
|
|
+ if(set_speed_1s < 0)
|
|
|
+ {
|
|
|
+ set_speed_1s = 0;
|
|
|
+ }
|
|
|
+
|
|
|
}
|
|
|
LOG(INFO)<< "RackTestOnTimer Start FUZZY PID";
|
|
|
|
|
@@ -2418,11 +2461,9 @@ void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
|
|
|
}
|
|
|
set_speed_last = set_speed;
|
|
|
nedc_speed_count = rack_test_send_count;
|
|
|
+ set_speed_last_1s = set_speed_1s;
|
|
|
|
|
|
- rack_set_speed = set_speed;
|
|
|
- rack_actual_speed = actual_speed;
|
|
|
-
|
|
|
- if(set_speed - actual_speed > over_threshold_value || set_speed - actual_speed < -over_threshold_value)
|
|
|
+ if(set_speed_1s - actual_speed > over_threshold_value || set_speed_1s - actual_speed < -over_threshold_value)
|
|
|
{
|
|
|
if(!over_speed_flag)
|
|
|
{
|
|
@@ -2434,6 +2475,8 @@ void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
|
|
|
{
|
|
|
over_speed_flag = false;
|
|
|
}
|
|
|
+ rack_set_speed = set_speed_1s;
|
|
|
+ rack_actual_speed = actual_speed;
|
|
|
//插入测试任务数据采集表
|
|
|
int data_task_already_num = task_already_num + 1;
|
|
|
long result = CanbusMySqlDataBase::InsertTaskData(task_id,set_speed,actual_speed,
|
|
@@ -2453,6 +2496,18 @@ void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
|
|
|
{
|
|
|
try
|
|
|
{
|
|
|
+ if(rack_set_speed - rack_actual_speed > over_threshold_value || rack_set_speed - rack_actual_speed < -over_threshold_value)
|
|
|
+ {
|
|
|
+ if(!over_speed_flag)
|
|
|
+ {
|
|
|
+ over_speed_flag = true;
|
|
|
+ over_speed_num ++;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ over_speed_flag = false;
|
|
|
+ }
|
|
|
if(rackTestSendTimerState)
|
|
|
{
|
|
|
rackTestReplyState = 1;
|
|
@@ -2605,6 +2660,7 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
|
|
|
|
float set_speed = 0;
|
|
|
float actual_speed = msg_RTK.Speed;
|
|
|
+ float set_speed_1s = 0;
|
|
|
if(!rackTestState)
|
|
|
{
|
|
|
if(!rackTestSendTimerState)
|
|
@@ -2689,6 +2745,34 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
|
set_speed = 0;
|
|
|
}
|
|
|
|
|
|
+ float set_speed1_1s = 0;
|
|
|
+ float set_speed2_1s = 0;
|
|
|
+ if(nedc_speed_count > 0)
|
|
|
+ {
|
|
|
+ set_speed1_1s = nedc_speeds[nedc_speed_count-1];
|
|
|
+ }
|
|
|
+ if(nedc_speed_count-1 == nedc_speed_number - 2)
|
|
|
+ {
|
|
|
+ set_speed2_1s = nedc_speeds[nedc_speed_count-1];
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ set_speed2_1s = nedc_speeds[nedc_speed_count];
|
|
|
+ }
|
|
|
+
|
|
|
+ if(set_speed1_1s == 0 && set_speed2_1s ==0)
|
|
|
+ {
|
|
|
+ set_speed_1s = 0;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ set_speed_1s = set_speed_last_1s + (set_speed2_1s - set_speed1_1s) * delta_time;
|
|
|
+ }
|
|
|
+ if(set_speed_1s < 0)
|
|
|
+ {
|
|
|
+ set_speed_1s = 0;
|
|
|
+ }
|
|
|
+
|
|
|
}
|
|
|
LOG(INFO)<< "delta time:"<<std::setprecision(10)<<delta_time<<" s";
|
|
|
LOG(INFO)<< "RTKLocationCallback Start FUZZY PID";
|
|
@@ -2816,21 +2900,11 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
|
set_speed_last = set_speed;
|
|
|
nedc_speed_count = rack_test_send_count;
|
|
|
|
|
|
- rack_set_speed = set_speed;
|
|
|
+ rack_set_speed = set_speed_1s;
|
|
|
rack_actual_speed = actual_speed;
|
|
|
+ set_speed_last_1s = set_speed_1s;
|
|
|
+
|
|
|
|
|
|
- if(set_speed - actual_speed > over_threshold_value || set_speed - actual_speed < -over_threshold_value)
|
|
|
- {
|
|
|
- if(!over_speed_flag)
|
|
|
- {
|
|
|
- over_speed_flag = true;
|
|
|
- over_speed_num ++;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- over_speed_flag = false;
|
|
|
- }
|
|
|
//插入测试任务数据采集表
|
|
|
int data_task_already_num = task_already_num + 1;
|
|
|
long result = CanbusMySqlDataBase::InsertTaskData(task_id,set_speed,actual_speed,
|