bool ArTCMCompassRobot::packetHandler(ArRobotPacket *packet)
{
if (packet->getID() != 0xC0)
return false;
myHeading = ArMath::fixAngle(packet->bufToByte2() / 10.0);
myPitch = ArMath::fixAngle(packet->bufToByte2() / 10.0);
myRoll = ArMath::fixAngle(packet->bufToByte2() / 10.0);
myXMag = packet->bufToByte2() / 100.0;
myYMag = packet->bufToByte2() / 100.0;
myZMag = packet->bufToByte2() / 100.0;
myTemperature = packet->bufToByte2() / 10.0;
myError = packet->bufToByte2();
myCalibrationH = packet->bufToByte();
myCalibrationV = packet->bufToByte();
myCalibrationM = packet->bufToByte2() / 100.0;
myHaveHeading =
myHavePitch =
myHaveRoll =
myHaveXMag =
myHaveYMag =
myHaveZMag =
myHaveTemperature =
myHaveCalibrationH =
myHaveCalibrationV =
myHaveCalibrationM = true;
incrementPacketCount();
invokeHeadingDataCallbacks(myHeading);
return true;
}
void ArTCMCompassDirect::handleHCHDM(ArNMEAParser::Message m)
{
myHeading = ArMath::fixAngle(atof((*m.message)[1].c_str()));
#ifdef DEBUG_ARTCMCOMPASSDIRECT
printf("XXX ArTCMCompassDirect: recieved HCHDM message with compass heading %f.\n", myHeading);
#endif
myHaveHeading = true;
incrementPacketCount();
invokeHeadingDataCallbacks(myHeading);
}
