void sendAllMapData()
{
for(uint8_t i = 0; i < 16; ++i)
{
for(uint8_t j = 0; j < 16; ++j)
{
if(glob_map[j][i] != UNKNOWN)
{
cbWrite(&glob_mapDataToSend, i);
cbWrite(&glob_mapDataToSend, j);
}
}
}
}
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 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);
}
/* Write an element, overwriting oldest element if buffer is full. App can
choose to avoid the overwrite by checking cbIsFull(). */
void cbWriteBlock(CircularBuffer *cb, short *elem, int length)
{
int i;
for(i = 0; i < length; i++)
cbWrite(cb, elem[i]);
}
void RSI_KEYBOARD(){
unsigned char key = inb(0x60);
unsigned char mask = 0b10000000;
unsigned char make = key & mask;
char c = '0';
if(make == 0) {
mask = 0b01111111;
c = char_map[key & mask];
if(!(c>=0x20 && c<=0x7E))
c = 'C';
cbWrite(&keybuffer, &c);
//MIRAR
if (!(cbIsFull(&keybuffer) && list_empty(&keyboardqueue))) {
if(!list_empty(&keyboardqueue)) {
struct list_head * to_unblock;
struct task_struct * pcb_to_unlock;
to_unblock = list_first(&keyboardqueue);
pcb_to_unlock = list_head_to_task_struct(to_unblock);
if ((cbIsFull(&keybuffer) || pcb_to_unlock->characters_left <= cbCount(&keybuffer))) {
list_del(to_unblock);
list_add_tail(to_unblock, &readyqueue);
}
}
}
}
else
c = ' ';
printc_xy(0x3f, 0x15, c);
}
uint8_t Api_add_cmd_to_cb(void* api_ptr) {
struct ApiHandlerVars* handler = global_api_handler(0);
ElemType elem;
elem.value = api_ptr;
cbWrite(&(handler->tx_buffer), &elem);
return 1;
}
bool WriteFileContents(const wxString& filename, const wxString& contents)
{
wxFileName fname(Manager::Get()->GetMacrosManager()->ReplaceMacros(filename));
NormalizePath(fname, wxEmptyString);
if (!SecurityAllows(_T("CreateFile"), fname.GetFullPath()))
return false;
wxFile f(fname.GetFullPath(), wxFile::write);
return cbWrite(f, contents);
}
__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;
}
void printp(const char *fmt, ...) {
int count = 0;
int i;
va_list args;
spin_lock(&cb_spinlock);
va_start(args, fmt);
count += vsnprintf(printproc_buf, 1024, fmt, args);
for (i = 0; i<count; i++) {
cbWrite(&cb, &printproc_buf[i]);
}
va_end(args);
spin_unlock(&cb_spinlock);
}
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;
}
void * stdio_thread_func(void * ptr) {
char command [10];
int delaytime;
while (run) {
printf("> ");
delaytime = 0;
scanf("%s %d", command, &delaytime);
if (strcmp("l", command) == 0) {
cbWrite(&commands, goleft);
printf("Go left\n");
}
else if (strcmp("r", command) == 0) {
cbWrite(&commands, goright);
printf("Go right\n");
}
else if (strcmp("f", command) == 0) {
cbWrite(&commands, goforward);
printf("Go forward\n");
}
else if (strcmp("b", command) == 0) {
cbWrite(&commands, goback);
printf("Go backward\n");
}
else if (strcmp("q", command) == 0) {
cbWrite(&commands, goquit);
}
if (delaytime > 0) {
cbWrite(&commands, gowait);
cbWrite(&commands, delaytime);
}
printf("\n");
}
}
void U2puts(const char* msg) {
if (uart2BlockMs > 0) return;
while (*msg != '\0')
cbWrite(&cbUart2Tx, *msg++);
}
/**
* 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);
}
}
}
int main(int argc, char *argv[]){
write_uart_error_ptr = &write_uart_error; //initialize the function pointer to write error
write_udp_error_ptr = &write_udp_error;
write_decode_error_ptr = &write_decode_error;
write_log_error_ptr = &write_log_error;
//parse arguments
if(argc == 4){
//first argument is always name of program or empty string
connection.server_ip=argv[1];
connection.port_number_lisa_to_pc=atoi(argv[2]);
connection.port_number_pc_to_lisa=atoi(argv[3]);
}else{
printf("wrong parameters: server ip - send port number - receive port number\n");
exit(EXIT_FAILURE);
}
//init log (mount sd card if necessary)
int err = init_log();
LOG_err_handler(err,write_log_error_ptr);
if(err != LOG_ERR_NONE){
exit(EXIT_FAILURE); //mounting SD card failed
}
enable_ptp();
#if LOGGING > 0
//init circular data log buffers
cbInit(cb_read_lisa, CBSIZE);
cbInit(cb_write_lisa, CBSIZE);
cbInit(cb_read_ground, CBSIZE);
cbInit(cb_write_ground, CBSIZE);
#endif
err = serial_port_setup();
UART_err_handler(err,write_uart_error_ptr);
if(err != UART_ERR_NONE){
exit(EXIT_FAILURE);
}
//thread variables
pthread_t thread_lisa_to_pc,thread_data_logging_lisa,thread_data_logging_ground;
//create a second thread which executes lisa_to_pc
if(pthread_create(&thread_lisa_to_pc, NULL, lisa_to_pc,NULL)) {
error_write(FILENAME,"error creating lisa thread");
exit(EXIT_FAILURE);
}
#if LOGGING > 0
//create a third thread which executes data_logging_lisa
if(pthread_create(&thread_data_logging_lisa, NULL, data_logging_lisa,NULL)) {
error_write(FILENAME,"error creating lisa logging thread");
exit(EXIT_FAILURE);
}
//create a fourth thread which executes data_logging_groundstation
if(pthread_create(&thread_data_logging_ground, NULL, data_logging_groundstation,NULL)) {
error_write(FILENAME,"error creating groundstation logging thread");
exit(EXIT_FAILURE);
}
#endif
/*-------------------------START OF FIRST THREAD: PC TO LISA------------------------*/
static UDP udp_server;
uint8_t input_stream[OUTPUT_BUFFER];
ElemType cb_elem = {0};
//init the data decode pointers
init_decoding();
UDP_err_handler(openUDPServerSocket(&udp_server,connection.port_number_pc_to_lisa,UDP_SOCKET_TIMEOUT),write_udp_error_ptr);
while(1){
//1. retreive UDP data form PC from ethernet port.
err=receiveUDPServerData(&udp_server,(void *)&input_stream,sizeof(input_stream)); //blocking !!!
UDP_err_handler(err,write_udp_error_ptr);
if(err==UDP_ERR_NONE){
#if LOGGING > 0
if(!cbIsFull(cb_write_ground)){
memcpy (&cb_elem.value, &input_stream, sizeof(input_stream));
cbWrite(cb_write_ground, &cb_elem);
}else{
if(reading_flag_ground==0){
switch_cb_ground_pointers();
}else{
printf("GROUND WRITE WAS NOT READY \n");
}
}
#endif
int new_length = strip_timestamp(input_stream); //lisa expects a package without a timestamp
UART_err_handler(serial_port_write(input_stream,new_length),write_uart_error_ptr);
}
}
UART_err_handler(serial_port_close(),write_uart_error_ptr);
UDP_err_handler(closeUDPServerSocket(&udp_server),write_udp_error_ptr);
/*------------------------END OF FIRST THREAD------------------------*/
//wait for the second thread to finish
if(pthread_join(thread_lisa_to_pc, NULL)) {
error_write(FILENAME,"error joining thread_lisa_to_pc");
}
#if LOGGING > 0
//wait for the third thread to finish
if(pthread_join(thread_data_logging_lisa, NULL)) {
error_write(FILENAME,"error joining thread_data_logging_lisa");
}
//wait for the fourth thread to finish
if(pthread_join(thread_data_logging_ground, NULL)) {
error_write(FILENAME,"error joining thread_data_logging_ground");
}
//free circular buffers
cbFree(cb_read_lisa);
cbFree(cb_write_lisa);
cbFree(cb_read_ground);
cbFree(cb_write_ground);
#endif
return 0;
}
/************************************
* FUNCTIONS
* **********************************/
static void *lisa_to_pc(void *arg){
/*-------------------------START OF SECOND THREAD: LISA TO PC------------------------*/
static UDP udp_client;
int message_length;
ElemType cb_elem = {0};
uint8_t input_buffer[INPUT_BUFFER_SIZE];
UDP_err_handler(openUDPClientSocket(&udp_client,connection.server_ip,connection.port_number_lisa_to_pc,UDP_SOCKET_TIMEOUT),write_udp_error_ptr);
while(1)
{
message_length = serial_input_get_lisa_data(input_buffer); //blocking !!!
if(message_length > 0){
//to test latency from lisa pull pin high when airspeed package arrives
/*if(serial_input.buffer[3]==AIRSPEED_ETS){
if (test==0){
system("echo 1 > /sys/class/gpio/gpio60/value");
test=1;
}else{
system("echo 0 > /sys/class/gpio/gpio60/value");
test=0;
}
}*/
//add timestamp
message_length=add_timestamp(input_buffer);
//send data to eth port using UDP
UDP_err_handler(sendUDPClientData(&udp_client,input_buffer,message_length),write_udp_error_ptr);
#if LOGGING > 0
//write the data to circual buffer for log thread
if(!cbIsFull(cb_write_lisa)){
memcpy (&cb_elem.value, input_buffer, message_length);
cbWrite(cb_write_lisa, &cb_elem);
}else{
if(reading_flag_lisa==0){
switch_cb_lisa_pointers();
//printf("switching lisa pointers\n");
}else{
printf("LISA WRITE WAS NOT READY \n");
exit(1); //FOR DEBUGGING
}
}
#endif
}else{
//send error message to server: not receiving data on uart port
printf("error on uart, see log...\n"); //FOR DEBUGGING
UART_err_handler(message_length,write_uart_error_ptr);
}
}
UART_err_handler(serial_port_close(),write_uart_error_ptr);
UDP_err_handler(closeUDPClientSocket(&udp_client),write_udp_error_ptr);
return NULL;
/*------------------------END OF SECOND THREAD------------------------*/
}
//#define SENSOR_OFF
//#define KOM_OFF
void timedInterupt(void)
{
//Toggla
if(temp==0){
PORTB &= 0b11111110;
temp = 1;
}else{
PORTB |= 0b00000001;
temp = 0;
}
uint8_t msgRecieve[32];
uint8_t type;
uint8_t len;
uint8_t msgSend[32];
//skicka request till sensorenehten att skicka data
#ifndef SENSOR_OFF
SPI_set_sensor(START);
SPI_MASTER_write(msgSend, TYPE_REQUEST_SENSOR_DATA, 0);
//ta emot data från sensorenheten
uint8_t answerSensor=0;
uint8_t answerCounterSensor=0;
while((!answerSensor)&&(answerCounterSensor<254))//räknare så vi inte fastnar här om vi hamnar aout of sync med kom.
{
answerSensor = SPI_MASTER_read(msgRecieve, &type, &len);
answerCounterSensor++;
}
SPI_set_sensor(END);
if(answerCounterSensor == 255)
return;
glob_longFront_old = glob_longFront; // cm
glob_longRight_old = glob_longRight; // cm
glob_longRear_old = glob_longRear;// cm
glob_longLeft_old = glob_longLeft;// cm
glob_shortFrontRight_old = glob_shortFrontRight; // cm/2
glob_shortFrontLeft_old = glob_shortFrontLeft; // cm/2
glob_shortRearRight_old = glob_shortRearRight; // cm/2
glob_shortRearLeft_old = glob_shortRearLeft;
//tolka/spara sensordata
if(type==TYPE_SENSOR_DATA && len!=0)
{
for(uint8_t i = 0; i < len; ++i)
{
uint8_t id = msgRecieve[i];
switch(id)
{
case LONGFRONT:
glob_longFront = msgRecieve[i+1];
++i;
break;
case LONGRIGHT:
glob_longRight = msgRecieve[i+1];
++i;
break;
case LONGREAR:
glob_longRear = msgRecieve[i+1];
++i;
break;
case LONGLEFT:
glob_longLeft = msgRecieve[i+1];
++i;
break;
case SHORTFRONTRIGHT:
glob_shortFrontRight = msgRecieve[i+1];
++i;
break;
case SHORTFRONTLEFT:
glob_shortFrontLeft = msgRecieve[i+1];
++i;
break;
case SHORTREARRIGHT:
glob_shortRearRight = msgRecieve[i+1];
++i;
break;
case SHORTREARLEFT:
glob_shortRearLeft = msgRecieve[i+1];
++i;
break;
case IDGYROSENSOR:
glob_gyro = (msgRecieve[i+1]<<8)|msgRecieve[i+2];
i = i+2;
break;
case IDSPEEDRIGHT:
glob_vRight = msgRecieve[i+1];
++i;
break;
case IDSPEEDLEFT:
glob_vLeft = msgRecieve[i+1];
++i;
break;
}
}
//uppdatera tillstånd
updateState();
}
#endif
#ifndef KOM_OFF
sendcounter++;
if(sendcounter==8)
{
sendcounter=0;
//skicka vidare till PC
SPI_set_kom(START);
//väldigt ful kod gör om, gör rätt
msgRecieve[len] = 12;
msgRecieve[len+1] = glob_x;
msgRecieve[len+2] = 13;
msgRecieve[len+3] = glob_y;
msgRecieve[len+4] = 14;
msgRecieve[len+5] = glob_theta;
SPI_MASTER_write(msgRecieve, TYPE_DEBUG_DATA, len+6);
/*int8_t vinkelHastHjul = (glob_vRight-glob_vLeft)>>4; // 17 ca 16
cbWrite(&glob_debugMesssageBuffer, 21);
cbWrite(&glob_debugMesssageBuffer, vinkelHastHjul);*/
cbWrite(&glob_debugMesssageBuffer, 25);
cbWrite(&glob_debugMesssageBuffer, (glob_sum_y&0xFF00)>>8);
cbWrite(&glob_debugMesssageBuffer, (glob_sum_y&0x00FF));
//send debug data
uint8_t bytesToSend = 0;
while(cbBytesUsed(&glob_debugMesssageBuffer) != 0)
{
msgSend[bytesToSend] = cbRead(&glob_debugMesssageBuffer);
++bytesToSend;
}
if(bytesToSend != 0)
{
SPI_MASTER_write(msgSend, TYPE_DEBUG_DATA, bytesToSend);
}
//end send debug data
//uint8_t msg[4]={12, glob_x, 13, glob_y};
//SPI_MASTER_write(msg, TYPE_DEBUG_DATA, 4);
volatile uint8_t answer = 0;
uint8_t answerCounter=0;
do
{
SPI_MASTER_write(msgSend, TYPE_REQUEST_PC_MESSAGE, 0);
answer = 0;
answerCounter=0;
while((!answer)&&(answerCounter<254))//räknare så vi inte fastnar här om vi hamnar aout of sync med kom.
{
answer = SPI_MASTER_read(msgRecieve, &type, &len);
answerCounter++;
}
if(type == TYPE_MANUAL_COMMAND)
{
//lägg till msg[0] först i route
for(uint8_t i = glob_routeLength; i > 0; --i)
{
glob_route[glob_routeLength] = glob_route[glob_routeLength-1];
}
glob_route[0] = msgRecieve[0];
glob_routeLength = glob_routeLength+1;
break;
}
else if(type == TYPE_CHANGE_PARM)
{
uint8_t ID = msgRecieve[0];
uint8_t val = msgRecieve[1];
if (ID==PARAMLEFTCUSTOM)
{
glob_paramCustomLeft = val;
}
else if (ID==PARAMRIGHTCUSTOM)
{
glob_paramCustomRight = val;
}
else if (ID==L1_STRAIGHTX)
{
glob_L1_straightX = val;
}
else if (ID==L2_STRAIGHTTHETA)
{
glob_L2_straightTheta = val;
}
else if (ID==L3_STRAIGHTOMEGA)
{
glob_L3_straightOmega = val;
}
else if (ID==L1_TURNTHETA)
{
glob_L1_turnTheta = val;
}
else if (ID==L2_TURNOMEGA)
{
glob_L2_turnOmega = val;
}
}
}while((type != TYPE_NO_PC_MESSAGES) && (type != TYPE_REQUEST_PC_MESSAGE) && (answerCounter<254));//type != TYPE_REQUEST_PC_MESSAGE betyder att vi ej laggt in ny data i kom. dvs ej handskakat.
//skicka all kartdata till komm
while(cbBytesUsed(&glob_mapDataToSend) > 1)
{
uint8_t x = cbRead(&glob_mapDataToSend);
uint8_t y = cbRead(&glob_mapDataToSend);
msgSend[0] = x;
msgSend[1] = y;
msgSend[2] = glob_map[y][x];
SPI_MASTER_write(msgSend, TYPE_MAP_DATA, 3);
}
SPI_set_kom(END);
}
int main(int argc, char *argv[])
{
int print;
printf("%d %s\n",argc, argv[0]);
if(argc >1 && strcmp("-p", argv[1]) == 0) print=1;
time_t nosso_tempo;
//************************************************/
CircularBuffer cb;
ElemType elem = {0};
int testBufferSize = 11264; // 176 blocos de 64 palavras
cbInit(&cb, testBufferSize);
//************************************************/
FILE *arquivo;
int fd;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
int res;
#pragma GCC diagnostic pop
unsigned short crc16;
int i,j=0;
int detected= 0, pronto = 0;
char *ptr;
uint16_t crc;
uint16_t n_mesg, msg_num = 0;
int offset;
struct termios oldtio,newtio;
char buf[BUFFER_SIZE];
fd = open(MODEMDEVICE, O_RDWR | O_NOCTTY );
if (fd < 0) {
perror(MODEMDEVICE);
cbFree(&cb);
exit(-1);
}
tcgetattr(fd,&oldtio); /* save current port settings */
bzero(&newtio, sizeof(newtio));
newtio.c_cflag = BAUDRATE | CRTSCTS | CS8 | CLOCAL | CREAD;
newtio.c_iflag = IGNPAR;
newtio.c_oflag = 0;
/* set input mode (non-canonical, no echo,...) */
newtio.c_lflag = 0;
newtio.c_cc[VTIME] = 0; /* inter-character timer unused */
newtio.c_cc[VMIN] = 132; /* blocking read until 132 chars received */
tcflush(fd, TCIFLUSH);
tcsetattr(fd,TCSANOW,&newtio);
arquivo = fopen("raios_detetados.bin", "wb");
if (arquivo == NULL) {
perror("Nao foi possível abrir o arquivo!");
cbFree(&cb);
close(fd);
exit(30);
}
while (1) { /* loop until we have a terminating condition */
/* read blocks program execution until a line terminating character is
input, even if more than 255 chars are input. If the number
of characters read is smaller than the number of chars available,
subsequent reads will return the remaining chars. res will be set
to the actual number of characters actually read */
res=read(fd,buf,BUFFER_SIZE);
// printf("%d chars - ",res);
crc=((uint16_t) (0xFF00 & (buf[MSG_SIZE+1]<<8)) + (uint16_t) (0x00FF & buf[MSG_SIZE]));
n_mesg=((uint16_t) (0xFF00 & (buf[1]<<8)) + (uint16_t) (0x00FF & buf[0]));
// char test[]="CRC16 stream simulation.";
ptr=buf;
crc16= 0;
for (i = 0; i < MSG_SIZE; i++) {
crc16 = update_crc_16(crc16, *ptr);
ptr++;
}
if(crc==crc16){
// printf(" CRC OK! - %4X %4X %d\n", crc, crc16, n_mesg);
for(i=1;i<65;i++){
elem.value=((int16_t) (0xFF00 & (buf[2*(i)+1]<<8)) + (int16_t) (0x00FF & buf[2*i]));
if (detected == 1) {
// if (n_mesg != msg_num) {
if (j < 13632) {
vetor_disco[j] = elem.value;
j++;
// printf("j = %d\n", j);
}
else {
pronto = 1;
detected = 0;
cbWrite(&cb, &elem);
}
}
else {
cbWrite(&cb, &elem);
}
if(print==1){
printf("%4d ",elem.value);
if (((i+2) % 32) ==1)
printf("\n");
}
}
} else {
clearScreen();
// printf("not OK! %X %X\n", crc, crc16);
tcflush(fd, TCIFLUSH);
}
if (pronto == 1) {
fwrite(vetor_disco, sizeof(vetor_disco), 1, arquivo);
printf("Write!!!!!");
j = 0;
pronto = 0;
}
// if(j>=testBufferSize){
// while (!cbIsEmpty(&cb)) {
// elem = cbRead(&cb);i
// for (offset=0;offset<(cb.end-cb.start);offset++){
// elem = cbPeek(&cb,offset);
// printf("%d\t%d\t%d\t%d\n", elem.value, cb.start, cb.end, cb.size);
// }
if (detected == 0) {
offset = r_change(&cb,n_mesg); // TODO: o que acontece quando n_mesg retorna a zero?
if(offset){
nosso_tempo = time(&nosso_tempo);
fwrite(&nosso_tempo, sizeof(time_t),1, arquivo);
// for (i = offset - 2263, j = 0; i < cb.end; i++, j++){ // TODO :: fazer a conta do offset levando em consideração o wrap around.
for (i = (offset%64 - 37)*64, j = 0; i < (offset%64 +1)*64; i++, j++){
ElemType temp_elem;
temp_elem = cbPeek(&cb, i);
vetor_disco[j] = temp_elem.value;
}
cb.end = cb.start; // Esvazia o buffer circular
detected = 1;
msg_num = n_mesg + 176;
printf("Detected: %ld %d %d\n",nosso_tempo, n_mesg,msg_num);
}
}
// }
// break;
// }
// for(i=0;i<64;i++){
// printf("%4d ",((uint16_t) (0xFF00 & (buf[2*(i)+1]<<8)) + (uint16_t) (0x00FF & buf[2*i])));
// if (((i+2) % 32) ==1) printf("\n");
// }
}
tcsetattr(fd,TCSANOW,&oldtio);
return(0);
}
int sdr_demod(struct demapped_transmission_frame_t *tf, struct sdr_state_t *sdr){
int i,j;
tf->has_fic = 0;
/* resetting coarse freqshift */
sdr->coarse_freq_shift = 0;
/* write input data into fifo */
for (i=0;i<sdr->input_buffer_len;i++) {
cbWrite(&(sdr->fifo),&sdr->input_buffer[i]);
}
/* Check for data in fifo */
if (sdr->fifo.count < 196608*3) {
return 0;
}
/* read fifo */
sdr_read_fifo(&(sdr->fifo),196608*2,sdr->coarse_timeshift+sdr->fine_timeshift,sdr->buffer);
/* give the AGC some time to settle */
if (sdr->startup_delay<=GAIN_SETTLE_TIME) {
sdr->startup_delay+=1;
fprintf(stderr,"startup_delay=%i\n",sdr->startup_delay);
return 0;
}
/* complex data conversion */
for (j=0;j<196608*2;j+=2){
sdr->real[j/2]=sdr->buffer[j]-127;
sdr->imag[j/2]=sdr->buffer[j+1]-127;
}
/* resetting coarse timeshift */
sdr->coarse_timeshift = 0;
/* coarse time sync */
/* performance bottleneck atm */
sdr->coarse_timeshift = dab_coarse_time_sync(sdr->real,sdr->filt,sdr->force_timesync);
// we are not in sync so -> next frame
sdr->force_timesync=0;
if (sdr->coarse_timeshift) {
//printf("coarse time shift\n");
return 0;
}
/* create complex frame */
for (j=0;j<196608;j++){
sdr->dab_frame[j][0] = sdr->real[j];
sdr->dab_frame[j][1] = sdr->imag[j];
}
/* fine time sync */
sdr->fine_timeshift = dab_fine_time_sync(sdr->dab_frame);
if (sdr->coarse_freq_shift) {
sdr->fine_timeshift = 0;
}
/* coarse_frequency shift */
fftw_plan p;
p = fftw_plan_dft_1d(2048, &sdr->dab_frame[2656+505+sdr->fine_timeshift], sdr->symbols[0], FFTW_FORWARD, FFTW_ESTIMATE);
fftw_execute(p);
fftw_destroy_plan(p);
fftw_complex tmp;
for (i = 0; i < 2048/2; i++)
{
tmp[0] = sdr->symbols[0][i][0];
tmp[1] = sdr->symbols[0][i][1];
sdr->symbols[0][i][0] = sdr->symbols[0][i+2048/2][0];
sdr->symbols[0][i][1] = sdr->symbols[0][i+2048/2][1];
sdr->symbols[0][i+2048/2][0] = tmp[0];
sdr->symbols[0][i+2048/2][1] = tmp[1];
}
sdr->coarse_freq_shift = dab_coarse_freq_sync_2(sdr->symbols[0]);
if (abs(sdr->coarse_freq_shift)>1) {
sdr->force_timesync = 1;
return 0;
}
/* fine freq correction */
sdr->fine_freq_shift = dab_fine_freq_corr(sdr->dab_frame,sdr->fine_timeshift);
/* d-qpsk */
for (i=0;i<76;i++) {
p = fftw_plan_dft_1d(2048, &sdr->dab_frame[2656+(2552*i)+504],
sdr->symbols[i], FFTW_FORWARD, FFTW_ESTIMATE);
fftw_execute(p);
fftw_destroy_plan(p);
for (j = 0; j < 2048/2; j++)
{
tmp[0] = sdr->symbols[i][j][0];
tmp[1] = sdr->symbols[i][j][1];
sdr->symbols[i][j][0] = sdr->symbols[i][j+2048/2][0];
sdr->symbols[i][j][1] = sdr->symbols[i][j+2048/2][1];
sdr->symbols[i][j+2048/2][0] = tmp[0];
sdr->symbols[i][j+2048/2][1] = tmp[1];
}
}
//
for (j=1;j<76;j++) {
for (i=0;i<2048;i++)
{
sdr->symbols_d[j*2048+i][0] =
((sdr->symbols[j][i][0]*sdr->symbols[j-1][i][0])
+(sdr->symbols[j][i][1]*sdr->symbols[j-1][i][1]))
/(sdr->symbols[j-1][i][0]*sdr->symbols[j-1][i][0]+sdr->symbols[j-1][i][1]*sdr->symbols[j-1][i][1]);
sdr->symbols_d[j*2048+i][1] =
((sdr->symbols[j][i][0]*sdr->symbols[j-1][i][1])
-(sdr->symbols[j][i][1]*sdr->symbols[j-1][i][0]))
/(sdr->symbols[j-1][i][0]*sdr->symbols[j-1][i][0]+sdr->symbols[j-1][i][1]*sdr->symbols[j-1][i][1]);
}
}
uint8_t* dst = tf->fic_symbols_demapped[0];
tf->has_fic = 1; /* Always true for SDR input */
int k,kk;
for (j=1;j<76;j++) {
if (j == 4) { dst = tf->msc_symbols_demapped[0]; }
k = 0;
for (i=0;i<2048;i++){
if ((i>255) && i!=1024 && i < 1793) {
/* Frequency deinterleaving and QPSK demapping combined */
kk = rev_freq_deint_tab[k++];
dst[kk] = (sdr->symbols_d[j*2048+i][0]>0)?0:1;
dst[1536+kk] = (sdr->symbols_d[j*2048+i][1]>0)?1:0;
}
}
dst += 3072;
}
return 1;
}
int main(void)
{
// Initiera SPI som slav
SPI_SLAVE_init();
// Initiera UART
UART_init();
// Sätt på interrupts
sei();
// Skapa meddelandebuffert för meddelanden som ska skickas till styrenheten
CircularBuffer messageQueue;
cbInit(&messageQueue, 32);
uint8_t spiMsg[32];
uint8_t spiType;
uint8_t spiLen;
uint8_t spiMsgR[32];
uint8_t spiTypeR;
uint8_t spiLenR;
uint8_t uartMsg[16];
uint8_t uartType;
uint8_t uartLen;
uint8_t stopped = 0;
while(1){
// Kolla om det har kommit ett helt meddelande från PCn
if(UART_readMessage(uartMsg,&uartType,&uartLen)){
// Kolla om det är ett nödstoppsanrop
if(uartType == TYPE_EMERGENCY_STOP){
SETBIT(PORTB, PORTB3);
stopped=1;
}else{
// Annars, lägg hela meddelandet i meddelandebuffern
cbWrite(&messageQueue, (uartType<<5)|uartLen);
for(uint8_t i=0; i < uartLen; i++)
{
cbWrite(&messageQueue, uartMsg[i]);
}
}
if(uartType == TYPE_CHANGE_PARM && stopped==1)
{
CLEARBIT(PORTB, PORTB3);
stopped=0;
}
}
// Kolla om vi har tagit emot ett helt meddelande från styrenheten
if(SPI_SLAVE_read(spiMsg, &spiType, &spiLen))
{
if(spiType == TYPE_REQUEST_PC_MESSAGE)
{
// Om det är en efterfrågan av PC-meddelanden, skicka det första meddelandet i buffern, om det finns något
if(cbBytesUsed(&messageQueue)!=0)
{
uint8_t head = cbPeek(&messageQueue);
uint8_t len = head&0b00011111;
uint8_t type = 0b11100000&head;
type = type>>5;
if(len < cbBytesUsed(&messageQueue) && cbBytesUsed(&messageQueue) != 0)
{
head = cbRead(&messageQueue); // Läs headern igen
for(uint8_t i = 0; i < len; i++)
{
spiMsgR[i]=cbRead(&messageQueue);
}
SPI_SLAVE_write(spiMsgR, type, len);
}
else
{
SPI_SLAVE_write(spiMsgR, TYPE_NO_PC_MESSAGES, 0);
}
}
else
{
SPI_SLAVE_write(spiMsgR, TYPE_NO_PC_MESSAGES, 0);
}
}else if(spiType == TYPE_MAP_DATA)