diff --git a/CLOCK.CFG b/CLOCK.CFG old mode 100644 new mode 100755 diff --git a/JConfig.CFG b/JConfig.CFG old mode 100644 new mode 100755 diff --git a/RapportInfo2txt b/RapportInfo2txt old mode 100644 new mode 100755 diff --git a/RapportInfo2txt.c b/RapportInfo2txt.c old mode 100644 new mode 100755 diff --git a/log2wav b/log2wav old mode 100644 new mode 100755 index b6e6e5e04bc69b16de4399099cc49083de2f25bf..306bdaeb081e7de54002e9853746a7ec90fad9f5 Binary files a/log2wav and b/log2wav differ diff --git a/log2wav.c b/log2wav.c old mode 100644 new mode 100755 index 5aee4fd67a13f915c7de1681b9f43680f2b6ebf2..725984ee780e5c4f4afb6b194edc45aa27e49c94 --- a/log2wav.c +++ b/log2wav.c @@ -8,139 +8,6 @@ #define MAX_PERIPHERAL 5 //Nombre max de peripheriques externes -#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain -#define twoKiDef (2.0f * 0.0f) // 2 * integral gain -#define PI 3.14159265358979323846 - -// now = micros(); -float twoKp = twoKpDef; // 2 * proportional gain (Kp) -float twoKi = twoKiDef; // 2 * integral gain (Ki) -float q0 = 1.0f; -float q1 = 0.0f; -float q2 = 0.0f; -float q3 = 0.0f; -float integralFBx = 0.0f; -float integralFBy = 0.0f; -float integralFBz = 0.0f; -float anglesComputed = 0; - -void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) -{ - float recipNorm; - float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; - float hx, hy, bx, bz; - float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz; - float halfex, halfey, halfez; - float qa, qb, qc; - - // Convert gyroscope degrees/sec to radians/sec - gx *= 0.0174533f; - gy *= 0.0174533f; - gz *= 0.0174533f; - - // Use IMU algorithm if magnetometer measurement invalid - // (avoids NaN in magnetometer normalisation) - if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { - // updateIMU(gx, gy, gz, ax, ay, az); - return; - } - - // now = micros(); - // // Set integration time by time elapsed since last filter update - // delta_t = ((now - last_update) / 1000000.0f); - // last_update = now; - - // Compute feedback only if accelerometer measurement valid - // (avoids NaN in accelerometer normalisation) - if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { - - // Normalise accelerometer measurement - recipNorm = powf(ax * ax + ay * ay + az * az, -0.5); - ax *= recipNorm; - ay *= recipNorm; - az *= recipNorm; - - // Normalise magnetometer measurement - recipNorm = powf(mx * mx + my * my + mz * mz, -0.5); - mx *= recipNorm; - my *= recipNorm; - mz *= recipNorm; - - // Auxiliary variables to avoid repeated arithmetic - q0q0 = q0 * q0; - q0q1 = q0 * q1; - q0q2 = q0 * q2; - q0q3 = q0 * q3; - q1q1 = q1 * q1; - q1q2 = q1 * q2; - q1q3 = q1 * q3; - q2q2 = q2 * q2; - q2q3 = q2 * q3; - q3q3 = q3 * q3; - - // Reference direction of Earth's magnetic field - hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2)); - hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1)); - bx = sqrtf(hx * hx + hy * hy); - bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2)); - - // Estimated direction of gravity and magnetic field - halfvx = q1q3 - q0q2; - halfvy = q0q1 + q2q3; - halfvz = q0q0 - 0.5f + q3q3; - halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2); - halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3); - halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2); - - // Error is sum of cross product between estimated direction - // and measured direction of field vectors - halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy); - halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz); - halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx); - - // Compute and apply integral feedback if enabled - if(false){ //twoKi > 0.0f) { - // integral error scaled by Ki - integralFBx += twoKi * halfex * 0.01; - integralFBy += twoKi * halfey * 0.01; - integralFBz += twoKi * halfez * 0.01; - gx += integralFBx; // apply integral feedback - gy += integralFBy; - gz += integralFBz; - } else { - integralFBx = 0.0f; // prevent integral windup - integralFBy = 0.0f; - integralFBz = 0.0f; - } - - // Apply proportional feedback - gx += twoKp * halfex; - gy += twoKp * halfey; - gz += twoKp * halfez; - } - - // Integrate rate of change of quaternion - gx *= (0.5f * 0.01); // pre-multiply common factors - gy *= (0.5f * 0.01); - gz *= (0.5f * 0.01); - qa = q0; - qb = q1; - qc = q2; - q0 += (-qb * gx - qc * gy - q3 * gz); - q1 += (qa * gx + qc * gz - q3 * gy); - q2 += (qa * gy - qb * gz + q3 * gx); - q3 += (qa * gz + qb * gy - qc * gx); - - // Normalise quaternion - recipNorm = powf(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3, -0.5); - q0 *= recipNorm; - q1 *= recipNorm; - q2 *= recipNorm; - q3 *= recipNorm; - anglesComputed = 0; -} - - typedef struct{ char Type; //type du peripherique (0x01 accel, 0x02 gyro, 0x03 magneto, 0x04 temperature, 0x05 pressure, 0x06 light,...) char ID; //ID du peripherique @@ -245,7 +112,7 @@ void parseMPU(unsigned char* additionnalDataBlock, int size, bool verbose, FILE* val = *((short int*) (curData + i)); val = ((val & 0x00FF)<<8) | ((val & 0xFF00)>>8); //printf("curData %x %x %hd", *(curData+i), *(curData + i +1), val); - if( true){//i<29){ + if(i<29){ fprintf(mpuFile, "%hd,", val); }else{ fprintf(mpuFile, "%hd\n", val); @@ -258,27 +125,17 @@ void parseMPU(unsigned char* additionnalDataBlock, int size, bool verbose, FILE* 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 - // update(ax, ay, az, gx, gy, gz, mx, my, mz); - // fprintf(mpuFile, "%f,", atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2) * 360 / PI); // roll - // fprintf(mpuFile, "%f,", asinf(-2.0f * (q1*q3 - q0*q2)) * 360 / PI); // pitch - // fprintf(mpuFile, "%f\n", atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3) * 360 / PI); // yaw - float pitch = 180 * atan (ax/sqrt(ay*ay + az*az))/PI; - float roll = 180 * atan (ay/sqrt(ax*ax + az*az))/PI; - float yaw = 180 * atan (az/sqrt(ax*ax + az*az))/PI; - fprintf(mpuFile, "%f,", pitch); - fprintf(mpuFile, "%f,", yaw); - fprintf(mpuFile, "%f\n", roll); + 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 < 3){ - printf("Script needs to be called with at least 2 arguments : \n \t input file name (.log file) \n \t output filename (.wav file)\n \t (optionnal) verbose (1 / void)\n"); + 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; @@ -306,7 +163,14 @@ int main(int argc, char* argv[]){ printf("resolution %d not supported yet sorry\n", hdr.resolutionBits); return 0; } - FILE* wavfile = fopen(argv[2], "wb"); // open wav file + + 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; @@ -321,7 +185,14 @@ int main(int argc, char* argv[]){ whdr.subChunk2Size = whdr.chunkSize-36; fwrite(&whdr, sizeof(WaveHeader), 1, wavfile); - FILE* mpuFile = fopen(argv[3], "w+"); // open mpu file + 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; @@ -345,7 +216,7 @@ int main(int argc, char* argv[]){ } fwrite(samples, resolutionBytes, hdr.numberOfChan, wavfile); } - printf("\r %s : ", argv[2]); + printf("\r %s : ", argv[1]); pos = ftell(logfile); printf(" %ld%%", pos*100/filesize); }while(pos < filesize - 1); diff --git a/plot_mpu.py b/plot_mpu.py old mode 100644 new mode 100755 index e3defd70b846f2af4c1d78708c0e90fd7ef0954a..de164d64fc054a2990ed48b17f1c56352ceb95d3 --- a/plot_mpu.py +++ b/plot_mpu.py @@ -5,11 +5,13 @@ import os time, accelX, accelY, accelZ, gyroX, gyroY, gyroZ, magX, magY, magZ = [], [], [], [], [], [], [], [], [], [] -for f in os.listdir('./test/'): +for f in os.listdir('./'): if not f.endswith('mpu.csv'): continue print(f) - a = np.loadtxt('./test/'+f, delimiter=',') + plt.figure() + plt.title(f) + a = np.loadtxt('./'+f, delimiter=',') time.extend(a[:,0]) accelX.extend(a[:,1]) accelY.extend(a[:,2]) @@ -17,9 +19,9 @@ for f in os.listdir('./test/'): gyroX.extend(a[:,4]) gyroY.extend(a[:,5]) gyroZ.extend(a[:,6]) - magX.extend(a[:,10]) - magY.extend(a[:,11]) - magZ.extend(a[:,12]) + magX.extend(a[:,7]) + magY.extend(a[:,8]) + magZ.extend(a[:,9]) plt.scatter(time, magX, label='x') plt.scatter(time, magY, label='y') diff --git a/trame IMU.txt b/trame IMU.txt old mode 100644 new mode 100755