private const double CHR_QUATERNION_YAW_TRUE_NORTH_OFFSET = 14.0; // manual correction to the Quaternion yaw, to align it with true North reading.
/// <summary>
/// Handle CH Robotics UM6 Orientation Sensor Notification - Euler
/// </summary>
/// <param name="notification">Euler notification</param>
private void ChrEulerHandler(chrum6orientationsensor.EulerNotification notification)
{
//Tracer.Trace(string.Format("the UM6 Sensor reported Euler: {0} PHI={1} THETA={2} PSI={3}", notification.Body.LastUpdate, notification.Body.phi, notification.Body.theta, notification.Body.psi));
if (USE_DIRECTION_UM6_EULER)
{
try
{
// mag heading is reported as a short within +-16200 range
double magHeading = Direction.to180(notification.Body.psi * CHR_EULER_YAW_FACTOR + CHR_EULER_YAW_TRUE_NORTHOFFSET); // convert to degrees and ensure that it is within +- 180 degrees and related to True North.
// we still use proxibrick.DirectionDataDssSerializable because it is the direction data in the State:
proxibrick.DirectionDataDssSerializable newDir = new proxibrick.DirectionDataDssSerializable()
{
TimeStamp = DateTime.Now, heading = magHeading
};
setCurrentDirection(newDir);
}
catch (Exception exc)
{
Tracer.Trace("ChrEulerHandler() - " + exc);
}
}
}
private const double CHR_EILER_YAW_FACTOR = 180.0d / 16200.0d; // mag heading is reported as a ahort within +-16200 range. This is to convert it to degrees.
/// <summary>
/// Handle CH Robotics UM6 Orientation Sensor Notification - Euler
/// </summary>
/// <param name="notification">Euler notification</param>
private void ChrEulerHandler(chrum6orientationsensor.EulerNotification notification)
{
Tracer.Trace(string.Format("the UM6 Sensor reported Euler: {0} PHI={1} THETA={2} PSI={3}", notification.Body.LastUpdate, notification.Body.phi, notification.Body.theta, notification.Body.psi));
try
{
// mag heading is reported as a ahort within +-16200 range
double magHeading = Direction.to180(notification.Body.psi * CHR_EILER_YAW_FACTOR); // convert to degrees and ensure that it is within +- 180 degrees.
DirectionData newDir = new DirectionData()
{
TimeStamp = DateTime.Now.Ticks, heading = magHeading
};
DirectionData curDir = MostRecentDirection;
if (curDir == null || Math.Abs(newDir.heading - curDir.heading) > 0.9d) // only react on significant changes in direction
{
MostRecentDirection = newDir; //.Clone()
if (_mapperVicinity.robotDirection.bearing.HasValue)
{
_currentGoalBearing = (double)_mapperVicinity.robotDirection.bearing;
}
if (_mapperVicinity.turnState != null && !_mapperVicinity.turnState.hasFinished)
{
_mapperVicinity.turnState.directionCurrent = new Direction()
{
heading = newDir.heading, TimeStamp = newDir.TimeStamp
};
}
// update mapper with Direction data:
_mapperVicinity.robotDirection = new Direction()
{
TimeStamp = newDir.TimeStamp, heading = newDir.heading, bearing = _currentGoalBearing
};
// update mapper with Odometry data:
//updateMapperWithOdometryData();
// update GUI (compass control):
setGuiCurrentDirection(newDir);
//if (!_doUnitTest)
//{
// Decide(SensorEventSource.Compass, null);
//}
}
}
catch (Exception exc)
{
Tracer.Trace("ChrEulerHandler() - " + exc);
}
}
