public void consumeMessage(MachineStateMessage message) { if (message.quaternions.main_boom_orientation != null) { var quat = message.quaternions.main_boom_orientation; this.mainBoomAngle = xAngleFromRawQuaternion(quat); } if (message.quaternions.digging_arm_orientation != null) { var quat = message.quaternions.digging_arm_orientation; this.diggingArmAngle = xAngleFromRawQuaternion(quat); } if (message.quaternions.bucket_orientation != null) { var quat = message.quaternions.bucket_orientation; this.bucketAngle = xAngleFromRawQuaternion(quat); } }
public void consumeMessage(MachineStateMessage message) { // Quaternions & derived angles if (message.quaternions.main_boom_orientation != null) { var quat = message.quaternions.main_boom_orientation; this.mainBoomQuaternion = new Quaternion(quat.x, quat.y, quat.z, quat.w); this.mainBoomQuaternionAngle = getEulerXAngle(quat.w, quat.x, quat.y, quat.z); } if (message.quaternions.digging_arm_orientation != null) { var quat = message.quaternions.digging_arm_orientation; this.diggingArmQuaternion = new Quaternion(quat.x, quat.y, quat.z, quat.w); this.diggingArmQuaternionAngle = getEulerXAngle(quat.w, quat.x, quat.y, quat.z); } if (message.quaternions.bucket_orientation != null) { var quat = message.quaternions.bucket_orientation; this.bucketQuaternion = new Quaternion(quat.x, quat.y, quat.z, quat.w); this.bucketQuaternionAngle = getEulerXAngle(quat.w, quat.x, quat.y, quat.z); } if (message.quaternions.frame_orientation != null) { var quat = message.quaternions.frame_orientation; this.frameQuaternion = new Quaternion(quat.x, quat.y, quat.z, quat.w); } // Euler angles if (message.angles.main_boom != null) { this.mainBoomEulerAngle = message.angles.main_boom; } if (message.angles.digging_arm != null) { this.diggingArmEulerAngle = message.angles.digging_arm; } if (message.angles.bucket != null) { this.bucketEulerAngle = message.angles.bucket; } if (message.angles.heading != null) { this.headingEulerAngle = message.angles.heading; } if (message.angles.frame_pitch != null) { this.framePitchEulerAngle = message.angles.frame_pitch; } if (message.angles.frame_roll != null) { this.frameRollEulerAngle = message.angles.frame_roll; } // Limit warnings if (message.limitWarnings.left != null) { this.limitLeft = message.limitWarnings.left; } if (message.limitWarnings.right != null) { this.limitRight = message.limitWarnings.right; } if (message.limitWarnings.upper != null) { this.limitUpper = message.limitWarnings.upper; } if (message.limitWarnings.lower != null) { this.limitLower = message.limitWarnings.lower; } if (message.limitWarnings.forward != null) { this.limitForward = message.limitWarnings.forward; } if (message.limitWarnings.property != null) { this.limitProperty = message.limitWarnings.property; } if (message.limitWarnings.overload != null) { this.limitOverload = message.limitWarnings.overload; } // Zero level if (message.zeroLevel.height_from_zero != null) { this.zeroHeightFrom = message.zeroLevel.height_from_zero; } if (message.zeroLevel.distance_to_zero != null) { this.zeroDistanceTo = message.zeroLevel.distance_to_zero; } if (message.zeroLevel.height_to_slope_from_zero != null) { this.zeroHeightToSlope = message.zeroLevel.height_to_slope_from_zero; } }