Merge branch 'jeely_xingyueL' of http://111.33.136.149:3000/adc_pilot/modularization into jeely_xingyueL

src/controller/controller_Geely_xingyueL/controller_Geely_xingyueL.pro

@@ -39,7 +39,7 @@ HEADERS += \
     include/kx_11_can_frame/asdm_sim_canfd_data_type.h \
     include/kx_11_can_frame/asdm_sim_canfd_frame.h \
     include/kx_11_can_frame/can_canfd_common.h \
-    include/kx_11_can_frame/chassis_can1_frame.h \
+#    include/kx_11_can_frame/chassis_can1_frame.h \
     include/socket_can_interface/socket_can_interface.h \
     include/socket_udp/socket_udp.h \
     include/basic_types.h \
@@ -48,4 +48,5 @@ HEADERS += \
 
 LIBS += $$PWD/lib/libcar_control.a
 
+unix|win32: LIBS += -lglog
 

src/controller/controller_Geely_xingyueL/include/car_control.h

@@ -1,17 +1,9 @@
 #ifndef CAR_CONTROL_H
 #define CAR_CONTROL_H
 
-#include "kx_11_can_frame/chassis_can1_frame.h"
 #include "kx_11_can_frame/asdm_sim_canfd_frame.h"
 #include <thread>
 
-enum class AppCtrlSm   //cxw,20220622
-{
-  kStandby = 0,
-  kStartup = 1,
-  kActive = 2,
-  kShutDown = 3
-};
 
 enum class StsMach
 {
@@ -73,9 +65,9 @@ private:
   void chassis_handshake_off();
   void apa_startup_action();
   void hwa_pre_active();
+  bool is_car_vin_matched();
 private:
-  ChassisCAN1_Process *chassis_obj_ptr_{nullptr};
-  ASDM_Sim_Process *asdm_obj_ptr_{nullptr};
+  std::unique_ptr<ASDM_Sim_Process>  asdm_obj_ptr_{nullptr};
   StsMach sm_{StsMach::kStandby};
   std::unique_ptr<std::thread> thread_sm_task_{nullptr};
   bool is_chassis_handshake_timeout{false};
@@ -90,11 +82,13 @@ private:
   uint16_t timeout_apa_shutdown = 0;
   uint16_t timeout_hwa_startup = 0;
   uint16_t timeout_hwa_shutdown = 0;
+  uint32_t time_vin_check = 0;
   const uint16_t kMaxStartupDlyTime = (3000 / 20); // uinit:ms
   const uint16_t kMaxApaStartupTime = (3000 / 20); // uinit:ms
   const uint16_t kMaxApaShutdownTime = (10000 / 20); // uinit:ms
   const uint16_t kMaxHwaStartupTime = (5000 / 20); // uinit:ms
   const uint16_t kMaxHwaShutdownTime = (10000 / 20); // uinit:ms
+  const uint32_t kMaxVinCheckTime = (5 * 60 * 1000 / 20); // uinit:ms,  5minites
   float32_t kSpeedForCtrlModSwitch = CONVERT_KPH_2_MPS(6);   // the APA longitude speed uppper limit is 10km/h
   const float32_t kSteerDiffMax = 90;
   const float32_t kSteerDiffMin = -90;
@@ -103,6 +97,11 @@ private:
   const float32_t kApaShutDownDecel = -1.28; //uinit: m/s^2, apa max deceleration.
   const float32_t kHwaShutDownDecel = -5; //uinit: m/s^2, hwa shutdown deceleration value.
 
+  const float32_t kApaAccelerationMax = 1.27; //uinit: m/s^2
+  const float32_t kApaAccelerationMin = -1.27; //uinit: m/s^2
+  const float32_t kHwaAccelerationMax = 11; //uinit: m/s^2
+  const float32_t kHwaAccelerationMin = -11; //uinit: m/s^2
+
   // debug parameter:
   bool debug_enable_{false};
   uint16_t print_period_ms_{20};

src/controller/controller_Geely_xingyueL/include/kx_11_can_frame/asdm_sim_canfd_data_type.h

@@ -321,4 +321,60 @@ struct VddmFeedback
   CfmdLgtCtrlMod lgt_ctrl_mod_feedback{CfmdLgtCtrlMod::kNoReq};
 };
 
+
+// Chassis CAN date type 
+enum class SteerStsToParkAssis
+{
+  kNormOper = 0,
+  kAbortBySpdHi = 1,
+  kCtrlDifHi = 2,
+  kCtrlIntErr = 3,
+  kAbortByDrvIntv = 4,
+  kSteerTqHi = 5,
+  kSpare1 = 6,
+  kSpare2 = 7
+};
+
+enum class DrvrWheelHoldSts
+{
+  kNoInfo = 0,
+  kOffDetected = 1,
+  kNotOnOrOffDetected = 2,
+  kOnDetected = 3
+};
+
+struct AccFuncPinionCtrReq
+{
+  float32_t torque_upper_limit{0};
+  float32_t torque_lower_limit{0};
+  float32_t pinion_ang_req_in_deg{0};
+};
+
+struct ChassisCanCtrlReq
+{
+    // input parameters
+  float32_t pas_pinion_ang_req_in_deg{0};
+  bool is_pas_func_active{false};
+  struct AccFuncPinionCtrReq acc_func_pinion_ctrl_req;
+};
+
+
+struct ChassisCanFeedback
+{
+  // output parameters
+  SteerStsToParkAssis steer_sts{SteerStsToParkAssis::kNormOper};
+  DrvrWheelHoldSts driver_wheel_hold_sts{DrvrWheelHoldSts::kNoInfo};
+  bool driver_steering_is_active{false};
+  bool is_steer_servious_sts{false};
+  float32_t cur_pinion_steer_ag1_in_deg{0};
+  
+  bool pinion_steer_ag1_valid{false};
+  float32_t pinion_steerAgSpd1_deg_in_per_sec{0};
+  bool pinion_steer_ag_spd1_valid{false};
+  float32_t steer_wheel_torque{0};
+  bool steer_wheel_torque_valid{false};
+  bool vfc_info_enable{false};
+};
+
+
 #endif

src/controller/controller_Geely_xingyueL/include/kx_11_can_frame/asdm_sim_canfd_frame.h

@@ -18,60 +18,39 @@ public:
   ~ASDM_Sim_Process();
 
 public:
-
-  ReceiveFrameCanfd vddm_fr03_;
-  ReceiveFrameCanfd bcm_fr00_;
-  ReceiveFrameCanfd bcm_fr03_;
-  ReceiveFrameCanfd bcm_fr06_;
+  ChassisCanCtrlReq chassis_can_request;
+  ChassisCanFeedback chassis_can_feedback;
 
   AsdmControlRequest asdm_control_request;
   VddmFeedback vddm_feedback;
 private:
   void receive_process(canfd_frame receive_frame);
-  static void* tx_rx_polling(void* arg);
-  static void* rx_5ms(void* arg);
-  static void* tx_5ms(void* arg);
+  void tx_rx_polling();
+  void rx_2ms();
+  void tx_3ms();
   void set_filters();
