diff --git a/controller/build.sh b/controller/build.sh index bf7b87f..0ba43a2 100755 --- a/controller/build.sh +++ b/controller/build.sh @@ -1 +1 @@ -gcc -o controller controller.c -li2c +gcc -o controller ringbuffer.c controller.c -li2c diff --git a/controller/controller.c b/controller/controller.c index 60a9f6d..3639987 100644 --- a/controller/controller.c +++ b/controller/controller.c @@ -12,8 +12,11 @@ #include #include "global.h" +#include "ringbuffer.h" #define IIC_DEVICE "/dev/i2c-7" +#define BUFFER_BLOCKS 128 +#define BUFFER_BLOCK_SIZE 8 #define HEADING_SIM_UP "sim/autopilot/heading_up" #define HEADING_SIM_DOWN "sim/autopilot/heading_down" @@ -24,7 +27,10 @@ #define ERROR_IIC_OPEN "open failed" -pthread_mutex_t i2cLock; +pthread_mutex_t i2cDataWriteLock; +pthread_mutex_t i2cDataReadLock; +struct ringBuffer i2cDataWrite; +struct ringBuffer i2cDataRead; struct device { uint8_t address; // I2C address of device @@ -74,7 +80,7 @@ void _log(char *format, ...) { int setI2CAddress(int i2c, uint8_t address) { if (ioctl(i2c, I2C_SLAVE, address) < 0) { - _log("setting periphal address 0x%02X failed\n", address); + _log("setting i2c address 0x%02X failed\n", address); return 0; } else { return 1; @@ -116,15 +122,29 @@ void sendI2CData(int i2c, struct device d, uint8_t triggerBit, char *data, uint8 i2c_smbus_write_block_data(i2c, 0, bufLen, buf); } -void* i2cSend(void* arg) { - int i2c; +int tempCounter = 0; +void* mqttHandler(void* arg) { + char block[BUFFER_BLOCK_SIZE]; + + while (1) { + sprintf(block, "%03d", tempCounter); + pthread_mutex_lock(&i2cDataWriteLock); + ringBufferWrite(&i2cDataWrite, block); + pthread_mutex_unlock(&i2cDataWriteLock); + tempCounter++; + sleep(1); + } } -void* i2cReceive(void* arg) { - int i2c; +void* i2cHandler(void* arg) { + int i2c, tmp; __s32 i2cResponse; + char block[BUFFER_BLOCK_SIZE]; - i2c = *((int*)arg); + i2c = open(IIC_DEVICE, O_RDWR); + if (i2c < 0) { + _log("Unable to open I2C device %s\n", IIC_DEVICE); + } while (1) { for (int i = 0; i < numDevices; i++) { @@ -142,6 +162,16 @@ void* i2cReceive(void* arg) { sendI2CData(i2c, devices[i], TRIGGER_BIT_2, "222", 3); } else if (i2cResponse == DATA_STOP_BYTE) { // expect no forther data from device + // send data to device + do { + pthread_mutex_lock(&i2cDataWriteLock); + tmp = ringBufferRead(&i2cDataWrite, block); + pthread_mutex_unlock(&i2cDataWriteLock); + if (tmp == 0) { + // we got something to send + sendI2CData(i2c, devices[i], TRIGGER_BIT_1, block, 3); + } + } while (tmp == 0); usleep(50 * 1000); continue; } else { @@ -149,7 +179,6 @@ void* i2cReceive(void* arg) { _log("data: 0x%02x\n", i2cResponse); /* XXX: put data into cache and handle with another core / loop / fork / whatever - */ if (i2cResponse & TRIGGER_BIT_1) { if (i2cResponse & HIGH_BIT_1) { sendI2CData(i2c, devices[i], TRIGGER_BIT_1, "+1+", 3); @@ -165,6 +194,7 @@ void* i2cReceive(void* arg) { sendI2CData(i2c, devices[i], TRIGGER_BIT_2, "-2-", 3); } } + */ } } } @@ -174,22 +204,31 @@ void* i2cReceive(void* arg) { } int main() { - int i2c; - pthread_t i2cReceiver; + pthread_t i2cThread, mqttThread; + // create the ring buffers for i2c devices + ringBufferCreate(BUFFER_BLOCKS, BUFFER_BLOCK_SIZE, &i2cDataRead); + ringBufferCreate(BUFFER_BLOCKS, BUFFER_BLOCK_SIZE, &i2cDataWrite); + + // alloc rrefSubscriptions = calloc(sizeof(struct device), numDevices); - i2c = open(IIC_DEVICE, O_RDWR); - if (i2c < 0) { - _log("Unable to open I2C device %s\n", IIC_DEVICE); - } + // init mutex for ring buffer access + pthread_mutex_init(&i2cDataReadLock, NULL); + pthread_mutex_init(&i2cDataWriteLock, NULL); - pthread_mutex_init(&i2cLock, NULL); + // start and join threads + pthread_create(&i2cThread, NULL, i2cHandler, NULL); + pthread_create(&mqttThread, NULL, mqttHandler, NULL); + pthread_join(i2cThread, NULL); + pthread_join(mqttThread, NULL); - pthread_create(&i2cReceiver, NULL, i2cReceive, &i2c); - pthread_join(i2cReceiver, NULL); - - pthread_mutex_destroy(&i2cLock); + // free resources + pthread_mutex_destroy(&i2cDataReadLock); + pthread_mutex_destroy(&i2cDataWriteLock); + free(rrefSubscriptions); + ringBufferDestroy(&i2cDataRead); + ringBufferDestroy(&i2cDataWrite); return 0; } diff --git a/controller/ringbuffer.c b/controller/ringbuffer.c index cc1d7c6..ea1554e 100644 --- a/controller/ringbuffer.c +++ b/controller/ringbuffer.c @@ -2,18 +2,20 @@ #include #include +#include + #include "ringbuffer.h" void _ringBufferIncReader(struct ringBuffer *buf) { - buf->reader++; - if (buf->reader >= buf->capacity) { + buf->reader += buf->blockSize; + if (buf->reader >= buf->blocks * buf->blockSize) { buf->reader = 0; } } void _ringBufferIncWriter(struct ringBuffer *buf) { - buf->writer++; - if (buf->writer >= buf->capacity) { + buf->writer += buf->blockSize; + if (buf->writer >= buf->blocks * buf->blockSize) { buf->writer = 0; } @@ -26,9 +28,10 @@ void _ringBufferIncWriter(struct ringBuffer *buf) { } } -void ringBufferCreate(int capacity, struct ringBuffer *out) { - out->buffer = malloc(capacity * sizeof(uint8_t)); - out->capacity = capacity; +void ringBufferCreate(int blocks, size_t blockSize, struct ringBuffer *out) { + out->buffer = malloc(blocks * blockSize * sizeof(char)); + out->blocks = blocks; + out->blockSize = blockSize; out->reader = 0; out->writer = 0; } @@ -37,10 +40,10 @@ void ringBufferDestroy(struct ringBuffer *buf) { free(buf->buffer); } -int ringBufferRead(struct ringBuffer *buf, uint8_t *out) { +int ringBufferRead(struct ringBuffer *buf, char *out) { if (buf->reader != buf->writer) { // we have data to read - memcpy(out, &buf->buffer[buf->reader], sizeof(uint8_t)); + memcpy(out, &buf->buffer[buf->reader], buf->blockSize * sizeof(char)); _ringBufferIncReader(buf); return 0; } else { @@ -49,7 +52,7 @@ int ringBufferRead(struct ringBuffer *buf, uint8_t *out) { } } -void ringBufferWrite(struct ringBuffer *buf, uint8_t *in) { - memcpy(&buf->buffer[buf->writer], in, sizeof(uint8_t)); +void ringBufferWrite(struct ringBuffer *buf, char *in) { + memcpy(&buf->buffer[buf->writer], in, buf->blockSize * sizeof(char)); _ringBufferIncWriter(buf); } diff --git a/controller/ringbuffer.h b/controller/ringbuffer.h index 244182f..fbddd45 100644 --- a/controller/ringbuffer.h +++ b/controller/ringbuffer.h @@ -1,11 +1,12 @@ struct ringBuffer { - uint8_t *buffer; - int capacity; + char *buffer; + int blocks; + size_t blockSize; int reader; int writer; }; -void ringBufferCreate(int capacity, struct ringBuffer *out); +void ringBufferCreate(int blocks, size_t blockSize, struct ringBuffer *out); void ringBufferDestroy(struct ringBuffer *buf); -int ringBufferRead(struct ringBuffer *buf, uint8_t *out); -void ringBufferWrite(struct ringBuffer *buf, uint8_t *in); +int ringBufferRead(struct ringBuffer *buf, char *out); +void ringBufferWrite(struct ringBuffer *buf, char *in);