|
@@ -509,6 +509,16 @@ int ProcShenLanCANFDChassis(void *pa, iv::can::canmsg *pmsg)
|
|
|
value = (value<<8) + static_cast<unsigned int >( byte[13]);
|
|
|
vehspeed = static_cast<double>(value) * 0.05625;
|
|
|
|
|
|
+ value = byte[2]&0x1F;
|
|
|
+ value = (value<<8) + static_cast<unsigned int >( byte[3]);
|
|
|
+ double frlvehspeed = static_cast<double>(value) * 0.05625;
|
|
|
+
|
|
|
+ value = byte[0]&0x1F;
|
|
|
+ value = (value<<8) + static_cast<unsigned int >( byte[1]);
|
|
|
+ double frrvehspeed = static_cast<double>(value) * 0.05625;
|
|
|
+
|
|
|
+ vehspeed = (frlvehspeed + frrvehspeed)/2.0;
|
|
|
+
|
|
|
xchassis.set_vel(static_cast<float>(vehspeed));
|
|
|
xchassis.set_time(std::chrono::system_clock::now().time_since_epoch().count());
|
|
|
|