private void ChargerPoseMessage(IChargerPoseEvent eventResponse)
{
// Using averaged value of charger pose.
if (_poseLock)
{
return;
}
_poseLock = true;
try
{
if (_previousChargerPose.Count > 5)
{
_previousChargerPose.Dequeue();
}
var latestPose = new ChargerPosition()
{
X = eventResponse.Pose.HomogeneousMatrix[12],
Y = eventResponse.Pose.HomogeneousMatrix[13],
Z = eventResponse.Pose.HomogeneousMatrix[14],
//EulerYaw = (float)(180.0 * Math.Atan2(eventResponse.Pose.HomogeneousMatrix[6], eventResponse.Pose.HomogeneousMatrix[2]) / Math.PI)
EulerYaw = (float)(-180.0 * Math.Asin(eventResponse.Pose.HomogeneousMatrix[2]) / Math.PI)
};
_previousChargerPose.Enqueue(latestPose);
var meanPose = new ChargerPosition();
foreach (var pose in _previousChargerPose)
{
meanPose.X += pose.X;
meanPose.Y += pose.Y;
meanPose.Z += pose.Z;
meanPose.EulerYaw += pose.EulerYaw;
}
meanPose.X /= _previousChargerPose.Count;
meanPose.Y /= _previousChargerPose.Count;
meanPose.Z /= _previousChargerPose.Count;
meanPose.EulerYaw /= _previousChargerPose.Count;
//_skillHelper.LogMessage($"{latestPose.X} {meanPose.X}");
// Apply offset so that we can treat charger position as relative to the center
// of the Occipital sensor.
meanPose.X += CENTER_OFFSET;
//_skillHelper.LogMessage($"Charger pose: X {meanPose.X} Y {meanPose.Y} Z {meanPose.Z} EY {meanPose.EulerYaw}");
_chargerPose = meanPose;
}
finally
{
_poseLock = false;
}
}
private void ChargerPoseMessage(IChargerPoseEvent eventResponse)
{
float[] matrix = eventResponse.Pose.HomogeneousMatrix;
//_skillHelper.LogMessage($"Charger pose: [{matrix[0]}, {matrix[1]}, {matrix[2]}, {matrix[3]}, {matrix[4]}, {matrix[5]}, {matrix[6]}, {matrix[7]}, {matrix[8]}, {matrix[9]}, {matrix[10]}, {matrix[11]}, {matrix[12]}, {matrix[13]}, {matrix[14]}, {matrix[15]}]");
// Occipital coordinates have left-right axis as positive to the right.
// Occipital origin is 0.035mm to the right of Misty.
// So we add 0.035 to the received value to get a left-right value for Misty's body.
// The _charger_y_offset is an optionally manually set offset that may help some Misty's dock more reliably.
matrix[12] += (0.035f + _charger_y_offset);
_chargerPose = NavigationHelper.ConvertPose(matrix);
//_skillHelper.LogMessage("Charger pose: " + _chargerPose.ToString());
}
