void PlayDrillCommandClass::LogSteps() const {
const Step* pStep= NULL;
int index= 1;
for (int i= 0; i < m_Steps.getUsedElems(); ++i)
{
pStep= m_Steps[i];
LOGF3("Step [", i+1, "]");
pStep->log(F(""));
}
}
void PlayDrillCommandClass::bumperOff()
{
LOGF("bumperOff");
#if USE_RF24
for (int id= RF24_BUMPER_ID_BASE; id < RF24NodeId(RF24_BUMPER_ID_BASE + MAX_BUMPER); ++ id)
{
if (g_Rf24Master.isDeviceConnected( RF24NodeId(id) ))
{
LOGF3("Off(", id, ")");
g_Rf24Master.sendOff(RF24NodeId(id));
}
}
#endif
}
void PlayDrillCommandClass::bumperOn(bool resetCounter)
{
LOGF("bumperOn");
#if USE_RF24
// TODO: improve communication with bumper
for (int id= RF24_BUMPER_ID_BASE; id < RF24NodeId(RF24_BUMPER_ID_BASE + MAX_BUMPER); ++ id)
{
if (g_Rf24Master.isDeviceConnected( RF24NodeId(id) ))
{
LOGF3("On(", id, ")");
g_Rf24Master.sendOn(RF24NodeId(id), resetCounter);
}
}
#endif
}
void KineticCountingSemaphore_Give(KineticCountingSemaphore * const sem) // SIGNAL
{
KINETIC_ASSERT(sem != NULL);
pthread_mutex_lock(&sem->mutex);
if (sem->count == 0 && sem->num_waiting > 0) {
pthread_cond_signal(&sem->available);
}
uint32_t before = sem->count++;
uint32_t after = sem->count;
uint32_t waiting = sem->num_waiting;
pthread_mutex_unlock(&sem->mutex);
LOGF3("Concurrent ops throttle -- GIVE: %u => %u (waiting=%u)", before, after, waiting);
KINETIC_ASSERT(sem->max >= after);
}
KineticStatus bus_to_kinetic_status(bus_send_status_t const status)
{
KineticStatus res = KINETIC_STATUS_INVALID;
switch(status) {
// TODO scrutinize all these mappings
case BUS_SEND_SUCCESS:
res = KINETIC_STATUS_SUCCESS;
break;
case BUS_SEND_TX_TIMEOUT_NOTIFYING_LISTENER:
case BUS_SEND_TX_TIMEOUT:
res = KINETIC_STATUS_SOCKET_TIMEOUT;
break;
case BUS_SEND_TX_FAILURE:
res = KINETIC_STATUS_SOCKET_ERROR;
break;
case BUS_SEND_RX_TIMEOUT:
res = KINETIC_STATUS_OPERATION_TIMEDOUT;
break;
case BUS_SEND_RX_FAILURE:
res = KINETIC_STATUS_SOCKET_ERROR;
break;
case BUS_SEND_BAD_RESPONSE:
res = KINETIC_STATUS_SOCKET_ERROR;
break;
case BUS_SEND_UNREGISTERED_SOCKET:
res = KINETIC_STATUS_SOCKET_ERROR;
break;
case BUS_SEND_RX_TIMEOUT_EXPECT:
res = KINETIC_STATUS_OPERATION_TIMEDOUT;
break;
case BUS_SEND_UNDEFINED:
default:
{
LOGF0("bus_to_kinetic_status: UNMATCHED %d", status);
KINETIC_ASSERT(false);
return KINETIC_STATUS_INVALID;
}
}
LOGF3("bus_to_kinetic_status: mapping status %d => %d",
status, res);
return res;
}
void KineticCountingSemaphore_Take(KineticCountingSemaphore * const sem) // WAIT
{
KINETIC_ASSERT(sem != NULL);
pthread_mutex_lock(&sem->mutex);
sem->num_waiting++;
while (sem->count == 0) {
pthread_cond_wait(&sem->available, &sem->mutex);
}
sem->num_waiting--;
uint32_t before = sem->count--;
uint32_t after = sem->count;
uint32_t waiting = sem->num_waiting;
pthread_mutex_unlock(&sem->mutex);
LOGF3("Concurrent ops throttle -- TAKE: %u => %u (waiting=%u)", before, after, waiting);
}
KineticStatus KineticSocket_Write(int socket, ByteBuffer* src)
{
LOGF3("Writing %zu bytes to socket...", src->bytesUsed);
for (unsigned int bytesSent = 0; bytesSent < src->bytesUsed;) {
int bytesRemaining = src->bytesUsed - bytesSent;
int status = write(socket, &src->array.data[bytesSent], bytesRemaining);
if (status == -1 &&
((errno == EINTR) || (errno == EAGAIN) || (errno == EWOULDBLOCK))) {
LOG2("Write interrupted. retrying...");
continue;
}
else if (status <= 0) {
LOGF0("Failed to write to socket! status=%d, errno=%d\n", status, errno);
return KINETIC_STATUS_SOCKET_ERROR;
}
else {
bytesSent += status;
LOGF2("Wrote %d bytes (%d of %zu sent)", status, bytesSent, src->bytesUsed);
}
}
LOG3("Socket write completed successfully");
return KINETIC_STATUS_SUCCESS;
}
int KineticSocket_Connect(const char* host, int port, bool nonBlocking)
{
char port_str[32];
struct addrinfo hints;
struct addrinfo* ai_result = NULL;
struct addrinfo* ai = NULL;
socket99_result result;
// Setup server address info
socket99_config cfg = {
.host = (char*)host,
.port = port,
.nonblocking = nonBlocking,
};
sprintf(port_str, "%d", port);
// Open socket
LOGF0("Connecting to %s:%d", host, port);
if (!socket99_open(&cfg, &result)) {
LOGF0("Failed to open socket connection with host: status %d, errno %d",
result.status, result.saved_errno);
return KINETIC_SOCKET_DESCRIPTOR_INVALID;
}
// Configure the socket
socket99_set_hints(&cfg, &hints);
if (getaddrinfo(cfg.host, port_str, &hints, &ai_result) != 0) {
LOGF0("Failed to get socket address info: errno %d", errno);
close(result.fd);
return KINETIC_SOCKET_DESCRIPTOR_INVALID;
}
for (ai = ai_result; ai != NULL; ai = ai->ai_next) {
int setsockopt_result;
int buffer_size = KINETIC_OBJ_SIZE;
#if defined(SO_NOSIGPIPE) && !defined(__APPLE__)
// On BSD-like systems we can set SO_NOSIGPIPE on the socket to
// prevent it from sending a PIPE signal and bringing down the whole
// application if the server closes the socket forcibly
int enable = 1;
setsockopt_result = setsockopt(result.fd,
SOL_SOCKET, SO_NOSIGPIPE,
&enable, sizeof(enable));
// Allow ENOTSOCK because it allows tests to use pipes instead of
// real sockets
if (setsockopt_result != 0 && setsockopt_result != ENOTSOCK) {
LOG0("Failed to set SO_NOSIGPIPE on socket");
continue;
}
#endif
// Increase send buffer to KINETIC_OBJ_SIZE
// Note: OS allocates 2x this value for its overhead
setsockopt_result = setsockopt(result.fd,
SOL_SOCKET, SO_SNDBUF,
&buffer_size, sizeof(buffer_size));
if (setsockopt_result == -1) {
LOG0("Error setting socket send buffer size");
continue;
}
// Increase receive buffer to KINETIC_OBJ_SIZE
// Note: OS allocates 2x this value for its overheadbuffer_size
setsockopt_result = setsockopt(result.fd,
SOL_SOCKET, SO_RCVBUF,
&buffer_size, sizeof(buffer_size));
if (setsockopt_result == -1) {
LOG0("Error setting socket receive buffer size");
continue;
}
break;
}
freeaddrinfo(ai_result);
if (ai == NULL || result.fd == KINETIC_SOCKET_DESCRIPTOR_INVALID) {
// we went through all addresses without finding one we could bind to
LOGF0("Could not connect to %s:%d", host, port);
return KINETIC_SOCKET_DESCRIPTOR_INVALID;
}
else {
LOGF1("Successfully connected to %s:%d (fd=%d)", host, port, result.fd);
return result.fd;
}
}
void KineticSocket_Close(int socket)
{
if (socket == -1) {
LOG1("Not connected so no cleanup needed");
}
else {
LOGF0("Closing socket with fd=%d", socket);
if (close(socket) == 0) {
LOG2("Socket closed successfully");
}
else {
LOGF0("Error closing socket file descriptor!"
" (fd=%d, errno=%d, desc='%s')",
socket, errno, strerror(errno));
}
}
}
KineticWaitStatus KineticSocket_WaitUntilDataAvailable(int socket, int timeout)
{
if (socket < 0) {return -1;}
struct pollfd fd = {
.fd = socket,
.events = POLLIN,
.revents = 0,
};
int res = poll(&fd, 1, timeout);
if (res > 0) {
//if (fd.revents & POLLHUP) // hung up
if (fd.revents & POLLIN)
{
return KINETIC_WAIT_STATUS_DATA_AVAILABLE;
}
else
{
return KINETIC_WAIT_STATUS_FATAL_ERROR;
}
}
else if (res == 0)
{
return KINETIC_WAIT_STATUS_TIMED_OUT;
}
else if ((errno & (EAGAIN | EINTR)) != 0)
{
return KINETIC_WAIT_STATUS_RETRYABLE_ERROR;
}
else
{
return KINETIC_WAIT_STATUS_FATAL_ERROR;
}
}
int KineticSocket_DataBytesAvailable(int socket)
{
if (socket < 0) {return -1;}
int count = -1;
ioctl(socket, FIONREAD, &count);
return count;
}
KineticStatus KineticSocket_Read(int socket, ByteBuffer* dest, size_t len)
{
LOGF2("Reading %zd bytes into buffer @ 0x%zX from fd=%d",
len, (size_t)dest->array.data, socket);
KineticStatus status = KINETIC_STATUS_INVALID;
// Read "up to" the allocated number of bytes into dest buffer
size_t bytesToReadIntoBuffer = len;
if (dest->array.len < len) {
bytesToReadIntoBuffer = dest->array.len;
}
while (dest->bytesUsed < bytesToReadIntoBuffer) {
int opStatus;
fd_set readSet;
struct timeval timeout;
// Time out after 5 seconds
timeout.tv_sec = 5;
timeout.tv_usec = 0;
FD_ZERO(&readSet);
FD_SET(socket, &readSet);
opStatus = select(socket + 1, &readSet, NULL, NULL, &timeout);
if (opStatus < 0) { // Error occurred
LOGF0("Failed waiting to read from socket!"
" status=%d, errno=%d, desc='%s'",
opStatus, errno, strerror(errno));
return KINETIC_STATUS_SOCKET_ERROR;
}
else if (opStatus == 0) { // Timeout occurred
LOG0("Timed out waiting for socket data to arrive!");
return KINETIC_STATUS_SOCKET_TIMEOUT;
}
else if (opStatus > 0) { // Data available to read
// The socket is ready for reading
opStatus = read(socket,
&dest->array.data[dest->bytesUsed],
dest->array.len - dest->bytesUsed);
// Retry if no data yet...
if (opStatus == -1 &&
((errno == EINTR) ||
(errno == EAGAIN) ||
(errno == EWOULDBLOCK)
)) {
continue;
}
else if (opStatus <= 0) {
LOGF0("Failed to read from socket!"
" status=%d, errno=%d, desc='%s'",
opStatus, errno, strerror(errno));
return KINETIC_STATUS_SOCKET_ERROR;
}
else {
dest->bytesUsed += opStatus;
LOGF3("Received %d bytes (%zd of %zd)",
opStatus, dest->bytesUsed, len);
}
}
}
// Flush any remaining data, in case of a truncated read w/short dest buffer
if (dest->bytesUsed < len) {
bool abortFlush = false;
uint8_t* discardedBytes = malloc(len - dest->bytesUsed);
if (discardedBytes == NULL) {
LOG0("Failed allocating a socket read discard buffer!");
abortFlush = true;
status = KINETIC_STATUS_MEMORY_ERROR;
}
while (!abortFlush && dest->bytesUsed < len) {
int opStatus;
fd_set readSet;
struct timeval timeout;
size_t remainingLen = len - dest->bytesUsed;
// Time out after 5 seconds
timeout.tv_sec = 5;
timeout.tv_usec = 0;
FD_ZERO(&readSet);
FD_SET(socket, &readSet);
opStatus = select(socket + 1, &readSet, NULL, NULL, &timeout);
if (opStatus < 0) { // Error occurred
LOGF0("Failure trying to flush read socket data!"
" status=%d, errno=%d, desc='%s'",
status, errno, strerror(errno));
abortFlush = true;
status = KINETIC_STATUS_SOCKET_ERROR;
continue;
}
else if (opStatus == 0) { // Timeout occurred
LOG0("Timed out waiting to flush socket data!");
abortFlush = true;
status = KINETIC_STATUS_SOCKET_TIMEOUT;
continue;
}
else if (opStatus > 0) { // Data available to read
// The socket is ready for reading
opStatus = read(socket, discardedBytes, remainingLen);
// Retry if no data yet...
if (opStatus == -1 &&
((errno == EINTR) ||
(errno == EAGAIN) ||
(errno == EWOULDBLOCK)
)) {
continue;
}
else if (opStatus <= 0) {
LOGF0("Failed to read from socket while flushing!"
" status=%d, errno=%d, desc='%s'",
opStatus, errno, strerror(errno));
abortFlush = true;
status = KINETIC_STATUS_SOCKET_ERROR;
}
else {
dest->bytesUsed += opStatus;
LOGF3("Flushed %d bytes from socket read pipe (%zd of %zd)",
opStatus, dest->bytesUsed, len);
}
}
}
// Free up dynamically allocated memory before returning
if (discardedBytes != NULL) {
free(discardedBytes);
}
// Report any error that occurred during socket flush
if (abortFlush) {
LOG0("Socket read pipe flush aborted!");
assert(status == KINETIC_STATUS_SUCCESS);
return status;
}
// Report truncation of data for any variable length byte arrays
LOGF1("Socket read buffer was truncated due to buffer overrun!"
" received=%zu, copied=%zu",
len, dest->array.len);
return KINETIC_STATUS_BUFFER_OVERRUN;
}
LOGF3("Received %zd of %zd bytes requested", dest->bytesUsed, len);
return KINETIC_STATUS_SUCCESS;
}
KineticStatus KineticSocket_ReadProtobuf(int socket, KineticPDU* pdu)
{
size_t bytesToRead = pdu->header.protobufLength;
LOGF2("Reading %zd bytes of protobuf", bytesToRead);
uint8_t* packed = (uint8_t*)malloc(bytesToRead);
if (packed == NULL) {
LOG0("Failed allocating memory for protocol buffer");
return KINETIC_STATUS_MEMORY_ERROR;
}
ByteBuffer recvBuffer = ByteBuffer_Create(packed, bytesToRead, 0);
KineticStatus status = KineticSocket_Read(socket, &recvBuffer, bytesToRead);
if (status != KINETIC_STATUS_SUCCESS) {
LOG0("Protobuf read failed!");
free(packed);
return status;
}
else {
pdu->proto = KineticProto_Message__unpack(
NULL, recvBuffer.bytesUsed, recvBuffer.array.data);
}
free(packed);
if (pdu->proto == NULL) {
pdu->protobufDynamicallyExtracted = false;
LOG0("Error unpacking incoming Kinetic protobuf message!");
return KINETIC_STATUS_DATA_ERROR;
}
else {
pdu->protobufDynamicallyExtracted = true;
LOG3("Protobuf unpacked successfully!");
return KINETIC_STATUS_SUCCESS;
}
}
void PlayDrillCommandClass::execute()
{
if (IsExecuting())
{
LOGF("SKIPPING PlayDrill command");
return;
}
parse();
LogSteps();
StopCommandClass::reset();
PauseCommandClass::reset();
resetInterruptCounter();
LOGF("=========== Initializing Bumper ==========");
bumperOn(true/*resetCounter*/);
LOGF("=========== Play Drill ==========");
s_pExecutedDrill = this;
bool bContinue= true;
char pref[10];
while (true)
{
// handle incoming commands
executeCommands();
if (StopCommandClass::IsStopped())
{
m_execData.error= false;
m_execData.feedData.result=MotorHelperClass::FR_NONE;
break;
}
else if (PauseCommandClass::IsPaused())
{
static unsigned long s_LedSetTime= 0;
static bool s_yellowLed= false;
unsigned long currentTime= millis();
if (currentTime - s_LedSetTime >= 250) {
if (s_yellowLed) {
Helper::setLedColor(0,0,255); // turn the RGB LED blue
}
else {
Helper::setLedColor(255,215,0); // turn the RGB LED yellow
}
s_LedSetTime= currentTime;
s_yellowLed= !s_yellowLed;
}
g_motorHelper.stopMotorsIfIdle();
}
else
{
Step* pStep= NULL;
for (int stepIndex= 0; stepIndex < m_Steps.getUsedElems(); ++stepIndex)
{
if (m_execData.nextStepIndex > 0) {
// goto
stepIndex= m_execData.nextStepIndex - 1; // 1-based
m_execData.nextStepIndex= -1;
}
pStep= m_Steps[stepIndex];
LOGF3("============ Executing Step [", stepIndex+1, F("] ============"));
bContinue= pStep->execute(m_execData);
// handle incoming commands
executeCommands();
if (StopCommandClass::IsStopped())
{
m_execData.error= false;
m_execData.feedData.result=MotorHelperClass::FR_NONE;
bContinue= false;
}
else if (PauseCommandClass::IsPaused())
{
g_motorHelper.stopMotorsIfIdle();
break;
}
if (!bContinue)
break;
g_TaskQueue.executeReadyTasks();
}
if (!bContinue)
break;
}
} // EOF while(true)
g_motorHelper.stopMotors();
s_pExecutedDrill = NULL;
bumperOff();
g_serialCom.fireEvent(EVENT_EXECUTE_STOPPED);
// in case of error no sound
if (!m_execData.error)
{
Helper::playSound(Helper::SND_FINISHED);
}
}
