超差阈值范围逻辑修改

src/canbus/canbus.cpp

@@ -2554,8 +2554,58 @@ void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
       publish_rack_speed();
       return;
     }
+    double set_speed_over = 0;
+    double set_speed_over_front = 0;
+    double set_speed_over_back = 0;
+    double delta_over_value_max = 0;
+    double delta_over_value_min = 0;
+    if(standard_name == "CruiseControl")  //定速巡航模式
+    {
+      delta_over_value_min = 0;
+      delta_over_value_max = 0;
+    }
+    else  //规范文件模式
+    {
+      if(nedc_speed_count > 0)
+      {
+        set_speed_over = nedc_speeds[nedc_speed_count-1];
+      }
+      if(nedc_speed_count == nedc_speed_number)
+      {
+        set_speed_over_back = nedc_speeds[nedc_speed_count-1];
+      }
+      else
+      {
+        set_speed_over_back = nedc_speeds[nedc_speed_count];
+      }
+      if(nedc_speed_count -1 > 0)
+      {
+        set_speed_over_front = nedc_speeds[nedc_speed_count-2];
+      }
+      delta_over_value_max =
+          set_speed_over >= set_speed_over_front ? set_speed_over : set_speed_over_front;
+      delta_over_value_max =
+          delta_over_value_max >= set_speed_over_back ? delta_over_value_max : set_speed_over_back;
+
+      delta_over_value_min =
+          set_speed_over <= set_speed_over_front ? set_speed_over : set_speed_over_front;
+      delta_over_value_min =
+          delta_over_value_min <= set_speed_over_back ? delta_over_value_min : set_speed_over_back;
+    }
+
+
+    double over_max = 0;
+    double over_min = 0;
+
+    over_max = over_threshold_value + delta_over_value_max;
+    over_min = delta_over_value_min - over_threshold_value;
+    if(over_min < 0)
+    {
+      over_min = 0;
+    }
 
-    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_max
+       || rack_set_speed - rack_actual_speed < over_min)
     {
       if(!over_speed_flag)
       {