123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210 |
- #include <QCoreApplication>
- #include <QTime>
- #include "control/control_status.h"
- #include "control/controller.h"
- #include "xmlparam.h"
- #include "modulecomm.h"
- #include "ivversion.h"
- #include "ivbacktrace.h"
- #include "canmsg.pb.h"
- #include "decition.pb.h"
- #include <thread>
- #include <QMutex>
- void * gpacansend;
- void * gpadecition;
- std::string gstrmemdecition;
- std::string gstrmemcansend;
- bool gbSendRun = true;
- iv::brain::decition gdecition_def;
- iv::brain::decition gdecition;
- QMutex gMutexDecision;
- QTime gTime;
- int gnLastSendTime = 0;
- int gnLastRecvDecTime = -1000;
- int gnDecitionNum = 0; //when is zero,send default;
- const int gnDecitionNumMax = 100;
- int gnIndex = 0;
- boost::shared_ptr<iv::control::Controller> gcontroller; //实际车辆控制器
- void executeDecition(const iv::brain::decition decition)
- {
- // std::cout<<"acc is "<<decition.accelerator()<<" ang is "<<decition.wheelangle()<<std::endl;
- // std::cout<<"brake is "<<decition.brake()<<std::endl;
- gcontroller->inialize();
- gcontroller->control_wheel(decition.wheelangle());
- gcontroller->control_accelerate(decition.accelerator());
- // gcontroller->control_braking(decition.brake()); //vv7不用brake
- gcontroller->control_accmode(true);
- gcontroller->control_turnmode(true);
- gcontroller->control_drivemode(1);
- #if 0
- optional bool air_ac = 37;
- optional bool air_circle = 38;
- optional bool air_auto = 39;
- optional bool air_off = 40;
- optional uint32 window_fl = 41;
- optional uint32 window_fr = 42;
- optional uint32 window_rl = 43;
- optional uint32 window_rr = 44;
- #endif
- if(decition.has_air_ac())
- gcontroller->control_air_AC_ctrl(decition.air_ac());
- if(decition.has_air_auto())
- gcontroller->control_air_auto_ctrl(decition.air_auto());
- if(decition.has_air_circle())
- gcontroller->control_air_inloop_ctrl(decition.air_circle());
- if(decition.has_air_off())
- gcontroller->control_air_close(decition.air_off());
- if(decition.has_window_fl())
- gcontroller->control_air_lf_win(decition.window_fl());
- if(decition.has_window_fr())
- gcontroller->control_air_rf_win(decition.window_fr());
- if(decition.has_window_rl())
- gcontroller->control_air_lr_win(decition.window_rl());
- if(decition.has_window_rr())
- gcontroller->control_air_rr_win(decition.window_rr());
- bool leftlampSig = false;
- bool rightlampSig = false;
- if(decition.has_leftlamp())
- leftlampSig = decition.leftlamp();
- if(decition.has_rightlamp())
- rightlampSig = decition.rightlamp();
- gcontroller->control_turnsignals(leftlampSig,rightlampSig);
- // if(decition.has_engine())
- // gcontroller->control_engine(decition.engine());
- if(decition.has_speak())
- gcontroller->control_speaker(decition.speak());
- }
- void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- iv::brain::decition xdecition;
- if(!xdecition.ParseFromArray(strdata,nSize))
- {
- std::cout<<"ListenDecition parse error."<<std::endl;
- return;
- }
- gMutexDecision.lock();
- gdecition.CopyFrom(xdecition);
- gMutexDecision.unlock();
- gnDecitionNum = gnDecitionNumMax;
- }
- void ExecSend()
- {
- iv::can::canmsg xmsg;
- iv::can::canraw xraw;
- xraw.set_id(ServiceControlStatus.command_ID);
- xraw.set_data(ServiceControlStatus.command.byte,8);
- // qDebug("%02x %02x",ServiceControlStatus.command.byte[2],ServiceControlStatus.command.byte[3]);
- xraw.set_bext(false);
- xraw.set_bremote(false);
- xraw.set_len(8);
- iv::can::canraw * pxraw = xmsg.add_rawmsg();
- pxraw->CopyFrom(xraw);
- xmsg.set_channel(0);
- xmsg.set_index(gnIndex);
- gnIndex++;
- xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
- int ndatasize = xmsg.ByteSize();
- char * strser = new char[ndatasize];
- std::shared_ptr<char> pstrser;
- pstrser.reset(strser);
- if(xmsg.SerializePartialToArray(strser,ndatasize))
- {
- iv::modulecomm::ModuleSendMsg(gpacansend,strser,ndatasize);
- }
- else
- {
- std::cout<<"MainWindow::onTimer serialize error."<<std::endl;
- }
- }
- void sendthread()
- {
- iv::brain::decition xdecition;
- while(gbSendRun)
- {
- if(gnDecitionNum <= 0)
- {
- xdecition.CopyFrom(gdecition_def);
- }
- else
- {
- gMutexDecision.lock();
- xdecition.CopyFrom(gdecition);
- gMutexDecision.unlock();
- gnDecitionNum--;
- }
- executeDecition(xdecition);
- ExecSend();
- std::this_thread::sleep_for(std::chrono::milliseconds(10));
- }
- }
- int main(int argc, char *argv[])
- {
- RegisterIVBackTrace();
- showversion("controller_tju_vv7");
- QCoreApplication a(argc, argv);
- QString strpath = QCoreApplication::applicationDirPath();
- if(argc < 2)
- strpath = strpath + "/controller_vv7.xml";
- else
- strpath = argv[1];
- std::cout<<strpath.toStdString()<<std::endl;
- gdecition_def.set_accelerator(-0.5);
- gdecition_def.set_brake(1.0);
- gdecition_def.set_torque(0);
- gdecition_def.set_rightlamp(true);
- // gdecition_def.set_doublespark(true);
- gTime.start();
- gcontroller = boost::shared_ptr<iv::control::Controller>(new iv::control::Controller());
- iv::xmlparam::Xmlparam xp(strpath.toStdString());
- gstrmemcansend = xp.GetParam("cansend","cansend0");
- gstrmemdecition = xp.GetParam("dection","deciton");
- const char * strx = gstrmemcansend.data();
- gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,3);
- gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
- std::thread xthread(sendthread);
- return a.exec();
- }
|