235 lines
6.4 KiB
C
235 lines
6.4 KiB
C
#include <linux/i2c-dev.h>
|
|
#include <i2c/smbus.h>
|
|
#include <sys/ioctl.h>
|
|
#include <fcntl.h>
|
|
#include <unistd.h>
|
|
#include <stdio.h>
|
|
#include <errno.h>
|
|
#include <string.h>
|
|
#include <stdint.h>
|
|
#include <stdarg.h>
|
|
#include <stdlib.h>
|
|
#include <pthread.h>
|
|
|
|
#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"
|
|
#define ALTITUDE_SIM_UP "sim/autopilot/altitude_up"
|
|
#define ALTITUDE_SIM_DOWN "sim/autopilot/altitude_down"
|
|
#define AIRSPEED_SIM_UP "sim/autopilot/airspeed_up"
|
|
#define AIRSPEED_SIM_DOWN "sim/autopilot/airspeed_down"
|
|
|
|
#define ERROR_IIC_OPEN "open failed"
|
|
|
|
pthread_mutex_t i2cDataWriteLock;
|
|
pthread_mutex_t i2cDataReadLock;
|
|
struct ringBuffer i2cDataWrite;
|
|
struct ringBuffer i2cDataRead;
|
|
|
|
struct device {
|
|
uint8_t address; // I2C address of device
|
|
char *name; // name of this device
|
|
char name1[3]; // first name to send on init of device
|
|
char *highCommand1; // command to use if TRIGGER_BIT_1 is set and HIGH_BIT_1 is set
|
|
char *lowCommand1; // command to use if TRIGGER_BIT_1 is set and HIGH_BIT_1 is not set
|
|
int rrefRefresh1; // refresh rate of subscribing rref
|
|
char *rref1; // rref to subscribe to
|
|
char name2[3]; // second name to send on init of device
|
|
char *highCommand2; // command to use if TRIGGER_BIT_2 is set and HIGH_BIT_2 is set
|
|
char *lowCommand2; // command to use if TRIGGER_BIT_2 is set and HIGH_BIT_2 is not set
|
|
int rrefRefresh2; // refresh rate of subscribing rref
|
|
char *rref2; // rref to subscribe to
|
|
};
|
|
|
|
struct rrefSubscription {
|
|
uint8_t address;
|
|
uint8_t triggerBit;
|
|
};
|
|
|
|
const struct device devices[] = {
|
|
{
|
|
0x08,
|
|
"Heading Speed",
|
|
{'H', 'D', 'G'},
|
|
HEADING_SIM_UP,
|
|
HEADING_SIM_DOWN,
|
|
200,
|
|
"sim/cockpit/autopilot/heading_mag",
|
|
{'S', 'P', 'D'},
|
|
AIRSPEED_SIM_UP,
|
|
AIRSPEED_SIM_DOWN,
|
|
200,
|
|
"sim/cockpit/autopilot/airspeed"
|
|
}
|
|
};
|
|
const int numDevices = sizeof(devices) / sizeof(struct device);
|
|
struct rrefSubscription *rrefSubscriptions;
|
|
|
|
void _log(char *format, ...) {
|
|
va_list args;
|
|
va_start(args, format);
|
|
vprintf(format, args);
|
|
va_end(args);
|
|
}
|
|
|
|
int setI2CAddress(int i2c, uint8_t address) {
|
|
if (ioctl(i2c, I2C_SLAVE, address) < 0) {
|
|
_log("setting i2c address 0x%02X failed\n", address);
|
|
return 0;
|
|
} else {
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
void sendI2CInit(int i2c, struct device d) {
|
|
_log("Sending init to %s\n", d.name);
|
|
|
|
int bufLen = 1 + 1 + sizeof(d.name1) + 1 + sizeof(d.name2);
|
|
char buf[bufLen];
|
|
int i = 0;
|
|
buf[i++] = DATA_RESET_BYTE;
|
|
buf[i++] = DATA_INIT_BYTE_1;
|
|
buf[i++] = d.name1[0];
|
|
buf[i++] = d.name1[1];
|
|
buf[i++] = d.name1[2];
|
|
buf[i++] = DATA_INIT_BYTE_2;
|
|
buf[i++] = d.name2[0];
|
|
buf[i++] = d.name2[1];
|
|
buf[i++] = d.name2[2];
|
|
|
|
setI2CAddress(i2c, d.address);
|
|
i2c_smbus_write_block_data(i2c, 0, bufLen, buf);
|
|
}
|
|
|
|
void sendI2CData(int i2c, struct device d, uint8_t triggerBit, char *data, uint8_t dataLen) {
|
|
int bufLen = 1 + 1 + 1 + dataLen;
|
|
char buf[bufLen];
|
|
int i = 0;
|
|
buf[i++] = DATA_BYTE;
|
|
buf[i++] = triggerBit;
|
|
buf[i++] = dataLen;
|
|
for (int j = 0; j < dataLen; j++) {
|
|
buf[i++] = data[j];
|
|
}
|
|
|
|
setI2CAddress(i2c, d.address);
|
|
i2c_smbus_write_block_data(i2c, 0, bufLen, buf);
|
|
}
|
|
|
|
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* i2cHandler(void* arg) {
|
|
int i2c, tmp;
|
|
__s32 i2cResponse;
|
|
char block[BUFFER_BLOCK_SIZE];
|
|
|
|
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++) {
|
|
setI2CAddress(i2c, devices[i].address);
|
|
i2cResponse = i2c_smbus_read_byte_data(i2c, 0);
|
|
if (i2cResponse < 0) {
|
|
_log("I2C read from device %s failed: %s (%d)\n", devices[i].name, strerror(errno), errno);
|
|
usleep(100 * 1000);
|
|
continue;
|
|
}
|
|
|
|
if (i2cResponse == DATA_RESET_BYTE) {
|
|
sendI2CInit(i2c, devices[i]);
|
|
sendI2CData(i2c, devices[i], TRIGGER_BIT_1, "111", 3);
|
|
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 {
|
|
// real data
|
|
_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);
|
|
} else {
|
|
sendI2CData(i2c, devices[i], TRIGGER_BIT_1, "-1-", 3);
|
|
}
|
|
}
|
|
|
|
if (i2cResponse & TRIGGER_BIT_2) {
|
|
if (i2cResponse & HIGH_BIT_2) {
|
|
sendI2CData(i2c, devices[i], TRIGGER_BIT_2, "+2+", 3);
|
|
} else {
|
|
sendI2CData(i2c, devices[i], TRIGGER_BIT_2, "-2-", 3);
|
|
}
|
|
}
|
|
*/
|
|
}
|
|
}
|
|
}
|
|
|
|
close(i2c);
|
|
pthread_exit(NULL);
|
|
}
|
|
|
|
int main() {
|
|
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);
|
|
|
|
// init mutex for ring buffer access
|
|
pthread_mutex_init(&i2cDataReadLock, NULL);
|
|
pthread_mutex_init(&i2cDataWriteLock, 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);
|
|
|
|
// free resources
|
|
pthread_mutex_destroy(&i2cDataReadLock);
|
|
pthread_mutex_destroy(&i2cDataWriteLock);
|
|
free(rrefSubscriptions);
|
|
ringBufferDestroy(&i2cDataRead);
|
|
ringBufferDestroy(&i2cDataWrite);
|
|
|
|
return 0;
|
|
}
|