-  void vddm_fr03_decode(void);
-  void bcm_fr00_decode(void);
-  void bcm_fr03_decode(void);
-  void bcm_fr06_decode(void);
+  void vddm_fr03_decode();
+  void bcm_fr00_decode();
+  void bcm_fr03_decode();
+  void bcm_fr06_decode();
+  void cem_fr08_decode();
+  void chassis_can_feedback_fr_decode();
 
-  void asdm_sim_fr03_frame_send(void);
-  void asdm_sim_fr02_frame_send(void);
-  void asdm_sim_fr04_frame_send(void);
-  void asdm_sim_fr11_frame_send(void);
-  void asdm_sim_fr30_frame_send(void);
-  void asdm_sim_nm_frame_send(void);
-  void asdm_sim_fr20_frame_send(void);
-  void asdm_sim_fr10_frame_send(void);
-  void asdm_sim_fr13_frame_send(void);
+  void asdm_sim_total_signals_frame_send();
 
 private:
-  SendFrameCanfd asdm_sim_fr02_;
-  SendFrameCanfd asdm_sim_fr03_;
-  SendFrameCanfd asdm_sim_fr04_;
-  SendFrameCanfd asdm_sim_fr11_;
-  SendFrameCanfd asdm_sim_fr30_;
-  SendFrameCanfd asdm_sim_nm_fr_;
-  SendFrameCanfd asdm_sim_fr20_;
-  SendFrameCanfd asdm_sim_fr10_;
-  SendFrameCanfd asdm_sim_fr13_;
+  std::unique_ptr<std::thread> thread_poll_{nullptr};
+  std::unique_ptr<std::thread> thread_rx_5ms_{nullptr};
+  std::unique_ptr<std::thread> thread_tx_5ms_{nullptr};
 
-  pthread_mutex_t mtx_asdm_sim_fr02_;
-  pthread_mutex_t mtx_asdm_sim_fr03_;
-  pthread_mutex_t mtx_asdm_sim_fr04_;
-  pthread_mutex_t mtx_asdm_sim_fr11_;
-  pthread_mutex_t mtx_asdm_sim_fr30_;
-  pthread_mutex_t mtx_asdm_sim_nm_fr_;
-  pthread_mutex_t mtx_asdm_sim_fr20_;
-  pthread_mutex_t mtx_asdm_sim_fr10_;
-  pthread_mutex_t mtx_asdm_sim_fr13_;
+  SendFrameCanfd asdm_sim_total_fr_;
 
-  pthread_mutex_t mtx_vddm_fr03_;
-  pthread_mutex_t mtx_bcm_fr00_;
-  pthread_mutex_t mtx_bcm_fr03_;
-  pthread_mutex_t mtx_bcm_fr06_;
+  ReceiveFrameCanfd vddm_fr03_;
+  ReceiveFrameCanfd bcm_fr00_;
+  ReceiveFrameCanfd bcm_fr03_;
+  ReceiveFrameCanfd bcm_fr06_;
+  ReceiveFrameCanfd cem_fr08_;
+  ReceiveFrameCanfd chassis_feedback_fr_;
 };
 
 #endif

src/controller/controller_Geely_xingyueL/include/kx_11_can_frame/can_canfd_common.h

@@ -2,7 +2,10 @@
 #define CAN_CANFD_COMMON_H
 
 #include <pthread.h>
+#include <thread>
+#include <mutex>
 #include "./include/socket_can_interface/socket_can_interface.h"
+#include "./include/glog_config.h"
 
 
 #define DOMAIN_ECU_PASSWORD "nvidia"
@@ -16,12 +19,14 @@
 #define CONVERT_KPH_2_MPS(kph)  ((float32_t)kph / 3.6)
 #define CONVERT_MPS_2_KPH(ms)   ((float32_t)ms * 3.6)
 
+using mlock = std::unique_lock<std::mutex>;
 
 typedef struct
 {
   /* data */
   can_frame fr{0, 0, 0, 0, 0, {0}};       // can frame 
   bool is_updated{false}; // if receive a new frame , it's updated , so need be processed
+  std::mutex mtx;
 } ReceiveFrame;
 
 typedef struct
@@ -29,6 +34,7 @@ typedef struct
   /* data */
   can_frame fr{0, 0, 0, 0, 0, {0}};       // can frame 
   bool need_send{false};  // if need send can frame to can bus ,set this to true
+  std::mutex mtx;
 } SendFrame;
 
 typedef struct
@@ -36,6 +42,7 @@ typedef struct
   /* data */
   canfd_frame fr{0, 0, 0, 0, 0, {0}};       // can fd frame 
   bool is_updated{false}; // if receive a new frame , it's updated , so need be processed
+  std::mutex mtx;
 } ReceiveFrameCanfd;
 
 typedef struct
@@ -43,6 +50,7 @@ typedef struct
   /* data */
   canfd_frame fr{0, 0, 0, 0, 0, {0}};       // can fd frame 
   bool need_send{false};  // if need send can frame to can bus ,set this to true
+  std::mutex mtx;
 } SendFrameCanfd;
 
 enum class GenQf1

src/controller/controller_Geely_xingyueL/include/kx_11_can_frame/chassis_can1_frame.h

