|
@@ -496,6 +496,30 @@ int ProcShenLanCANFDChassis(void *pa, iv::can::canmsg *pmsg)
|
|
|
std::cout<<" 1CC not 64 bytes. 1CC bytes: "<<praw->len()<<std::endl;
|
|
|
}
|
|
|
}
|
|
|
+ if(praw->id() == 0x18a)
|
|
|
+ {
|
|
|
+ unsigned char byte[64];
|
|
|
+ double ang;
|
|
|
+ if(praw->len() == 64)
|
|
|
+ {
|
|
|
+ memcpy(byte,praw->data().data(),64);
|
|
|
+ unsigned int value;
|
|
|
+ value = byte[0]&0xFF;
|
|
|
+ value = (value<<8) + static_cast<unsigned int >( byte[1]);
|
|
|
+ if(value<32767)
|
|
|
+ ang = static_cast<double>(value) * 0.1;
|
|
|
+ else
|
|
|
+ ang=static_cast<double>(value) * 0.1-65536*0.1;
|
|
|
+
|
|
|
+ xchassis.set_angle_feedback(static_cast<float>(ang));
|
|
|
+ ShareChassis(pa,&xchassis);
|
|
|
+ std::cout<<"ang: "<<xchassis.angle_feedback()<<std::endl;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::cout<<" 1CC not 64 bytes. 1CC bytes: "<<praw->len()<<std::endl;
|
|
|
+ }
|
|
|
+ }
|
|
|
|
|
|
}
|
|
|
|