|
|
@@ -1,6 +1,7 @@
|
|
|
|
|
|
#include "decodechassis.h"
|
|
|
#include <iostream>
|
|
|
+#include <chrono>
|
|
|
#include "ivlog.h"
|
|
|
#include "ivfault.h"
|
|
|
|
|
|
@@ -18,7 +19,7 @@ void PrintCANRaw(const iv::can::canraw * praw )
|
|
|
char strout[512];
|
|
|
int ndatalen = praw->len();
|
|
|
int i;
|
|
|
- snprintf(strout,512,"id: %02X data: ",praw->id());
|
|
|
+ snprintf(strout,512,"%ld id: %02X data: ",std::chrono::system_clock::now().time_since_epoch().count()/1000000, praw->id());
|
|
|
char * pdata = new char[8];
|
|
|
if(ndatalen>0)pdata = new char[ndatalen];
|
|
|
memcpy(pdata,praw->data().data(),ndatalen);
|
|
|
@@ -562,6 +563,7 @@ int ProcHunterChassis(void *pa, iv::can::canmsg *pmsg)
|
|
|
float fbatt = 0.0;
|
|
|
float fmileage_left = 0.0;
|
|
|
float fmileage_right = 0.0;
|
|
|
+ double fwheelangle_feedback = 0.0;
|
|
|
for(i=0;i<pmsg->rawmsg_size();i++)
|
|
|
{
|
|
|
|
|
|
@@ -579,7 +581,17 @@ int ProcHunterChassis(void *pa, iv::can::canmsg *pmsg)
|
|
|
memcpy(&svalue,vswap,2);
|
|
|
double vehspeed = svalue;
|
|
|
vehspeed = vehspeed * 0.001 *3.6;
|
|
|
+
|
|
|
+ memcpy(&vdata,praw->data().data() + 6,2);
|
|
|
+ vswap[0] = vdata[1];
|
|
|
+ vswap[1] = vdata[0];
|
|
|
+ memcpy(&svalue,vswap,2);
|
|
|
+ fwheelangle_feedback = svalue;
|
|
|
+ fwheelangle_feedback = fwheelangle_feedback * 0.001;
|
|
|
+
|
|
|
+
|
|
|
xchassis.set_vel(static_cast<float>(vehspeed));
|
|
|
+ xchassis.set_angle_feedback(fwheelangle_feedback);
|
|
|
bHave0x221 = true;
|
|
|
}
|
|
|
if(praw->id() == 0x361)
|
|
|
@@ -654,7 +666,7 @@ int ProcHunterChassis(void *pa, iv::can::canmsg *pmsg)
|
|
|
bHave0x361 = false;
|
|
|
ShareChassis(pa,&xchassis);
|
|
|
|
|
|
- std::cout<<"veh: "<<xchassis.vel()<<" soc: "<<fsoc<<" soh:"<<fsoh<< std::endl;
|
|
|
+ std::cout<<"veh: "<<xchassis.vel()<<" wheel: "<<fwheelangle_feedback<<" soc: "<<fsoc<<" soh:"<<fsoh<< std::endl;
|
|
|
std::cout<<"bat v:"<<fbatv<<" i: "<<fbati<<" Temp:"<<fbatt<<" mil left:"<<fmileage_left<<" right: "<<fmileage_right<<std::endl;
|
|
|
}
|
|
|
|
