add car control tool, but still cann't work

src/tool/carControlTester/mainwindow.cpp

@@ -7,13 +7,21 @@ MainWindow::MainWindow(QWidget *parent) :
     ui(new Ui::MainWindow)
 {
     ui->setupUi(this);
-    car_control_module.set_lat_lgt_ctrl_req(false);
-    car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
-    car_control_module.sm_task_20ms();
+        car_control_module.set_lat_lgt_ctrl_req(false);
+        car_control_module.set_target_gear( GearPrkgAssistReq::kNoRequest);
+        car_control_module.set_target_pinion_ag_in_deg(0);
+        car_control_module.set_target_acc_mps2(0);
+        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
+    //    car_control_module.sm_task_20ms();
+    QTimer * timerTask = new QTimer();
+    connect(timerTask,SIGNAL(timeout()),this,SLOT(onTimerTask()));
+    timerTask->setTimerType(Qt::PreciseTimer);
+    timerTask->start(20);
+
     QTimer * timer = new QTimer();
     connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
     timer->setTimerType(Qt::PreciseTimer);
-    timer->start(200);
+    timer->start(20);
 }
 
 MainWindow::~MainWindow()
@@ -25,7 +33,7 @@ MainWindow::~MainWindow()
 
 void MainWindow::on_cBox_Handshake_clicked(bool checked)
 {
-    car_control_module.set_lat_lgt_ctrl_req(checked);
+    bctrlAct = checked;
     ui->cBoxGearD->setEnabled(checked);
     ui->cBoxGearR->setEnabled(checked);
     ui->cBoxGearP->setEnabled(checked);
@@ -42,13 +50,13 @@ void MainWindow::on_cBoxGearD_clicked(bool checked)
 {
     if(checked)
     {
-        car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearD);
+        gearSetVal = GearPrkgAssistReq::kTargetGearD;
         ui->cBoxGearP->setChecked(false);
         ui->cBoxGearR->setChecked(false);
     }
     else
     {
-        car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
+        gearSetVal = GearPrkgAssistReq::kTargetGearP;
         ui->cBoxGearD->setChecked(false);
         ui->cBoxGearR->setChecked(false);
         ui->cBoxGearP->setChecked(true);
@@ -59,13 +67,13 @@ void MainWindow::on_cBoxGearR_clicked(bool checked)
 {
     if(checked)
     {
-        car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearR);
+        gearSetVal = GearPrkgAssistReq::kTargetGearR;
         ui->cBoxGearP->setChecked(false);
         ui->cBoxGearD->setChecked(false);
     }
     else
     {
-        car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
+        gearSetVal =GearPrkgAssistReq::kTargetGearP;
         ui->cBoxGearD->setChecked(false);
         ui->cBoxGearR->setChecked(false);
         ui->cBoxGearP->setChecked(true);
@@ -74,7 +82,7 @@ void MainWindow::on_cBoxGearR_clicked(bool checked)
 
 void MainWindow::on_cBoxGearP_clicked(bool checked)
 {
-    car_control_module.set_target_gear(GearPrkgAssistReq::kTargetGearP);
+    gearSetVal =GearPrkgAssistReq::kTargetGearP;
     ui->cBoxGearP->setChecked(true);
     if(checked)
     {
@@ -87,13 +95,13 @@ void MainWindow::on_cBoxLampL_clicked(bool checked)
 {
     if(checked)
     {
-        car_control_module.set_turn_light_status(TurnLightIndicReq::kLeft);
+        turnLightSetVal = TurnLightIndicReq::kLeft;
         ui->cBoxLampR->setChecked(false);
         ui->cBoxLampH->setChecked(false);
     }
     else
     {
-        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
+        turnLightSetVal = TurnLightIndicReq::kOff;
         ui->cBoxLampL->setChecked(false);
         ui->cBoxLampR->setChecked(false);
         ui->cBoxLampH->setChecked(true);
@@ -104,13 +112,13 @@ void MainWindow::on_cBoxLampR_clicked(bool checked)
 {
     if(checked)
     {
-        car_control_module.set_turn_light_status(TurnLightIndicReq::kRight);
+        turnLightSetVal = TurnLightIndicReq::kRight;
         ui->cBoxLampL->setChecked(false);
         ui->cBoxLampH->setChecked(false);
     }
     else
     {
-        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
+        turnLightSetVal = TurnLightIndicReq::kOff;
         ui->cBoxLampL->setChecked(false);
         ui->cBoxLampR->setChecked(false);
         ui->cBoxLampH->setChecked(true);
@@ -119,7 +127,7 @@ void MainWindow::on_cBoxLampR_clicked(bool checked)
 
 void MainWindow::on_cBoxLampH_clicked(bool checked)
 {
-    car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
+    turnLightSetVal = TurnLightIndicReq::kOff;
     ui->cBoxLampH->setChecked(true);
     if(checked)
     {
@@ -130,28 +138,66 @@ void MainWindow::on_cBoxLampH_clicked(bool checked)
 
 void MainWindow::on_sBoxSpeedSet_valueChanged(double arg1)
 {
-    car_control_module.set_target_acc_mps2(arg1);
+    speedSetVal = arg1;
     ui->vSliderSpeedSet->setValue(int(arg1));
 }
 
 void MainWindow::on_vSliderSpeedSet_valueChanged(int value)
 {
-    car_control_module.set_target_acc_mps2(double(value));
+    speedSetVal = double(value);
     ui->sBoxSpeedSet->setValue(double(value));
 }
 
 void MainWindow::on_sBoxEpsSet_valueChanged(double arg1)
 {
-    car_control_module.set_target_pinion_ag_in_deg(arg1);
+    EpsSetVal = arg1;
     ui->vSliderEpsSet->setValue(int(arg1));
 }
 
 void MainWindow::on_vSliderEpsSet_valueChanged(int value)
 {
-    car_control_module.set_target_pinion_ag_in_deg(double(value));
+    EpsSetVal = double(value);
     ui->sBoxEpsSet->setValue(double(value));
 }
 
+
+void MainWindow::onTimerTask()
+{
+    if(!car_control_module.is_lat_lgt_ctrl_active() && bctrlAct){
+        car_control_module.set_lat_lgt_ctrl_req(true);
+        car_control_module.set_target_gear( GearPrkgAssistReq::kTargetGearP);
+        car_control_module.set_target_pinion_ag_in_deg(car_control_module.get_current_steer_ang_in_deg());
+        car_control_module.set_target_acc_mps2(0);
+        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
+//        qDebug()<<1;
+    }
+    else if(car_control_module.is_lat_lgt_ctrl_active() && !bctrlAct){
+        car_control_module.set_lat_lgt_ctrl_req(false);
+        car_control_module.set_target_gear( GearPrkgAssistReq::kNoRequest);
+        car_control_module.set_target_pinion_ag_in_deg(car_control_module.get_current_steer_ang_in_deg());
+        car_control_module.set_target_acc_mps2(0);
+        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
+
+//        qDebug()<<2;
+    }
+    else if(!car_control_module.is_lat_lgt_ctrl_active() && !bctrlAct){
+//        qDebug()<<3;
+        car_control_module.set_lat_lgt_ctrl_req(false);
+        car_control_module.set_target_gear( GearPrkgAssistReq::kNoRequest);
+        car_control_module.set_target_pinion_ag_in_deg(car_control_module.get_current_steer_ang_in_deg());
+        car_control_module.set_target_acc_mps2(0);
+        car_control_module.set_turn_light_status(TurnLightIndicReq::kOff);
+    }
+    else if(car_control_module.is_lat_lgt_ctrl_active() && bctrlAct){
+//        qDebug()<<4;
+        car_control_module.set_target_gear( GearPrkgAssistReq::kTargetGearP);
+        car_control_module.set_target_pinion_ag_in_deg(90);
+        car_control_module.set_target_acc_mps2(0);
+        car_control_module.set_turn_light_status(TurnLightIndicReq::kRight);
+    }
+    car_control_module.sm_task_20ms();
+}
+
 void MainWindow::onTimer()
 {
     bool status;
@@ -209,6 +255,7 @@ void MainWindow::onTimer()
     ui->lineEChaissErr->setText(QString::number(tmp1[static_cast<int>(chassErr)]));
     stsMach = car_control_module.get_chassis_statemachine_sts();
     ui->lineEMachErr->setText(QString::number(tmp1[static_cast<int>(stsMach)]));
+    qDebug("chassisErr %d  state machine %d", chassErr,stsMach);
     speed = car_control_module.get_current_vehicle_spd_in_ms();
     ui->lineESpeed->setText(QString::number(speed));
     steerDeg = car_control_module.get_current_steer_ang_in_deg();

src/tool/carControlTester/mainwindow.h

@@ -3,6 +3,7 @@
 
 #include <QMainWindow>
 #include <QTimer>
+#include <QDebug>
 #include "include/car_control.h"
 
 namespace Ui {
@@ -42,11 +43,17 @@ private slots:
 
     void onTimer();
 
+    void onTimerTask();
+
 private:
     Ui::MainWindow *ui;
 
     CarControl car_control_module;
-
+    bool bctrlAct = false;
+    GearPrkgAssistReq gearSetVal= GearPrkgAssistReq::kNoRequest;
+    TurnLightIndicReq turnLightSetVal = TurnLightIndicReq::kOff;
+    double speedSetVal = 0;
+    double EpsSetVal = 0;
 };
 
 #endif // MAINWINDOW_H

src/tool/carControlTester/mainwindow.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>561</width>
-    <height>574</height>
+    <width>731</width>
+    <height>565</height>
    </rect>
   </property>
   <property name="windowTitle">