// Jan. 2021
// by paul, inspired / recycled from microchip code by SMIOT
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <math.h>
#define MAX_PERIPHERAL 5 //Nombre max de peripheriques externes
typedef struct{
char Type; //type du peripherique (0x01 accel, 0x02 gyro, 0x03 magneto, 0x04 temperature, 0x05 pressure, 0x06 light,...)
char ID; //ID du peripherique
char Range; //Range de la mesure (ex: 2G, 4G, 6G, 8G, 16G pour un accel)
char Resolution; //Resolution de mesure du peripherique
short Frequency; //Frequence d'echantillonage du peripherique
}PERIPHERAL_CONFIGURATION;
typedef struct{
int headerSize; //Taille du header ce champ exclu
int versionNumber;
char numberOfChan;
char resolutionBits;
int samplingFrequency;
int dmaBlockSize;
int sizeOfAdditionnalDataBuffer;
char numberOfExternalPeripheral;
int timeStampOfStart;
PERIPHERAL_CONFIGURATION periphConfig[MAX_PERIPHERAL];
}HighBlueHeader;
struct WaveHeader_s {
char chunkId[4]; // Riff Wave Header
int chunkSize;
char format[4];
char subChunk1Id[4]; // Format Subchunk
int subChunk1Size;
short int audioFormat;
short int numChannels;
int sampleRate;
int byteRate;
short int blockAlign;
short int bitsPerSample;
//short int extraParamSize;
char subChunk2Id[4]; // Data Subchunk
int subChunk2Size;
} WaveHeader_default = {{'R','I','F','F'}, 36, {'W','A','V','E'}, {'f','m','t',' '}, 16, 1, 0, 0, 0, 0, 0, {'d','a','t','a'}, 0};
typedef struct WaveHeader_s WaveHeader;
void parseLogFileHeader(FILE* logfile, HighBlueHeader* hdr, int verbose){
// header parse
fread(&hdr->headerSize, 4, 1, logfile);
fread(&hdr->versionNumber, 2, 1, logfile);
fread(&hdr->numberOfChan, 1, 1, logfile);
fread(&hdr->resolutionBits, 1, 1, logfile);
fread(&hdr->samplingFrequency, 4, 1, logfile);
fread(&hdr->dmaBlockSize, 4, 1, logfile);
fread(&hdr->sizeOfAdditionnalDataBuffer, 4, 1, logfile);
fread(&hdr->numberOfExternalPeripheral, 1, 1, logfile);
fread(&hdr->timeStampOfStart, 4, 1, logfile);
if(verbose){
printf("header size : %d\n", hdr->headerSize);
printf("version number : %d\n", hdr->versionNumber);
printf("number of chans : %d\n", hdr->numberOfChan);
printf("resolutionBits : %d\n", hdr->resolutionBits);
printf("samplingFrequency : %d\n", hdr->samplingFrequency);
printf("dmaBlockSize : %d\n", hdr->dmaBlockSize);
printf("sizeOfAdditionnalDataBuffer : %d\n", hdr->sizeOfAdditionnalDataBuffer);
printf("numberOfExternalPeripheral : %d\n", hdr->numberOfExternalPeripheral);
printf("timeStampOfStart : %d\n", hdr->timeStampOfStart);
}
// load external periph config
for(int i=0; i<hdr->numberOfExternalPeripheral; i++){
fread(&hdr->periphConfig[i].Type, 1, 1, logfile);
fread(&hdr->periphConfig[i].ID, 1, 1, logfile);
fread(&hdr->periphConfig[i].Range, 1, 1, logfile);
fread(&hdr->periphConfig[i].Resolution, 1, 1, logfile);
fread(&hdr->periphConfig[i].Frequency, 2, 1, logfile);
}
return;
}
short int toLittleEndian(short int val){
return val = ((val & 0x00FF)<<8) | ((val & 0xFF00)>>8);
}
void parseMPU(unsigned char* additionnalDataBlock, int size, bool verbose, FILE* mpuFile){
static int maxtimeStamp = 0;
int i, timestamp;
short int trameSize = 31, val; // fixed for now
unsigned char* curData = additionnalDataBlock + 6; // first 6 bytes are for usb device
if(verbose){
printf("MPU Range : %hdG\n", *(curData + 5 + 3));
printf("MPU Resolution : %hd\n", *(curData + 5 + 3 + 1));
printf("MPU Sampling Frequency : %hd\n", *(curData + 5 + 3 + 3));
}
while(curData + trameSize + 6 < additionnalDataBlock + size){
if(!(curData[0]==0xFE && curData[1]==0x0A && curData[2]==0x0A && curData[5]==0x08)){
// skip trame if header is incorrect
curData += trameSize + 6;
continue;
}
curData += 3 + 2; // skip trame header, trame length
timestamp = *((int*) (curData + 9));
timestamp = ((timestamp & 0xFF000000)>>24) | ((timestamp & 0x00FF0000)>>8) | ((timestamp & 0x0000FF00)<<8) | ((timestamp & 0x000000FF)<<24);
if(timestamp > maxtimeStamp){
fprintf(mpuFile, "%d,", timestamp);
//printf("%d\n", timestamp);
// treat payload
for(i=13; i<31; i+=2){
val = *((short int*) (curData + i));
val = ((val & 0x00FF)<<8) | ((val & 0xFF00)>>8);
//printf("curData %x %x %hd", *(curData+i), *(curData + i +1), val);
if(i<29){
fprintf(mpuFile, "%hd,", val);
}else{
fprintf(mpuFile, "%hd\n", val);
}
}
maxtimeStamp = timestamp;
}
curData += trameSize + 1; // shift of trame size + 1 byte of checksum
}
}
int main(int argc, char* argv[]){
if(argc < 2){
printf("Script needs to be called with at least 1 arguments :\n\t input file name (.log file)\n\t(optionnal) output filename (.wav file)\n\t(optionnal) mpu filename (.csv file)\n\t(optionnal) verbose (1 / void)\n");
return 0;
}
HighBlueHeader hdr;
FILE* logfile = fopen(argv[1], "rb");
if(logfile==NULL){
printf("Failed to open input file\n");
return 0;
}
// get file size
fseek(logfile, 0, SEEK_END);
long filesize = ftell(logfile);
if(filesize == 0){
printf("skipped empty file : %s\n", argv[1]);
return 0;
}
fseek(logfile, 0, SEEK_SET);
int verbose = 0;
if(argc==5){
verbose = *argv[4]=='1';
}
// file opened successfully, we read the header
parseLogFileHeader(logfile, &hdr, verbose);
int resolutionBytes = hdr.resolutionBits/8;
long dataBlockSampleSize = hdr.dmaBlockSize / ( hdr.numberOfChan * resolutionBytes);
if(verbose){
printf("file size (bytes) %ld \n", filesize);
printf("dataBlockSampleSize %ld\n", dataBlockSampleSize);
}
if(hdr.resolutionBits!=16 && hdr.resolutionBits!=24 && hdr.resolutionBits!=32){
printf("resolution %d not supported yet sorry\n", hdr.resolutionBits);
return 0;
}
// move to start of the data
fseek(logfile, hdr.headerSize + 4, SEEK_SET);
FILE* wavfile;// open wav file
if(argc>2){
wavfile = fopen(argv[2], "wb");
}else{
strcpy(argv[1] + strlen(argv[1])-3, "wav\0");
wavfile = fopen(argv[1], "wb");
}
if(wavfile==NULL){
printf("Failed to open wav output file\n");
return 0;
}
WaveHeader whdr = WaveHeader_default;
whdr.numChannels = hdr.numberOfChan;
whdr.sampleRate = hdr.samplingFrequency;
whdr.bitsPerSample = hdr.resolutionBits;
whdr.byteRate = whdr.sampleRate * whdr.numChannels * resolutionBytes;
whdr.blockAlign = whdr.numChannels * resolutionBytes;
whdr.chunkSize = 36 + (filesize - hdr.headerSize - 4) / (hdr.dmaBlockSize + hdr.sizeOfAdditionnalDataBuffer) * hdr.numberOfChan * dataBlockSampleSize * resolutionBytes;
whdr.subChunk2Size = whdr.chunkSize-36;
fwrite(&whdr, sizeof(WaveHeader), 1, wavfile);
FILE* mpuFile = NULL; // open mpu file
if(argc>3){
mpuFile = fopen(argv[3], "w+");
if(mpuFile==NULL){
printf("Failed to open mpu output file\n");
return 0;
}
}
// }else{
// strcpy(argv[1] + strlen(argv[1])-4, "_mpu.csv\0");
// mpuFile = fopen(argv[1], "w+");
// argv[1][strlen(argv[1])-8] = '\0';
// }
char* dmaBlock = (char*) malloc(hdr.dmaBlockSize);
char* additionnalDataBlock = (char*) malloc(hdr.sizeOfAdditionnalDataBuffer);
char* samples = (char*) malloc(hdr.numberOfChan * resolutionBytes);
int ichan, isample;
long pos = 0;
bool isFirst = true;
// read each dataBlock
do{
fread(additionnalDataBlock, hdr.sizeOfAdditionnalDataBuffer, 1, logfile);
if(mpuFile != NULL){
parseMPU(additionnalDataBlock, hdr.sizeOfAdditionnalDataBuffer, isFirst && verbose, mpuFile);
isFirst = false;
}
fread(dmaBlock, hdr.dmaBlockSize, 1, logfile);
for(isample=0; isample<dataBlockSampleSize; isample++){
for(ichan=0; ichan<hdr.numberOfChan; ichan++){
memcpy(samples + ichan * resolutionBytes, dmaBlock + (ichan * dataBlockSampleSize + isample) * resolutionBytes, resolutionBytes);
}
fwrite(samples, resolutionBytes, hdr.numberOfChan, wavfile);
}
printf("\r %s : ", argv[1]);
pos = ftell(logfile);
printf(" %ld%%", pos*100/filesize);
}while(pos < filesize - 1);
printf("\r\n");
fclose(wavfile);
fclose(logfile);
if(mpuFile!=NULL){
fclose(mpuFile);
}
return 0;
}