@@ -1,107 +0,0 @@
-#ifndef CHASSIS_CAN1_FRAME_H
-#define CHASSIS_CAN1_FRAME_H
-
-#include "./include/socket_can_interface/socket_can_interface.h"
-#include "can_canfd_common.h"
-
-
-#define CHASSIS_CAN1_BITRATE (500000)
-
-enum class SteerStsToParkAssis
-{
-  kNormOper = 0,
-  kAbortBySpdHi = 1,
-  kCtrlDifHi = 2,
-  kCtrlIntErr = 3,
-  kAbortByDrvIntv = 4,
-  kSteerTqHi = 5,
-  kSpare1 = 6,
-  kSpare2 = 7
-};
-
-enum class DrvrWheelHoldSts
-{
-  kNoInfo = 0,
-  kOffDetected = 1,
-  kNotOnOrOffDetected = 2,
-  kOnDetected = 3
-};
-
-struct AccFuncPinionCtrReq
-{
-  float32_t torque_upper_limit{0};
-  float32_t torque_lower_limit{0};
-  float32_t pinion_ang_req_in_deg{0};
-};
-
-
-
-class ChassisCAN1_Process : public SocketCanInterface
-{
-public:
-  ChassisCAN1_Process();
-  ~ChassisCAN1_Process();
-  void set_filters();
-public:
-  // output parameters
-  SteerStsToParkAssis steer_sts{SteerStsToParkAssis::kNormOper};
-  DrvrWheelHoldSts driver_wheel_hold_sts{DrvrWheelHoldSts::kNoInfo};
-  bool driver_steering_is_active{false};
-  bool is_steer_servious_sts{false};
-  float32_t cur_pinion_steer_ag1_in_deg{0};
-  
-  bool pinion_steer_ag1_valid{false};
-  float32_t pinion_steerAgSpd1_deg_in_per_sec{0};
-  bool pinion_steer_ag_spd1_valid{false};
-  float32_t steer_wheel_torque{0};
-  bool steer_wheel_torque_valid{false};
-  bool vfc_info_enable{false};
-
-  // input parameters
-  float32_t pas_pinion_ang_req_in_deg{0};
-  bool is_pas_func_active{false};
-  struct AccFuncPinionCtrReq acc_func_pinion_ctrl_req;
-
-private:
-  static void* chassis_tx_rx_polling(void* arg);
-  static void* chassis_rx_5ms(void* arg);
-  static void* chassis_tx_15ms(void* arg);
-  static void* chassis_asdm_tx(void* arg);
-  void receive_process(can_frame receive_frame);
-  void pscm_fr03_decode(void);
-  void pscm_fr07_decode(void);
-  void vddm_fr21_decode(void);
-  void pscm_fr01_decode(void);
-  void pscm_fr02_decode(void);
-
-  void pas_fr02_frame_send(void);
-  void pas_fr04_frame_send(void);
-  void pas_nm_frame_send(void);
-  void asdm_fr01_frame_send(void);
-  void asdm_fr03_frame_send(void);
-
-private:
-  ReceiveFrame pscm_chas1_fr03_;
-  ReceiveFrame pscm_chas1_fr07_;
-  ReceiveFrame pscm_chas1_fr01_;
-  ReceiveFrame pscm_chas1_fr02_;
-  ReceiveFrame vddm_chas1_fr21_;
-
-  SendFrame pas_chas1_fr02_;
-  SendFrame pas_chas1_fr04_;
-  SendFrame pas_chas1_nmfr_;
-  SendFrame asdm_chas1_fr01_;
-  SendFrame asdm_chas1_fr03_;
-
-  pthread_mutex_t mtx_pscm_chas1_fr03_;
-  pthread_mutex_t mtx_pscm_chas1_fr07_;
-  pthread_mutex_t mtx_pscm_chas1_fr01_;
-  pthread_mutex_t mtx_pscm_chas1_fr02_;
-  pthread_mutex_t mtx_vddm_chas1_fr21_;
-
-  pthread_mutex_t mtx_pas_chas1_fr02_;
-  pthread_mutex_t mtx_asdm_chas1_fr01_;
-  pthread_mutex_t mtx_asdm_chas1_fr03_;
-};
-
-#endif

src/controller/controller_Geely_xingyueL/include/socket_can_interface/socket_can_interface.h

@@ -42,7 +42,7 @@ public:
     SocketCanInterface() = default;
     ~SocketCanInterface() = default;
 
-    bool Init(const std::string &canDevice, const std::uint32_t &bitRate, const std::string &passWord,  const std::uint32_t &dbitRate = 0); // data bit rate default 0 : mean the can is CAN, set the dbitrate mean CANFD!
+    bool Init(const std::string &canDevice, bool canfd_on);
     bool SetRecvTimeout(const struct timeval &tv) const;
     bool SetCanFiLter(const struct can_filter &filter) const;
     bool SetCanFilters(const std::vector<can_filter> &filters) const;
@@ -66,7 +66,7 @@ public:
     void SetSignedSignalRawData(can_frame &send_can_frame, const SignalAttribute &sig_attr, const int32_t signal_raw);
     void SetSignedSignalRawData(canfd_frame &send_canfd_frame, const SignalAttribute &sig_attr, const int32_t signal_raw);
 
-    void SetUnsignedSignalPhyData(canfd_frame &send_canfd_frame, const SignalAttribute &sig_attr, const float32_t signal_phy);
+    void SetUnsignedSignalPhyData(canfd_frame &send_canfd_frame, const SignalAttribute &sig_attr, const float64_t signal_phy);
     void SetUnsignedSignalPhyData(can_frame &send_can_frame, const SignalAttribute &sig_attr, const float64_t signal_phy);
     void cntr_inc(uint32_t &cntr);
 private:

src/controller/controller_Geely_xingyueL/include/socket_udp/socket_udp.h

@@ -11,6 +11,7 @@
 #include <string.h>
 #include <sys/socket.h>
 #include <unistd.h>
+#include <thread>
 
 // define the socket UDP communication IP and Ports
 #define UDP_SEND_TARGET_IP_STRING "192.168.4.132"
@@ -88,11 +89,13 @@ class RemoteUdpCommunication
 
   private:
   int socket_udp_init();
-  static void* socket_send(void* arg);
-  static void* socket_receive(void* arg);
+  void socket_send();
+  void socket_receive();
   void close_socket_udp();
   void setnonblocking(int sock);
   private:
+  std::unique_ptr<std::thread> thread_socket_send_;
+  std::unique_ptr<std::thread> thread_socket_receive_;
   struct sockaddr_in socksend_addr_;
   struct sockaddr_in sockrcv_addr_;
   int send_fd_ = -1;

src/controller/controller_Geely_xingyueL/main.cpp

@@ -1,3 +1,4 @@
+#if 1
 #include <QCoreApplication>
 
 #include <QTime>
@@ -14,12 +15,23 @@
 #include <fstream>
 #include "include/car_control.h"
 
+
+enum class AppCtrlSm   //cxw,20220622
+{
+  kStandby = 0,
+  kStartup = 1,
+  kActive = 2,
+  kShutDown = 3
+};
+
 CarControl car_control_module;
 AppCtrlSm app_ctrl_car_sm_ = AppCtrlSm::kStandby;
 
 using namespace std;
 
 
+
+
 void * gpadecition;
 
 iv::brain::decition gdecition_def;
@@ -74,7 +86,7 @@ static void ShareChassis(void * pa,iv::chassis  * pchassis)
 
 void executeDecition(const iv::brain::decition decition)
 {
-//    std::cout<<"Command: Acc is "<<decition.accelerator()<<" Angle is "<<decition.wheelangle()<<" DangWei is "<<decition.gear()<<std::endl;
+    std::cout<<"Command: Acc is "<<decition.accelerator()<<" Angle is "<<decition.wheelangle()<<" DangWei is "<<decition.gear()<<std::endl;
 
     GearPrkgAssistReq gearSetVal= GearPrkgAssistReq::kNoRequest;
     double speedSetVal = 0;
@@ -235,6 +247,7 @@ void sendthread()
             communicate_cnt=gCommunctionNum+1;
         }
 
+#if 1
         switch (app_ctrl_car_sm_)
         {
             case AppCtrlSm::kStandby://ctrl_car节点有消息输入，否则退出与汽车的控制
@@ -307,7 +320,7 @@ void sendthread()
                 break;
         }
 
-
+#endif
 
 
 
@@ -315,7 +328,8 @@ void sendthread()
 
 
 //        bool bstatus = car_control_module.is_lat_lgt_ctrl_active();
-
+                    std::cout<<" lat lgt req. status: "<<(int)car_control_module.get_chassis_err_state()
+                            <<"  machine state: "<<(int)car_control_module.get_chassis_statemachine_sts()<< std::endl;
 //        if(bstatus == true)
 //        {
 //            //            std::cout<<"di pan ke kong "<<std::endl;
@@ -329,6 +343,9 @@ void sendthread()
 //        }
 
 
+
+
+#if 0
                        string filename="log.txt";
                        ofstream outfile;
                        outfile.open(filename, ostream::app);
