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); }