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