log2wav.c

// 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);
        }
      }
      float ax = ((float) toLittleEndian(*((short int*) (curData + 13)))) / 32756 * 19.62; // resolution +- 2g
      float ay = ((float) toLittleEndian(*((short int*) (curData + 15)))) / 32756 * 19.62;
      float az = ((float) toLittleEndian(*((short int*) (curData + 17)))) / 32756 * 19.62;
      float gx = ((float) toLittleEndian(*((short int*) (curData + 19)))) / 32756 * 250; // resolution +- 255deg/sec
      float gy = ((float) toLittleEndian(*((short int*) (curData + 21)))) / 32756 * 250;
      float gz = ((float) toLittleEndian(*((short int*) (curData + 23)))) / 32756 * 250;
      float mx = ((float) toLittleEndian(*((short int*) (curData + 25)))) / 32756 * 0.15; // utesla 
      float my = ((float) toLittleEndian(*((short int*) (curData + 27)))) / 32756 * (-0.15); // - car magneto oppose a accelero
      float mz = ((float) toLittleEndian(*((short int*) (curData + 29)))) / 32756 * (-0.15); // - car magneto oppose a accelero
      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;  // open mpu file
  if(argc>3){
    mpuFile = fopen(argv[3], "w+");
  }else{
    strcpy(argv[1] + strlen(argv[1])-4, "_mpu.csv\0");
    mpuFile = fopen(argv[1], "w+");
    argv[1][strlen(argv[1])-8] = '\0';
  }
  if(mpuFile==NULL){
    printf("Failed to open mpu output file\n");
    return 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);
    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);
  fclose(mpuFile);
  return 0;
}