|
@@ -17,6 +17,7 @@
|
|
#include "chassis.pb.h"
|
|
#include "chassis.pb.h"
|
|
|
|
|
|
#include "torquebrake.h"
|
|
#include "torquebrake.h"
|
|
|
|
+#include "remotectrl.pb.h"
|
|
|
|
|
|
#include <thread>
|
|
#include <thread>
|
|
|
|
|
|
@@ -27,10 +28,12 @@
|
|
static void * gpacansend;
|
|
static void * gpacansend;
|
|
static void * gpadecition;
|
|
static void * gpadecition;
|
|
static void * gpachassis;
|
|
static void * gpachassis;
|
|
|
|
+static void * gparemote;
|
|
|
|
|
|
static std::string gstrmemdecition;
|
|
static std::string gstrmemdecition;
|
|
static std::string gstrmemcansend;
|
|
static std::string gstrmemcansend;
|
|
static std::string gstrmemchassis;
|
|
static std::string gstrmemchassis;
|
|
|
|
+std::string gstrmemremote; //Remote Ctrl
|
|
static bool gbSendRun = true;
|
|
static bool gbSendRun = true;
|
|
|
|
|
|
static bool gbChassisEPS = false;
|
|
static bool gbChassisEPS = false;
|
|
@@ -38,6 +41,14 @@ static bool gbChassisEPS = false;
|
|
static iv::brain::decition gdecition_def;
|
|
static iv::brain::decition gdecition_def;
|
|
static iv::brain::decition gdecition;
|
|
static iv::brain::decition gdecition;
|
|
|
|
|
|
|
|
+bool gbAllowRemote = false; //Default, Not Allow Remote
|
|
|
|
+
|
|
|
|
+qint64 gLastRemoteTime = 0;
|
|
|
|
+
|
|
|
|
+bool gbAutoDriving = true; //if true, Auto Driving, false, remote controll
|
|
|
|
+
|
|
|
|
+iv::brain::decition gdecition_remote;
|
|
|
|
+
|
|
static QTime gTime;
|
|
static QTime gTime;
|
|
static int gnLastSendTime = 0;
|
|
static int gnLastSendTime = 0;
|
|
static int gnLastRecvDecTime = -1000;
|
|
static int gnLastRecvDecTime = -1000;
|
|
@@ -553,6 +564,51 @@ void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+void ListenRemotectrl(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
|
|
|
|
+{
|
|
|
|
+ (void)index; (void)dt; (void)strmemname;
|
|
|
|
+ iv::brain::decition xdecition;
|
|
|
|
+
|
|
|
|
+ iv::remotectrl xrc;
|
|
|
|
+
|
|
|
|
+ if(!xrc.ParseFromArray(strdata,nSize))
|
|
|
|
+ {
|
|
|
|
+ std::cout<<"ListenRemotectrl parse error."<<std::endl;
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ if(xrc.ntype() == iv::remotectrl_CtrlType_AUTO)
|
|
|
|
+ {
|
|
|
|
+ gbAutoDriving = true;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ gbAutoDriving = false;
|
|
|
|
+ xdecition.set_accelerator(xrc.acc());
|
|
|
|
+ xdecition.set_brake(xrc.brake());
|
|
|
|
+ xdecition.set_wheelangle(xrc.wheel());
|
|
|
|
+
|
|
|
|
+ if(xrc.acc()>0.0)xdecition.set_torque(xrc.acc() * 100);
|
|
|
|
+
|
|
|
|
+ xdecition.set_angle_mode(1);
|
|
|
|
+ xdecition.set_angle_active(1);
|
|
|
|
+ xdecition.set_acc_active(1);
|
|
|
|
+ xdecition.set_brake_active(1);
|
|
|
|
+ // xdecition.set_brake_type(1);
|
|
|
|
+ xdecition.set_auto_mode(3);
|
|
|
|
+
|
|
|
|
+ gMutex.lock();
|
|
|
|
+ gdecition_remote.CopyFrom(xdecition);
|
|
|
|
+ gMutex.unlock();
|
|
|
|
+ gLastRemoteTime = QDateTime::currentMSecsSinceEpoch();
|
|
|
|
+
|
|
|
|
+ gnDecitionNum = gnDecitionNumMax;
|
|
|
|
+ gbChassisEPS = false;
|
|
|
|
+
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+
|
|
void ExecSend()
|
|
void ExecSend()
|
|
{
|
|
{
|
|
static int nCount = 0;
|
|
static int nCount = 0;
|
|
@@ -640,6 +696,37 @@ void sendthread()
|
|
// Activate();
|
|
// Activate();
|
|
while(gbSendRun)
|
|
while(gbSendRun)
|
|
{
|
|
{
|
|
|
|
+
|
|
|
|
+ if(gbAutoDriving)
|
|
|
|
+ {
|
|
|
|
+ if(gnDecitionNum <= 0)
|
|
|
|
+ {
|
|
|
|
+ gMutex.lock();
|
|
|
|
+ xdecition.CopyFrom(gdecition_def);
|
|
|
|
+ gMutex.unlock();
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ xdecition.CopyFrom(gdecition);
|
|
|
|
+ gnDecitionNum--;
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ if((QDateTime::currentMSecsSinceEpoch() - gLastRemoteTime)> 1000)
|
|
|
|
+ {
|
|
|
|
+ gbAutoDriving = true;
|
|
|
|
+ continue;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ gMutex.lock();
|
|
|
|
+ xdecition.CopyFrom(gdecition_remote);
|
|
|
|
+ gMutex.unlock();
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+
|
|
if(gnDecitionNum <= 0)
|
|
if(gnDecitionNum <= 0)
|
|
{
|
|
{
|
|
if(nstate == 1)
|
|
if(nstate == 1)
|
|
@@ -656,9 +743,18 @@ void sendthread()
|
|
Activate();
|
|
Activate();
|
|
nstate = 1;
|
|
nstate = 1;
|
|
}
|
|
}
|
|
- gMutex.lock();
|
|
|
|
- xdecition.CopyFrom(gdecition);
|
|
|
|
- gMutex.unlock();
|
|
|
|
|
|
+ if(gbAutoDriving)
|
|
|
|
+ {
|
|
|
|
+ gMutex.lock();
|
|
|
|
+ xdecition.CopyFrom(gdecition);
|
|
|
|
+ gMutex.unlock();
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ gMutex.lock();
|
|
|
|
+ xdecition.CopyFrom(gdecition_remote);
|
|
|
|
+ gMutex.unlock();
|
|
|
|
+ }
|
|
gnDecitionNum--;
|
|
gnDecitionNum--;
|
|
}
|
|
}
|
|
|
|
|
|
@@ -832,6 +928,8 @@ int main(int argc, char *argv[])
|
|
gstrmemcansend = xp.GetParam("cansend","cansend0");
|
|
gstrmemcansend = xp.GetParam("cansend","cansend0");
|
|
gstrmemdecition = xp.GetParam("dection","deciton");
|
|
gstrmemdecition = xp.GetParam("dection","deciton");
|
|
gstrmemchassis = xp.GetParam("chassismsgname","chassis");
|
|
gstrmemchassis = xp.GetParam("chassismsgname","chassis");
|
|
|
|
+ gstrmemremote = xp.GetParam("remotectrl","remotectrl");
|
|
|
|
+
|
|
kp = xp.GetParam("kp",0.1);
|
|
kp = xp.GetParam("kp",0.1);
|
|
ki = xp.GetParam("ki",0.0);
|
|
ki = xp.GetParam("ki",0.0);
|
|
kd = xp.GetParam("kd",0.0);
|
|
kd = xp.GetParam("kd",0.0);
|
|
@@ -839,10 +937,21 @@ int main(int argc, char *argv[])
|
|
gbUsePID = xp.GetParam("UsePID",false);
|
|
gbUsePID = xp.GetParam("UsePID",false);
|
|
gbPrintPIDOut = xp.GetParam("PrintPIDOut",false);
|
|
gbPrintPIDOut = xp.GetParam("PrintPIDOut",false);
|
|
|
|
|
|
|
|
+ std::string strremote = xp.GetParam("allowremote","true");
|
|
|
|
+ if(strremote == "true")
|
|
|
|
+ {
|
|
|
|
+ gbAllowRemote = true;
|
|
|
|
+ }
|
|
|
|
+
|
|
gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,1);
|
|
gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,1);
|
|
gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
|
|
gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
|
|
gpachassis = iv::modulecomm::RegisterRecv(gstrmemchassis.data(),UpdateChassis);
|
|
gpachassis = iv::modulecomm::RegisterRecv(gstrmemchassis.data(),UpdateChassis);
|
|
|
|
|
|
|
|
+ if(gbAllowRemote)
|
|
|
|
+ {
|
|
|
|
+ gparemote = iv::modulecomm::RegisterRecv(gstrmemremote.data(),ListenRemotectrl);
|
|
|
|
+ }
|
|
|
|
+
|
|
#ifdef TORQUEBRAKETEST
|
|
#ifdef TORQUEBRAKETEST
|
|
EnableTorqueBrakeTest();
|
|
EnableTorqueBrakeTest();
|
|
#endif
|
|
#endif
|