void SideConfidenceProvider::updateBallConfidences(SideConfidence& sideConfidence)
{
// Check, if a mirrorcle has occured in the last frame
if(sideConfidence.mirror)
{
confidenceBuffer.clear();
averageBallConfidence = UNKNOWN;
sideConfidence.mirror = false;
}
// Special handling for certain game states:
if(theGameInfo.state != STATE_PLAYING || theRobotInfo.penalty != PENALTY_NONE)
{
confidenceBuffer.clear();
averageBallConfidence = UNKNOWN;
}
// Save current local ball observation:
if((theBallModel.timeWhenLastSeen == theFrameInfo.time && theFrameInfo.time != 0) &&
(theFieldDimensions.isInsideField(theRobotPose * theBallModel.estimate.position)) &&
(theBallModel.estimate.velocity.norm() <= maxBallVelocity) &&
(theRobotPose * theBallModel.estimate.position).norm() > centerBanZoneRadius)
{
lastBallObservation = theBallModel.estimate.position;
timeOfLastBallObservation = theFrameInfo.time;
}
// Odometry update for buffered perception
else if(theFrameInfo.getTimeSince(timeOfLastBallObservation) < ballBufferingInterval)
{
lastBallObservation = theOdometer.odometryOffset.inverse() * lastBallObservation;
}
// Add current confidence to buffer:
if((theFrameInfo.getTimeSince(timeOfLastBallObservation) < ballBufferingInterval) &&
(theLocalizationTeamBall.isValid) &&
(theLocalizationTeamBall.position.norm() > centerBanZoneRadius) &&
(theLocalizationTeamBall.lastObservation > timeOfLastTeamBallObservation))
{
SideConfidenceMeasurement scm;
scm.ballConfidence = computeCurrentBallConfidence();
scm.timeStamp = timeOfLastBallObservation;
confidenceBuffer.push_front(scm);
timeOfLastBallObservation = 0; //For next confidence computation, a "fresh" observation is needed
timeOfLastTeamBallObservation = theLocalizationTeamBall.lastObservation;
}
else
{
return;
}
// Check current buffer content for possible mirror situation
if(!confidenceBuffer.full())
{
return;
}
size_t mirrorCount(0);
size_t okCount(0);
for(const SideConfidenceMeasurement& confidence : confidenceBuffer)
{
switch(confidence.ballConfidence)
{
case MIRROR: ++mirrorCount; break;
case OK: ++okCount; break;
}
}
size_t unknownCount = confidenceBuffer.size() - mirrorCount - okCount;
if((okCount == 0) && (mirrorCount > unknownCount))
averageBallConfidence = MIRROR;
else if((okCount > unknownCount) && (mirrorCount == 0))
averageBallConfidence = OK;
else
averageBallConfidence = UNKNOWN;
}
uint8_t StatusClient::read()
{
// Read the message type
int32_t message_type;
size_t bytes = _client.read((uint8_t *) &message_type, sizeof(message_type));
if (bytes < 1) {
Serial1.println("Error reading message type");
return STATE_READ_ERROR;
}
// Decode message type as big-endian
message_type = __REV(message_type);
switch (message_type) {
case MESSAGE_UPDATE:
break;
case MESSAGE_ERROR:
Serial1.println("Upstream error");
return STATE_UPSTREAM_ERROR;
default:
Serial1.print("Unknown message type ");
Serial1.print(message_type, HEX);
Serial1.println();
return STATE_READ_ERROR;
}
// Read the message contents
bytes = _client.read((uint8_t *) &_buffer, sizeof(NagiosStatus));
if (bytes < sizeof(_buffer)) {
Serial1.println("Error reading message contents");
return STATE_READ_ERROR;
}
// Switch to little-endian values
uint32_t *ptr = (uint32_t *) &_buffer;
uint32_t *max_ptr = ptr + sizeof(NagiosStatus);
for (; ptr < max_ptr; ++ptr)
*ptr = __REV(*ptr);
// Update the notification flags
if (_buffer.critical_serial > _status.critical_serial)
_notifications |= NOTIFICATION_CRITICAL;
if (_buffer.warning_serial > _status.warning_serial)
_notifications |= NOTIFICATION_WARNING;
if (_buffer.ok_serial > _status.ok_serial)
_notifications |= NOTIFICATION_OK;
// Store the new status
memcpy(&_status, &_buffer, sizeof(NagiosStatus));
// Debug logging
Serial1.print("New status: ");
Serial1.print(criticalCount());
Serial1.print(" ");
Serial1.print(warningCount());
Serial1.print(" ");
Serial1.print(okCount());
Serial1.print(", notifications: 0x");
Serial1.print(notifications(), HEX);
Serial1.println();
return STATE_OK;
}
