int main() {
CircularBuffer cb; /* */
ElemType elem = {0};
int testBufferSize = 10; /* arbitrary size */
cbInit(&cb, testBufferSize);
/* Fill buffer with test elements 3 times */
for (elem.value = 0; elem.value < 3 * testBufferSize; ++ elem.value)
{
if(! cbIsFull(&cb)) { /* if not full */
cbWrite(&cb, &elem);
}
}
/* Remove and print all elements */
while (!cbIsEmpty(&cb)) {
cbRead(&cb, &elem);
printf("%d\n", elem.value);
}
cbFree(&cb);
return(0);
}
void * roomba_thread_func(void * ptr) {
while (run) {
if (!cbIsEmpty(&commands)) {
buff_t dat;
cbRead(&commands, &dat);
switch (dat) {
case goleft:
roomba_direct_drive(-1000, 1000);
break;
case goright:
roomba_direct_drive(1000, -1000);
break;
case goforward:
roomba_direct_drive(1000, 1000);
break;
case goback:
roomba_direct_drive(-1000, -1000);
break;
case goquit:
run = 0;
break;
case gowait:
cbRead(&commands, &dat);
delay(dat);
roomba_direct_drive(0, 0);
break;
}
}
}
}
int main() {
CircularBuffer cb;
int *value;
value = (int *)0; // cast must be
printf("[ ] %d %0x\n", (int)value,(int)value); // cast must be
cbInit(&cb);
int i;
for(i=0;i<20;++i)
{
value = (int *)i;
printf("[T] %d %0x\n", (int)value,(int)value); // cast must be
if(! cbIsFull(&cb)) { /* if not full */
cbWrite(&cb, (int)value);
}
}
while(!cbIsEmpty(&cb))
{
cbRead(&cb, (int*)&value);
printf("[P] %d %0x\n", (int)value,(int)value); // cast must be
}
return(0);
}
int read_log(char *page, char **start, off_t off, int count, int *eof, void *data_unused) {
char *buf;
char elem = {0};
buf = page;
spin_lock(&cb_spinlock);
while (!cbIsEmpty(&cb)) {
cbRead(&cb, &elem);
buf += sprintf(buf, &elem);
}
spin_unlock(&cb_spinlock);
*eof = 1;
return buf - page;
}
__attribute__((section(".rodata"))) int main( void ) {
CircularBuffer buff;
cbInit(&buff, 10);
cbWrite(&buff, 3);
cbWrite(&buff, 23);
cbWrite(&buff, 43);
buff_t dat;
cbRead(&buff, &dat);
printf("%d\n", dat);
cbRead(&buff, &dat);
printf("%d\n", dat);
cbRead(&buff, &dat);
printf("%d\n", dat);
printf("%d\n", cbIsEmpty(&buff));
return 0;
}
int main(int argc, char **argv) {
CircularBuffer cb;
ElemType elem = {0};
int testBufferSize = 50; /* arbitrary size */
cbInit(&cb, testBufferSize);
/* Fill buffer with test elements 3 times */
for (elem.value = 0; elem.value < 3 * testBufferSize; ++ elem.value){
cbWrite(&cb, &elem);
}
/* Remove and print all elements */
while (!cbIsEmpty(&cb)) {
cbRead(&cb, &elem);
printf("%d\n", elem.value);
}
cbFree(&cb);
return 0;
}
static void *data_logging_lisa(void *arg){
/*-------------------------START OF THIRD THREAD: LISA TO PC LOGGING------------------------*/
ElemType cb_elem = {0};
LOG_err_handler(open_data_lisa_log(),write_log_error_ptr);
while(1){
if (!cbIsEmpty(cb_read_lisa)) {
reading_flag_lisa=1;
cbRead(cb_read_lisa, &cb_elem);
LOG_err_handler(write_data_lisa_log(cb_elem.value,cb_elem.value[1]),write_log_error_ptr);
usleep(100);
}else{
reading_flag_lisa=0;
usleep(1000);
}
}
LOG_err_handler(close_data_lisa_log(),write_log_error_ptr);
return NULL;
/*-------------------------END OF THIRD THREAD: LISA TO PC LOGGING------------------------*/
}
static void *data_logging_groundstation(void *arg){
/*-------------------------START OF FOURTH THREAD: GROUNDSTATION TO LISA LOGGING------------------------*/
ElemType cb_elem = {0};
LOG_err_handler(open_data_groundstation_log(),write_log_error_ptr);
while(1){
if (!cbIsEmpty(cb_read_ground)) {
reading_flag_ground=1;
cbRead(cb_read_ground, &cb_elem);
LOG_err_handler(write_data_groundstation_log(cb_elem.value,cb_elem.value[1]),write_log_error_ptr);
usleep(100);
}else{
reading_flag_ground=0;
usleep(1000);
}
}
LOG_err_handler(close_data_groundstation_log(),write_log_error_ptr);
return NULL;
/*-------------------------END OF FOURTH THREAD: GROUNDSTATION TO LISA LOGGING------------------------*/
}
/**
* Send an 'S' command (read sensor data) to the station and get the packet back.
*/
BOOL send_S() {
char cmd[CMD_LENGTH] = "S\r";
int bytes = write(fd, cmd, strlen(cmd));
if (bytes == -1) {
perror("kestrel2ivy: Error writing command bytes");
fprintf(stderr,"You may need to reset the connection.\n");
}
bzero(tempBuf, BUF_LENGTH);
// repeat a few times within time limit, just return if done
int result = 0;
int j = 0;
for(j = 0; j < 3; j++) {
// delay to allow kestrel to give some data
usleep(250000); // 0.25sec
// read in one chunk, skip reads for the period if there is a read error
if (result >= 0) {
result = read(fd, tempBuf, cbAvailable(&cb));
if (result < 0) {
//perror("kestrel2ivy: Got a read error on the port");
} else {
int i = 0;
for (i = 0; i < result; i++) {
ElemType elemBuf;
elemBuf.value = tempBuf[i];
cbWrite(&cb, &elemBuf);
}
}
}
while (!cbIsEmpty(&cb)) {
// First get the (approximate) start indicator
// Then copy in the rest of the data
// Stop copying in data at the (approximate) end indicator
// Return if it looks like the last packet in the buffer, otherwise keep processing buffer
ElemType tempElem;
cbRead(&cb,&tempElem);
char tempChar = tempElem.value;
switch (msgState) {
case UNINIT:
if (tempChar == '>') {
bzero(packet, PACKET_LENGTH); // clear the packet after getting values
packetidx = 0;
packet[packetidx++] = tempChar;
msgState = GOT_START;
}
break;
case GOT_START:
if (tempChar == '>') {
if (packetidx < MIN_PACKET_LENGTH) {
// False alarm, not actually the start
bzero(packet, PACKET_LENGTH);
packetidx = 0;
packet[packetidx++] = tempChar;
} else {
msgState = UNINIT;
// if we think we have another full packet available, don't return
if (cbAvailable(&cb) > (BUF_LENGTH - (MIN_PACKET_LENGTH + 80)))
return TRUE;
}
}
else {
packet[packetidx++] = tempChar;
}
break;
default:
fprintf(stderr, "kestrel2ivy: Unknown parser state!\n");
}
}
}
return FALSE;
}
//#include <termios.h>
void *com_handler(void * peer){
struct peer* pinf = (struct peer*) peer;
struct peer p;
p.socket = pinf->socket; // creating a local copy of the peer object
p.ip = pinf->ip;
struct peer * pp = nw_get(p);
printf("Peer connected (ip %lu socket %i).\n", pinf->ip, pinf->socket);
struct msg newpeermsg = {.msgtype = OPCODE_NEWPEER,.from = p.ip,};
handle_msg(newpeermsg, 0);
char recv_msg[MAXRECVSIZE];
int read_size;
struct timeval ctime, ptime, ttime;
gettimeofday(&ttime,0);
/* Set non-blocking state */
int flags;
if (-1 == (flags = fcntl(pinf->socket, F_GETFL, 0))){
flags = 0;
}
fcntl(p.socket, F_SETFL, flags | O_NONBLOCK);
char * string = calloc(MAXRECVSIZE,1);
char * cjsonstr = calloc(MAXRECVSIZE,1);
/** Normal operating mode **/
while(1){
/** Maintain connection by passing and receiving I'm alive **/
gettimeofday(&ctime, 0);
if((ctime.tv_usec - ptime.tv_usec) >= IMALIVE_UPPERIOD || (ctime.tv_usec<ptime.tv_usec)){
struct msg packet = { .msgtype = OPCODE_IMALIVE,};
char * cout = pack(packet);
send(p.socket, cout, strlen(cout), 0);
gettimeofday(&ptime, 0);
}
/** Receive data **/
read_size = recv(pinf->socket, recv_msg, MAXRECVSIZE, 0);
if(read_size <= 0){ // ERROR/RECOVERY mode
if(read_size == 0){
printf("Peer disconnected (ip %lu socket %i).\n", pinf->ip, pinf->socket);
break;
}
else if((read_size==-1) ){
if( (errno != EAGAIN) && (errno != EWOULDBLOCK) ) {
printf("Peer disconnected (ip %lu socket %i).\n", pinf->ip, pinf->socket);
break;
}
}
else{
0;// Did not receive anything, but no error
}
}
else { // Received something
strcpy(string, recv_msg);
string[read_size]='\0';
int start_i = 0;
int end_i = cjsonendindex(string, start_i);
/* Separate packets arriving together */
while(end_i<(strlen(string)-1)){
strncpy(cjsonstr, (string+start_i), (end_i-start_i+1));
cjsonstr[end_i-start_i+1] = '\0';
struct msg packetin = unpack(cjsonstr);
packetin.from = p.ip;
handle_msg(packetin, &ttime);
start_i = end_i+1;
end_i = cjsonendindex(string, start_i);
}
if(end_i!=-1){
strncpy(cjsonstr, (string+start_i), (end_i-start_i+1));
cjsonstr[end_i-start_i+1] = '\0';
struct msg packetin = unpack(cjsonstr);
packetin.from = p.ip;
handle_msg(packetin, &ttime);
}
}
// tcflush(p.socket, TCIOFLUSH);
/** Check for timeout **/
gettimeofday(&ctime, 0);
if((ctime.tv_sec-ttime.tv_sec) > IMALIVE_TIMEOUT){
printf("Timeout on I'm alive, socket: %i\n", pinf->socket);
break;
}
/** Send data **/
struct msg elem;
while(!cbIsEmpty(&pp->bufout)){ // Send data from buffer
cbRead(&pp->bufout, &elem);
if (elem.msgtype!=OPCODE_CORRUPT){
char * cout = pack(elem);
send(pinf->socket, cout, strlen(cout), 0);
free(cout);
}
}
}
/** Recovery mode: **/
free(cjsonstr);
free(string);
nw_rm(p);
nw_setevent(DISCONNECTION);
struct msg recovermsg = { .from = p.ip, .to = highest_ip()};
if(recovermsg.to == root->p.ip){ // I have the highest ip on the network and should take over orders
recovermsg.msgtype = OPCODE_PEERLOSTTAKEOVER;
}
else{
recovermsg.msgtype = OPCODE_PEERLOST;
}
handle_msg(recovermsg, 0);
close(p.socket);
pthread_exit(0);
}
int sendtoallpeer(struct msg package){
struct peer p = {
.ip = TOALLIP
};
return sendtopeer(package, p);
}
int sendtopeer(struct msg package, struct peer p){
struct nw_node * iter;
iter = root;
iter = iter->next;
while(iter!=0){
if((iter->p.ip) == p.ip || p.ip == TOALLIP){
struct peer * pp = nw_get(iter->p);
cbWrite(&pp->bufout, &package);
}
iter = iter->next;
}
return 1;
}
int connect_to_peer(in_addr_t peer_ip){
int peer_socket = socket(AF_INET, SOCK_STREAM, 0);
struct sockaddr_in peer;
peer.sin_family = AF_INET;
peer.sin_addr.s_addr = peer_ip;
peer.sin_port = htons(LISTEN_PORT); // Connect to listen port.
if (connect(peer_socket, (struct sockaddr *)&peer , sizeof(peer)) < 0){
perror("err: connect. Connecting to peer failed\n");
return -1;
}
struct peer p = peer_object(peer_socket, peer_ip);
nw_add(p);
assign_com_thread(p);
return 1;
}
void assign_com_thread(struct peer p){
// We have a connection! Now assign a communication handler thread.
struct peer *pinf = malloc(sizeof(struct peer));
pinf->socket = p.socket;
pinf->ip = p.ip;
if( pthread_create( &pinf->com_thread , NULL , com_handler , pinf)<0){
perror("err: pthread_create\n");
exit(1);
}
}
/* \!brief Listen for TCP connections and accept incoming.
*
*/
void *listen_tcp(){
int opt = TRUE;
struct sockaddr_in listen_addr;
listen_addr.sin_family = AF_INET;
listen_addr.sin_port = htons(LISTEN_PORT);
listen_addr.sin_addr.s_addr = htons(INADDR_ANY); // Binding local IP-address
listen_socket = socket(AF_INET, SOCK_STREAM, 0);
if (setsockopt(listen_socket, SOL_SOCKET, SO_REUSEADDR, (char*)&opt, sizeof(opt)) == -1){
perror("err: setsockopt\n");
exit(1);
}
if ( bind(listen_socket, (struct sockaddr *)&listen_addr, sizeof listen_addr) == -1){
perror("err: bind\n");
exit(1);
}
struct sockaddr_in peer;
int new_peer_socket, structsize;
structsize = sizeof(struct sockaddr_in);
while(1){
listen(listen_socket, LISTEN_BACKLOG);
if ( (new_peer_socket = accept(listen_socket, (struct sockaddr *)&peer, (socklen_t*)&structsize)) == -1 ){
perror("err: accept\n");
exit(1);
}
struct peer newpeer = peer_object(new_peer_socket, peer.sin_addr.s_addr);
if(!nw_find(newpeer)){
nw_add(newpeer);
assign_com_thread(newpeer);
nw_setevent(CONNECTION);
}
else{ // Already in connected list
close(new_peer_socket);
}
}
}
