Browse Source

change detection_chassis for shenlan.

yuchuli 4 months ago
parent
commit
78cc8b7187
1 changed files with 10 additions and 0 deletions
  1. 10 0
      src/detection/detection_chassis/decodechassis.cpp

+ 10 - 0
src/detection/detection_chassis/decodechassis.cpp

@@ -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());