static void *
listen_thread(void *data)
{
long length;
DNS_query *query;
while (running) {
if (queueIsEmpty(&queries_unused)) {
if ((query = malloc(sizeof (*query))) == NULL) {
syslog(LOG_ERR, "out of memory");
sleep(2);
continue;
}
} else {
query = queueDequeue(&queries_unused)->data;
}
do {
length = socketReadFrom(service, (unsigned char *) &query->packet.header, UDP_PACKET_SIZE, &query->client);
if (0 < debug && 0 < length) {
char *from = socketAddressToString(&query->client);
syslog(LOG_DEBUG, "from=%s length=%ld", from, length);
free(from);
}
} while (length <= 0);
query->node.data = query;
query->packet.length = length;
queueEnqueue(&queries_waiting, &query->node);
}
return NULL;
}
/**
* \brief Enqueue data to be sent over SPI
*
* This function gets data ready to be sent over SPI.
* Note that the actual transfer is done in sgSerialReceive with
* the current scheme.
* \param sg_serial[in] Data to be sent. Contains a Sansgrid Payload
* \param size[in] Size of data. Not currently used.
*/
int8_t sgSerialSend(SansgridSerial *sg_serial, uint32_t size) {
// Send size bytes of serialdata
SansgridSerial *sg_serial_cp = NULL;
sg_serial_cp = (SansgridSerial*)malloc(sizeof(SansgridSerial));
memcpy(sg_serial_cp, sg_serial, sizeof(SansgridSerial));
queueEnqueue(tx_buffer, sg_serial_cp);
sem_post(&wait_on_slave);
return 0;
}
void *
produce(void *arg)
{
list *l = (list *)arg;
for(int i = 0; i < NUM_JOBS; i++) {
char buf[32];
sprintf(buf, "msg.%d", i);
queueEnqueue(l, strdup(buf));
}
return 0;
}
static void *
answer_thread(void *data)
{
DNS_query *query;
while (running) {
query = queueDequeue(&queries_waiting)->data;
find_answer(query);
socketWriteTo(service, (unsigned char *) &query->packet.header, query->packet.length, &query->client);
queueEnqueue(&queries_unused, &query->node);
}
return NULL;
}
int main()
{
int i = 0;
//int *queuedata;
void * dataout;
//void * dummyqueuezero = &dataqueue[1];
//void * dummyqueueone = &dataqueue[1];
queueInit(&queueTest, QUEUESIZE-1);
while( queueEnqueue(&queueTest, &dataqueue[i]) == noError)
{
i++;
}
while( queueDequeue(&queueTest, &dataout) == noError)
{
//queuedata = dataout;
//blaat = *(int *) dataout;
printf("dequeued element %d\n", *((int *) dataout));
}
return 0;
}
// to_goalがtrueのとき、ゴールまでの経路を探索する
// to_goalがfalseのとき、未探索の地点までの経路を探索する
enum action_t agent_explore(void) {
queueInit();
for(int y = 0; y < size; ++y) {
for(int x = 0; x < size; ++x) {
if(check_goal(y, x)) {
d[y][x] = 0;
inQueue[y][x] = true;
queueEnqueue(y, x);
}
else {
d[y][x] = UCHAR_MAX - 1;
inQueue[y][x] = false;
}
}
}
while(queueEmpty() == false) {
int vy, vx;
queueDequeue(&vy, &vx);
inQueue[vy][vx] = false;
for (int k = 0; k < 4; k++) {
if (wall[vy][vx][k] == false) {
int ny = vy + dy[k];
int nx = vx + dx[k];
if(d[ny][nx] > d[vy][vx] + 1) {
d[ny][nx] = d[vy][vx] + 1;
if(inQueue[ny][nx] == false) {
inQueue[ny][nx] = true;
queueEnqueue(ny, nx);
}
}
}
}
}
if(wall[curY][curX][(dir + 0)%4] == false &&
d[curY + dy[(dir + 0)%4]][curX + dx[(dir + 0)%4]] + 1 == d[curY][curX]) {
// GoForward
curY += dy[dir];
curX += dx[dir];
return GO_FORWARD;
}
if(wall[curY][curX][(dir + 1)%4] == false &&
d[curY + dy[(dir + 1)%4]][curX + dx[(dir + 1)%4]] + 1 == d[curY][curX]) {
// TurnLeft
dir = (dir + 1) % 4;
return TURN_LEFT;
}
if(wall[curY][curX][(dir + 3)%4] == false &&
d[curY + dy[(dir + 3)%4]][curX + dx[(dir + 3)%4]] + 1 == d[curY][curX]) {
// TurnRight
dir = (dir + 3) % 4;
return TURN_RIGHT;
}
if(wall[curY][curX][(dir + 2)%4] == false &&
d[curY + dy[(dir + 2)%4]][curX + dx[(dir + 2)%4]] + 1 == d[curY][curX]) {
// TurnLeft
dir = (dir + 1) % 4;
return TURN_LEFT;
}
return NO_OPERATION;
}
/**
* \brief Receive data full-duplex over SPI
*
* If there is data waiting to be sent, that is transferred.
* Data is also received over serial wire and then converted
* into a SansgridSerial structure.
* \param sg_serial[out] Where received data is placed.
* \param size[out] Size of the returned payload, zero if not packet
* received
*/
int8_t sgSerialReceive(SansgridSerial **sg_serial, uint32_t *size) {
// Transmit/Receive serialdata, size of packet stored in size
// Code from
// https://git.drogon.net/?p=wiringPi;a=blob;f=examples/isr.c;h=2bef54af13a60b95ad87fbfc67d2961722eb016e;hb=HEAD
int i;
SansgridSerial *sg_serial_out = NULL;
char buffer[sizeof(SansgridSerial)+1];
int fd;
// Set when we're sending data
int sending = 0;
int exit_code = 0;
*size = sizeof(SansgridSerial);
syslog(LOG_INFO, "Waiting for data over serial");
if (!sem_initd) {
sem_init(&wait_on_slave, 0, 0);
sem_initd = 1;
}
memset(buffer, 0x0, sizeof(buffer));
buffer[0] = SG_SERIAL_CTRL_NO_DATA;
// receive data
// first make sure semaphore is zeroed out
while (sem_trywait(&wait_on_slave) == 0);
// If there are no pending requests, wait until we get one
if (digitalRead((SLAVE_INT_PIN)) && !queueSize(tx_buffer))
sem_wait(&wait_on_slave);
// Transfer about to occur
if (queueTryDequeue(tx_buffer, (void**)&sg_serial_out) >= 0) {
// Data needs to be sent
sg_serial_out->control = SG_SERIAL_CTRL_VALID_DATA;
*sg_serial = sg_serial_out;
memcpy(buffer, sg_serial_out, *size);
sending = 1;
} else {
// No data needs to be sent
*sg_serial = (SansgridSerial*)malloc(sizeof(SansgridSerial));
(*sg_serial)->control = SG_SERIAL_CTRL_NO_DATA;
sending = 0;
}
pthread_mutex_lock(&transfer_lock);
// LOCKED
// Make sure only one thread can access SPI at a time
syslog(LOG_INFO, "Sending data over Serial");
// this needs to be atomic, since fd comes from static global g_fd
if ((fd = spiOpen()) == -1) {
return -1;
}
// (debug) Print our data to send
printf("Sending:\n");
spiPrintSgSerial(buffer, *size);
spiTransfer(buffer, *size);
close(fd);
// (debug) Print what we got
printf("Receiving:\n");
spiPrintSgSerial(buffer, *size);
memcpy(*sg_serial, buffer, *size);
if (buffer[0] == SG_SERIAL_CTRL_VALID_DATA) {
queueEnqueue(dispatch, sgSerialCP(*sg_serial));
*size = sizeof(SansgridSerial);
// Radio can't get spammed. Throttle sending
exit_code = 0;
} else {
// No need for sg_serial anymore
//free(sg_serial);
//*size = 0;
if (buffer[0] != SG_SERIAL_CTRL_VALID_DATA
&& sending == 0) {
// Didn't send valid data, and didn't get valid data
syslog(LOG_WARNING, "Bad data on SPI");
exit_code = -1;
}
}
for (i=0; i<3; i++) {
sleep(1);
if (!digitalRead(SLAVE_INT_PIN))
break;
}
pthread_mutex_unlock(&transfer_lock);
// UNLOCKED
return exit_code;
}