@@ -350,6 +367,278 @@ void sendthread()
 //                              <<"OBS_Distance"<< ","<<obsDistance<< ","                            
                               <<endl;
                        outfile.close();
+#endif
+//        car_control_module.sm_task_20ms();  // 线控状态机函数
+        std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    }
+}
+
+void * gpa;
+
+void recvthread()
+{
+    iv::chassis xchassis;
+    int tmp1[10] = {0,1,2,3,4,5,6,7,8,9};
+    while(1)
+    {
+        gstatus = car_control_module.is_lat_lgt_ctrl_active();
+        ggearSetVal = car_control_module.get_setted_tar_gear();
+        ggearRealVal = car_control_module.get_cur_disp_gear();
+        gchassErr = car_control_module.get_chassis_err_state();
+        gstsMach = car_control_module.get_chassis_statemachine_sts();
+        gspeed = car_control_module.get_current_vehicle_spd_in_ms();
+        gsteerDeg = car_control_module.get_current_steer_ang_in_deg();
+
+        std::cout<<"FeedBack: current_vehicle_spd_in_ms is "<<gspeed<<"err code :"<<(int)gchassErr<<std::endl;
+
+
+        xchassis.set_angle_feedback(gsteerDeg);
+
+        ShareChassis(gpa,&xchassis);
+        std::this_thread::sleep_for(std::chrono::milliseconds(20));
+        /*
+            bool is_lat_lgt_ctrl_active();        // 底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
+            ChassisErrCode get_chassis_err_state();// 底盘错误状态码
+            StsMach get_chassis_statemachine_sts(); // 内部状态机运行状态
+            float32_t get_current_steer_ang_in_deg();// 当前方向盘实际角度
+            float32_t get_current_vehicle_spd_in_ms();// 当前车辆实际车速，单位ｍ／ｓ
+            GearPrkgAssistReq get_setted_tar_gear(); // 获取当前设定目标档位值
+            GearLevelIndicate get_cur_disp_gear(); // 当前实际显示档位状态
+        */
+    }
+}
+
+int main(int argc, char *argv[])
+{
+    auto ret = google_glog_config("/home/nvidia/log1/.");
+
+    RegisterIVBackTrace();
+    showversion("controller_Geely_xingyueL");
+    QCoreApplication a(argc, argv);
+
+    QString strpath = QCoreApplication::applicationDirPath();
+
+    if(argc < 2)
+        strpath = strpath + "/controller_Geely_xingyueL.xml";
+    else
+        strpath = argv[1];
+    std::cout<<strpath.toStdString()<<std::endl;
+
+    //    car_control_module.set_lat_lgt_ctrl_req(true);
+    //    car_control_module.sm_task_20ms();  // 线控状态机函数
+
+    //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
+
+    iv::xmlparam::Xmlparam xp(strpath.toStdString());
+    std::string gstrmemdecition = xp.GetParam("dection","deciton");
+    std::string strchassismsgname = xp.GetParam("chassismsgname","chassis");
+
+    gpa = iv::modulecomm::RegisterSend(strchassismsgname.data(),1000,1);
+    gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
+
+    std::thread xthread(sendthread);
+//    std::thread myxthread(recvthread);
+
+    return a.exec();
+}
+
+#else
+
+#include <QCoreApplication>
+
+#include <QTime>
+#include <QMutex>
+#include <QTimer>
+#include "xmlparam.h"
+#include "modulecomm.h"
+#include "ivbacktrace.h"
+#include "ivversion.h"
+#include "decition.pb.h"
+#include "chassis.pb.h"
+#include <thread>
+#include "include/car_control.h"
+
+CarControl car_control_module;
+
+void * gpadecition;
+
+iv::brain::decition gdecition_def;
+iv::brain::decition gdecition;
+
+int gnDecitionNum = 0; //when is zero,send default;
+const int gnDecitionNumMax = 100;
+
+static QMutex gMutex;
+
+bool gstatus;
+GearPrkgAssistReq ggearSetVal;
+GearLevelIndicate ggearRealVal;
+ChassisErrCode gchassErr;
+StsMach gstsMach;
+float gsteerDeg, gspeed;
+double lastspeedSetVal = 0;
+double lastEpsSetVal = 0;
+
+GearPrkgAssistReq lastgearSetVal= GearPrkgAssistReq::kNoRequest;
+
+static void ShareChassis(void * pa,iv::chassis  * pchassis)
+{
+    char * str;
+    int ndatasize = pchassis->ByteSize();
+    str = new char[ndatasize];
+    std::shared_ptr<char> pstr;
+    pstr.reset(str);
+    if(!pchassis->SerializeToArray(str,ndatasize))
+    {
+        std::cout<<"ShareChassis Error."<<std::endl;
+        return;
+    }
+    iv::modulecomm::ModuleSendMsg(pa,str,ndatasize);
+
+}
+
+void executeDecition(const iv::brain::decition decition)
+{
+    std::cout<<"Command: Acc is "<<decition.accelerator()<<" Angle is "<<decition.wheelangle()<<" DangWei is "<<decition.gear()<<std::endl;
+
+    GearPrkgAssistReq gearSetVal= GearPrkgAssistReq::kNoRequest;
+    double speedSetVal = 0;
+    double EpsSetVal = 0;
+
+    if(decition.has_gear()){
+        switch (decition.gear()) {
+        case 1:
+            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
+            gearSetVal=GearPrkgAssistReq::kTargetGearP;
+            break;
+        case 2:
+            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearR);
+            gearSetVal=GearPrkgAssistReq::kTargetGearR;
+            break;
+        case 3:
+            //car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
+            gearSetVal=GearPrkgAssistReq::kTargetGearD;
+            break;
+        default:
+            //car_control_module.set_target_gear(GearPrkgAssistReq::kNoRequest);
+            gearSetVal=GearPrkgAssistReq::kNoRequest;;
+            break;
+        }
+        lastgearSetVal=gearSetVal;
+    }
+    else
+    {
+        gearSetVal=lastgearSetVal;
+    }
+
+    if(decition.has_accelerator()){
+        speedSetVal=decition.accelerator();
+        lastspeedSetVal=speedSetVal;
+        //speedSetVal=0.1;
+       // car_control_module.set_target_acc_mps2(decition.accelerator());
+//        car_control_module.set_target_acc_mps2(0.1);
+    }
+    else
+    {
+        speedSetVal=lastspeedSetVal;
+    }
+
+    if(decition.has_wheelangle())
+    {
+       // car_control_module.set_target_pinion_ag_in_deg(0.0);
+        EpsSetVal=decition.wheelangle();
+        lastEpsSetVal=EpsSetVal;
+       // EpsSetVal=0.0;//
+  // car_control_module.set_target_pinion_ag_in_deg(decition.wheelangle());
+    }
+    else
+    {
+        EpsSetVal=lastEpsSetVal;
+    }
+    // gearSetVal=GearPrkgAssistReq::kTargetGearD;
+    car_control_module.set_target_gear(gearSetVal);
+    car_control_module.set_target_acc_mps2(speedSetVal);
+    car_control_module.set_target_pinion_ag_in_deg(EpsSetVal);
+
+    if(decition.has_leftlamp() && decition.leftlamp()==true)
+        car_control_module.set_turn_light_status(TurnLightIndicReq::kLeft);
+    else if(decition.has_rightlamp() && decition.rightlamp()==true)
+        car_control_module.set_turn_light_status(TurnLightIndicReq::kRight);
+    else
+        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
+
+    /*
+    void set_lat_lgt_ctrl_req(bool req);  // 握手请求, true:请求握手， false:退出握手
+    void set_target_gear(GearPrkgAssistReq tar);  // 目标档位请求
+    bool is_lat_lgt_ctrl_active();        // 底盘控制状态反馈， true: 底盘可线控， false: 底盘不可控
+    void set_target_pinion_ag_in_deg(float32_t ang_req);// 设置目标方向盘角度请求， -450°至+450°（车辆实测范围值），方向盘左正，右负。
+    void set_target_acc_mps2(float32_t tar_acc);// 目标加减速度值。
+    void set_turn_light_status(TurnLightIndicReq req);// 转向灯控制请求
+*/
+}
+
+
+
+
+void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    qint64 oldtime;
+    iv::brain::decition xdecition;
+
+    if(!xdecition.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"ListenDecition parse error."<<std::endl;
+        return;
+    }
+
+    if(xdecition.gear() != 3)
+    {
+        qDebug("not D");
+    }
+    if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)
+        // qDebug("dection time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
+        oldtime = QDateTime::currentMSecsSinceEpoch();
+    gMutex.lock();
+    gdecition.CopyFrom(xdecition);
+    gMutex.unlock();
+    gnDecitionNum = gnDecitionNumMax;
+    std::cout<<"update decision. "<<std::endl;
+}
+
+
+void sendthread()
+{
+    iv::brain::decition xdecition;
+//    car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
+//    car_control_module.sm_task_20ms();  // 线控状态机函数
+
+    while(1)
+    {
+        if(gnDecitionNum <= 0)
+        {
+            xdecition.CopyFrom(gdecition_def);
+        }
+        else
+        {
+            gMutex.lock();
+            xdecition.CopyFrom(gdecition);
+            gMutex.unlock();
+            gnDecitionNum--;
+        }
+
+        bool bstatus = car_control_module.is_lat_lgt_ctrl_active();
+
+        if(bstatus == true)
+        {
+            //            std::cout<<"di pan ke kong "<<std::endl;
+            executeDecition(xdecition);
+        }
+        else
+        {
+            std::cout<<" lat lgt req. status: "<<(int)car_control_module.get_chassis_err_state()
+                    <<"  machine state: "<<(int)car_control_module.get_chassis_statemachine_sts()<< std::endl;
+            car_control_module.set_lat_lgt_ctrl_req(true);
+        }
 
 //        car_control_module.sm_task_20ms();  // 线控状态机函数
         std::this_thread::sleep_for(std::chrono::milliseconds(20));
@@ -393,6 +682,7 @@ void recvthread()
 
 int main(int argc, char *argv[])
 {
+    auto ret = google_glog_config("/home/nvidia/log1/.");
     RegisterIVBackTrace();
     showversion("controller_Geely_xingyueL");
     QCoreApplication a(argc, argv);
@@ -422,3 +712,5 @@ int main(int argc, char *argv[])
 
     return a.exec();
 }
+
+#endif

src/decition/common/common/car_status.h

@@ -163,8 +163,8 @@ namespace iv {
         iv::roadmode_vel mroadmode_vel;
         int vehGroupId;
         double mfEPSOff = 0.0;
-        float socfDis=15;   //停障触发距离
-        float aocfDis=25;   //避障触发距离//20
+        float socfDis=10;   //停障触发距离
+        float aocfDis=42;   //避障触发距离//20
         float aocfTimes=3; //避障触发次数
         float aobzDis=5;   //避障保障距离
         /// traffice light : 0x90

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_adapter/jeely_xingyueL_adapter.cpp

@@ -84,23 +84,47 @@ iv::decition::Decition iv::decition::Jeely_xingyueL_adapter::getAdapterDeciton(G
     }
 
     //0227 10m nei xianzhi shache
-    if(obsDistance<10 &&obsDistance>0){
-        controlSpeed=min(controlSpeed,-0.8f);
+    if(obsDistance<5 &&obsDistance>0){
+        controlSpeed=min(controlSpeed,-0.9f);
+      //  controlSpeed=max(controlSpeed,-0.4f);
     }
+//    if(controlSpeed<-0.2)//debug ,lianxu,zhangaiwu buwen
+//    {
+//        controlSpeed=-0.1;
+//    }
 
 
     if(DecideGps00::minDecelerate<0){
-        controlSpeed = min(DecideGps00::minDecelerate, controlSpeed);
+        controlSpeed = min(DecideGps00::minDecelerate, controlSpeed);//-1 from here apollo_fu 20220704
     }
+
+
+//        if(controlSpeed<-0.5)//debug,zhandianqiting
+//        {
+//            controlSpeed=-0.5;
+//        }
+
     controlSpeed = limitSpeed(realSpeed, controlBrake, dSpeed, controlSpeed);
 
+
+
     if(realSpeed<6.0)//xw
     {
-        controlSpeed=min(controlSpeed,0.1f);
+//        if(controlSpeed>0.0)
+//        {
+//            controlSpeed=0.5;
+
+//        }
+//        else
+//        {
+//           controlSpeed=max(controlSpeed,-0.5f);
+//        }
+        controlSpeed=min(controlSpeed,0.5f);
     }
 
     //controlSpeed=0.1;
 
+      (*decition)->mode=1;
     (*decition)->accelerator=controlSpeed;
 
     if((*decition)->leftlamp){

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_planner/spline_planner.cpp

@@ -36,7 +36,7 @@ std::vector<iv::Point2D> iv::decition::SplinePlanner::chooseRoadBySpline(GPS_INS
         if(fabs(obsSpline[j].d - nowX)<= 1.0e-6){
                     s_obs_current_relative=obsSpline[j].s;
                     if(s_obs_current_relative>40)
-                        s_obs_current_relative=40;
+                        s_obs_current_relative=40;//40gaicheng20
         }
     }
     iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);

src/decition/decition_brain_sf_Jeely_xingyueL/decition/adc_tools/transfer.cpp

@@ -228,7 +228,7 @@ iv::Point2D iv::decition::frenet_to_cartesian1D(std::vector<GPSData> gpsMap,doub
     int s_index=(int)(s_condition-(*gpsMap[index]).frenet_s)*10+index;
     int start_index=max(0,s_index-500);
     int map_size=gpsMap.size();
-    int end_index=min(s_index+500,map_size);
+    int end_index=min(s_index+500,map_size-1);
 
     int map_max=(*gpsMap[map_size-1]).frenet_s;
     double s_index_min=(*gpsMap[start_index]).frenet_s;

src/decition/decition_brain_sf_Jeely_xingyueL/decition/brain.cpp

@@ -1558,7 +1558,7 @@ void iv::decition::BrainDecition::UpdateFusion(std::shared_ptr<iv::fusion::fusio
                 fusion_ptr_[dx*(iv::gry) + dy].type = xobs.type();
                 fusion_ptr_[dx*(iv::gry) + dy].high = xobs.dimensions().z();
                 fusion_ptr_[dx*(iv::gry) + dy].speed_x = xobs.vel_relative().x();
-                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.vel_relative().y();
+                fusion_ptr_[dx*(iv::gry) + dy].speed_y = xobs.velocity_linear_x();
                 fusion_ptr_[dx*(iv::gry) + dy].yaw = xobs.yaw();
                 fusion_ptr_[dx*(iv::gry) + dy].pointcount = 5;
                 fusion_ptr_[dx*(iv::gry) + dy].ob = 2;

src/decition/decition_brain_sf_Jeely_xingyueL/decition/decide_gps_00.cpp

@@ -1170,7 +1170,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         if((now_s_d.s+20>(gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s))&&((gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s)<((*gpsMapLine[gpsMapLine.size()-1]).frenet_s))){
             double sPathFinal=gpsTraceAvoidXY[gpsTraceAvoidXY.size()-1].s;
             if(sPathFinal+20<=(*gpsMapLine[gpsMapLine.size()-1]).frenet_s){
-                sPathFinal=sPathFinal+20;
+                sPathFinal=sPathFinal+20;//20gaicheng 6
             }else{
                 sPathFinal=(*gpsMapLine[gpsMapLine.size()-1]).frenet_s;
             }
@@ -1183,13 +1183,13 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if((vehState == avoiding || vehState == backOri)&&(gpsTraceAvoidXY.size()>0)&&(obstacle_avoid_flag==1))
     {
+       // gpsTraceOri.clear();//20220811,cxw,add
         gpsTraceOri=getGpsTraceAvoid(now_gps_ins, gpsTraceAvoidXY, lastGpsIndex,circleMode);
     }
 
     if(gpsTraceOri.empty()){
         gps_decition->wheel_angle = 0;
         gps_decition->speed = dSpeed;
-
         gps_decition->accelerator = -0.5;
         gps_decition->brake=10;
         return gps_decition;
@@ -1347,18 +1347,18 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     //2020-0106
-    if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
+//    if(ServiceCarStatus.msysparam.mvehtype !="zhongche"){
 
-        if(vehState==avoiding){
-            ServiceCarStatus.msysparam.vehWidth=2.4;
-            controlAng=max(-150.0,controlAng);//35  zj-150
-            controlAng=min(150.0,controlAng);//35   zj-150
-        }
-        if(vehState==backOri){
-            controlAng=max(-150.0,controlAng);//35   zj-150
-            controlAng=min(150.0,controlAng);//35     zj-150
-        }
-    }
+//        if(vehState==avoiding){
+//            ServiceCarStatus.msysparam.vehWidth=2.4;
+//            controlAng=max(-150.0,controlAng);//35  zj-150
+//            controlAng=min(150.0,controlAng);//35   zj-150
+//        }
+//        if(vehState==backOri){
+//            controlAng=max(-150.0,controlAng);//35   zj-150
+//            controlAng=min(150.0,controlAng);//35     zj-150
+//        }
+//    }
 
 //    givlog->debug("brain_decition","vehState: %d,controlAng: %f",
 //            vehState,controlAng);
@@ -1599,7 +1599,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     static double obs_speed_for_avoid=-1,obs_filter_speed_memory=0;   //obs_speed_for_avoid: obstacle actual speed in km/h
     if(!ServiceCarStatus.daocheMode){
         //computeObsOnRoad(lidarGridPtr, gpsTraceNow, roadNow,gpsMapLine,lidar_per);
-        computeObsOnRoadXY(lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
+        computeObsOnRoadXY(now_gps_ins,lidarGridPtr, gpsTraceNow,gpsTraceNowLeft,gpsTraceNowRight,roadNow,gpsMapLine,lidar_per);
 
 
         if(obs_filter_flag==0)
@@ -1647,6 +1647,8 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
         obs_speed_for_avoid=(secSpeed+obsSpeed)*3.6;
+//        obsSpeed=0-secSpeed;
+//        obs_speed_for_avoid=0;
 
 
     }else{
@@ -1714,7 +1716,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //            gps_decition->rightlamp = false;
 //        }
 
-        dSpeed = min(6.0,dSpeed);
+        dSpeed = min(10.0,dSpeed); //6gaicheng 10,20220818
         if (turnLampFlag>0)
         {
             gps_decition->leftlamp = false;
@@ -1876,7 +1878,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if(PathPoint+400<gpsMapLine.size()){
         int road_permit_sum=0;
         for(int k=PathPoint;k<PathPoint+400;k++){
-                if((gpsMapLine[k]->mbnoavoid==false)&&(gpsMapLine[k]->roadSum>1))
+                if((gpsMapLine[k]->mbnoavoid==false))//&&(gpsMapLine[k]->roadSum>1))
                                  road_permit_sum++;
         }
         if(road_permit_sum>=400)
@@ -1884,7 +1886,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     }
 
     //shiyanbizhang 0929
-    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>1)//&& (avoid_speed_flag==true)        //
+    if(obsDistance>0&& (obsDistance<ServiceCarStatus.aocfDis)&&(obs_speed_for_avoid>-8.0)&&(obs_speed_for_avoid<3.0)&&(realspeed>0.1)//&& (avoid_speed_flag==true)//sudugaiwei0.1        //
             &&(vehState==normalRun||vehState==backOri || vehState==avoiding) //&&(ObsTimeStart==-1)
             && (gpsMapLine[PathPoint]->runMode==1)&& (road_permit_avoid==true)&&(gpsMapLine[PathPoint]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=3000)&&(gpsMapLine[PathPoint+100]->mode2!=3000)&&(gpsMapLine[PathPoint+300]->mode2!=23)){
 //        ObsTimeStart=GetTickCount();
@@ -1913,7 +1915,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     if (vehState == avoidObs   || vehState==waitAvoid ) {
         if (obsDistance <ServiceCarStatus.aocfDis && obsDistance >0)
         {
-            dSpeed = min(6.0,dSpeed);
+            dSpeed = min(10.0,dSpeed);//6gaiwei10 ,20220818
             avoidTimes++;
             //          if (avoidTimes>=6)
             if (avoidTimes>=ServiceCarStatus.aocfTimes)
@@ -1925,7 +1927,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         }
         else if (obsDistance < 40 && obsDistance >0 && dSpeed>15)
         {
-            dSpeed =  min(15.0,dSpeed);
+            dSpeed =  min(10.0,dSpeed);//15gaiceng10,20220816
             avoidTimes = 0;
         }
         else
@@ -1996,15 +1998,18 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
 
 
-    if((vehState == preAvoid)||(avoidXNew!=0))
+    if((vehState == preAvoid)||((avoidXNew!=0)))
+//    if((vehState == preAvoid)||(avoidXNew!=0))
     {
+        dSpeed = min(10.0,dSpeed);//6gaiwei 10
+        static int count_avoidx_0=0;
         int avoidLeft_value=0;
         int avoidRight_value=0;
         int* avoidLeft_p=&avoidLeft_value;
         int* avoidRight_p=&avoidRight_value;
         computeAvoidBoundary(roadOri,roadSum,gpsMapLine[PathPoint]->mfLaneWidth,ServiceCarStatus.msysparam.vehWidth,avoidLeft_p,avoidRight_p);
         avoidXNewPre=computeAvoidXDistance(lidarGridPtr,now_gps_ins, gpsMapLine,lidar_per,avoidLeft_value,avoidRight_value,avoidXNewLast);
-        if(avoidXNewPreVector.size()<5){
+        if(avoidXNewPreVector.size()<20){
             avoidXNewPreVector.push_back(avoidXNewPre);
         }else{
             if(avoidXNewPreVector[0]!=avoidXNewLast){
@@ -2028,12 +2033,29 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             else
                 turnLampFlag=0;
 
-           gpsTraceNow.clear();
+//           gpsTraceNow.clear();
            gpsTraceAvoidXY.clear();
            givlog->debug("decition_brain","avoidXNew: %d,avoidXNewLast: %d",
                            avoidXNew,avoidXNewLast);
            //gpsTraceAvoidXY = getGpsTraceOffsetAvoid(gpsTraceOri, avoidXNew,now_gps_ins);
-           gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+//           gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+           if(vehState == preAvoid)
+           {
+               gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+           }
+           else if(avoidXNew==0)
+           {
+               count_avoidx_0++;
+               if(count_avoidx_0>60)
+               {
+                  gpsTraceAvoidXY = splinePlanner->chooseRoadBySpline(now_gps_ins,gpsMapLine,PathPoint,obsSpline,avoidXNew,avoidXNewLast);
+
+               }
+            }
+           if(avoidXNew!=0)
+           {
+               count_avoidx_0=0;
+           }
            vehState = avoiding;
            startAvoidGpsIns = now_gps_ins;
            obstacle_avoid_flag=1;
@@ -2042,7 +2064,16 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
            avoidXNewPreFilter=avoidXNew;
         }
     }
-
+if (vehState == preAvoid)
+{
+    //dSpeed = min(6.0,dSpeed);
+    iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x, now_gps_ins.gps_y);
+            if ((obsDistance>ServiceCarStatus.aocfDis)&&(fabs(now_s_d.d)<0.05))
+            {
+                // vehState = avoidObs; 0929
+                vehState=normalRun;
+            }
+}
 
 
 //    if (vehState == preAvoid)
@@ -2390,7 +2421,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         if(obsDistance>0 && obsDistance<8){
                 dSpeed=0;
             }
-    }else if(obsDistance>0 && obsDistance<15){
+    }else if(obsDistance>0 && obsDistance<10){
         dSpeed=0;
     }
 
@@ -2601,17 +2632,19 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
                                <<"Decide_ACC"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
                                <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
                                <<"Vehicle_RealSpd"<< ","  <<setprecision(2)<<now_gps_ins.speed<< ","
-//                               <<"zuo_ultra"<< ","<<ServiceCarStatus.mbUltraDis[3]<< ","
-//                               <<"you_ultra"<< ","<<ServiceCarStatus.mbUltraDis[1]<< ","
-    //                          <<"OBS_Distance"<< ","<<obsDistance_log<< ","
-    //                           <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
-    //                           <<"Vehicle_state"<< ","<<vehState<< ","
-    //                           <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
-    //                           <<"avoidXNew"<<","<<avoidXNew<<","
-    //                           <<"avoidXNewPre"<<","<<avoidXNewPre<<","
-    //                           <<"avoidXPre"<<","<<avoidXPre<<","
-    //                           <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
-    //                           <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
+//                            <<"readyParkMode"<< ","<<readyParkMode<< ","
+//                              <<"readyZhucheMode"<< ","<<readyZhucheMode<< ","
+                              <<"OBS_Distance"<< ","<<obsDistance_log<< ","
+                               <<"OBS_Speed"<< ","<<obs_speed_for_avoid<< ","
+                              <<"obsSpeed_fusion"<<","<<obsSpeed<<","
+                              <<"SecSpeed"<<","<<secSpeed<<","
+                               <<"Vehicle_state"<< ","<<vehState<< ","
+                              <<"avoid_flag"<<","<<obstacle_avoid_flag<<","
+                               <<"avoidXNew"<<","<<avoidXNew<<","
+                               <<"avoidXNewPre"<<","<<avoidXNewPre<<","
+                               //<<"avoidXPre"<<","<<avoidXPre<<","
+                               <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
+                               <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
                                <<"Min_Decelation"","<<minDecelerate<< ","
     //                           <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
     //                           <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
@@ -2619,7 +2652,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
     //                           <<"Vehicle_GPS_Y"<< ","<< setprecision(10)<<now_gps_ins.gps_lng<< ","
     //                           <<"Trace_Point"<< ","<<PathPoint<< ","
     //                           <<"OBS_Speed"<< ","<<obsSpeed<< ","
-    //                           <<"OBS_Distance"<< ","<<obsDistance<< ","
+                               <<"gpsTraceOri"<< ","<<gpsTraceOri.size()<< ","
     //                           <<"TTC"<< ","<<ttc<< ","
 //                               <<"Decide_torque"  << ","  <<setprecision(2)<<gps_decition->torque<< ","
 //                               <<"Decide_Brake"<< ","  <<gps_decition->brake<< ","
@@ -2632,7 +2665,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 //                               <<"avoidXNewPre"<<","<<avoidXNewPre<<","
 ////                               <<"avoidXPre"<<","<<avoidXPre<<","
 //                               <<"ObsTimeEnd"<<","<<ObsTimeEnd<<","
-//                               <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
+                             <<"avoidXY_size"<<","<<gpsTraceAvoidXY.size()<<","
 //                               <<"Min_Decelation"","<<minDecelerate<< ","
                                <<"Decide_Angle"<< ","  << setprecision(2)<<gps_decition->wheel_angle<< ","
                               // <<"Vehicle_GPS_heading"<< ","<< setprecision(10)<<now_gps_ins.ins_heading_angle<< ","
@@ -3583,13 +3616,20 @@ void iv::decition::DecideGps00::computeObsOnRoad(iv::LidarGridPtr lidarGridPtr,
 
 }
 
-void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+//void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
+//                                                 const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
+void iv::decition::DecideGps00::computeObsOnRoadXY( iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,
                                                  const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per) {
 
+
     //    QTime xTime;
     //   xTime.start();
     //    qDebug("time 0 is %d ",xTime.elapsed());
     double obs,obsSd;
+    double obsCarCoordinateX,obsCarCoordinateY;
+    GPS_INS obsGeodetic;
+    Point2D obsFrenet,obsFrenetMid;
+    double obsCarCoordinateDistance=-1;
 
     if (lidarGridPtr == NULL)
     {
@@ -3604,7 +3644,15 @@ void iv::decition::DecideGps00::computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr
         if(!ServiceCarStatus.useMobileEye){
             lidarXiuZheng=0-ServiceCarStatus.msysparam.frontGpsXiuzheng;
         }
-        lidarDistance = obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
+
+        obsCarCoordinateX=obsPoint.x;
+        obsCarCoordinateY=obsPoint.y;
+        obsGeodetic = Coordinate_UnTransfer(obsCarCoordinateX, obsCarCoordinateY, now_gps_ins);   //车体转大地
+        obsFrenetMid=cartesian_to_frenet1D(gpsMapLine,obsGeodetic.gps_x,obsGeodetic.gps_y);          //大地转frenet
+        iv::Point2D now_s_d=cartesian_to_frenet1D(gpsMapLine,now_gps_ins.gps_x,now_gps_ins.gps_y);
+        givlog->debug("decition_brain","road_num: %d,obsFrenetMid_s: %f,now_s: %f",roadNum,obsFrenetMid.s,now_s_d.s);
+        obsFrenet.s=obsFrenetMid.s-now_s_d.s;
+        lidarDistance = obsFrenet.s;//obsPoint.y + lidarXiuZheng;   //激光距离推到车头  gjw20191110 lidar
         //    lidarDistance=-1;
         if (lidarDistance<0)
         {

src/decition/decition_brain_sf_Jeely_xingyueL/decition/decide_gps_00.h

@@ -201,7 +201,7 @@ public:
     bool checkReturnEnable(double avoidX, iv::LidarGridPtr lidarGridPtr, int roadNum);
 
     void computeObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
-    void computeObsOnRoadXY(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
+    void computeObsOnRoadXY(iv::GPS_INS now_gps_ins,iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace,std::vector<Point2D> gpsTraceLeft,std::vector<Point2D> gpsTraceRight,int roadNum,const std::vector<GPSData> gpsMapLine, std::vector<iv::Perception::PerceptionOutput> lidar_per);
     static void getEsrObsDistanceByFrenet(const std::vector<Point2D>& gpsTrace, iv::decition::FrenetPoint car_now_frenet_point,iv::decition::FrenetPoint &esrObs_F_Point, const std::vector<iv::GPSData>& gpsMap,int pathpoint,GPS_INS nowGps);
 
     void computeRearObsOnRoad(iv::LidarGridPtr lidarGridPtr, std::vector<Point2D> gpsTrace, int roadNum,const std::vector<GPSData> gpsMapLine);
@@ -271,7 +271,7 @@ public:
     float  ObsTimeSpan=-1;
     double ObsTimeStart=-1;
     double ObsTimeEnd=-1;
-    float ObsTimeWidth=70.0;   //500   zj-30
+    float ObsTimeWidth=40;//70.0;   //500   zj-30
     std::vector<iv::TracePoint> planTrace;
 
     bool roadNowStart=true;

src/tool/data_serials/mainwindow.cpp

@@ -8,9 +8,9 @@ MainWindow::MainWindow(QWidget *parent)
     ui->setupUi(this);
     mp_tabWidget = ui->tabWidget;
 
-    ui->checkBoxAcc->setEnabled(false);
+    ui->checkBoxAcc->setEnabled(true);
     ui->checkBoxBrake->setEnabled(false);
-    ui->checkBoxEps->setEnabled(false);
+    ui->checkBoxEps->setEnabled(true);
     ui->checkBoxHeading->setEnabled(false);
     ui->checkBoxLidarObs->setEnabled(false);
     ui->checkBoxObsDis->setEnabled(false);
@@ -26,14 +26,14 @@ MainWindow::MainWindow(QWidget *parent)
 //    iniTabWidgetLines(mp_widgetOnlineLines, &mq_plotOnlineLines, true);
 //    iniTabWidgetLines(mp_widgetOfflineLines, &mq_plotOffineLines, false);
     //初始化地图曲线
-    iniTabWidgetMap();
+//    iniTabWidgetMap();
     //显示导入的离线决策数据
 //    initGraphOffline();
-    mp_tabWidget->addTab(mp_plotOnlineMap,"地图轨迹");
+//    mp_tabWidget->addTab(mp_plotOnlineMap,"地图轨迹");
     mp_tabWidget->addTab(mp_widgetOnlineLines,"在线曲线");
 //    mp_tabWidget->addTab(mp_widgetOffline,"离线数据");
 
-    connect(mp_dataParser,SIGNAL(signalUpGPS()),this,SLOT(on_updataGPS()));
+//    connect(mp_dataParser,SIGNAL(signalUpGPS()),this,SLOT(on_updataGPS()));
     connect(mp_dataParser,SIGNAL(signalUpDecision()),this,SLOT(on_updataDecision()));
 
 }

src/ui/ui_ads_hmi/ADCIntelligentVehicle.cpp

@@ -1304,9 +1304,9 @@ void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
         // draw plan trace end
 /////////////////////////////////////apollo add car icon  20200409
         QPixmap pix;
-        //pix.load("car.png");
+        pix.load("car.png");
 //        pix.load(":/ADCIntelligentVehicle/car1.png");
-//        painter->drawPixmap(435,667,30,67,pix);
+        painter->drawPixmap(435,667,30,67,pix);
 //        painter_small->drawPixmap(442,683,16,34,pix);
 ///////////////////////////////////////////////////////////////////
 

src/v2x/obuUdpClient/udpreciver.cpp

@@ -33,8 +33,50 @@ void UdpReciver::readDatagrams()
     {
         m_data.resize(m_udpSocket->pendingDatagramSize());
         m_udpSocket->readDatagram(m_data.data(), m_data.size(), &client_address);
-        QString strclient_address = client_address.toString();
-        deliverInfo(m_data, strclient_address);
+//        QString strclient_address = client_address.toString();
+//        deliverInfo(m_data, strclient_address);
         qDebug() << "receive UDP data: " << m_data;
+        ReceiveDecode(m_data);
+    }
+}
+
+void UdpReciver::ReceiveDecode(QByteArray &data)
+{
+    static int BATH_LENTH = 31;
+    // 防包太大
+    if (data.size() > 2048) {
+        //qDebug() << "size too large";
+        return;
+    }
+    //  ##
+    char first;
+    char second;
+    // 寻找报文开头
+    while (data.size() >= BATH_LENTH) {
+        while (1) {
+            first = data[0];
+            second = data[1];
+            if (first == '#' && second == '#') {
+                break;
+            }
+            // 删除一个字符
+            data.remove(0, 1);
+            if (data.size() < BATH_LENTH)
+                return;
+        }
+        int high = data.at(22);
+        int low = data.at(23);
+        int dataLen = (high << 8) | low;
+        //qDebug() << "dataLen:" << dataLen;
+        // 长度不对
+        if ((dataLen + sizeof(packageDataHead)) > data.size()) {
+            //qDebug() << "message len error";
+            return;
+        }
+        QByteArray temp = data.left(sizeof(packageDataHead) + dataLen + 1);
+        //queue_mutex.lock();
+        readData.enqueue(temp);
+        //queue_mutex.unlock();
+        data.remove(0, sizeof(packageDataHead) + dataLen + 1);//need check,20210915,jiaolili
     }
 }

src/v2x/obuUdpClient/udpreciver.h

@@ -20,6 +20,7 @@ private:
     int m_port;
     QByteArray m_data;
     QThread *m_thread;
+    void ReceiveDecode(QByteArray &data);
 };