change controller_shenlan_v2.

src/controller/controller_changan_shenlan_v2/main.cpp

@@ -48,6 +48,11 @@ static int gnIndex = 0;
 static bool gbHaveVehSpd = false;
 static double gfVehSpd = 0.0;
 
+static bool gbHaveWheelAngle = false;
+static double gfWheelAngle = 0.0;
+
+static double gfWheelSpeedLim = 300; //300 degrees per second
+
 static boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器
 
 static QMutex gMutex;
@@ -136,7 +141,22 @@ void executeDecition(const iv::brain::decition &decition)
 //     std::cout<<"brake_type is "<<decition.brake_type()<<" acc_active is "<<decition.acc_active()<<std::endl;
 //     std::cout<<"brake is "<<decition.brake()<<" brake_active is "<<decition.brake_active()<<std::endl;
 //     std::cout<<"auto_mode is "<<decition.auto_mode()<<" rightlamp is "<<decition.rightlamp()<<std::endl;
-    _m1C4.ACC_LatAngReq = ECU_1C4_ACC_LatAngReq_toS(decition.wheelangle());
+
+    double fWheelAngleReq = decition.wheelangle();
+    double fsendinter = 0.02;
+    if(fabs(fWheelAngleReq - gfWheelAngle)/fsendinter > gfWheelSpeedLim)
+    {
+        if(fWheelAngleReq > gfWheelAngle)
+        {
+            fWheelAngleReq = fWheelAngleReq + fsendinter * gfWheelSpeedLim;
+        }
+        else
+        {
+            fWheelAngleReq = fWheelAngleReq - fsendinter * gfWheelSpeedLim;
+        }
+    }
+
+    _m1C4.ACC_LatAngReq = ECU_1C4_ACC_LatAngReq_toS(fWheelAngleReq);
     //_m1C4.ADS_Reqmode = decition.angle_mode(); //20221102,新车没有此信号
     _m1C4.ACC_MotorTorqueMaxLimitRequest = ECU_1C4_ACC_MotorTorqueMaxLimitRequest_toS(10);
     _m1C4.ACC_LatAngReqActive = decition.angle_active();
@@ -418,6 +438,12 @@ void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned
         gbHaveVehSpd = true;
   //      std::cout<<" gf Veh speed : "<<gfVehSpd<<std::endl;
     }
+
+    if(xchassis.has_angle_feedback())
+    {
+        gfWheelAngle = xchassis.angle_feedback();
+        gbHaveWheelAngle = true;
+    }
 }