There's no need for |port| to be dynamically allocated.

Also fixes the bug that if |nav_fd| open fails, port->isOpen and
port->isInitialised are not cleared.
This commit is contained in:
Ben Laurie
2013-09-08 05:49:55 +01:00
parent 057c842045
commit b28fae799a
+23 -25
View File
@@ -45,20 +45,18 @@ typedef struct {
uint8_t buffer[NAVDATA_BUFFER_SIZE]; uint8_t buffer[NAVDATA_BUFFER_SIZE];
} navdata_port; } navdata_port;
static navdata_port* port; static navdata_port port;
static int nav_fd; static int nav_fd;
int navdata_init() int navdata_init()
{ {
port = malloc(sizeof(navdata_port));
nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK); nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
if (nav_fd == -1) if (nav_fd == -1)
{ {
perror("navdata_init: Unable to open /dev/ttyO1 - "); perror("navdata_init: Unable to open /dev/ttyO1 - ");
return 1; return 1;
} else { } else {
port->isOpen = 1; port.isOpen = 1;
} }
fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking
@@ -93,10 +91,10 @@ int navdata_init()
navdata_imu_available = 0; navdata_imu_available = 0;
navdata_baro_available = 0; navdata_baro_available = 0;
port->bytesRead = 0; port.bytesRead = 0;
port->totalBytesRead = 0; port.totalBytesRead = 0;
port->packetsRead = 0; port.packetsRead = 0;
port->isInitialized = 1; port.isInitialized = 1;
previousUltrasoundHeight = 0; previousUltrasoundHeight = 0;
@@ -158,7 +156,7 @@ void acquire_baro_calibration()
void navdata_close() void navdata_close()
{ {
port->isOpen = 0; port.isOpen = 0;
close(nav_fd); close(nav_fd);
} }
@@ -166,19 +164,19 @@ void navdata_read()
{ {
int newbytes = 0; int newbytes = 0;
if (port->isInitialized != 1) if (port.isInitialized != 1)
navdata_init(); navdata_init();
if (port->isOpen != 1) if (port.isOpen != 1)
return; return;
newbytes = read(nav_fd, port->buffer+port->bytesRead, NAVDATA_BUFFER_SIZE-port->bytesRead); newbytes = read(nav_fd, port.buffer+port.bytesRead, NAVDATA_BUFFER_SIZE-port.bytesRead);
// because non-blocking read returns -1 when no bytes available // because non-blocking read returns -1 when no bytes available
if (newbytes > 0) if (newbytes > 0)
{ {
port->bytesRead += newbytes; port.bytesRead += newbytes;
port->totalBytesRead += newbytes; port.totalBytesRead += newbytes;
} }
} }
@@ -241,15 +239,15 @@ void navdata_update()
navdata_read(); navdata_read();
// while there is something interesting to do... // while there is something interesting to do...
while (port->bytesRead >= 60) while (port.bytesRead >= 60)
{ {
if (port->buffer[0] == NAVDATA_START_BYTE) if (port.buffer[0] == NAVDATA_START_BYTE)
{ {
// if checksum is OK // if checksum is OK
if ( 1 ) // we dont know how to calculate the checksum if ( 1 ) // we dont know how to calculate the checksum
// if ( navdata_checksum() == 0 ) // if ( navdata_checksum() == 0 )
{ {
memcpy(navdata, port->buffer, NAVDATA_PACKET_SIZE); memcpy(navdata, port.buffer, NAVDATA_PACKET_SIZE);
// Invert byte order so that TELEMETRY works better // Invert byte order so that TELEMETRY works better
uint8_t tmp; uint8_t tmp;
@@ -266,7 +264,7 @@ void navdata_update()
navdata_imu_available = 1; navdata_imu_available = 1;
port->packetsRead++; port.packetsRead++;
// printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum)); // printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum));
//navdata_getHeight(); //navdata_getHeight();
} }
@@ -276,13 +274,13 @@ void navdata_update()
{ {
// find start byte, copy all data from startbyte to buffer origin, update bytesread // find start byte, copy all data from startbyte to buffer origin, update bytesread
uint8_t * pint; uint8_t * pint;
pint = memchr(port->buffer, NAVDATA_START_BYTE, port->bytesRead); pint = memchr(port.buffer, NAVDATA_START_BYTE, port.bytesRead);
if (pint != NULL) { if (pint != NULL) {
navdata_CropBuffer(pint - port->buffer); navdata_CropBuffer(pint - port.buffer);
} else { } else {
// if the start byte was not found, it means there is junk in the buffer // if the start byte was not found, it means there is junk in the buffer
port->bytesRead = 0; port.bytesRead = 0;
} }
} }
} }
@@ -290,14 +288,14 @@ void navdata_update()
void navdata_CropBuffer(int cropsize) void navdata_CropBuffer(int cropsize)
{ {
if (port->bytesRead - cropsize < 0) { if (port.bytesRead - cropsize < 0) {
// TODO think about why the amount of bytes read minus the cropsize gets below zero // TODO think about why the amount of bytes read minus the cropsize gets below zero
printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", port->bytesRead, cropsize, port->buffer); printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", port.bytesRead, cropsize, port.buffer);
return; return;
} }
memmove(port->buffer, port->buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize); memmove(port.buffer, port.buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
port->bytesRead -= cropsize; port.bytesRead -= cropsize;
} }
int16_t navdata_getHeight() { int16_t navdata_getHeight() {