|
@@ -88,7 +88,8 @@ int Canbus::Start() {
|
|
return ROBOT_FAILTURE;
|
|
return ROBOT_FAILTURE;
|
|
}
|
|
}
|
|
LOG(INFO)<< Name() <<": read param rcu_canbus_ip:port = " << canbus_ip << ":" << canbus_port;
|
|
LOG(INFO)<< Name() <<": read param rcu_canbus_ip:port = " << canbus_ip << ":" << canbus_port;
|
|
- canbus_socket = socket(AF_INET, SOCK_STREAM, 0);
|
|
|
|
|
|
+ canbus_socket = socket(AF_INET, SOCK_STREAM | SOCK_NONBLOCK, IPPROTO_TCP);
|
|
|
|
+// canbus_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
|
|
struct sockaddr_in serv_addr;
|
|
struct sockaddr_in serv_addr;
|
|
memset(&serv_addr, 0, sizeof(serv_addr)); //每个字节都用0填充
|
|
memset(&serv_addr, 0, sizeof(serv_addr)); //每个字节都用0填充
|
|
serv_addr.sin_family = AF_INET; //使用IPv4地址
|
|
serv_addr.sin_family = AF_INET; //使用IPv4地址
|
|
@@ -336,7 +337,18 @@ int Canbus::read_loop()
|
|
if (canbus_mode == "tcp") {
|
|
if (canbus_mode == "tcp") {
|
|
int recv_len = read(canbus_socket, s_g_pBufGet, sizeof(s_g_pBufGet));
|
|
int recv_len = read(canbus_socket, s_g_pBufGet, sizeof(s_g_pBufGet));
|
|
if (recv_len != sizeof(s_g_pBufGet)) {
|
|
if (recv_len != sizeof(s_g_pBufGet)) {
|
|
- LOG(ERROR) << Name() <<" canbus socket read error: " << recv_len;
|
|
|
|
|
|
+ int len = 13 - recv_len;
|
|
|
|
+ size_t a = static_cast<size_t>(len);
|
|
|
|
+ unsigned char s_g_pBufGet2[len];
|
|
|
|
+ int recv_len2 = read(canbus_socket, s_g_pBufGet2, sizeof(s_g_pBufGet2));
|
|
|
|
+ int sizeArr1 = sizeof(s_g_pBufGet) / sizeof(s_g_pBufGet[0]);
|
|
|
|
+ int sizeArr2 = sizeof(s_g_pBufGet2) / sizeof(s_g_pBufGet2[0]);
|
|
|
|
+ std::copy(s_g_pBufGet2, s_g_pBufGet2 + sizeArr2, s_g_pBufGet + sizeArr1);
|
|
|
|
+ if(recv_len + recv_len2 != 13)
|
|
|
|
+ {
|
|
|
|
+ // LOG(ERROR) << Name() <<" canbus socket read error: " << recv_len;
|
|
|
|
+ }
|
|
|
|
+// LOG(ERROR) << Name() <<" canbus socket read error: " << recv_len;
|
|
}
|
|
}
|
|
else {
|
|
else {
|
|
msg.Msg.ID = static_cast<int>(s_g_pBufGet[3]) * 256 + static_cast<int>(s_g_pBufGet[4]);
|
|
msg.Msg.ID = static_cast<int>(s_g_pBufGet[3]) * 256 + static_cast<int>(s_g_pBufGet[4]);
|
|
@@ -514,7 +526,8 @@ int Canbus::write_data(TPCANMsg *pMsgBuff, bool print)
|
|
// cout << hex << setw(2) << setfill('0') << static_cast<int>(s_g_pBufSet[i]) << " ";
|
|
// cout << hex << setw(2) << setfill('0') << static_cast<int>(s_g_pBufSet[i]) << " ";
|
|
// }
|
|
// }
|
|
// cout << endl;
|
|
// cout << endl;
|
|
- int send_length = write(canbus_socket, s_g_pBufSet, sizeof(s_g_pBufSet));
|
|
|
|
|
|
+// int send_length = write(canbus_socket, s_g_pBufSet, sizeof(s_g_pBufSet));
|
|
|
|
+ int send_length = send(canbus_socket, s_g_pBufSet, sizeof(s_g_pBufSet), 0);
|
|
if (send_length != sizeof(s_g_pBufSet)){
|
|
if (send_length != sizeof(s_g_pBufSet)){
|
|
LOG(ERROR) << Name() <<" canbus socket write error: " << send_length;
|
|
LOG(ERROR) << Name() <<" canbus socket write error: " << send_length;
|
|
return errno;
|
|
return errno;
|
|
@@ -2115,16 +2128,31 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
|
|
try{
|
|
try{
|
|
if(req.uiStart == 0)
|
|
if(req.uiStart == 0)
|
|
{
|
|
{
|
|
- rackTestState = false;
|
|
|
|
|
|
+ rackTestState = 0;
|
|
res.uiReturn = 1;
|
|
res.uiReturn = 1;
|
|
return true;
|
|
return true;
|
|
}
|
|
}
|
|
- if(rackTestState || rackTestSendTimerState)
|
|
|
|
|
|
+ if(req.uiStart == 2)
|
|
|
|
+ {
|
|
|
|
+ if(rack_set_speed == 0 && rack_actual_speed == 0)
|
|
|
|
+ {
|
|
|
|
+ rackTestState = 2;
|
|
|
|
+ res.uiReturn = 1;
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 3;
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+ if(rackTestState == 1)
|
|
{
|
|
{
|
|
res.uiReturn = 2;
|
|
res.uiReturn = 2;
|
|
return true;
|
|
return true;
|
|
}
|
|
}
|
|
- rackTestState = true;
|
|
|
|
|
|
+
|
|
nedc_speeds[1810] = {0};
|
|
nedc_speeds[1810] = {0};
|
|
string str = "/home/madesheng/publish/wwwroot/Upload/File/CLTC Speed.txt";
|
|
string str = "/home/madesheng/publish/wwwroot/Upload/File/CLTC Speed.txt";
|
|
if(req.uiStandard == 1)
|
|
if(req.uiStandard == 1)
|
|
@@ -2142,12 +2170,29 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
|
|
standard_name = "CLTC";
|
|
standard_name = "CLTC";
|
|
str = "/home/madesheng/publish/wwwroot/Upload/File/CLTC Speed.txt";
|
|
str = "/home/madesheng/publish/wwwroot/Upload/File/CLTC Speed.txt";
|
|
}
|
|
}
|
|
- int flag = Canbus::read_txt(str,6);
|
|
|
|
- if(flag != ROBOT_SUCCESS)
|
|
|
|
|
|
+ else if(req.uiStandard == 100)
|
|
{
|
|
{
|
|
- res.uiReturn = 0;
|
|
|
|
- return false;
|
|
|
|
|
|
+ standard_name = "CruiseControl";
|
|
|
|
+ rack_constant_speed = req.uiConstantSpeed;
|
|
}
|
|
}
|
|
|
|
+ if(req.uiStandard != 100)
|
|
|
|
+ {
|
|
|
|
+ int flag = Canbus::read_txt(str,6);
|
|
|
|
+ if(flag != ROBOT_SUCCESS)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 0;
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ if(rackTestState == 2)
|
|
|
|
+ {
|
|
|
|
+ rackTestState = 1;
|
|
|
|
+ res.uiReturn = 1;
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ rackTestState = 1;
|
|
nedc_speed_count = 0;
|
|
nedc_speed_count = 0;
|
|
rack_test_send_count = 0;
|
|
rack_test_send_count = 0;
|
|
set_speed_last = 0;
|
|
set_speed_last = 0;
|
|
@@ -2180,7 +2225,15 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
|
|
std::string datetime_now;
|
|
std::string datetime_now;
|
|
datetime_now = get_datetime_mill();
|
|
datetime_now = get_datetime_mill();
|
|
//插入测试任务信息表
|
|
//插入测试任务信息表
|
|
- task_num = req.iTaskNum;
|
|
|
|
|
|
+ if(req.uiStandard == 100)
|
|
|
|
+ {
|
|
|
|
+ task_num = 1;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ task_num = req.iTaskNum;
|
|
|
|
+ }
|
|
|
|
+
|
|
long result = CanbusMySqlDataBase::InsertTaskInfo(req.uiCarId,standard_name,
|
|
long result = CanbusMySqlDataBase::InsertTaskInfo(req.uiCarId,standard_name,
|
|
datetime_now,over_threshold_value,task_num);
|
|
datetime_now,over_threshold_value,task_num);
|
|
if(result <= 0)
|
|
if(result <= 0)
|
|
@@ -2344,7 +2397,6 @@ void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
|
|
// LOG(INFO)<< "actual_speed too more";
|
|
// LOG(INFO)<< "actual_speed too more";
|
|
// return;
|
|
// return;
|
|
// }
|
|
// }
|
|
- set_speed_reply = set_speed;
|
|
|
|
LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
|
|
LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
|
|
// string str = "/home/madesheng/FuzzyPid.txt";
|
|
// string str = "/home/madesheng/FuzzyPid.txt";
|
|
// int flag = Canbus::read_txt(str,7);
|
|
// int flag = Canbus::read_txt(str,7);
|
|
@@ -2496,6 +2548,13 @@ void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
|
|
{
|
|
{
|
|
try
|
|
try
|
|
{
|
|
{
|
|
|
|
+ if(rackTestState == 2)
|
|
|
|
+ {
|
|
|
|
+ rackTestReplyState = 2;
|
|
|
|
+ publish_rack_speed();
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+
|
|
if(rack_set_speed - rack_actual_speed > over_threshold_value || rack_set_speed - rack_actual_speed < -over_threshold_value)
|
|
if(rack_set_speed - rack_actual_speed > over_threshold_value || rack_set_speed - rack_actual_speed < -over_threshold_value)
|
|
{
|
|
{
|
|
if(!over_speed_flag)
|
|
if(!over_speed_flag)
|
|
@@ -2530,6 +2589,10 @@ void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
|
|
|
|
|
|
void Canbus::RackTestReplyDetailOnTimer(const ros::TimerEvent &event)
|
|
void Canbus::RackTestReplyDetailOnTimer(const ros::TimerEvent &event)
|
|
{
|
|
{
|
|
|
|
+ if(rackTestState == 2)
|
|
|
|
+ {
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
try
|
|
try
|
|
{
|
|
{
|
|
gettimeofday(&now_time_ui, NULL);
|
|
gettimeofday(&now_time_ui, NULL);
|
|
@@ -2551,8 +2614,31 @@ void Canbus::RackTestReplyDetailOnTimer(const ros::TimerEvent &event)
|
|
rackTestReplyState = 1;
|
|
rackTestReplyState = 1;
|
|
//插入测试任务数据采集表界面显示
|
|
//插入测试任务数据采集表界面显示
|
|
int data_task_already_num = task_already_num + 1;
|
|
int data_task_already_num = task_already_num + 1;
|
|
- long result = CanbusMySqlDataBase::InsertTaskDataUI(task_id,rack_set_speed,rack_actual_speed,
|
|
|
|
- task_time_stamp_ui,over_speed_num,data_task_already_num);
|
|
|
|
|
|
+// long result = CanbusMySqlDataBase::InsertTaskDataUI(task_id,rack_set_speed,rack_actual_speed,
|
|
|
|
+// task_time_stamp_ui,over_speed_num,data_task_already_num);
|
|
|
|
+ auto ThreadInsertTaskDataUI = [&](long TaskId,
|
|
|
|
+ double SetSpeed,
|
|
|
|
+ double ActualSpeed,
|
|
|
|
+ double StandardSpeedTimeStamp,
|
|
|
|
+ int OverSpeedNum,
|
|
|
|
+ int StandardTaskCount) -> void {
|
|
|
|
+ LOG(INFO)<< "Start ThreadInsertTaskDataUI";
|
|
|
|
+ mtx_taskdataui.lock();
|
|
|
|
+
|
|
|
|
+ CanbusMySqlDataBase::InsertTaskDataUI(TaskId,SetSpeed,ActualSpeed,
|
|
|
|
+ StandardSpeedTimeStamp,OverSpeedNum,
|
|
|
|
+ StandardTaskCount);
|
|
|
|
+
|
|
|
|
+ mtx_taskdataui.unlock();
|
|
|
|
+ LOG(INFO)<< "END ThreadInsertTaskDataUI";
|
|
|
|
+ };
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ std::thread thread_taskdataui (ThreadInsertTaskDataUI,
|
|
|
|
+ task_id,rack_set_speed,rack_actual_speed,
|
|
|
|
+ task_time_stamp_ui,over_speed_num,
|
|
|
|
+ data_task_already_num);
|
|
|
|
+ thread_taskdataui.join();
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
@@ -2606,7 +2692,15 @@ void Canbus::publish_rack_speed()
|
|
rackMsg.RackTestReplyState = rackTestReplyState;
|
|
rackMsg.RackTestReplyState = rackTestReplyState;
|
|
rackMsg.SetSpeed = rack_set_speed;
|
|
rackMsg.SetSpeed = rack_set_speed;
|
|
rackMsg.TaskCurrentTime = nedc_speed_count +(nedc_speed_number * task_already_num);
|
|
rackMsg.TaskCurrentTime = nedc_speed_count +(nedc_speed_number * task_already_num);
|
|
- rackMsg.TaskTotalTime = nedc_speed_number * task_num;
|
|
|
|
|
|
+ if(standard_name == "CruiseControl")
|
|
|
|
+ {
|
|
|
|
+ rackMsg.TaskTotalTime = rackMsg.TaskCurrentTime;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ rackMsg.TaskTotalTime = nedc_speed_number * task_num;
|
|
|
|
+ }
|
|
|
|
+
|
|
rackMsg.OverSpeedNum =over_speed_num;
|
|
rackMsg.OverSpeedNum =over_speed_num;
|
|
rackMsg.TaskId = task_id;
|
|
rackMsg.TaskId = task_id;
|
|
rackMsg.TaskAlreadyNum = task_already_num + 1;
|
|
rackMsg.TaskAlreadyNum = task_already_num + 1;
|
|
@@ -2624,9 +2718,6 @@ void Canbus::publis_rack_detail_speed()
|
|
try
|
|
try
|
|
{
|
|
{
|
|
canbus::rackTestReplyDetailMsg detailMsg;
|
|
canbus::rackTestReplyDetailMsg detailMsg;
|
|
- detailMsg.ActualSpeed = set_speed_reply;
|
|
|
|
- detailMsg.SetSpeed = set_speed_reply;
|
|
|
|
- detailMsg.RackTestReplyState = rackTestReplyState;
|
|
|
|
canRackDetailData_pub.publish(detailMsg);
|
|
canRackDetailData_pub.publish(detailMsg);
|
|
}
|
|
}
|
|
catch (ros::Exception ex)
|
|
catch (ros::Exception ex)
|
|
@@ -2661,7 +2752,11 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
float set_speed = 0;
|
|
float set_speed = 0;
|
|
float actual_speed = msg_RTK.Speed;
|
|
float actual_speed = msg_RTK.Speed;
|
|
float set_speed_1s = 0;
|
|
float set_speed_1s = 0;
|
|
- if(!rackTestState)
|
|
|
|
|
|
+ if(rackTestState == 2)
|
|
|
|
+ {
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+ if(rackTestState == 0)
|
|
{
|
|
{
|
|
if(!rackTestSendTimerState)
|
|
if(!rackTestSendTimerState)
|
|
{
|
|
{
|
|
@@ -2685,93 +2780,104 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
set_speed = 0;
|
|
set_speed = 0;
|
|
|
|
|
|
}
|
|
}
|
|
- else
|
|
|
|
|
|
+ else if(rackTestState == 1)
|
|
{
|
|
{
|
|
- if(nedc_speed_count == nedc_speed_number)
|
|
|
|
|
|
+ if(standard_name == "CruiseControl") //定速巡航模式
|
|
{
|
|
{
|
|
- task_already_num ++;
|
|
|
|
- if(task_already_num < task_num)
|
|
|
|
|
|
+ set_speed = rack_constant_speed;
|
|
|
|
+ set_speed_1s = rack_constant_speed;
|
|
|
|
+ task_already_num = 0;
|
|
|
|
+ nedc_speed_number = 0;
|
|
|
|
+ }
|
|
|
|
+ else //规范文件模式
|
|
|
|
+ {
|
|
|
|
+ if(nedc_speed_count == nedc_speed_number)
|
|
{
|
|
{
|
|
- nedc_speed_count = 0;
|
|
|
|
- rack_test_send_count = 0;
|
|
|
|
- set_speed_last = 0;
|
|
|
|
- task_time_stamp = 0;
|
|
|
|
- task_time_stamp_ui = 0;
|
|
|
|
|
|
+ task_already_num ++;
|
|
|
|
+ if(task_already_num < task_num)
|
|
|
|
+ {
|
|
|
|
+ nedc_speed_count = 0;
|
|
|
|
+ rack_test_send_count = 0;
|
|
|
|
+ set_speed_last = 0;
|
|
|
|
+ task_time_stamp = 0;
|
|
|
|
+ task_time_stamp_ui = 0;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ rackTestState = 0;
|
|
|
|
+ // rack_test_timer_.stop();
|
|
|
|
+ rack_reply_timer_.stop();
|
|
|
|
+ rack_reply_detail_timer_.stop();
|
|
|
|
+ rackTestSendTimerState = false;
|
|
|
|
+ rackTestReplyState = 0;
|
|
|
|
+ task_id = 0;
|
|
|
|
+ task_num = 0;
|
|
|
|
+ task_time_stamp =0;
|
|
|
|
+ publish_rack_speed();
|
|
|
|
+ return;
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+
|
|
}
|
|
}
|
|
- else
|
|
|
|
|
|
+ if(delta_time > 0 && delta_time < 1)
|
|
{
|
|
{
|
|
- rackTestState = false;
|
|
|
|
- rack_test_timer_.stop();
|
|
|
|
- rack_reply_timer_.stop();
|
|
|
|
- rack_reply_detail_timer_.stop();
|
|
|
|
- rackTestSendTimerState = false;
|
|
|
|
- rackTestReplyState = 0;
|
|
|
|
- task_id = 0;
|
|
|
|
- task_num = 0;
|
|
|
|
- task_time_stamp =0;
|
|
|
|
- publish_rack_speed();
|
|
|
|
- return;
|
|
|
|
-
|
|
|
|
|
|
+ task_time_stamp += delta_time;
|
|
}
|
|
}
|
|
|
|
|
|
- }
|
|
|
|
- if(delta_time > 0 && delta_time < 1)
|
|
|
|
- {
|
|
|
|
- task_time_stamp += delta_time;
|
|
|
|
- }
|
|
|
|
|
|
+ //根据时间差计算速度插值
|
|
|
|
+ float set_speed1 = nedc_speeds[nedc_speed_count];
|
|
|
|
+ float set_speed2 = 0;
|
|
|
|
+ if(nedc_speed_count == nedc_speed_number - 1)
|
|
|
|
+ {
|
|
|
|
+ set_speed2 = nedc_speeds[nedc_speed_count];
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ set_speed2 = nedc_speeds[nedc_speed_count + 1];
|
|
|
|
+ }
|
|
|
|
|
|
- //根据时间差计算速度插值
|
|
|
|
- float set_speed1 = nedc_speeds[nedc_speed_count];
|
|
|
|
- float set_speed2 = 0;
|
|
|
|
- if(nedc_speed_count == nedc_speed_number - 1)
|
|
|
|
- {
|
|
|
|
- set_speed2 = nedc_speeds[nedc_speed_count];
|
|
|
|
- }
|
|
|
|
- else
|
|
|
|
- {
|
|
|
|
- set_speed2 = nedc_speeds[nedc_speed_count + 1];
|
|
|
|
- }
|
|
|
|
|
|
+ if(set_speed1 == 0 && set_speed2 ==0)
|
|
|
|
+ {
|
|
|
|
+ set_speed = 0;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ set_speed = set_speed_last + (set_speed2 - set_speed1) * delta_time;
|
|
|
|
+ }
|
|
|
|
+ if(set_speed < 0)
|
|
|
|
+ {
|
|
|
|
+ set_speed = 0;
|
|
|
|
+ }
|
|
|
|
|
|
- if(set_speed1 == 0 && set_speed2 ==0)
|
|
|
|
- {
|
|
|
|
- set_speed = 0;
|
|
|
|
- }
|
|
|
|
- else
|
|
|
|
- {
|
|
|
|
- set_speed = set_speed_last + (set_speed2 - set_speed1) * delta_time;
|
|
|
|
- }
|
|
|
|
- if(set_speed < 0)
|
|
|
|
- {
|
|
|
|
- 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];
|
|
|
|
+ }
|
|
|
|
|
|
- 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;
|
|
|
|
+ }
|
|
}
|
|
}
|
|
|
|
|
|
- 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)<< "delta time:"<<std::setprecision(10)<<delta_time<<" s";
|
|
@@ -2781,21 +2887,9 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
// {
|
|
// {
|
|
// LOG(INFO)<< "actual_speed too more";
|
|
// LOG(INFO)<< "actual_speed too more";
|
|
// return;
|
|
// return;
|
|
-// }
|
|
|
|
- set_speed_reply = set_speed;
|
|
|
|
- LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
|
|
|
|
-// string str = "/home/madesheng/FuzzyPid.txt";
|
|
|
|
-// int flag = Canbus::read_txt(str,7);
|
|
|
|
-// if(flag != ROBOT_SUCCESS)
|
|
|
|
-// {
|
|
|
|
-// LOG(INFO)<< "Get FuzzyPID.txt error";
|
|
|
|
-// return;
|
|
|
|
// }
|
|
// }
|
|
|
|
|
|
-// int brkFactor = 0;
|
|
|
|
-// int brkMax = 0;
|
|
|
|
-// brkFactor = fuzzy_pid[6];
|
|
|
|
-// brkMax = fuzzy_pid[7];
|
|
|
|
|
|
+ LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
|
|
|
|
|
|
float pi_out = 0.01;
|
|
float pi_out = 0.01;
|
|
pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
|
|
pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
|
|
@@ -2907,11 +3001,39 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
|
|
|
|
//插入测试任务数据采集表
|
|
//插入测试任务数据采集表
|
|
int data_task_already_num = task_already_num + 1;
|
|
int data_task_already_num = task_already_num + 1;
|
|
- long result = CanbusMySqlDataBase::InsertTaskData(task_id,set_speed,actual_speed,
|
|
|
|
- requestAcc.uiAccPsngCtrReq/10,
|
|
|
|
- requestBrk.uiBrkPsngCtrReq / 10,
|
|
|
|
- task_time_stamp,over_speed_num,
|
|
|
|
- data_task_already_num);
|
|
|
|
|
|
+// CanbusMySqlDataBase::InsertTaskData(task_id,set_speed,actual_speed,
|
|
|
|
+// requestAcc.uiAccPsngCtrReq/10,
|
|
|
|
+// requestBrk.uiBrkPsngCtrReq / 10,
|
|
|
|
+// task_time_stamp,over_speed_num,
|
|
|
|
+// data_task_already_num);
|
|
|
|
+ auto ThreadInsertTaskData = [&](long TaskId,
|
|
|
|
+ double SetSpeed,
|
|
|
|
+ double ActualSpeed,
|
|
|
|
+ long AccOrderPsng,
|
|
|
|
+ long BrkOrderPsng,
|
|
|
|
+ double StandardSpeedTimeStamp,
|
|
|
|
+ int OverSpeedNum,
|
|
|
|
+ int StandardTaskCount) -> void {
|
|
|
|
+ LOG(INFO)<< "Start ThreadInsertTaskData";
|
|
|
|
+ mtx_taskdata.lock();
|
|
|
|
+
|
|
|
|
+ CanbusMySqlDataBase::InsertTaskData(TaskId,SetSpeed,ActualSpeed,
|
|
|
|
+ AccOrderPsng,
|
|
|
|
+ BrkOrderPsng,
|
|
|
|
+ StandardSpeedTimeStamp,OverSpeedNum,
|
|
|
|
+ StandardTaskCount);
|
|
|
|
+
|
|
|
|
+ mtx_taskdata.unlock();
|
|
|
|
+ LOG(INFO)<< "End ThreadInsertTaskData";
|
|
|
|
+ };
|
|
|
|
+
|
|
|
|
+ std::thread thread_taskdata(ThreadInsertTaskData,
|
|
|
|
+ task_id,set_speed,actual_speed,
|
|
|
|
+ requestAcc.uiAccPsngCtrReq/10,
|
|
|
|
+ requestBrk.uiBrkPsngCtrReq / 10,
|
|
|
|
+ task_time_stamp,over_speed_num,
|
|
|
|
+ data_task_already_num);
|
|
|
|
+ thread_taskdata.join();
|
|
|
|
|
|
}
|
|
}
|
|
catch (ros::Exception ex)
|
|
catch (ros::Exception ex)
|