From 615ec6de0d5f40f7534100946b535bc55208b17b Mon Sep 17 00:00:00 2001 From: rodrigosiqueira Date: Mon, 29 Jun 2015 03:02:33 -0300 Subject: [PATCH] First section of refactoring. --- rpi/Makefile | 91 +++++++++++++++- rpi/{def.h => include/def.hpp} | 4 +- rpi/{dfs.h => include/dfs.hpp} | 9 +- rpi/{packet.h => include/packet.hpp} | 10 +- rpi/include/pathFinder.hpp | 10 ++ rpi/{pathNode.h => include/pathNode.hpp} | 4 +- rpi/{position.h => include/position.hpp} | 6 +- rpi/{serial.h => include/serial.hpp} | 4 +- rpi/main.cpp | 125 --------------------- rpi/obj/.keep | 0 rpi/pathfinder.h | 12 --- rpi/serial.cpp | 119 -------------------- rpi/{ => src}/dfs.cpp | 88 +++++++-------- rpi/src/main.cpp | 131 +++++++++++++++++++++++ rpi/{ => src}/packet.cpp | 45 ++++---- rpi/{ => src}/pathNode.cpp | 2 +- rpi/{ => src}/position.cpp | 2 +- rpi/src/serial.cpp | 125 +++++++++++++++++++++ 18 files changed, 439 insertions(+), 348 deletions(-) rename rpi/{def.h => include/def.hpp} (97%) rename rpi/{dfs.h => include/dfs.hpp} (92%) rename rpi/{packet.h => include/packet.hpp} (80%) create mode 100644 rpi/include/pathFinder.hpp rename rpi/{pathNode.h => include/pathNode.hpp} (79%) rename rpi/{position.h => include/position.hpp} (83%) rename rpi/{serial.h => include/serial.hpp} (91%) delete mode 100644 rpi/main.cpp create mode 100644 rpi/obj/.keep delete mode 100644 rpi/pathfinder.h delete mode 100644 rpi/serial.cpp rename rpi/{ => src}/dfs.cpp (87%) create mode 100644 rpi/src/main.cpp rename rpi/{ => src}/packet.cpp (81%) rename rpi/{ => src}/pathNode.cpp (82%) rename rpi/{ => src}/position.cpp (85%) create mode 100644 rpi/src/serial.cpp diff --git a/rpi/Makefile b/rpi/Makefile index 9324062..36a0d94 100644 --- a/rpi/Makefile +++ b/rpi/Makefile @@ -1,2 +1,89 @@ -all: - clang++ *.cpp -o bin +#============================================================================== +# Project folders +#============================================================================== + +#Main folders +SRCFOLDER := src/ +INCFOLDER := include/ +BINFOLDER := bin/ +LIBFOLDER := lib/ +OBJFOLDER := obj/ +TESTFOLDER := test/ +OTHERFOLDER := other/ +DOCFOLDER := doc/ + +#============================================================================== +# Project flags +#============================================================================== +CC = g++ $(SIMPLEWARNINGS) +SIMPLEWARNINGS := -Wall +ALLWARNINGS := -Wall -Wextra -pedantic -Wshadow -Wredundant-decls -Woverloaded-virtual -Wsynth +IFLAG = -I./$(INCFOLDER) +GOOGLETESTFLAG = -lgtest -lgtest_main + +#============================================================================== +# Project Strings +#============================================================================== +TESTSEPARATOR = "+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" +CLEANSEPARATOR = "- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - " +#============================================================================== +# Project files +#============================================================================== +CPPFILES = $(wildcard src/*.cpp) +TESTCPPFILES = $(wildcard test/*.cpp) + +all: prepare rpi + +rpi: srcObjects + $(CC) obj/*.o -o bin/app/$@ $(IFLAG) + +#============================================================================== +# Preparing object files +#============================================================================== +# Files +srcObjects: $(CPPFILES:src/%.cpp=obj/%.o) + +#Necessary use include for PostgreSQL library here too, if it is used in other file. +obj/%.o: src/%.cpp + $(CC) -c $< -o $@ $(IFLAG) + +#============================================================================== +# Testing +# About: The command test compile all tests and put it in the folder bin/test. +# You should have a file in the src folder mapped into test. To make it +# organized, you should keep the same folder structure in test folder. +# Example: +# make test -> Compile all the tests +# make testModel -> Compile all the tests in model folder. +#============================================================================== +test: tests + +# Test Model +tests: $(TESTCPPFILES:test/%_test.cpp=bin/test/%.bin) + +bin/test/%.bin: test/%_test.cpp src/%.cpp + $(CC) $^ -o $@ $(IFLAG) $(GOOGLETESTFLAG) + +#===================================================================================== +# Project Actions +#===================================================================================== +PHONY: clean prepare doxy runTest + +doxy: + doxygen Doxyfile + +run: + $(APPBINFOLDER)rpi + +runTest: + $(TESTBINFOLDER)*.bin + +prepare: + mkdir -p $(BINFOLDER)test + mkdir -p $(BINFOLDER)app + +clean: + @echo $(CLEANSEPARATOR) + rm -fr bin/* + rm -fr obj/* + diff --git a/rpi/def.h b/rpi/include/def.hpp similarity index 97% rename from rpi/def.h rename to rpi/include/def.hpp index f2b8899..ef8b417 100644 --- a/rpi/def.h +++ b/rpi/include/def.hpp @@ -1,5 +1,5 @@ -#ifndef _DEF_H -#define _DEF_H +#ifndef _DEF_HPP_ +#define _DEF_HPP_ typedef unsigned char byte; diff --git a/rpi/dfs.h b/rpi/include/dfs.hpp similarity index 92% rename from rpi/dfs.h rename to rpi/include/dfs.hpp index df4c516..57927af 100644 --- a/rpi/dfs.h +++ b/rpi/include/dfs.hpp @@ -1,15 +1,15 @@ -#ifndef DEPTH_FIRST_SEARCH -#define DEPTH_FIRST_SEARCH +#ifndef _DEPTH_FIRST_SEARCH_HPP_ +#define _DEPTH_FIRST_SEARCH_HPP_ #include -#include "position.h" -#include "pathNode.h" #include #include #include #include #include +#include "position.hpp" +#include "pathNode.hpp" class DepthFirstSearch { @@ -61,5 +61,4 @@ class DepthFirstSearch }; - #endif diff --git a/rpi/packet.h b/rpi/include/packet.hpp similarity index 80% rename from rpi/packet.h rename to rpi/include/packet.hpp index 5c1c38a..e0d7c81 100644 --- a/rpi/packet.h +++ b/rpi/include/packet.hpp @@ -1,9 +1,10 @@ -#ifndef _PACKET_H -#define _PACKET_H +#ifndef _PACKET_HPP_ +#define _PACKET_HPP_ #define MAX_PACKETS 16 -struct _packet { +struct _packet +{ unsigned char tag; unsigned char length; unsigned char* value; @@ -12,7 +13,8 @@ struct _packet { typedef struct _packet packet; /* Cria o pacote com o rótulo, tamanho e valoe especificado */ -packet* packet_create(unsigned char tag, unsigned char length, unsigned char* data); +packet* packet_create(unsigned char tag, unsigned char length, + unsigned char* data); /* Destrói um pacote da memória */ void packet_destroy(packet* p); diff --git a/rpi/include/pathFinder.hpp b/rpi/include/pathFinder.hpp new file mode 100644 index 0000000..85666fc --- /dev/null +++ b/rpi/include/pathFinder.hpp @@ -0,0 +1,10 @@ +#ifndef _PATH_FINDER_HPP_ +#define _PATH_FINDER_HPP_ + +class PathFinder +{ + +}; + + +#endif diff --git a/rpi/pathNode.h b/rpi/include/pathNode.hpp similarity index 79% rename from rpi/pathNode.h rename to rpi/include/pathNode.hpp index a7ababe..eed85ec 100644 --- a/rpi/pathNode.h +++ b/rpi/include/pathNode.hpp @@ -1,5 +1,5 @@ -#ifndef PATH_NODE_H -#define PATH_NODE_H +#ifndef _PATH_NODE_HPP_ +#define _PATH_NODE_HPP_ class PathNode { diff --git a/rpi/position.h b/rpi/include/position.hpp similarity index 83% rename from rpi/position.h rename to rpi/include/position.hpp index 4b8be03..1e444ac 100644 --- a/rpi/position.h +++ b/rpi/include/position.hpp @@ -1,7 +1,7 @@ -#ifndef POSITION_H -#define POSITION_H +#ifndef _POSITION_HPP_ +#define _POSITION_HPP_ -#include "def.h" +#include "def.hpp" class Position { diff --git a/rpi/serial.h b/rpi/include/serial.hpp similarity index 91% rename from rpi/serial.h rename to rpi/include/serial.hpp index 3fc8a4f..7368757 100644 --- a/rpi/serial.h +++ b/rpi/include/serial.hpp @@ -1,5 +1,5 @@ -#ifndef _SERIAL_H -#define _SERIAL_H +#ifndef _SERIAL_HPP_ +#define _SERIAL_HPP_ #define SERIAL_DEVICE_NAME "/dev/ttyACM0" #define SERIAL_DEVICE_BAUDRATE B57600 diff --git a/rpi/main.cpp b/rpi/main.cpp deleted file mode 100644 index 12e3f54..0000000 --- a/rpi/main.cpp +++ /dev/null @@ -1,125 +0,0 @@ -#include "def.h" -#include "serial.h" -#include "dfs.h" -#include "packet.h" - -#include -#include -#include - -#include - -using namespace std; - -#define RXTX_BUFFER_SIZE 256 - -void print_bytes(byte* buff, int len) { - - for (int i = 0; i < len; i++) { - cout << (int) buff[i] << " "; - } - - cout << endl; -} - - -int main() { - cout << "Init" << endl; - - byte* tx_data; - byte* rx_data; - - rx_data = new byte[RXTX_BUFFER_SIZE]; - if(!rx_data) { - return 1; - } - - int fd = serial_open(); - - if(fd < 0) { - cout << "Could not open serial" << endl; - return 1; - } - - cout << "Serial opened: fd=" << fd << endl; - /* handshake*/ - // send: TAG_CMD, 1, CMD_WAKEUP - tx_data = new byte[1]; - tx_data[0] = CMD_WAKEUP; //TAG_CMD; -// tx_data[1] = 1; - // tx_data[2] = CMD_WAKEUP; - - cout << "Sending handshake" << endl; - byte* ack = new byte[1]; - ack[0] = 0; - - while(ack[0] == 0) { - cout << "Trying..." << endl; - serial_write(fd, tx_data, 1); - usleep(1000); - serial_read(fd, ack, 1); - } - - delete[] ack; - - DepthFirstSearch* dfs = new DepthFirstSearch(); - dfs->init(25, 25); // TODO - - /* main loop */ - - while(true) { - // read: TAG_DEVICE_STATE, 1, STATE_WAITING_CMD - // TAG_SENSOR_DATA, 2, SENSOR_0, distancia - // TAG_SENSOR_DATA, 2, SENSOR_1, obstáculo - cout << "Waiting data..." << endl; - int bytes_read = serial_read(fd, rx_data, RXTX_BUFFER_SIZE, 9); - - if(bytes_read != 9) { - // alguma coisa deu errado - continue; - } - - cout << "Bytes lidos: "; - print_bytes(rx_data, bytes_read); - - packet* pkt_distance = packet_create(rx_data[0], rx_data[1], rx_data + 2); - packet* pkt_obstacle = packet_create(rx_data[5], rx_data[6], rx_data + 7); - - //TODO verificar integridade dos pacotes - - bool obstacle = pkt_obstacle->value[1] == 49; - - unsigned char b_low, b_high; - b_high = pkt_distance->value[1]; - b_low = pkt_distance->value[2]; - - int distance = (b_high << 8) | b_low; - //cout << "obst: " << obstacle << ": " << (int) pkt_obstacle->value[3] << endl; - //cout << "dist: " << distance << ": bh=" << (int) b_high << "; bl=" << (int) b_low << endl; - - // processa no algoritmo: tocado pelo pai - byte cmd_direction = dfs->move(obstacle, distance) & 0xFF; - byte cmd_distance = dfs->getDistance();// & 0XFF; - - cout << "cmd_d:" << (int) cmd_distance << endl; - - //packet_destroy(pkt_state); - packet_destroy(pkt_distance); - packet_destroy(pkt_obstacle); - - tx_data = new byte[6]; - tx_data[0] = TAG_CMD; - tx_data[1] = 1; - tx_data[2] = cmd_direction; - - tx_data[3] = TAG_DATA; - tx_data[4] = 1; - tx_data[5] = cmd_distance; - - serial_write(fd, tx_data, 6); - print_bytes(tx_data, 6); - delete[] tx_data; - - } - return 0; -} diff --git a/rpi/obj/.keep b/rpi/obj/.keep new file mode 100644 index 0000000..e69de29 diff --git a/rpi/pathfinder.h b/rpi/pathfinder.h deleted file mode 100644 index 1882ebc..0000000 --- a/rpi/pathfinder.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef PATH_FINDER_H -#define PATH_FINDER_H - -class PathFinder -{ - - - -}; - - -#endif diff --git a/rpi/serial.cpp b/rpi/serial.cpp deleted file mode 100644 index c7c828d..0000000 --- a/rpi/serial.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include "serial.h" - -#include -#include -#include -#include -#include -#include - -using namespace std; - -typedef unsigned char byte; - -int _set_interface_attribs (int fd, int speed, int parity) -{ - struct termios tty; - memset (&tty, 0, sizeof tty); - if (tcgetattr (fd, &tty) != 0) - { - // TODO adicionar algum erro aqui - return -1; - } - - cfsetospeed (&tty, speed); - cfsetispeed (&tty, speed); - - tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars - // disable IGNBRK for mismatched speed tests; otherwise receive break - // as \000 chars - tty.c_iflag &= ~IGNBRK; // disable break processing - tty.c_lflag = 0; // no signaling chars, no echo, - // no canonical processing - tty.c_oflag = 0; // no remapping, no delays - tty.c_cc[VMIN] = 0; // read doesn't block - tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout - - tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl - - tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls, - // enable reading - tty.c_cflag &= ~(PARENB | PARODD); // shut off parity - tty.c_cflag |= parity; - tty.c_cflag &= ~CSTOPB; - tty.c_cflag &= ~CRTSCTS; - - if (tcsetattr (fd, TCSANOW, &tty) != 0) - { - //error_message ("error %d from tcsetattr", errno); - return -1; - } - return 0; -} - -void _set_blocking (int fd, int should_block) -{ - struct termios tty; - memset (&tty, 0, sizeof tty); - if (tcgetattr (fd, &tty) != 0) - { - //TODO adicionar algum erro aqui - return; - } - - tty.c_cc[VMIN] = should_block ? 1 : 0; - tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout - - if (tcsetattr (fd, TCSANOW, &tty) != 0) { - // TODO adicionar algum erro aqui - - } -} - - -int serial_open(std::string name) { - const char* port_name = name.c_str(); - int fd = open (port_name, O_RDWR | O_NOCTTY | O_SYNC); - - if (fd < 0) { - return -1; - } - - _set_interface_attribs (fd, SERIAL_DEVICE_BAUDRATE, SERIAL_DEVICE_PARITY); - _set_blocking (fd, SERIAL_DEVICE_BLOCKING); - - return fd; -} -// Nota: usleep -// Cada char demora ~100us para ser entregue. *Idealmente* deveria receber -// um ACK da placa, mas como esse código está longe do ideal, vai ser assim -// mesmo. - -int serial_write(int fd, unsigned char* data, int data_size) { - write(fd, data, data_size); - usleep(data_size * 100); - return 1; -} - -int serial_read(int fd, unsigned char* buffer, int buffer_size, int max) { - usleep(buffer_size * 100); - memset(buffer, 0, buffer_size); - - if(max == -1) { - int total_read = read(fd, buffer, buffer_size); - return total_read; - } - else { - int total_read = 0; - byte* tmp = new byte[buffer_size]; - - while(total_read < max) { - int bytes_read = read(fd, tmp, buffer_size); - //cout << "Entrou aqui :" << bytes_read << endl; - memcpy(buffer + total_read, tmp, bytes_read); - total_read += bytes_read; - } - return total_read; - } - -} diff --git a/rpi/dfs.cpp b/rpi/src/dfs.cpp similarity index 87% rename from rpi/dfs.cpp rename to rpi/src/dfs.cpp index 78407aa..d87164d 100644 --- a/rpi/dfs.cpp +++ b/rpi/src/dfs.cpp @@ -1,7 +1,8 @@ -#include "dfs.h" #include #include +#include "dfs.hpp" + const int DepthFirstSearch::INCREASING_SIZE = 50; DepthFirstSearch::PositionComparison::PositionComparison(Position _targetPosition) @@ -9,8 +10,8 @@ DepthFirstSearch::PositionComparison::PositionComparison(Position _targetPositio targetPosition = _targetPosition; } -int -DepthFirstSearch::PositionComparison::calculateHeuristic(const int& row, const int& column) const +int DepthFirstSearch::PositionComparison::calculateHeuristic( + const int& row, const int& column) const { int distance = 1; @@ -20,9 +21,9 @@ DepthFirstSearch::PositionComparison::calculateHeuristic(const int& row, const i return distance * (dx+dy); } - -bool -DepthFirstSearch::PositionComparison::operator() (const Position& p1, const Position& p2) const +bool DepthFirstSearch::PositionComparison::operator() ( + const Position& p1, + const Position& p2) const { int relativeCost1 = p1.cost + calculateHeuristic(p1.x, p1.y); @@ -38,16 +39,13 @@ DepthFirstSearch::PositionComparison::operator() (const Position& p1, const Posi return false; } - - DepthFirstSearch::DepthFirstSearch():pathAvailable(false) { } -void -DepthFirstSearch::display_visited_positions() +void DepthFirstSearch::display_visited_positions() { int row, column; @@ -68,8 +66,7 @@ DepthFirstSearch::display_visited_positions() } -int -DepthFirstSearch::init(int x, int y) +int DepthFirstSearch::init(int x, int y) { int cost = 0; Position pos = Position(x, y, Position::FRONT, cost); @@ -77,7 +74,7 @@ DepthFirstSearch::init(int x, int y) visitedPositions.resize(INCREASING_SIZE); - for(int i =0; i , PositionComparison > space((PositionComparison(targetPos))); std::vector< std::vector > visited_nodes; @@ -427,14 +416,13 @@ DepthFirstSearch::createPathToLocation() } for(unsigned int i =0; i +#include +#include + +#include + +#include "def.hpp" +#include "serial.hpp" +#include "dfs.hpp" +#include "packet.hpp" + +using namespace std; + +#define RXTX_BUFFER_SIZE 256 + +void print_bytes(byte* buff, int len) +{ + + for (int i = 0; i < len; i++) + { + cout << (int) buff[i] << " "; + } + + cout << endl; +} + +int main() +{ + cout << "Init" << endl; + + byte* tx_data; + byte* rx_data; + + rx_data = new byte[RXTX_BUFFER_SIZE]; + if(!rx_data) + { + return 1; + } + + int fd = serial_open(); + + if(fd < 0) + { + cout << "Could not open serial" << endl; + return 1; + } + + cout << "Serial opened: fd=" << fd << endl; + /* handshake*/ + // send: TAG_CMD, 1, CMD_WAKEUP + tx_data = new byte[1]; + tx_data[0] = CMD_WAKEUP; //TAG_CMD; +// tx_data[1] = 1; + // tx_data[2] = CMD_WAKEUP; + + cout << "Sending handshake" << endl; + byte* ack = new byte[1]; + ack[0] = 0; + + while(ack[0] == 0) + { + cout << "Trying..." << endl; + serial_write(fd, tx_data, 1); + usleep(1000); + serial_read(fd, ack, 1); + } + + delete[] ack; + + DepthFirstSearch* dfs = new DepthFirstSearch(); + dfs->init(25, 25); // TODO + + /* main loop */ + + while(true) + { + // read: TAG_DEVICE_STATE, 1, STATE_WAITING_CMD + // TAG_SENSOR_DATA, 2, SENSOR_0, distancia + // TAG_SENSOR_DATA, 2, SENSOR_1, obstáculo + cout << "Waiting data..." << endl; + int bytes_read = serial_read(fd, rx_data, RXTX_BUFFER_SIZE, 9); + + if(bytes_read != 9) + { + // alguma coisa deu errado + continue; + } + + cout << "Bytes lidos: "; + print_bytes(rx_data, bytes_read); + + packet* pkt_distance = packet_create(rx_data[0], rx_data[1], rx_data + 2); + packet* pkt_obstacle = packet_create(rx_data[5], rx_data[6], rx_data + 7); + + //TODO verificar integridade dos pacotes + bool obstacle = pkt_obstacle->value[1] == 49; + + unsigned char b_low, b_high; + b_high = pkt_distance->value[1]; + b_low = pkt_distance->value[2]; + + int distance = (b_high << 8) | b_low; + //cout << "obst: " << obstacle << ": " << (int) pkt_obstacle->value[3] << endl; + //cout << "dist: " << distance << ": bh=" << (int) b_high << "; bl=" << (int) b_low << endl; + + // processa no algoritmo: tocado pelo pai + byte cmd_direction = dfs->move(obstacle, distance) & 0xFF; + byte cmd_distance = dfs->getDistance();// & 0XFF; + + cout << "cmd_d:" << (int) cmd_distance << endl; + + //packet_destroy(pkt_state); + packet_destroy(pkt_distance); + packet_destroy(pkt_obstacle); + + tx_data = new byte[6]; + tx_data[0] = TAG_CMD; + tx_data[1] = 1; + tx_data[2] = cmd_direction; + + tx_data[3] = TAG_DATA; + tx_data[4] = 1; + tx_data[5] = cmd_distance; + + serial_write(fd, tx_data, 6); + print_bytes(tx_data, 6); + delete[] tx_data; + + } + return 0; +} diff --git a/rpi/packet.cpp b/rpi/src/packet.cpp similarity index 81% rename from rpi/packet.cpp rename to rpi/src/packet.cpp index 9be7d44..6d70acc 100644 --- a/rpi/packet.cpp +++ b/rpi/src/packet.cpp @@ -1,9 +1,11 @@ -#include "packet.h" - #include #include -packet* packet_create(unsigned char tag, unsigned char length, unsigned char* data) { +#include "packet.hpp" + +packet* packet_create(unsigned char tag, unsigned char length, + unsigned char* data) +{ packet* p = (packet*) malloc(sizeof(packet)); p->value = (unsigned char*) malloc(sizeof(unsigned char) * length); @@ -14,28 +16,32 @@ packet* packet_create(unsigned char tag, unsigned char length, unsigned char* da return p; } -void packet_destroy(packet* p) { +void packet_destroy(packet* p) +{ free(p->value); free(p); } -unsigned char* packet_pack(packet** packets, int packets_len, int* buffer_len) { +unsigned char* packet_pack(packet** packets, int packets_len, int* buffer_len) +{ int packet_index = 0; *buffer_len = 0; - - while(packet_index < packets_len) { + + while(packet_index < packets_len) + { packet* p = packets[packet_index]; unsigned char length; length = p->length; *buffer_len += length + 2; packet_index += 1; } - + unsigned char* buffer = (unsigned char*) malloc(sizeof(unsigned char) * (*buffer_len)); int buffer_index = 0; packet_index = 0; - - while(packet_index < packets_len) { + + while(packet_index < packets_len) + { packet* p = packets[packet_index]; buffer[buffer_index] = p->tag; buffer[buffer_index + 1] = p->length; @@ -47,11 +53,12 @@ unsigned char* packet_pack(packet** packets, int packets_len, int* buffer_len) { return buffer; } -packet** packet_unpack(unsigned char* buffer, int buffer_len, int* total_read) { +packet** packet_unpack(unsigned char* buffer, int buffer_len, int* total_read) +{ *total_read = 0; int index = 0; packet** list = (packet**) malloc(sizeof(packet*) * MAX_PACKETS); - + while(index < buffer_len) { unsigned char tag = buffer[index]; unsigned char length = buffer[index + 1]; @@ -63,19 +70,21 @@ packet** packet_unpack(unsigned char* buffer, int buffer_len, int* total_read) { *total_read += 1; index += 2 + length; } - + return list; } -packet* packet_find(unsigned char tag, packet** packets, int len) { - +packet* packet_find(unsigned char tag, packet** packets, int len) +{ int i; - for(i=0; i< len; i++) { - if(packets[i]->tag == tag) { + for(i = 0; i < len; i++) + { + if(packets[i]->tag == tag) + { return packets[i]; } } - + return 0; } diff --git a/rpi/pathNode.cpp b/rpi/src/pathNode.cpp similarity index 82% rename from rpi/pathNode.cpp rename to rpi/src/pathNode.cpp index 1673a2f..0392dae 100644 --- a/rpi/pathNode.cpp +++ b/rpi/src/pathNode.cpp @@ -1,4 +1,4 @@ -#include "pathNode.h" +#include "pathNode.hpp" PathNode::PathNode(int _x, int _y, int _direction, int _cost):x(_x), y(_y), direction(_direction), cost(_cost) { diff --git a/rpi/position.cpp b/rpi/src/position.cpp similarity index 85% rename from rpi/position.cpp rename to rpi/src/position.cpp index 5988cbe..792ed6e 100644 --- a/rpi/position.cpp +++ b/rpi/src/position.cpp @@ -1,4 +1,4 @@ -#include "position.h" +#include "position.hpp" Position::Position(int _x, int _y, int _direction, int _cost) { diff --git a/rpi/src/serial.cpp b/rpi/src/serial.cpp new file mode 100644 index 0000000..751ab7f --- /dev/null +++ b/rpi/src/serial.cpp @@ -0,0 +1,125 @@ +#include +#include +#include +#include +#include +#include + +#include "serial.hpp" + +using namespace std; + +typedef unsigned char byte; + +int _set_interface_attribs (int fd, int speed, int parity) +{ + struct termios tty; + memset (&tty, 0, sizeof tty); + + if (tcgetattr (fd, &tty) != 0) + { + // TODO adicionar algum erro aqui + return -1; + } + + cfsetospeed (&tty, speed); + cfsetispeed (&tty, speed); + + tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars + // disable IGNBRK for mismatched speed tests; otherwise receive break + // as \000 chars + tty.c_iflag &= ~IGNBRK; // disable break processing + tty.c_lflag = 0; // no signaling chars, no echo, + // no canonical processing + tty.c_oflag = 0; // no remapping, no delays + tty.c_cc[VMIN] = 0; // read doesn't block + tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout + + tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl + + tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls, + // enable reading + tty.c_cflag &= ~(PARENB | PARODD); // shut off parity + tty.c_cflag |= parity; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~CRTSCTS; + + if (tcsetattr (fd, TCSANOW, &tty) != 0) + { + //error_message ("error %d from tcsetattr", errno); + return -1; + } + return 0; +} + +void _set_blocking (int fd, int should_block) +{ + struct termios tty; + memset (&tty, 0, sizeof tty); + if (tcgetattr (fd, &tty) != 0) + { + //TODO adicionar algum erro aqui + return; + } + + tty.c_cc[VMIN] = should_block ? 1 : 0; + tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout + + if (tcsetattr (fd, TCSANOW, &tty) != 0) { + // TODO adicionar algum erro aqui + + } +} + +int serial_open(std::string name) +{ + const char* port_name = name.c_str(); + int fd = open (port_name, O_RDWR | O_NOCTTY | O_SYNC); + + if (fd < 0) { + return -1; + } + + _set_interface_attribs (fd, SERIAL_DEVICE_BAUDRATE, SERIAL_DEVICE_PARITY); + _set_blocking (fd, SERIAL_DEVICE_BLOCKING); + + return fd; +} +// Nota: usleep +// Cada char demora ~100us para ser entregue. *Idealmente* deveria receber +// um ACK da placa, mas como esse código está longe do ideal, vai ser assim +// mesmo. + +int serial_write(int fd, unsigned char* data, int data_size) +{ + write(fd, data, data_size); + usleep(data_size * 100); + return 1; +} + +int serial_read(int fd, unsigned char* buffer, int buffer_size, int max) +{ + usleep(buffer_size * 100); + memset(buffer, 0, buffer_size); + + if(max == -1) + { + int total_read = read(fd, buffer, buffer_size); + return total_read; + } + else + { + int total_read = 0; + byte* tmp = new byte[buffer_size]; + + while(total_read < max) + { + int bytes_read = read(fd, tmp, buffer_size); + //cout << "Entrou aqui :" << bytes_read << endl; + memcpy(buffer + total_read, tmp, bytes_read); + total_read += bytes_read; + } + return total_read; + } + +}