Project Football
as
Dependencies: CommonTypes ESC Matrix PID Servo kalman mbed-rtos mbed
Fork of
Nucleo_MPU_9250_
by 
Alan Huchin Herrera
Revision 0:89cf0851969b, committed 2018-06-26
- Comitter:
- AlanHuchin
- Date:
- Tue Jun 26 18:24:45 2018 +0000
- Commit message:
- hello
Changed in this revision
diff -r 000000000000 -r 89cf0851969b CID10DOF/CID10DOF.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/CID10DOF/CID10DOF.cpp Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,628 @@
+#include "CID10DOF.h"
+#include <math.h>
+#include "rtos.h"
+#include "Matrix.h"
+#include "Servo.h"
+#include "PIDcontroller.h"
+
+#ifndef min
+#define min(a,b) ( (a) < (b) ? (a) : (b) )
+#endif
+
+#ifndef max
+#define max(a,b) ( (a) > (b) ? (a) : (b) )
+#endif
+
+PID PID_ROLL(kp, ki, kd, Td);
+PID PID_PITCH(kp, ki, kd, Td);
+
+FIS_TYPE g_fisInput[fis_gcI];
+FIS_TYPE g_fisOutput[fis_gcO];
+
+
+CID10DOF::CID10DOF(PinName sda, PinName scl,PinName FL, PinName FR, PinName BL, PinName BR) : mpu9250(sda,scl),pc(USBTX,USBRX),telem(PC_0, PC_1)
+{
+
+ pc.baud(115200);
+ telem.baud(57600);
+
+ motor[0] = new Servo (FL); //motors are of class Servo as ESC are used in the similar manner
+ motor[1] = new Servo (FR);
+ motor[2] = new Servo (BL);
+ motor[3] = new Servo (BR);
+
+ min_calibrate = 0.0;
+ max_calibrate = 1.0;
+
+ e = 0;
+ last_e = 0;
+ l = 1.0f;
+
+
+}
+
+/**
+ * Initialize the FreeIMU I2C bus, sensors and performs gyro offsets calibration
+*/
+
+void CID10DOF::MPU9250init()
+{
+
+ uint8_t whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
+ pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
+
+ if (whoami == 0x71) // WHO_AM_I should always be 0x68
+ {
+
+ mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
+ mpu9250.MPU9250SelfTest(SelfTest);
+ mpu9250.getAres(); // Get accelerometer sensitivity
+ mpu9250.getGres(); // Get gyro sensitivity
+ mpu9250.getMres(); // Get magnetometer sensitivity
+ mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
+ wait(2);
+
+ mpu9250.initMPU9250();
+ pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+ wait(1);
+
+ mpu9250.initAK8963(magCalibration);
+ pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+ pc.printf("Accelerometer full-scale range = %f g\n\r", 2.0f*(float)(1<<Ascale));
+ pc.printf("Gyroscope full-scale range = %f deg/s\n\r", 250.0f*(float)(1<<Gscale));
+
+ if(Mscale == 0) pc.printf("Magnetometer resolution = 14 bits\n\r");
+ if(Mscale == 1) pc.printf("Magnetometer resolution = 16 bits\n\r");
+ if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
+ if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
+
+ pc.printf("Mag Calibration: Wave device in a figure eight until done!");
+ wait(4);
+
+ magbias[0] = -41.563381;
+ magbias[1] = 165.252426;
+ magbias[2] = 55.095879;
+
+ magScale[0] = 0.866279;
+ magScale[1] = 1.087591;
+ magScale[2] = 1.079710;
+
+ //mpu9250.magcalMPU9250(magbias, magScale);
+ pc.printf("Mag Calibration done!\n\r");
+ pc.printf("x mag bias = %f\n\r", magbias[0]);
+ pc.printf("y mag bias = %f\n\r", magbias[1]);
+ pc.printf("z mag bias = %f\n\r", magbias[2]);
+ //pc.printf("Mag Calibration done!\n\r");
+ pc.printf("x mag Scale = %f\n\r", magScale[0]);
+ pc.printf("y mag Scale = %f\n\r", magScale[1]);
+ pc.printf("z mag Scale = %f\n\r", magScale[2]);
+ wait(2);
+
+
+ }else{
+
+ while(1) ; // Loop forever if communication doesn't happen
+ }
+
+ mpu9250.getAres(); // Get accelerometer sensitivity
+ mpu9250.getGres(); // Get gyro sensitivity
+ mpu9250.getMres(); // Get magnetometer sensitivity
+
+ t.start();
+
+}
+
+void CID10DOF::MPU9250GetValues()
+{
+ mpu9250.readAccelData(accelCount);
+
+ ax = (float)accelCount[0]*aRes - accelBias[0];
+ ay = (float)accelCount[1]*aRes - accelBias[1];
+ az = (float)accelCount[2]*aRes - accelBias[2];
+
+ mpu9250.readGyroData(gyroCount);
+
+ gx = (float)gyroCount[0]*gRes - gyroBias[0];
+ gy = (float)gyroCount[1]*gRes - gyroBias[1];
+ gz = (float)gyroCount[2]*gRes - gyroBias[2];
+
+ mpu9250.readMagData(magCount);
+
+ mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];
+ my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
+ mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
+ mx *= magScale[0]; // poor man's soft iron calibration
+ my *= magScale[1];
+ mz *= magScale[2];
+
+ tempCount = mpu9250.readTempData();
+ temperature = ((float) tempCount) / 333.87f + 21.0f;
+}
+
+void CID10DOF::MPU9250getYawPitchRoll()
+{
+ yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+ pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+ roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+
+ //float Xh = mx*cos(pitch)+my*sin(roll)*sin(pitch)-mz*cos(roll)*sin(pitch);
+ //float Yh = my*cos(roll)+mz*sin(roll);
+
+ //yawmag = atan2(Yh,Xh)+PI;
+
+ pitch *= 180.0f / PI;
+ yaw *= 180.0f / PI;
+ yaw -= 4.28; //
+ if(yaw < 0) yaw += 360.0f;
+ roll *= 180.0f / PI;
+
+}
+
+void CID10DOF::MPU9250ReadAxis(double * dat)
+{
+
+ if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {
+ MPU9250GetValues();
+ }
+
+ Now = t.read_us();
+ deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+ lastUpdate = Now;
+
+ sum += deltat;
+ sumCount++;
+
+
+
+ mpu9250.MahonyQuaternionUpdate( ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
+
+ delt_t = t.read_ms() - count;
+
+ if (delt_t > 50) {
+ MPU9250getYawPitchRoll();
+ sum = 0;
+ sumCount = 0;
+ dat[0] = yaw;
+ dat[1] = pitch;
+ dat[2] = roll;
+ }
+
+
+}
+
+//----------------------------Function for ESC calibration---------------------
+void CID10DOF::Calibrate_Motors()
+{
+ for (int i = 0; i < 4; i++){ //run motors for some time in min speed
+ *motor[i] = max_calibrate;
+ }
+ wait(6.0); //wait for the response from ESC modules
+ for (int i = 0; i < 4; i++){
+ *motor[i] = min_calibrate; //run motors at maximum speed
+ }
+ wait(2.0);
+}
+
+//--------------------------Function for setting calibration limits-----------
+void CID10DOF::setLimits(double min, double max)
+{
+ if (min > max){ //here detect if someone tried making min to be more than max. If that is the case, then flip them together
+ min_calibrate = max;
+ max_calibrate = min;
+ } else {
+ min_calibrate = min;
+ max_calibrate = max;
+ }
+ if ((min_calibrate > 1.0) || (min_calibrate < 0.0)) //here chech if values are in correct range. If they are not, make them to be in correct range
+ min_calibrate = 0.0;
+ if ((max_calibrate > 1.0) || (max_calibrate < 0.0))
+ max_calibrate = 1.0;
+}
+
+//-------------------------------------Function for Stabilising---------------
+void CID10DOF::stabilise(double *speed, double *actSpeed, double *Diff){
+
+
+}
+
+//-----------------------Function for producing thrust in Z direction --------
+void CID10DOF::run (double *speed){
+ for (int i = 0; i < 4; i++){
+ *motor[i] = speed[i];
+ }
+}
+
+void CID10DOF::PID_Config(double *St_point, double *bias)
+{
+ PID_ROLL.setInputLimits(IN_MIN_ROLL,IN_MAX_ROLL);
+ PID_ROLL.setOutputLimits(OUT_MIN_ROLL, OUT_MAX_ROLL);
+ PID_ROLL.setBias(0.0);
+
+ PID_PITCH.setInputLimits(IN_MIN_PITCH,IN_MAX_PITCH);
+ PID_PITCH.setOutputLimits(OUT_MIN_PITCH, OUT_MAX_PITCH);
+ PID_PITCH.setBias(0.0);
+
+}
+
+void CID10DOF::PID_Run(double *processVariable, double *setPoint,double *pid_diff)
+{
+
+ PID_ROLL.setProcessValue(processVariable[2]);
+ PID_ROLL.setSetPoint(setPoint[1]);
+ pid_diff[2] = PID_ROLL.compute();
+
+ PID_PITCH.setProcessValue(processVariable[1]);
+ PID_PITCH.setSetPoint(setPoint[1]);
+ pid_diff[1] = PID_PITCH.compute();
+
+}
+
+float CID10DOF::FLC_roll(float Phi, float dPhi, float iPhi)
+{
+ g_fisInput[0] = Phi;// Read Input: Phi
+ g_fisInput[1] = dPhi;// Read Input: dPhi
+ g_fisInput[2] = iPhi;// Read Input: iPhi
+ g_fisOutput[0] = 0;
+ fis_evaluate();
+
+ return g_fisOutput[0];
+}
+
+
+
+//***********************************************************************
+// Support functions for Fuzzy Inference System
+//***********************************************************************
+// Trapezoidal Member Function
+FIS_TYPE fis_trapmf(FIS_TYPE x, FIS_TYPE* p)
+{
+ FIS_TYPE a = p[0], b = p[1], c = p[2], d = p[3];
+ FIS_TYPE t1 = ((x <= c) ? 1 : ((d < x) ? 0 : ((c != d) ? ((d - x) / (d - c)) : 0)));
+ FIS_TYPE t2 = ((b <= x) ? 1 : ((x < a) ? 0 : ((a != b) ? ((x - a) / (b - a)) : 0)));
+ return (FIS_TYPE) min(t1, t2);
+}
+
+// Triangular Member Function
+FIS_TYPE fis_trimf(FIS_TYPE x, FIS_TYPE* p)
+{
+ FIS_TYPE a = p[0], b = p[1], c = p[2];
+ FIS_TYPE t1 = (x - a) / (b - a);
+ FIS_TYPE t2 = (c - x) / (c - b);
+ if ((a == b) && (b == c)) return (FIS_TYPE) (x == a);
+ if (a == b) return (FIS_TYPE) (t2*(b <= x)*(x <= c));
+ if (b == c) return (FIS_TYPE) (t1*(a <= x)*(x <= b));
+ t1 = min(t1, t2);
+ return (FIS_TYPE) max(t1, 0);
+}
+
+FIS_TYPE fis_min(FIS_TYPE a, FIS_TYPE b)
+{
+ return min(a, b);
+}
+
+FIS_TYPE fis_max(FIS_TYPE a, FIS_TYPE b)
+{
+ return max(a, b);
+}
+
+FIS_TYPE fis_array_operation(FIS_TYPE *array, int size, _FIS_ARR_OP pfnOp)
+{
+ int i;
+ FIS_TYPE ret = 0;
+
+ if (size == 0) return ret;
+ if (size == 1) return array[0];
+
+ ret = array[0];
+ for (i = 1; i < size; i++)
+ {
+ ret = (*pfnOp)(ret, array[i]);
+ }
+
+ return ret;
+}
+
+
+//***********************************************************************
+// Data for Fuzzy Inference System
+//***********************************************************************
+// Pointers to the implementations of member functions
+_FIS_MF fis_gMF[] =
+{
+ fis_trapmf, fis_trimf
+};
+
+// Count of member function for each Input
+int fis_gIMFCount[] = { 3, 3, 3 };
+
+// Count of member function for each Output
+int fis_gOMFCount[] = { 5 };
+
+// Coefficients for the Input Member Functions
+FIS_TYPE fis_gMFI0Coeff1[] = { -10, -10, -0.2, 0 };
+FIS_TYPE fis_gMFI0Coeff2[] = { -0.1, 0, 0.1 };
+FIS_TYPE fis_gMFI0Coeff3[] = { 0, 0.2, 10, 10 };
+FIS_TYPE* fis_gMFI0Coeff[] = { fis_gMFI0Coeff1, fis_gMFI0Coeff2, fis_gMFI0Coeff3 };
+FIS_TYPE fis_gMFI1Coeff1[] = { -20, -20, -1, -0.25 };
+FIS_TYPE fis_gMFI1Coeff2[] = { -1, 0, 1 };
+FIS_TYPE fis_gMFI1Coeff3[] = { 0.25, 1, 20, 20 };
+FIS_TYPE* fis_gMFI1Coeff[] = { fis_gMFI1Coeff1, fis_gMFI1Coeff2, fis_gMFI1Coeff3 };
+FIS_TYPE fis_gMFI2Coeff1[] = { -20, -20, -0.8, -0.25 };
+FIS_TYPE fis_gMFI2Coeff2[] = { -0.6, 0, 0.6 };
+FIS_TYPE fis_gMFI2Coeff3[] = { 0, 1, 20, 20 };
+FIS_TYPE* fis_gMFI2Coeff[] = { fis_gMFI2Coeff1, fis_gMFI2Coeff2, fis_gMFI2Coeff3 };
+FIS_TYPE** fis_gMFICoeff[] = { fis_gMFI0Coeff, fis_gMFI1Coeff, fis_gMFI2Coeff };
+
+// Coefficients for the Input Member Functions
+FIS_TYPE fis_gMFO0Coeff1[] = { -0.1, -0.1, -0.05, -0.045 };
+FIS_TYPE fis_gMFO0Coeff2[] = { -0.05, -0.045, -0.01, -0.003 };
+FIS_TYPE fis_gMFO0Coeff3[] = { -0.01, 0, 0.01 };
+FIS_TYPE fis_gMFO0Coeff4[] = { 0.003, 0.01, 0.045, 0.05 };
+FIS_TYPE fis_gMFO0Coeff5[] = { 0.0455, 0.05, 0.1, 0.1 };
+FIS_TYPE* fis_gMFO0Coeff[] = { fis_gMFO0Coeff1, fis_gMFO0Coeff2, fis_gMFO0Coeff3, fis_gMFO0Coeff4, fis_gMFO0Coeff5 };
+FIS_TYPE** fis_gMFOCoeff[] = { fis_gMFO0Coeff };
+
+// Input membership function set
+int fis_gMFI0[] = { 0, 1, 0 };
+int fis_gMFI1[] = { 0, 1, 0 };
+int fis_gMFI2[] = { 0, 1, 0 };
+int* fis_gMFI[] = { fis_gMFI0, fis_gMFI1, fis_gMFI2};
+
+// Output membership function set
+int fis_gMFO0[] = { 0, 0, 1, 0, 0 };
+int* fis_gMFO[] = { fis_gMFO0};
+
+// Rule Weights
+FIS_TYPE fis_gRWeight[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };
+
+// Rule Type
+int fis_gRType[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 };
+
+// Rule Inputs
+int fis_gRI0[] = { 1, 1, 1 };
+int fis_gRI1[] = { 2, 1, 1 };
+int fis_gRI2[] = { 3, 1, 1 };
+int fis_gRI3[] = { 1, 1, 2 };
+int fis_gRI4[] = { 2, 1, 2 };
+int fis_gRI5[] = { 3, 1, 2 };
+int fis_gRI6[] = { 1, 1, 3 };
+int fis_gRI7[] = { 2, 1, 3 };
+int fis_gRI8[] = { 3, 1, 3 };
+int fis_gRI9[] = { 1, 2, 1 };
+int fis_gRI10[] = { 2, 2, 1 };
+int fis_gRI11[] = { 3, 2, 1 };
+int fis_gRI12[] = { 1, 2, 2 };
+int fis_gRI13[] = { 2, 2, 2 };
+int fis_gRI14[] = { 3, 2, 2 };
+int fis_gRI15[] = { 1, 2, 3 };
+int fis_gRI16[] = { 2, 2, 3 };
+int fis_gRI17[] = { 3, 2, 3 };
+int fis_gRI18[] = { 1, 3, 1 };
+int fis_gRI19[] = { 2, 3, 1 };
+int fis_gRI20[] = { 3, 3, 1 };
+int fis_gRI21[] = { 1, 3, 2 };
+int fis_gRI22[] = { 2, 3, 2 };
+int fis_gRI23[] = { 3, 3, 2 };
+int fis_gRI24[] = { 1, 3, 3 };
+int fis_gRI25[] = { 2, 3, 3 };
+int fis_gRI26[] = { 3, 3, 3 };
+int* fis_gRI[] = { fis_gRI0, fis_gRI1, fis_gRI2, fis_gRI3, fis_gRI4, fis_gRI5, fis_gRI6, fis_gRI7, fis_gRI8, fis_gRI9, fis_gRI10, fis_gRI11, fis_gRI12, fis_gRI13, fis_gRI14, fis_gRI15, fis_gRI16, fis_gRI17, fis_gRI18, fis_gRI19, fis_gRI20, fis_gRI21, fis_gRI22, fis_gRI23, fis_gRI24, fis_gRI25, fis_gRI26 };
+
+// Rule Outputs
+int fis_gRO0[] = { 5 };
+int fis_gRO1[] = { 4 };
+int fis_gRO2[] = { 2 };
+int fis_gRO3[] = { 5 };
+int fis_gRO4[] = { 4 };
+int fis_gRO5[] = { 2 };
+int fis_gRO6[] = { 5 };
+int fis_gRO7[] = { 4 };
+int fis_gRO8[] = { 2 };
+int fis_gRO9[] = { 4 };
+int fis_gRO10[] = { 4 };
+int fis_gRO11[] = { 2 };
+int fis_gRO12[] = { 4 };
+int fis_gRO13[] = { 3 };
+int fis_gRO14[] = { 2 };
+int fis_gRO15[] = { 4 };
+int fis_gRO16[] = { 2 };
+int fis_gRO17[] = { 2 };
+int fis_gRO18[] = { 4 };
+int fis_gRO19[] = { 2 };
+int fis_gRO20[] = { 1 };
+int fis_gRO21[] = { 4 };
+int fis_gRO22[] = { 2 };
+int fis_gRO23[] = { 1 };
+int fis_gRO24[] = { 4 };
+int fis_gRO25[] = { 2 };
+int fis_gRO26[] = { 1 };
+int* fis_gRO[] = { fis_gRO0, fis_gRO1, fis_gRO2, fis_gRO3, fis_gRO4, fis_gRO5, fis_gRO6, fis_gRO7, fis_gRO8, fis_gRO9, fis_gRO10, fis_gRO11, fis_gRO12, fis_gRO13, fis_gRO14, fis_gRO15, fis_gRO16, fis_gRO17, fis_gRO18, fis_gRO19, fis_gRO20, fis_gRO21, fis_gRO22, fis_gRO23, fis_gRO24, fis_gRO25, fis_gRO26 };
+
+// Input range Min
+FIS_TYPE fis_gIMin[] = { -10, -20, -20 };
+
+// Input range Max
+FIS_TYPE fis_gIMax[] = { 10, 20, 20 };
+
+// Output range Min
+FIS_TYPE fis_gOMin[] = { -0.1 };
+
+// Output range Max
+FIS_TYPE fis_gOMax[] = { 0.1 };
+
+//***********************************************************************
+// Data dependent support functions for Fuzzy Inference System
+//***********************************************************************
+FIS_TYPE fis_MF_out(FIS_TYPE** fuzzyRuleSet, FIS_TYPE x, int o)
+{
+ FIS_TYPE mfOut;
+ int r;
+
+ for (r = 0; r < fis_gcR; ++r)
+ {
+ int index = fis_gRO[r][o];
+ if (index > 0)
+ {
+ index = index - 1;
+ mfOut = (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
+ }
+ else if (index < 0)
+ {
+ index = -index - 1;
+ mfOut = 1 - (fis_gMF[fis_gMFO[o][index]])(x, fis_gMFOCoeff[o][index]);
+ }
+ else
+ {
+ mfOut = 0;
+ }
+
+ fuzzyRuleSet[0][r] = fis_min(mfOut, fuzzyRuleSet[1][r]);
+ }
+ return fis_array_operation(fuzzyRuleSet[0], fis_gcR, fis_max);
+}
+
+FIS_TYPE fis_defuzz_centroid(FIS_TYPE** fuzzyRuleSet, int o)
+{
+ FIS_TYPE step = (fis_gOMax[o] - fis_gOMin[o]) / (FIS_RESOLUSION - 1);
+ FIS_TYPE area = 0;
+ FIS_TYPE momentum = 0;
+ FIS_TYPE dist, slice;
+ int i;
+
+ // calculate the area under the curve formed by the MF outputs
+ for (i = 0; i < FIS_RESOLUSION; ++i){
+ dist = fis_gOMin[o] + (step * i);
+ slice = step * fis_MF_out(fuzzyRuleSet, dist, o);
+ area += slice;
+ momentum += slice*dist;
+ }
+
+ return ((area == 0) ? ((fis_gOMax[o] + fis_gOMin[o]) / 2) : (momentum / area));
+}
+
+//***********************************************************************
+// Fuzzy Inference System
+//***********************************************************************
+void CID10DOF::fis_evaluate()
+{
+ FIS_TYPE fuzzyInput0[] = { 0, 0, 0 };
+ FIS_TYPE fuzzyInput1[] = { 0, 0, 0 };
+ FIS_TYPE fuzzyInput2[] = { 0, 0, 0 };
+ FIS_TYPE* fuzzyInput[fis_gcI] = { fuzzyInput0, fuzzyInput1, fuzzyInput2, };
+ FIS_TYPE fuzzyOutput0[] = { 0, 0, 0, 0, 0 };
+ FIS_TYPE* fuzzyOutput[fis_gcO] = { fuzzyOutput0, };
+ FIS_TYPE fuzzyRules[fis_gcR] = { 0 };
+ FIS_TYPE fuzzyFires[fis_gcR] = { 0 };
+ FIS_TYPE* fuzzyRuleSet[] = { fuzzyRules, fuzzyFires };
+ FIS_TYPE sW = 0;
+
+ // Transforming input to fuzzy Input
+ int i, j, r, o;
+ for (i = 0; i < fis_gcI; ++i)
+ {
+ for (j = 0; j < fis_gIMFCount[i]; ++j)
+ {
+ fuzzyInput[i][j] =
+ (fis_gMF[fis_gMFI[i][j]])(g_fisInput[i], fis_gMFICoeff[i][j]);
+ }
+ }
+
+ int index = 0;
+ for (r = 0; r < fis_gcR; ++r)
+ {
+ if (fis_gRType[r] == 1)
+ {
+ fuzzyFires[r] = FIS_MAX;
+ for (i = 0; i < fis_gcI; ++i)
+ {
+ index = fis_gRI[r][i];
+ if (index > 0)
+ fuzzyFires[r] = fis_min(fuzzyFires[r], fuzzyInput[i][index - 1]);
+ else if (index < 0)
+ fuzzyFires[r] = fis_min(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
+ else
+ fuzzyFires[r] = fis_min(fuzzyFires[r], 1);
+ }
+ }
+ else
+ {
+ fuzzyFires[r] = FIS_MIN;
+ for (i = 0; i < fis_gcI; ++i)
+ {
+ index = fis_gRI[r][i];
+ if (index > 0)
+ fuzzyFires[r] = fis_max(fuzzyFires[r], fuzzyInput[i][index - 1]);
+ else if (index < 0)
+ fuzzyFires[r] = fis_max(fuzzyFires[r], 1 - fuzzyInput[i][-index - 1]);
+ else
+ fuzzyFires[r] = fis_max(fuzzyFires[r], 0);
+ }
+ }
+
+ fuzzyFires[r] = fis_gRWeight[r] * fuzzyFires[r];
+ sW += fuzzyFires[r];
+ }
+
+ if (sW == 0)
+ {
+ for (o = 0; o < fis_gcO; ++o)
+ {
+ g_fisOutput[o] = ((fis_gOMax[o] + fis_gOMin[o]) / 2);
+ }
+ }
+ else
+ {
+ for (o = 0; o < fis_gcO; ++o)
+ {
+ g_fisOutput[o] = fis_defuzz_centroid(fuzzyRuleSet, o);
+ }
+ }
+}
+
+
+
+
+
+const float def_sea_press = 1013.25;
+
+
+
+float CID10DOF::getBaroTem()
+{
+
+}
+
+float CID10DOF::getBaroPress()
+{
+
+}
+
+float CID10DOF::getBaroAlt()
+{
+
+}
+
+
+float invSqrt(float number)
+{
+ volatile long i;
+ volatile float x, y;
+ volatile const float f = 1.5F;
+
+ x = number * 0.5F;
+ y = number;
+ i = * ( long * ) &y;
+ i = 0x5f375a86 - ( i >> 1 );
+ y = * ( float * ) &i;
+ y = y * ( f - ( x * y * y ) );
+ return y;
+}
+
+double CID10DOF::map(double x, double in_min, double in_max, double out_min, double out_max){ //simply maps values in the given range
+ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+
+
+
+
diff -r 000000000000 -r 89cf0851969b CID10DOF/CID10DOF.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/CID10DOF/CID10DOF.h Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,187 @@
+/*
+FreeIMU.h - A libre and easy to use orientation sensing library for Arduino
+Copyright (C) 2011 Fabio Varesano <fabio at varesano dot net>
+
+Development of this code has been supported by the Department of Computer Science,
+Universita' degli Studi di Torino, Italy within the Piemonte Project
+http://www.piemonte.di.unito.it/
+
+
+This program is free software: you can redistribute it and/or modify
+it under the terms of the version 3 GNU General Public License as
+published by the Free Software Foundation.
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+GNU General Public License for more details.
+
+You should have received a copy of the GNU General Public License
+along with this program. If not, see <http://www.gnu.org/licenses/>.
+
+*/
+
+#ifndef CID10DOF_h
+#define CID10DOF_h
+
+// Uncomment the appropriated version of FreeIMU you are using
+//#define FREEIMU_v01
+//#define FREEIMU_v02
+//#define FREEIMU_v03
+//#define FREEIMU_v035
+//#define FREEIMU_v035_MS
+//#define FREEIMU_v035_BMP
+#define FREEIMU_v04
+
+
+
+#define FREEIMU_LIB_VERSION "20121122"
+
+#define FREEIMU_DEVELOPER "Fabio Varesano - varesano.net"
+
+#if F_CPU == 16000000L
+ #define FREEIMU_FREQ "16 MHz"
+#elif F_CPU == 8000000L
+ #define FREEIMU_FREQ "8 MHz"
+#endif
+
+
+// board IDs
+
+
+#define FREEIMU_ID "Zboub"
+
+#define IS_6DOM() (defined(SEN_10121) || defined(GEN_MPU6050))
+#define IS_9DOM()
+#define HAS_AXIS_ALIGNED() (defined(FREEIMU_v01) || defined(FREEIMU_v02) || defined(FREEIMU_v03) || defined(FREEIMU_v035) || defined(FREEIMU_v035_MS) || defined(FREEIMU_v035_BMP) || defined(FREEIMU_v04) || defined(SEN_10121) || defined(SEN_10736))
+
+
+
+
+#include "mbed.h"
+
+//#include "calibration.h"
+/*
+#ifndef CALIBRATION_H
+#include <EEPROM.h>
+#endif
+#define FREEIMU_EEPROM_BASE 0x0A
+#define FREEIMU_EEPROM_SIGNATURE 0x19
+*/
+
+//include des biblis des senseurs
+
+#include "MPU9250.h"
+#include "Matrix.h"
+#include "Servo.h"
+#include "PIDcontroller.h"
+
+//ESC calibration values
+#define MAX_TRHUST 1.0
+#define MIN_TRHUST 0.45
+
+#define IN_MIN_ROLL -30.0
+#define IN_MAX_ROLL 30.0
+#define OUT_MIN_ROLL -0.1
+#define OUT_MAX_ROLL 1.0
+
+#define IN_MIN_PITCH -30.0
+#define IN_MAX_PITCH 30.0
+#define OUT_MIN_PITCH -0.1
+#define OUT_MAX_PITCH 0.1
+
+#define kp 0.69 //0.82
+#define ki 0.0 //0.01
+#define kd 0.130//0.17245
+#define Td 0.01
+
+#define FIS_TYPE float
+#define FIS_RESOLUSION 101
+#define FIS_MIN -3.4028235E+38
+#define FIS_MAX 3.4028235E+38
+
+typedef FIS_TYPE(*_FIS_MF)(FIS_TYPE, FIS_TYPE*);
+typedef FIS_TYPE(*_FIS_ARR_OP)(FIS_TYPE, FIS_TYPE);
+typedef FIS_TYPE(*_FIS_ARR)(FIS_TYPE*, int, _FIS_ARR_OP);
+
+
+
+class CID10DOF
+{
+ public:
+ CID10DOF(PinName sda, PinName scl,PinName FL, PinName FR, PinName BL, PinName BR);
+ //I2C i2c_;
+
+ //void init();
+ //void init(int accgyro_addr, bool fastmode);
+ void MPU9250init();
+ void MatrixConfig();
+ #ifndef CALIBRATION_H
+ void calLoad();
+ #endif
+ void getRawValues(int * raw_values);
+ void MPU9250GetValues();
+ void MPU9250getYawPitchRoll();
+ void MPU9250ReadAxis(double * dat);
+
+ void Calibrate_Motors();
+ void stabilise(double *speed, double *actSpeed, double *Diff);
+ void setLimits(double min, double max);
+ void run (double *speed);
+ void fis_evaluate();
+
+ //PID
+ void PID_Config(double *St_point, double *bias);
+ void PID_Run(double *processVariable, double *setPoint, double *pid_diff);
+
+ float FLC_roll(float Phi, float iPhi, float dPhi);
+ //float FLC_Roll( float setpoint, float roll_ang, float Ts, float gain);
+
+ //Sensor de presión y temperatura
+ float getBaroTem();
+ float getBaroPress();
+ float getBaroAlt();
+
+ double map(double x, double in_min, double in_max, double out_min, double out_max);
+
+ // we make them public so that users can interact directly with device classes
+
+ MPU9250 mpu9250;
+ Serial pc;
+ Serial telem;
+
+ double inMin_, inMax_, inSpan_;
+ double outMin_, outMax_, outSpan_;
+
+ int16_t accelCount[3];
+ int16_t gyroCount[3];
+ int16_t magCount[3];
+ uint32_t sumCount;
+ int16_t tempCount;
+ float edv, e_ant, P, I, D, PID_fuzz;
+ float temperature, sum;
+ double value, outputs[3];
+ float yawmag,l;
+ double last_e;
+ float e;
+ char inData[50],byteIn;
+
+ Servo* motor[4];
+
+ private:
+ Timer t;
+ int dt_us;
+
+ float min_calibrate; //min value at which each motor is calibrated
+ float max_calibrate; //max value ...
+
+};
+ const int fis_gcI = 3;
+ // Number of outputs to the fuzzy inference system
+ const int fis_gcO = 1;
+ // Number of rules to the fuzzy inference system
+ const int fis_gcR = 27;
+
+ float invSqrt(float number);
+ void arr3_rad_to_deg(float * arr);
+#endif // FreeIMU_h
\ No newline at end of file
diff -r 000000000000 -r 89cf0851969b CID10DOF/helper_3dmath.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/CID10DOF/helper_3dmath.h Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,216 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
+// 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+// 2012-06-05 - add 3D math helper file to DMP6 example sketch
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2012 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#ifndef _HELPER_3DMATH_H_
+#define _HELPER_3DMATH_H_
+
+class Quaternion {
+ public:
+ float w;
+ float x;
+ float y;
+ float z;
+
+ Quaternion() {
+ w = 1.0f;
+ x = 0.0f;
+ y = 0.0f;
+ z = 0.0f;
+ }
+
+ Quaternion(float nw, float nx, float ny, float nz) {
+ w = nw;
+ x = nx;
+ y = ny;
+ z = nz;
+ }
+
+ Quaternion getProduct(Quaternion q) {
+ // Quaternion multiplication is defined by:
+ // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
+ // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
+ // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
+ // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
+ return Quaternion(
+ w*q.w - x*q.x - y*q.y - z*q.z, // new w
+ w*q.x + x*q.w + y*q.z - z*q.y, // new x
+ w*q.y - x*q.z + y*q.w + z*q.x, // new y
+ w*q.z + x*q.y - y*q.x + z*q.w); // new z
+ }
+
+ Quaternion getConjugate() {
+ return Quaternion(w, -x, -y, -z);
+ }
+
+ float getMagnitude() {
+ return sqrt(w*w + x*x + y*y + z*z);
+ }
+
+ void normalize() {
+ float m = getMagnitude();
+ w /= m;
+ x /= m;
+ y /= m;
+ z /= m;
+ }
+
+ Quaternion getNormalized() {
+ Quaternion r(w, x, y, z);
+ r.normalize();
+ return r;
+ }
+};
+
+class VectorInt16 {
+ public:
+ int16_t x;
+ int16_t y;
+ int16_t z;
+
+ VectorInt16() {
+ x = 0;
+ y = 0;
+ z = 0;
+ }
+
+ VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
+ x = nx;
+ y = ny;
+ z = nz;
+ }
+
+ float getMagnitude() {
+ return sqrt((float)(x*x + y*y + z*z));
+ }
+
+ void normalize() {
+ float m = getMagnitude();
+ x /= m;
+ y /= m;
+ z /= m;
+ }
+
+ VectorInt16 getNormalized() {
+ VectorInt16 r(x, y, z);
+ r.normalize();
+ return r;
+ }
+
+ void rotate(Quaternion *q) {
+ // http://www.cprogramming.com/tutorial/3d/quaternions.html
+ // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
+ // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
+ // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
+
+ // P_out = q * P_in * conj(q)
+ // - P_out is the output vector
+ // - q is the orientation quaternion
+ // - P_in is the input vector (a*aReal)
+ // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
+ Quaternion p(0, x, y, z);
+
+ // quaternion multiplication: q * p, stored back in p
+ p = q -> getProduct(p);
+
+ // quaternion multiplication: p * conj(q), stored back in p
+ p = p.getProduct(q -> getConjugate());
+
+ // p quaternion is now [0, x', y', z']
+ x = p.x;
+ y = p.y;
+ z = p.z;
+ }
+
+ VectorInt16 getRotated(Quaternion *q) {
+ VectorInt16 r(x, y, z);
+ r.rotate(q);
+ return r;
+ }
+};
+
+class VectorFloat {
+ public:
+ float x;
+ float y;
+ float z;
+
+ VectorFloat() {
+ x = 0;
+ y = 0;
+ z = 0;
+ }
+
+ VectorFloat(float nx, float ny, float nz) {
+ x = nx;
+ y = ny;
+ z = nz;
+ }
+
+ float getMagnitude() {
+ return sqrt(x*x + y*y + z*z);
+ }
+
+ void normalize() {
+ float m = getMagnitude();
+ x /= m;
+ y /= m;
+ z /= m;
+ }
+
+ VectorFloat getNormalized() {
+ VectorFloat r(x, y, z);
+ r.normalize();
+ return r;
+ }
+
+ void rotate(Quaternion *q) {
+ Quaternion p(0, x, y, z);
+
+ // quaternion multiplication: q * p, stored back in p
+ p = q -> getProduct(p);
+
+ // quaternion multiplication: p * conj(q), stored back in p
+ p = p.getProduct(q -> getConjugate());
+
+ // p quaternion is now [0, x', y', z']
+ x = p.x;
+ y = p.y;
+ z = p.z;
+ }
+
+ VectorFloat getRotated(Quaternion *q) {
+ VectorFloat r(x, y, z);
+ r.rotate(q);
+ return r;
+ }
+};
+
+#endif /* _HELPER_3DMATH_H_ */
diff -r 000000000000 -r 89cf0851969b CID10DOF/vector_math.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/CID10DOF/vector_math.h Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,1116 @@
+/*
+Copyright (c) 2007, Markus Trenkwalder
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice,
+ this list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+* Neither the name of the library's copyright owner nor the names of its
+ contributors may be used to endorse or promote products derived from this
+ software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
+CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef VECTOR_MATH_H
+#define VECTOR_MATH_H
+
+//#include <cmath>
+
+// "minor" can be defined from GCC and can cause problems
+#undef minor
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+namespace vmath {
+
+//using std::sin;
+//using std::cos;
+//using std::acos;
+//using std::sqrt;
+
+template <typename T>
+inline T rsqrt(T x)
+{
+ return T(1) / sqrt(x);
+}
+
+template <typename T>
+inline T inv(T x)
+{
+ return T(1) / x;
+}
+
+namespace detail {
+ // This function is used heavily in this library. Here is a generic
+ // implementation for it. If you can provide a faster one for your specific
+ // types this can speed up things considerably.
+ template <typename T>
+ inline T multiply_accumulate(int count, const T *a, const T *b)
+ {
+ T result = T(0);
+ for (int i = 0; i < count; ++i)
+ result += a[i] * b[i];
+ return result;
+ }
+}
+
+#define MOP_M_CLASS_TEMPLATE(CLASS, OP, COUNT) \
+ CLASS & operator OP (const CLASS& rhs) \
+ { \
+ for (int i = 0; i < (COUNT); ++i ) \
+ (*this)[i] OP rhs[i]; \
+ return *this; \
+ }
+
+#define MOP_M_TYPE_TEMPLATE(CLASS, OP, COUNT) \
+ CLASS & operator OP (const T & rhs) \
+ { \
+ for (int i = 0; i < (COUNT); ++i ) \
+ (*this)[i] OP rhs; \
+ return *this; \
+ }
+
+#define MOP_COMP_TEMPLATE(CLASS, COUNT) \
+ bool operator == (const CLASS & rhs) \
+ { \
+ bool result = true; \
+ for (int i = 0; i < (COUNT); ++i) \
+ result = result && (*this)[i] == rhs[i]; \
+ return result; \
+ } \
+ bool operator != (const CLASS & rhs) \
+ { return !((*this) == rhs); }
+
+#define MOP_G_UMINUS_TEMPLATE(CLASS, COUNT) \
+ CLASS operator - () const \
+ { \
+ CLASS result; \
+ for (int i = 0; i < (COUNT); ++i) \
+ result[i] = -(*this)[i]; \
+ return result; \
+ }
+
+#define COMMON_OPERATORS(CLASS, COUNT) \
+ MOP_M_CLASS_TEMPLATE(CLASS, +=, COUNT) \
+ MOP_M_CLASS_TEMPLATE(CLASS, -=, COUNT) \
+ /*no *= as this is not the same for vectors and matrices */ \
+ MOP_M_CLASS_TEMPLATE(CLASS, /=, COUNT) \
+ MOP_M_TYPE_TEMPLATE(CLASS, +=, COUNT) \
+ MOP_M_TYPE_TEMPLATE(CLASS, -=, COUNT) \
+ MOP_M_TYPE_TEMPLATE(CLASS, *=, COUNT) \
+ MOP_M_TYPE_TEMPLATE(CLASS, /=, COUNT) \
+ MOP_G_UMINUS_TEMPLATE(CLASS, COUNT) \
+ MOP_COMP_TEMPLATE(CLASS, COUNT)
+
+#define VECTOR_COMMON(CLASS, COUNT) \
+ COMMON_OPERATORS(CLASS, COUNT) \
+ MOP_M_CLASS_TEMPLATE(CLASS, *=, COUNT) \
+ operator const T* () const { return &x; } \
+ operator T* () { return &x; }
+
+#define FOP_G_SOURCE_TEMPLATE(OP, CLASS) \
+ { CLASS<T> r = lhs; r OP##= rhs; return r; }
+
+#define FOP_G_CLASS_TEMPLATE(OP, CLASS) \
+ template <typename T> \
+ inline CLASS<T> operator OP (const CLASS<T> &lhs, const CLASS<T> &rhs) \
+ FOP_G_SOURCE_TEMPLATE(OP, CLASS)
+
+#define FOP_G_TYPE_TEMPLATE(OP, CLASS) \
+ template <typename T> \
+ inline CLASS<T> operator OP (const CLASS<T> &lhs, const T &rhs) \
+ FOP_G_SOURCE_TEMPLATE(OP, CLASS)
+
+// forward declarations
+template <typename T> struct vec2;
+template <typename T> struct vec3;
+template <typename T> struct vec4;
+template <typename T> struct mat2;
+template <typename T> struct mat3;
+template <typename T> struct mat4;
+template <typename T> struct quat;
+
+#define FREE_MODIFYING_OPERATORS(CLASS) \
+ FOP_G_CLASS_TEMPLATE(+, CLASS) \
+ FOP_G_CLASS_TEMPLATE(-, CLASS) \
+ FOP_G_CLASS_TEMPLATE(*, CLASS) \
+ FOP_G_CLASS_TEMPLATE(/, CLASS) \
+ FOP_G_TYPE_TEMPLATE(+, CLASS) \
+ FOP_G_TYPE_TEMPLATE(-, CLASS) \
+ FOP_G_TYPE_TEMPLATE(*, CLASS) \
+ FOP_G_TYPE_TEMPLATE(/, CLASS)
+
+FREE_MODIFYING_OPERATORS(vec2)
+FREE_MODIFYING_OPERATORS(vec3)
+FREE_MODIFYING_OPERATORS(vec4)
+FREE_MODIFYING_OPERATORS(mat2)
+FREE_MODIFYING_OPERATORS(mat3)
+FREE_MODIFYING_OPERATORS(mat4)
+FREE_MODIFYING_OPERATORS(quat)
+
+#define FREE_OPERATORS(CLASS) \
+ template <typename T> \
+ inline CLASS<T> operator + (const T& a, const CLASS<T>& b) \
+ { CLASS<T> r = b; r += a; return r; } \
+ \
+ template <typename T> \
+ inline CLASS<T> operator * (const T& a, const CLASS<T>& b) \
+ { CLASS<T> r = b; r *= a; return r; } \
+ \
+ template <typename T> \
+ inline CLASS<T> operator - (const T& a, const CLASS<T>& b) \
+ { return -b + a; } \
+ \
+ template <typename T> \
+ inline CLASS<T> operator / (const T& a, const CLASS<T>& b) \
+ { CLASS<T> r(a); r /= b; return r; }
+
+FREE_OPERATORS(vec2)
+FREE_OPERATORS(vec3)
+FREE_OPERATORS(vec4)
+FREE_OPERATORS(mat2)
+FREE_OPERATORS(mat3)
+FREE_OPERATORS(mat4)
+FREE_OPERATORS(quat)
+
+template <typename T>
+struct vec2 {
+ T x, y;
+
+ vec2() {};
+ explicit vec2(const T i) : x(i), y(i) {}
+ explicit vec2(const T ix, const T iy) : x(ix), y(iy) {}
+ explicit vec2(const vec3<T>& v);
+ explicit vec2(const vec4<T>& v);
+
+ VECTOR_COMMON(vec2, 2)
+};
+
+template <typename T>
+struct vec3 {
+ T x, y, z;
+
+ vec3() {};
+ explicit vec3(const T i) : x(i), y(i), z(i) {}
+ explicit vec3(const T ix, const T iy, const T iz) : x(ix), y(iy), z(iz) {}
+ explicit vec3(const vec2<T>& xy, const T iz) : x(xy.x), y(xy.y), z(iz) {}
+ explicit vec3(const T ix, const vec2<T>& yz) : x(ix), y(yz.y), z(yz.z) {}
+ explicit vec3(const vec4<T>& v);
+
+ VECTOR_COMMON(vec3, 3)
+};
+
+template <typename T>
+struct vec4 {
+ T x, y, z, w;
+
+ vec4() {};
+ explicit vec4(const T i) : x(i), y(i), z(i), w(i) {}
+ explicit vec4(const T ix, const T iy, const T iz, const T iw) : x(ix), y(iy), z(iz), w(iw) {}
+ explicit vec4(const vec3<T>& xyz,const T iw) : x(xyz.x), y(xyz.y), z(xyz.z), w(iw) {}
+ explicit vec4(const T ix, const vec3<T>& yzw) : x(ix), y(yzw.x), z(yzw.y), w(yzw.z) {}
+ explicit vec4(const vec2<T>& xy, const vec2<T>& zw) : x(xy.x), y(xy.y), z(zw.x), w(zw.y) {}
+
+ VECTOR_COMMON(vec4, 4)
+};
+
+// additional constructors that omit the last element
+template <typename T> inline vec2<T>::vec2(const vec3<T>& v) : x(v.x), y(v.y) {}
+template <typename T> inline vec2<T>::vec2(const vec4<T>& v) : x(v.x), y(v.y) {}
+template <typename T> inline vec3<T>::vec3(const vec4<T>& v) : x(v.x), y(v.y), z(v.z) {}
+
+#define VEC_QUAT_FUNC_TEMPLATE(CLASS, COUNT) \
+ template <typename T> \
+ inline T dot(const CLASS & u, const CLASS & v) \
+ { \
+ const T *a = u; \
+ const T *b = v; \
+ using namespace detail; \
+ return multiply_accumulate(COUNT, a, b); \
+ } \
+ template <typename T> \
+ inline T length(const CLASS & v) \
+ { \
+ return sqrt(dot(v, v)); \
+ } \
+ template <typename T> inline CLASS normalize(const CLASS & v) \
+ { \
+ return v * rsqrt(dot(v, v)); \
+ } \
+ template <typename T> inline CLASS lerp(const CLASS & u, const CLASS & v, const T x) \
+ { \
+ return u * (T(1) - x) + v * x; \
+ }
+
+VEC_QUAT_FUNC_TEMPLATE(vec2<T>, 2)
+VEC_QUAT_FUNC_TEMPLATE(vec3<T>, 3)
+VEC_QUAT_FUNC_TEMPLATE(vec4<T>, 4)
+VEC_QUAT_FUNC_TEMPLATE(quat<T>, 4)
+
+#define VEC_FUNC_TEMPLATE(CLASS) \
+ template <typename T> inline CLASS reflect(const CLASS & I, const CLASS & N) \
+ { \
+ return I - T(2) * dot(N, I) * N; \
+ } \
+ template <typename T> inline CLASS refract(const CLASS & I, const CLASS & N, T eta) \
+ { \
+ const T d = dot(N, I); \
+ const T k = T(1) - eta * eta * (T(1) - d * d); \
+ if ( k < T(0) ) \
+ return CLASS(T(0)); \
+ else \
+ return eta * I - (eta * d + static_cast<T>(sqrt(k))) * N; \
+ }
+
+VEC_FUNC_TEMPLATE(vec2<T>)
+VEC_FUNC_TEMPLATE(vec3<T>)
+VEC_FUNC_TEMPLATE(vec4<T>)
+
+template <typename T> inline T lerp(const T & u, const T & v, const T x)
+{
+ return dot(vec2<T>(u, v), vec2<T>((T(1) - x), x));
+}
+
+template <typename T> inline vec3<T> cross(const vec3<T>& u, const vec3<T>& v)
+{
+ return vec3<T>(
+ dot(vec2<T>(u.y, -v.y), vec2<T>(v.z, u.z)),
+ dot(vec2<T>(u.z, -v.z), vec2<T>(v.x, u.x)),
+ dot(vec2<T>(u.x, -v.x), vec2<T>(v.y, u.y)));
+}
+
+
+#define MATRIX_COL4(SRC, C) \
+ vec4<T>(SRC.elem[0][C], SRC.elem[1][C], SRC.elem[2][C], SRC.elem[3][C])
+
+#define MATRIX_ROW4(SRC, R) \
+ vec4<T>(SRC.elem[R][0], SRC.elem[R][1], SRC.elem[R][2], SRC.elem[R][3])
+
+#define MATRIX_COL3(SRC, C) \
+ vec3<T>(SRC.elem[0][C], SRC.elem[1][C], SRC.elem[2][C])
+
+#define MATRIX_ROW3(SRC, R) \
+ vec3<T>(SRC.elem[R][0], SRC.elem[R][1], SRC.elem[R][2])
+
+#define MATRIX_COL2(SRC, C) \
+ vec2<T>(SRC.elem[0][C], SRC.elem[1][C])
+
+#define MATRIX_ROW2(SRC, R) \
+ vec2<T>(SRC.elem[R][0], SRC.elem[R][1])
+
+#define MOP_M_MATRIX_MULTIPLY(CLASS, SIZE) \
+ CLASS & operator *= (const CLASS & rhs) \
+ { \
+ CLASS result; \
+ for (int r = 0; r < SIZE; ++r) \
+ for (int c = 0; c < SIZE; ++c) \
+ result.elem[r][c] = dot( \
+ MATRIX_ROW ## SIZE((*this), r), \
+ MATRIX_COL ## SIZE(rhs, c)); \
+ return (*this) = result; \
+ }
+
+#define MATRIX_CONSTRUCTOR_FROM_T(CLASS, SIZE) \
+ explicit CLASS(const T v) \
+ { \
+ for (int r = 0; r < SIZE; ++r) \
+ for (int c = 0; c < SIZE; ++c) \
+ if (r == c) elem[r][c] = v; \
+ else elem[r][c] = T(0); \
+ }
+
+#define MATRIX_CONSTRUCTOR_FROM_LOWER(CLASS1, CLASS2, SIZE1, SIZE2) \
+ explicit CLASS1(const CLASS2<T>& m) \
+ { \
+ for (int r = 0; r < SIZE1; ++r) \
+ for (int c = 0; c < SIZE1; ++c) \
+ if (r < SIZE2 && c < SIZE2) elem[r][c] = m.elem[r][c]; \
+ else elem[r][c] = r == c ? T(1) : T(0); \
+ }
+
+#define MATRIX_COMMON(CLASS, SIZE) \
+ COMMON_OPERATORS(CLASS, SIZE*SIZE) \
+ MOP_M_MATRIX_MULTIPLY(CLASS, SIZE) \
+ MATRIX_CONSTRUCTOR_FROM_T(CLASS, SIZE) \
+ operator const T* () const { return (const T*) elem; } \
+ operator T* () { return (T*) elem; }
+
+template <typename T> struct mat2;
+template <typename T> struct mat3;
+template <typename T> struct mat4;
+
+template <typename T>
+struct mat2 {
+ T elem[2][2];
+
+ mat2() {}
+
+ explicit mat2(
+ const T m00, const T m01,
+ const T m10, const T m11)
+ {
+ elem[0][0] = m00; elem[0][1] = m01;
+ elem[1][0] = m10; elem[1][1] = m11;
+ }
+
+ explicit mat2(const vec2<T>& v0, const vec2<T>& v1)
+ {
+ elem[0][0] = v0[0];
+ elem[1][0] = v0[1];
+ elem[0][1] = v1[0];
+ elem[1][1] = v1[1];
+ }
+
+ explicit mat2(const mat3<T>& m);
+
+ MATRIX_COMMON(mat2, 2)
+};
+
+template <typename T>
+struct mat3 {
+ T elem[3][3];
+
+ mat3() {}
+
+ explicit mat3(
+ const T m00, const T m01, const T m02,
+ const T m10, const T m11, const T m12,
+ const T m20, const T m21, const T m22)
+ {
+ elem[0][0] = m00; elem[0][1] = m01; elem[0][2] = m02;
+ elem[1][0] = m10; elem[1][1] = m11; elem[1][2] = m12;
+ elem[2][0] = m20; elem[2][1] = m21; elem[2][2] = m22;
+ }
+
+ explicit mat3(const vec3<T>& v0, const vec3<T>& v1, const vec3<T>& v2)
+ {
+ elem[0][0] = v0[0];
+ elem[1][0] = v0[1];
+ elem[2][0] = v0[2];
+ elem[0][1] = v1[0];
+ elem[1][1] = v1[1];
+ elem[2][1] = v1[2];
+ elem[0][2] = v2[0];
+ elem[1][2] = v2[1];
+ elem[2][2] = v2[2];
+ }
+
+ explicit mat3(const mat4<T>& m);
+
+ MATRIX_CONSTRUCTOR_FROM_LOWER(mat3, mat2, 3, 2)
+ MATRIX_COMMON(mat3, 3)
+};
+
+template <typename T>
+struct mat4 {
+ T elem[4][4];
+
+ mat4() {}
+
+ explicit mat4(
+ const T m00, const T m01, const T m02, const T m03,
+ const T m10, const T m11, const T m12, const T m13,
+ const T m20, const T m21, const T m22, const T m23,
+ const T m30, const T m31, const T m32, const T m33)
+ {
+ elem[0][0] = m00; elem[0][1] = m01; elem[0][2] = m02; elem[0][3] = m03;
+ elem[1][0] = m10; elem[1][1] = m11; elem[1][2] = m12; elem[1][3] = m13;
+ elem[2][0] = m20; elem[2][1] = m21; elem[2][2] = m22; elem[2][3] = m23;
+ elem[3][0] = m30; elem[3][1] = m31; elem[3][2] = m32; elem[3][3] = m33;
+ }
+
+ explicit mat4(const vec4<T>& v0, const vec4<T>& v1, const vec4<T>& v2, const vec4<T>& v3)
+ {
+ elem[0][0] = v0[0];
+ elem[1][0] = v0[1];
+ elem[2][0] = v0[2];
+ elem[3][0] = v0[3];
+ elem[0][1] = v1[0];
+ elem[1][1] = v1[1];
+ elem[2][1] = v1[2];
+ elem[3][1] = v1[3];
+ elem[0][2] = v2[0];
+ elem[1][2] = v2[1];
+ elem[2][2] = v2[2];
+ elem[3][2] = v2[3];
+ elem[0][3] = v3[0];
+ elem[1][3] = v3[1];
+ elem[2][3] = v3[2];
+ elem[3][3] = v3[3];
+ }
+
+ MATRIX_CONSTRUCTOR_FROM_LOWER(mat4, mat3, 4, 3)
+ MATRIX_COMMON(mat4, 4)
+};
+
+#define MATRIX_CONSTRUCTOR_FROM_HIGHER(CLASS1, CLASS2, SIZE) \
+ template <typename T> \
+ inline CLASS1<T>::CLASS1(const CLASS2<T>& m) \
+ { \
+ for (int r = 0; r < SIZE; ++r) \
+ for (int c = 0; c < SIZE; ++c) \
+ elem[r][c] = m.elem[r][c]; \
+ }
+
+MATRIX_CONSTRUCTOR_FROM_HIGHER(mat2, mat3, 2)
+MATRIX_CONSTRUCTOR_FROM_HIGHER(mat3, mat4, 3)
+
+#define MAT_FUNC_TEMPLATE(CLASS, SIZE) \
+ template <typename T> \
+ inline CLASS transpose(const CLASS & m) \
+ { \
+ CLASS result; \
+ for (int r = 0; r < SIZE; ++r) \
+ for (int c = 0; c < SIZE; ++c) \
+ result.elem[r][c] = m.elem[c][r]; \
+ return result; \
+ } \
+ template <typename T> \
+ inline CLASS identity ## SIZE() \
+ { \
+ CLASS result; \
+ for (int r = 0; r < SIZE; ++r) \
+ for (int c = 0; c < SIZE; ++c) \
+ result.elem[r][c] = r == c ? T(1) : T(0); \
+ return result; \
+ } \
+ template <typename T> \
+ inline T trace(const CLASS & m) \
+ { \
+ T result = T(0); \
+ for (int i = 0; i < SIZE; ++i) \
+ result += m.elem[i][i]; \
+ return result; \
+ }
+
+MAT_FUNC_TEMPLATE(mat2<T>, 2)
+MAT_FUNC_TEMPLATE(mat3<T>, 3)
+MAT_FUNC_TEMPLATE(mat4<T>, 4)
+
+#define MAT_FUNC_MINOR_TEMPLATE(CLASS1, CLASS2, SIZE) \
+ template <typename T> \
+ inline CLASS2 minor(const CLASS1 & m, int _r = SIZE, int _c = SIZE) { \
+ CLASS2 result; \
+ for (int r = 0; r < SIZE - 1; ++r) \
+ for (int c = 0; c < SIZE - 1; ++c) { \
+ int rs = r >= _r ? 1 : 0; \
+ int cs = c >= _c ? 1 : 0; \
+ result.elem[r][c] = m.elem[r + rs][c + cs]; \
+ } \
+ return result; \
+ }
+
+MAT_FUNC_MINOR_TEMPLATE(mat3<T>, mat2<T>, 3)
+MAT_FUNC_MINOR_TEMPLATE(mat4<T>, mat3<T>, 4)
+
+template <typename T>
+inline T det(const mat2<T>& m)
+{
+ return dot(
+ vec2<T>(m.elem[0][0], -m.elem[0][1]),
+ vec2<T>(m.elem[1][1], m.elem[1][0]));
+}
+
+template <typename T>
+inline T det(const mat3<T>& m)
+{
+ return dot(cross(MATRIX_COL3(m, 0), MATRIX_COL3(m, 1)), MATRIX_COL3(m, 2));
+}
+
+template <typename T>
+inline T det(const mat4<T>& m)
+{
+ vec4<T> b;
+ for (int i = 0; i < 4; ++i)
+ b[i] = (i & 1 ? -1 : 1) * det(minor(m, 0, i));
+ return dot(MATRIX_ROW4(m, 0), b);
+}
+
+#define MAT_ADJOINT_TEMPLATE(CLASS, SIZE) \
+ template <typename T> \
+ inline CLASS adjoint(const CLASS & m) \
+ { \
+ CLASS result; \
+ for (int r = 0; r < SIZE; ++r) \
+ for (int c = 0; c < SIZE; ++c) \
+ result.elem[r][c] = ((r + c) & 1 ? -1 : 1) * det(minor(m, c, r)); \
+ return result; \
+ }
+
+MAT_ADJOINT_TEMPLATE(mat3<T>, 3)
+MAT_ADJOINT_TEMPLATE(mat4<T>, 4)
+
+template <typename T>
+inline mat2<T> adjoint(const mat2<T> & m)
+{
+ return mat2<T>(
+ m.elem[1][1], -m.elem[0][1],
+ -m.elem[1][0], m.elem[0][0]
+ );
+}
+
+#define MAT_INVERSE_TEMPLATE(CLASS) \
+ template <typename T> \
+ inline CLASS inverse(const CLASS & m) \
+ { \
+ return adjoint(m) * inv(det(m)); \
+ }
+
+MAT_INVERSE_TEMPLATE(mat2<T>)
+MAT_INVERSE_TEMPLATE(mat3<T>)
+MAT_INVERSE_TEMPLATE(mat4<T>)
+
+#define MAT_VEC_FUNCS_TEMPLATE(MATCLASS, VECCLASS, SIZE) \
+ template <typename T> \
+ inline VECCLASS operator * (const MATCLASS & m, const VECCLASS & v) \
+ { \
+ VECCLASS result; \
+ for (int i = 0; i < SIZE; ++i) {\
+ result[i] = dot(MATRIX_ROW ## SIZE(m, i), v); \
+ } \
+ return result; \
+ } \
+ template <typename T> \
+ inline VECCLASS operator * (const VECCLASS & v, const MATCLASS & m) \
+ { \
+ VECCLASS result; \
+ for (int i = 0; i < SIZE; ++i) \
+ result[i] = dot(v, MATRIX_COL ## SIZE(m, i)); \
+ return result; \
+ }
+
+MAT_VEC_FUNCS_TEMPLATE(mat2<T>, vec2<T>, 2)
+MAT_VEC_FUNCS_TEMPLATE(mat3<T>, vec3<T>, 3)
+MAT_VEC_FUNCS_TEMPLATE(mat4<T>, vec4<T>, 4)
+
+// Returns the inverse of a 4x4 matrix. It is assumed that the matrix passed
+// as argument describes a rigid-body transformation.
+template <typename T>
+inline mat4<T> fast_inverse(const mat4<T>& m)
+{
+ const vec3<T> t = MATRIX_COL3(m, 3);
+ const T tx = -dot(MATRIX_COL3(m, 0), t);
+ const T ty = -dot(MATRIX_COL3(m, 1), t);
+ const T tz = -dot(MATRIX_COL3(m, 2), t);
+
+ return mat4<T>(
+ m.elem[0][0], m.elem[1][0], m.elem[2][0], tx,
+ m.elem[0][1], m.elem[1][1], m.elem[2][1], ty,
+ m.elem[0][2], m.elem[1][2], m.elem[2][2], tz,
+ T(0), T(0), T(0), T(1)
+ );
+}
+
+// Transformations for points and vectors. Potentially faster than a full
+// matrix * vector multiplication
+
+#define MAT_TRANFORMS_TEMPLATE(MATCLASS, VECCLASS, VECSIZE) \
+ /* computes vec3<T>(m * vec4<T>(v, 0.0)) */ \
+ template <typename T> \
+ inline VECCLASS transform_vector(const MATCLASS & m, const VECCLASS & v) \
+ { \
+ VECCLASS result; \
+ for (int i = 0; i < VECSIZE; ++i) \
+ result[i] = dot(MATRIX_ROW ## VECSIZE(m, i), v); \
+ return result;\
+ } \
+ /* computes vec3(m * vec4(v, 1.0)) */ \
+ template <typename T> \
+ inline VECCLASS transform_point(const MATCLASS & m, const VECCLASS & v) \
+ { \
+ /*return transform_vector(m, v) + MATRIX_ROW ## VECSIZE(m, VECSIZE); */\
+ VECCLASS result; \
+ for (int i = 0; i < VECSIZE; ++i) \
+ result[i] = dot(MATRIX_ROW ## VECSIZE(m, i), v) + m.elem[i][VECSIZE]; \
+ return result; \
+ } \
+ /* computes VECCLASS(transpose(m) * vec4<T>(v, 0.0)) */ \
+ template <typename T> \
+ inline VECCLASS transform_vector_transpose(const MATCLASS & m, const VECCLASS& v) \
+ { \
+ VECCLASS result; \
+ for (int i = 0; i < VECSIZE; ++i) \
+ result[i] = dot(MATRIX_COL ## VECSIZE(m, i), v); \
+ return result; \
+ } \
+ /* computes VECCLASS(transpose(m) * vec4<T>(v, 1.0)) */ \
+ template <typename T> \
+ inline VECCLASS transform_point_transpose(const MATCLASS & m, const VECCLASS& v) \
+ { \
+ /*return transform_vector_transpose(m, v) + MATRIX_COL ## VECSIZE(m, VECSIZE); */\
+ VECCLASS result; \
+ for (int i = 0; i < VECSIZE; ++i) \
+ result[i] = dot(MATRIX_COL ## VECSIZE(m, i), v) + m.elem[VECSIZE][i]; \
+ return result; \
+ }
+
+MAT_TRANFORMS_TEMPLATE(mat4<T>, vec3<T>, 3)
+MAT_TRANFORMS_TEMPLATE(mat3<T>, vec2<T>, 2)
+
+#define MAT_OUTERPRODUCT_TEMPLATE(MATCLASS, VECCLASS, MATSIZE) \
+ template <typename T> \
+ inline MATCLASS outer_product(const VECCLASS & v1, const VECCLASS & v2) \
+ { \
+ MATCLASS r; \
+ for ( int j = 0; j < MATSIZE; ++j ) \
+ for ( int k = 0; k < MATSIZE; ++k ) \
+ r.elem[j][k] = v1[j] * v2[k]; \
+ return r; \
+ }
+
+MAT_OUTERPRODUCT_TEMPLATE(mat4<T>, vec4<T>, 4)
+MAT_OUTERPRODUCT_TEMPLATE(mat3<T>, vec3<T>, 3)
+MAT_OUTERPRODUCT_TEMPLATE(mat2<T>, vec2<T>, 2)
+
+template <typename T>
+inline mat4<T> translation_matrix(const T x, const T y, const T z)
+{
+ mat4<T> r(T(1));
+ r.elem[0][3] = x;
+ r.elem[1][3] = y;
+ r.elem[2][3] = z;
+ return r;
+}
+
+template <typename T>
+inline mat4<T> translation_matrix(const vec3<T>& v)
+{
+ return translation_matrix(v.x, v.y, v.z);
+}
+
+template <typename T>
+inline mat4<T> scaling_matrix(const T x, const T y, const T z)
+{
+ mat4<T> r(T(0));
+ r.elem[0][0] = x;
+ r.elem[1][1] = y;
+ r.elem[2][2] = z;
+ r.elem[3][3] = T(1);
+ return r;
+}
+
+template <typename T>
+inline mat4<T> scaling_matrix(const vec3<T>& v)
+{
+ return scaling_matrix(v.x, v.y, v.z);
+}
+
+template <typename T>
+inline mat4<T> rotation_matrix(const T angle, const vec3<T>& v)
+{
+ const T a = angle * T(M_PI/180) ;
+ const vec3<T> u = normalize(v);
+
+ const mat3<T> S(
+ T(0), -u[2], u[1],
+ u[2], T(0), -u[0],
+ -u[1], u[0], T(0)
+ );
+
+ const mat3<T> uut = outer_product(u, u);
+ const mat3<T> R = uut + T(cos(a)) * (identity3<T>() - uut) + T(sin(a)) * S;
+
+ return mat4<T>(R);
+}
+
+
+template <typename T>
+inline mat4<T> rotation_matrix(const T angle, const T x, const T y, const T z)
+{
+ return rotation_matrix(angle, vec3<T>(x, y, z));
+}
+
+// Constructs a shear-matrix that shears component i by factor with
+// Respect to component j.
+template <typename T>
+inline mat4<T> shear_matrix(const int i, const int j, const T factor)
+{
+ mat4<T> m = identity4<T>();
+ m.elem[i][j] = factor;
+ return m;
+}
+
+template <typename T>
+inline mat4<T> euler(const T head, const T pitch, const T roll)
+{
+ return rotation_matrix(roll, T(0), T(0), T(1)) *
+ rotation_matrix(pitch, T(1), T(0), T(0)) *
+ rotation_matrix(head, T(0), T(1), T(0));
+}
+
+template <typename T>
+inline mat4<T> frustum_matrix(const T l, const T r, const T b, const T t, const T n, const T f)
+{
+ return mat4<T>(
+ (2 * n)/(r - l), T(0), (r + l)/(r - l), T(0),
+ T(0), (2 * n)/(t - b), (t + b)/(t - b), T(0),
+ T(0), T(0), -(f + n)/(f - n), -(2 * f * n)/(f - n),
+ T(0), T(0), -T(1), T(0)
+ );
+}
+
+template <typename T>
+inline mat4<T> perspective_matrix(const T fovy, const T aspect, const T zNear, const T zFar)
+{
+ const T dz = zFar - zNear;
+ const T rad = fovy / T(2) * T(M_PI/180);
+ const T s = sin(rad);
+
+ if ( ( dz == T(0) ) || ( s == T(0) ) || ( aspect == T(0) ) ) {
+ return identity4<T>();
+ }
+
+ const T cot = cos(rad) / s;
+
+ mat4<T> m = identity4<T>();
+ m[0] = cot / aspect;
+ m[5] = cot;
+ m[10] = -(zFar + zNear) / dz;
+ m[14] = T(-1);
+ m[11] = -2 * zNear * zFar / dz;
+ m[15] = T(0);
+
+ return m;
+}
+
+template <typename T>
+inline mat4<T> ortho_matrix(const T l, const T r, const T b, const T t, const T n, const T f)
+{
+ return mat4<T>(
+ T(2)/(r - l), T(0), T(0), -(r + l)/(r - l),
+ T(0), T(2)/(t - b), T(0), -(t + b)/(t - b),
+ T(0), T(0), -T(2)/(f - n), -(f + n)/(f - n),
+ T(0), T(0), T(0), T(1)
+ );
+}
+
+template <typename T>
+inline mat4<T> lookat_matrix(const vec3<T>& eye, const vec3<T>& center, const vec3<T>& up) {
+ const vec3<T> forward = normalize(center - eye);
+ const vec3<T> side = normalize(cross(forward, up));
+
+ const vec3<T> up2 = cross(side, forward);
+
+ mat4<T> m = identity4<T>();
+
+ m.elem[0][0] = side[0];
+ m.elem[0][1] = side[1];
+ m.elem[0][2] = side[2];
+
+ m.elem[1][0] = up2[0];
+ m.elem[1][1] = up2[1];
+ m.elem[1][2] = up2[2];
+
+ m.elem[2][0] = -forward[0];
+ m.elem[2][1] = -forward[1];
+ m.elem[2][2] = -forward[2];
+
+ return m * translation_matrix(-eye);
+}
+
+template <typename T>
+inline mat4<T> picking_matrix(const T x, const T y, const T dx, const T dy, int viewport[4]) {
+ if (dx <= 0 || dy <= 0) {
+ return identity4<T>();
+ }
+
+ mat4<T> r = translation_matrix((viewport[2] - 2 * (x - viewport[0])) / dx,
+ (viewport[3] - 2 * (y - viewport[1])) / dy, 0);
+ r *= scaling_matrix(viewport[2] / dx, viewport[2] / dy, 1);
+ return r;
+}
+
+// Constructs a shadow matrix. q is the light source and p is the plane.
+template <typename T> inline mat4<T> shadow_matrix(const vec4<T>& q, const vec4<T>& p) {
+ mat4<T> m;
+
+ m.elem[0][0] = p.y * q[1] + p.z * q[2] + p.w * q[3];
+ m.elem[0][1] = -p.y * q[0];
+ m.elem[0][2] = -p.z * q[0];
+ m.elem[0][3] = -p.w * q[0];
+
+ m.elem[1][0] = -p.x * q[1];
+ m.elem[1][1] = p.x * q[0] + p.z * q[2] + p.w * q[3];
+ m.elem[1][2] = -p.z * q[1];
+ m.elem[1][3] = -p.w * q[1];
+
+
+ m.elem[2][0] = -p.x * q[2];
+ m.elem[2][1] = -p.y * q[2];
+ m.elem[2][2] = p.x * q[0] + p.y * q[1] + p.w * q[3];
+ m.elem[2][3] = -p.w * q[2];
+
+ m.elem[3][1] = -p.x * q[3];
+ m.elem[3][2] = -p.y * q[3];
+ m.elem[3][3] = -p.z * q[3];
+ m.elem[3][0] = p.x * q[0] + p.y * q[1] + p.z * q[2];
+
+ return m;
+}
+
+// Quaternion class
+template <typename T>
+struct quat {
+ vec3<T> v;
+ T w;
+
+ quat() {}
+ quat(const vec3<T>& iv, const T iw) : v(iv), w(iw) {}
+ quat(const T vx, const T vy, const T vz, const T iw) : v(vx, vy, vz), w(iw) {}
+ quat(const vec4<T>& i) : v(i.x, i.y, i.z), w(i.w) {}
+
+ operator const T* () const { return &(v[0]); }
+ operator T* () { return &(v[0]); }
+
+ quat& operator += (const quat& q) { v += q.v; w += q.w; return *this; }
+ quat& operator -= (const quat& q) { v -= q.v; w -= q.w; return *this; }
+
+ quat& operator *= (const T& s) { v *= s; w *= s; return *this; }
+ quat& operator /= (const T& s) { v /= s; w /= s; return *this; }
+
+ quat& operator *= (const quat& r)
+ {
+ //q1 x q2 = [s1,v1] x [s2,v2] = [(s1*s2 - v1*v2),(s1*v2 + s2*v1 + v1xv2)].
+ quat q;
+ q.v = cross(v, r.v) + r.w * v + w * r.v;
+ q.w = w * r.w - dot(v, r.v);
+ return *this = q;
+ }
+
+ quat& operator /= (const quat& q) { return (*this) *= inverse(q); }
+};
+
+// Quaternion functions
+
+template <typename T>
+inline quat<T> identityq()
+{
+ return quat<T>(T(0), T(0), T(0), T(1));
+}
+
+template <typename T>
+inline quat<T> conjugate(const quat<T>& q)
+{
+ return quat<T>(-q.v, q.w);
+}
+
+template <typename T>
+inline quat<T> inverse(const quat<T>& q)
+{
+ const T l = dot(q, q);
+ if ( l > T(0) ) return conjugate(q) * inv(l);
+ else return identityq<T>();
+}
+
+// quaternion utility functions
+
+// the input quaternion is assumed to be normalized
+template <typename T>
+inline mat3<T> quat_to_mat3(const quat<T>& q)
+{
+ // const quat<T> q = normalize(qq);
+
+ const T xx = q[0] * q[0];
+ const T xy = q[0] * q[1];
+ const T xz = q[0] * q[2];
+ const T xw = q[0] * q[3];
+
+ const T yy = q[1] * q[1];
+ const T yz = q[1] * q[2];
+ const T yw = q[1] * q[3];
+
+ const T zz = q[2] * q[2];
+ const T zw = q[2] * q[3];
+
+ return mat3<T>(
+ 1 - 2*(yy + zz), 2*(xy - zw), 2*(xz + yw),
+ 2*(xy + zw), 1 - 2*(xx + zz), 2*(yz - xw),
+ 2*(xz - yw), 2*(yz + xw), 1 - 2*(xx + yy)
+ );
+}
+
+// the input quat<T>ernion is assumed to be normalized
+template <typename T>
+inline mat4<T> quat_to_mat4(const quat<T>& q)
+{
+ // const quat<T> q = normalize(qq);
+
+ return mat4<T>(quat_to_mat3(q));
+}
+
+template <typename T>
+inline quat<T> mat_to_quat(const mat4<T>& m)
+{
+ const T t = m.elem[0][0] + m.elem[1][1] + m.elem[2][2] + T(1);
+ quat<T> q;
+
+ if ( t > 0 ) {
+ const T s = T(0.5) / sqrt(t);
+ q[3] = T(0.25) * inv(s);
+ q[0] = (m.elem[2][1] - m.elem[1][2]) * s;
+ q[1] = (m.elem[0][2] - m.elem[2][0]) * s;
+ q[2] = (m.elem[1][0] - m.elem[0][1]) * s;
+ } else {
+ if ( m.elem[0][0] > m.elem[1][1] && m.elem[0][0] > m.elem[2][2] ) {
+ const T s = T(2) * sqrt( T(1) + m.elem[0][0] - m.elem[1][1] - m.elem[2][2]);
+ const T invs = inv(s);
+ q[0] = T(0.25) * s;
+ q[1] = (m.elem[0][1] + m.elem[1][0] ) * invs;
+ q[2] = (m.elem[0][2] + m.elem[2][0] ) * invs;
+ q[3] = (m.elem[1][2] - m.elem[2][1] ) * invs;
+ } else if (m.elem[1][1] > m.elem[2][2]) {
+ const T s = T(2) * sqrt( T(1) + m.elem[1][1] - m.elem[0][0] - m.elem[2][2]);
+ const T invs = inv(s);
+ q[0] = (m.elem[0][1] + m.elem[1][0] ) * invs;
+ q[1] = T(0.25) * s;
+ q[2] = (m.elem[1][2] + m.elem[2][1] ) * invs;
+ q[3] = (m.elem[0][2] - m.elem[2][0] ) * invs;
+ } else {
+ const T s = T(2) * sqrt( T(1) + m.elem[2][2] - m.elem[0][0] - m.elem[1][1] );
+ const T invs = inv(s);
+ q[0] = (m.elem[0][2] + m.elem[2][0] ) * invs;
+ q[1] = (m.elem[1][2] + m.elem[2][1] ) * invs;
+ q[2] = T(0.25) * s;
+ q[3] = (m.elem[0][1] - m.elem[1][0] ) * invs;
+ }
+ }
+
+ return q;
+}
+
+template <typename T>
+inline quat<T> mat_to_quat(const mat3<T>& m)
+{
+ return mat_to_quat(mat4<T>(m));
+}
+
+// the angle is in radians
+template <typename T>
+inline quat<T> quat_from_axis_angle(const vec3<T>& axis, const T a)
+{
+ quat<T> r;
+ const T inv2 = inv(T(2));
+ r.v = sin(a * inv2) * normalize(axis);
+ r.w = cos(a * inv2);
+
+ return r;
+}
+
+// the angle is in radians
+template <typename T>
+inline quat<T> quat_from_axis_angle(const T x, const T y, const T z, const T angle)
+{
+ return quat_from_axis_angle<T>(vec3<T>(x, y, z), angle);
+}
+
+// the angle is stored in radians
+template <typename T>
+inline void quat_to_axis_angle(const quat<T>& qq, vec3<T>* axis, T *angle)
+{
+ quat<T> q = normalize(qq);
+
+ *angle = 2 * acos(q.w);
+
+ const T s = sin((*angle) * inv(T(2)));
+ if ( s != T(0) )
+ *axis = q.v * inv(s);
+ else
+ * axis = vec3<T>(T(0), T(0), T(0));
+}
+
+// Spherical linear interpolation
+template <typename T>
+inline quat<T> slerp(const quat<T>& qq1, const quat<T>& qq2, const T t)
+{
+ // slerp(q1,q2) = sin((1-t)*a)/sin(a) * q1 + sin(t*a)/sin(a) * q2
+ const quat<T> q1 = normalize(qq1);
+ const quat<T> q2 = normalize(qq2);
+
+ const T a = acos(dot(q1, q2));
+ const T s = sin(a);
+
+ #define EPS T(1e-5)
+
+ if ( !(-EPS <= s && s <= EPS) ) {
+ return sin((T(1)-t)*a)/s * q1 + sin(t*a)/s * q2;
+ } else {
+ // if the angle is to small use a linear interpolation
+ return lerp(q1, q2, t);
+ }
+
+ #undef EPS
+}
+
+// Sperical quadtratic interpolation using a smooth cubic spline
+// The parameters a and b are the control points.
+template <typename T>
+inline quat<T> squad(
+ const quat<T>& q0,
+ const quat<T>& a,
+ const quat<T>& b,
+ const quat<T>& q1,
+ const T t)
+{
+ return slerp(slerp(q0, q1, t),slerp(a, b, t), 2 * t * (1 - t));
+}
+
+#undef MOP_M_CLASS_TEMPLATE
+#undef MOP_M_TYPE_TEMPLATE
+#undef MOP_COMP_TEMPLATE
+#undef MOP_G_UMINUS_TEMPLATE
+#undef COMMON_OPERATORS
+#undef VECTOR_COMMON
+#undef FOP_G_SOURCE_TEMPLATE
+#undef FOP_G_CLASS_TEMPLATE
+#undef FOP_G_TYPE_TEMPLATE
+#undef VEC_QUAT_FUNC_TEMPLATE
+#undef VEC_FUNC_TEMPLATE
+#undef MATRIX_COL4
+#undef MATRIX_ROW4
+#undef MATRIX_COL3
+#undef MATRIX_ROW3
+#undef MATRIX_COL2
+#undef MATRIX_ROW2
+#undef MOP_M_MATRIX_MULTIPLY
+#undef MATRIX_CONSTRUCTOR_FROM_T
+#undef MATRIX_CONSTRUCTOR_FROM_LOWER
+#undef MATRIX_COMMON
+#undef MATRIX_CONSTRUCTOR_FROM_HIGHER
+#undef MAT_FUNC_TEMPLATE
+#undef MAT_FUNC_MINOR_TEMPLATE
+#undef MAT_ADJOINT_TEMPLATE
+#undef MAT_INVERSE_TEMPLATE
+#undef MAT_VEC_FUNCS_TEMPLATE
+#undef MAT_TRANFORMS_TEMPLATE
+#undef MAT_OUTERPRODUCT_TEMPLATE
+#undef FREE_MODIFYING_OPERATORS
+#undef FREE_OPERATORS
+
+} // end namespace vmath
+
+#endif
+
+
+
+
diff -r 000000000000 -r 89cf0851969b CommonTypes.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/CommonTypes.lib Tue Jun 26 18:24:45 2018 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/RichardE/code/CommonTypes/#033e112463bb
diff -r 000000000000 -r 89cf0851969b ESC.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ESC.lib Tue Jun 26 18:24:45 2018 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/MatteoT/code/ESC/#735702ea5519
diff -r 000000000000 -r 89cf0851969b MPU9250.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.cpp Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,743 @@
+#include "MPU9250.h"
+
+// Set initial input parameters
+enum Ascale {
+ AFS_2G = 0,
+ AFS_4G,
+ AFS_8G,
+ AFS_16G
+};
+
+enum Gscale {
+ GFS_250DPS = 0,
+ GFS_500DPS,
+ GFS_1000DPS,
+ GFS_2000DPS
+};
+
+enum Mscale {
+ MFS_14BITS = 0, // 0.6 mG per LSB
+ MFS_16BITS // 0.15 mG per LSB
+};
+
+float ax,ay,az;
+float gx,gy,gz;
+float mx,my,mz;
+int16_t accelData[3],gyroData[3],tempData;
+float accelBias[3] = {0, 0, 0}; // Bias corrections for acc
+float gyroBias[3] = {0, 0, 0};
+extern float magCalibration[3]= {0, 0, 0};
+extern float magbias[3]= {0, 0, 0}, magScale[3] = {0, 0, 0};
+ float SelfTest[6];
+
+int delt_t = 0; // used to control display output rate
+int count = 0; // used to control display output rate
+
+// parameters for 6 DoF sensor fusion calculations
+float PI = 3.14159265358979323846f;
+float GyroMeasError = PI * (60.0f / 180.0f); // gyroscope measurement error in rads/s (start at 60 deg/s), then reduce after ~10 s to 3
+float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
+float GyroMeasDrift = PI * (1.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
+float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
+#define Kp 0.98 // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
+#define Ki 0.0000001f
+
+float pitch, yaw, roll;
+float deltat = 0.0f; // integration interval for both filter schemes
+int lastUpdate = 0, firstUpdate = 0, Now = 0; // used to calculate integration interval // used to calculate integration interval
+float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
+float eInt[3] = {0.0f, 0.0f, 0.0f};
+
+uint8_t Ascale = AFS_16G; // AFS_2G, AFS_4G, AFS_8G, AFS_16G
+uint8_t Gscale = GFS_2000DPS; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
+uint8_t Mscale = MFS_16BITS; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
+uint8_t Mmode = 0x06; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR
+float aRes, gRes, mRes;
+
+MPU9250::MPU9250(PinName sda, PinName scl) : i2c(sda, scl) {
+ //this->setSleepMode(false);
+ i2c.frequency(400000);
+ //Initializations:
+ currentGyroRange = 0;
+ currentAcceleroRange=0;
+}
+
+//--------------------------------------------------
+//--------------------CONFIG------------------------
+//--------------------------------------------------
+
+void MPU9250::getAres()
+{
+ switch(Ascale)
+ {
+ case AFS_2G:
+ aRes = 2.0/32768.0;
+ break;
+ case AFS_4G:
+ aRes = 4.0/32768.0;
+ break;
+ case AFS_8G:
+ aRes = 8.0/32768.0;
+ break;
+ case AFS_16G:
+ aRes = 16.0/32768.0;
+ break;
+ }
+}
+
+void MPU9250::getGres()
+{
+ switch(Gscale)
+ {
+ case GFS_250DPS:
+ gRes = 250.0/32768.0;
+ break;
+ case GFS_500DPS:
+ gRes = 500.0/32768.0;
+ break;
+ case GFS_1000DPS:
+ gRes = 1000.0/32768.0;
+ break;
+ case GFS_2000DPS:
+ gRes = 2000.0/32768.0;
+ break;
+ }
+}
+
+void MPU9250::getMres() {
+ switch (Mscale)
+ {
+ // Possible magnetometer scales (and their register bit settings) are:
+ // 14 bit resolution (0) and 16 bit resolution (1)
+ case MFS_14BITS:
+ mRes = 10.0*4219.0/8190.0; // Proper scale to return milliGauss
+ break;
+ case MFS_16BITS:
+ mRes = 10.0*4219.0/32760.0; // Proper scale to return milliGauss
+ break;
+ }
+}
+//--------------------------------------------------
+//-------------------General------------------------
+//--------------------------------------------------
+
+void MPU9250::writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
+{
+ char data_write[2];
+ data_write[0] = subAddress;
+ data_write[1] = data;
+ i2c.write(address, data_write, 2, 0);
+}
+
+char MPU9250::readByte(uint8_t address, uint8_t subAddress)
+{
+ char data[1]; // `data` will store the register data
+ char data_write[1];
+ data_write[0] = subAddress;
+ i2c.write(address, data_write, 1, 1); // no stop
+ i2c.read(address, data, 1, 0);
+ return data[0];
+}
+
+void MPU9250::readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
+{
+ char data[14];
+ char data_write[1];
+ data_write[0] = subAddress;
+ i2c.write(address, data_write, 1, 1); // no stop
+ i2c.read(address, data, count, 0);
+ for(int ii = 0; ii < count; ii++) {
+ dest[ii] = data[ii];
+ }
+}
+
+
+void MPU9250::readAccelData(int16_t * destination)
+{
+ uint8_t rawData[6]; // x/y/z accel register data stored here
+ readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
+ destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+ destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+ destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+}
+
+void MPU9250::readGyroData(int16_t * destination)
+{
+ uint8_t rawData[6]; // x/y/z gyro register data stored here
+ readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+ destination[0] = (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+ destination[1] = (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+ destination[2] = (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+}
+
+void MPU9250::readMagData(int16_t * destination)
+{
+ uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
+ if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
+ readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
+ uint8_t c = rawData[6]; // End data read by reading ST2 register
+ if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
+ destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
+ destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
+ destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ;
+ }
+ }
+}
+
+int16_t MPU9250::readTempData()
+{
+ uint8_t rawData[2]; // x/y/z gyro register data stored here
+ readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
+ return (int16_t)(((int16_t)rawData[0]) << 8 | rawData[1]) ; // Turn the MSB and LSB into a 16-bit value
+}
+
+void MPU9250::resetMPU9250() {
+ // reset device
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
+ wait(0.1);
+ }
+
+void MPU9250::initAK8963(float * destination)
+{
+ // First extract the factory calibration for each magnetometer axis
+ uint8_t rawData[3]; // x/y/z gyro calibration data stored here
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
+ wait(0.01);
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
+ wait(0.01);
+ readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
+ destination[0] = (float)(rawData[0] - 128)/256.0f + 1.0f; // Return x-axis sensitivity adjustment values, etc.
+ destination[1] = (float)(rawData[1] - 128)/256.0f + 1.0f;
+ destination[2] = (float)(rawData[2] - 128)/256.0f + 1.0f;
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
+ wait(0.01);
+ // Configure the magnetometer for continuous read and highest resolution
+ // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
+ // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
+ writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
+ wait(0.01);
+}
+
+void MPU9250::initMPU9250()
+{
+ // Initialize MPU9250 device
+ // wake up device
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
+ wait(0.1); // Delay 100 ms for PLL to get established on x-axis gyro; should check for PLL ready interrupt
+
+ // get stable time source
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
+
+ // Configure Gyro and Accelerometer
+ // Disable FSYNC and set accelerometer and gyro bandwidth to 44 and 42 Hz, respectively;
+ // DLPF_CFG = bits 2:0 = 010; this sets the sample rate at 1 kHz for both
+ // Maximum delay is 4.9 ms which is just over a 200 Hz maximum rate
+ writeByte(MPU9250_ADDRESS, CONFIG, 0x03);
+
+ // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
+ writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; the same rate set in CONFIG above
+
+ // Set gyroscope full scale range
+ // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
+ uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value
+ // c = c & ~0xE0; // Clear self-test bits [7:5]
+ c = c & ~0x02; // Clear Fchoice bits [1:0]
+ c = c & ~0x18; // Clear AFS bits [4:3]
+ c = c | Gscale << 3; // Set full scale range for the gyro
+ // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register
+
+ // Set accelerometer full-scale range configuration
+ c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value
+ // c = c & ~0xE0; // Clear self-test bits [7:5]
+ c = c & ~0x18; // Clear AFS bits [4:3]
+ c = c | Ascale << 3; // Set full scale range for the accelerometer
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value
+
+ // Set accelerometer sample rate configuration
+ // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
+ // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
+ c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value
+ c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
+ c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value
+
+ // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
+ // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting
+
+ // Configure Interrupts and Bypass Enable
+ // Set interrupt pin active high, push-pull, and clear on read of INT_STATUS, enable I2C_BYPASS_EN so additional chips
+ // can join the I2C bus and all can be controlled by the Arduino as master
+
+#if USE_ISR
+ writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x12); // INT is 50 microsecond pulse and any read to clear
+#else
+ writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
+#endif
+ writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
+}
+
+// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
+// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
+void MPU9250::calibrateMPU9250(float * dest1, float * dest2)
+{
+ uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
+ uint16_t ii, packet_count, fifo_count;
+ int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};
+
+// reset device, reset all registers, clear gyro and accelerometer bias registers
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
+ wait(0.1);
+
+// get stable time source
+// Set clock source to be PLL with x-axis gyroscope reference, bits 2:0 = 001
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
+ wait(0.2);
+
+// Configure device for bias calculation
+ writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
+ writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
+ writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
+ writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
+ writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
+ writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
+ wait(0.015);
+
+// Configure MPU9250 gyro and accelerometer for bias calculation
+ writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
+ writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity
+
+ uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
+ uint16_t accelsensitivity = 16384; // = 16384 LSB/g
+
+// Configure FIFO to capture accelerometer and gyro data for bias calculation
+ writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
+ writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9250)
+ wait(0.04); // accumulate 40 samples in 80 milliseconds = 480 bytes
+
+// At end of sample accumulation, turn off FIFO sensor read
+ writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
+ readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
+ fifo_count = ((uint16_t)data[0] << 8) | data[1];
+ packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging
+
+ for (ii = 0; ii < packet_count; ii++) {
+ int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
+ readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
+ accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
+ accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
+ accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
+ gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
+ gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
+ gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;
+
+ accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
+ accel_bias[1] += (int32_t) accel_temp[1];
+ accel_bias[2] += (int32_t) accel_temp[2];
+ gyro_bias[0] += (int32_t) gyro_temp[0];
+ gyro_bias[1] += (int32_t) gyro_temp[1];
+ gyro_bias[2] += (int32_t) gyro_temp[2];
+
+}
+ accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
+ accel_bias[1] /= (int32_t) packet_count;
+ accel_bias[2] /= (int32_t) packet_count;
+ gyro_bias[0] /= (int32_t) packet_count;
+ gyro_bias[1] /= (int32_t) packet_count;
+ gyro_bias[2] /= (int32_t) packet_count;
+
+ if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
+ else {accel_bias[2] += (int32_t) accelsensitivity;}
+
+// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
+ data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
+ data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
+ data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
+ data[3] = (-gyro_bias[1]/4) & 0xFF;
+ data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
+ data[5] = (-gyro_bias[2]/4) & 0xFF;
+
+/// Push gyro biases to hardware registers
+/* writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
+ writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
+ writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
+ writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
+ writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
+ writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);
+*/
+ dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; // construct gyro bias in deg/s for later manual subtraction
+ dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
+ dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;
+
+// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
+// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
+// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
+// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
+// the accelerometer biases calculated above must be divided by 8.
+
+ int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
+ readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
+ accel_bias_reg[0] = (int16_t) ((int16_t)data[0] << 8) | data[1];
+ readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
+ accel_bias_reg[1] = (int16_t) ((int16_t)data[0] << 8) | data[1];
+ readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
+ accel_bias_reg[2] = (int16_t) ((int16_t)data[0] << 8) | data[1];
+
+ uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
+ uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis
+
+ for(ii = 0; ii < 3; ii++) {
+ if(accel_bias_reg[ii] & mask) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
+ }
+
+ // Construct total accelerometer bias, including calculated average accelerometer bias from above
+ accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
+ accel_bias_reg[1] -= (accel_bias[1]/8);
+ accel_bias_reg[2] -= (accel_bias[2]/8);
+
+ data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
+ data[1] = (accel_bias_reg[0]) & 0xFF;
+ data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
+ data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
+ data[3] = (accel_bias_reg[1]) & 0xFF;
+ data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
+ data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
+ data[5] = (accel_bias_reg[2]) & 0xFF;
+ data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers
+
+// Apparently this is not working for the acceleration biases in the MPU-9250
+// Are we handling the temperature correction bit properly?
+// Push accelerometer biases to hardware registers
+/* writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
+ writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
+ writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
+ writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
+ writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
+ writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);
+*/
+// Output scaled accelerometer biases for manual subtraction in the main program
+ dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
+ dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
+ dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
+}
+
+void MPU9250::magcalMPU9250(float * dest1, float * dest2)
+{
+ uint16_t ii = 0, sample_count = 0;
+ int32_t mag_bias[3] = {0, 0, 0}, mag_scale[3] = {0, 0, 0};
+ int16_t mag_max[3] = {-32767, -32767, -32767}, mag_min[3] = {32767, 32767, 32767}, mag_temp[3] = {0, 0, 0};
+
+ // shoot for ~fifteen seconds of mag data
+ if(Mmode == 0x02) sample_count = 128; // at 8 Hz ODR, new mag data is available every 125 ms
+ if(Mmode == 0x06) sample_count = 1500; // at 100 Hz ODR, new mag data is available every 10 ms
+
+ for(ii = 0; ii < sample_count; ii++) {
+ readMagData(mag_temp); // Read the mag data
+
+ for (int jj = 0; jj < 3; jj++) {
+ if(mag_temp[jj] > mag_max[jj]) mag_max[jj] = mag_temp[jj];
+ if(mag_temp[jj] < mag_min[jj]) mag_min[jj] = mag_temp[jj];
+ }
+
+ if(Mmode == 0x02) wait(0.135); // at 8 Hz ODR, new mag data is available every 125 ms
+ if(Mmode == 0x06) wait(0.012); // at 100 Hz ODR, new mag data is available every 10 ms
+ }
+
+ // Get hard iron correction
+ mag_bias[0] = (mag_max[0] + mag_min[0])/2; // get average x mag bias in counts
+ mag_bias[1] = (mag_max[1] + mag_min[1])/2; // get average y mag bias in counts
+ mag_bias[2] = (mag_max[2] + mag_min[2])/2; // get average z mag bias in counts
+
+ dest1[0] = (float) mag_bias[0]*mRes*magCalibration[0]; // save mag biases in G for main program
+ dest1[1] = (float) mag_bias[1]*mRes*magCalibration[1];
+ dest1[2] = (float) mag_bias[2]*mRes*magCalibration[2];
+
+ // Get soft iron correction estimate
+ mag_scale[0] = (mag_max[0] - mag_min[0])/2; // get average x axis max chord length in counts
+ mag_scale[1] = (mag_max[1] - mag_min[1])/2; // get average y axis max chord length in counts
+ mag_scale[2] = (mag_max[2] - mag_min[2])/2; // get average z axis max chord length in counts
+
+ float avg_rad = mag_scale[0] + mag_scale[1] + mag_scale[2];
+ avg_rad /= 3.0;
+
+ dest2[0] = avg_rad/((float)mag_scale[0]);
+ dest2[1] = avg_rad/((float)mag_scale[1]);
+ dest2[2] = avg_rad/((float)mag_scale[2]);
+}
+
+
+// Accelerometer and gyroscope self test; check calibration wrt factory settings
+void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
+{
+ uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
+ uint8_t selfTest[6];
+ int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
+ float factoryTrim[6];
+ uint8_t FS = 0;
+
+ writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz
+ writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1<<FS); // Set full scale range for the gyro to 250 dps
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 1<<FS); // Set full scale range for the accelerometer to 2 g
+
+ for( int ii = 0; ii < 200; ii++) { // get average current values of gyro and acclerometer
+
+ readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
+ aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+ aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+ aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+
+ readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+ gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+ gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+ gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+ }
+
+ for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average current readings
+ aAvg[ii] /= 200;
+ gAvg[ii] /= 200;
+ }
+
+// Configure the accelerometer for self-test
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
+ wait(0.025); // Delay a while to let the device stabilize
+
+ for( int ii = 0; ii < 200; ii++) { // get average self-test values of gyro and acclerometer
+
+ readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
+ aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+ aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+ aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+
+ readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
+ gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ; // Turn the MSB and LSB into a signed 16-bit value
+ gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
+ gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
+ }
+
+ for (int ii =0; ii < 3; ii++) { // Get average of 200 values and store as average self-test readings
+ aSTAvg[ii] /= 200;
+ gSTAvg[ii] /= 200;
+ }
+
+ // Configure the gyro and accelerometer for normal operation
+ writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00);
+ writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00);
+ wait(0.025); // Delay a while to let the device stabilize
+
+ // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
+ selfTest[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis accel self-test results
+ selfTest[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis accel self-test results
+ selfTest[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis accel self-test results
+ selfTest[3] = readByte(MPU9250_ADDRESS, SELF_TEST_X_GYRO); // X-axis gyro self-test results
+ selfTest[4] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_GYRO); // Y-axis gyro self-test results
+ selfTest[5] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_GYRO); // Z-axis gyro self-test results
+
+ // Retrieve factory self-test value from self-test code reads
+ factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
+ factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
+ factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
+ factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
+ factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
+ factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
+
+ // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
+ // To get percent, must multiply by 100
+ for (int i = 0; i < 3; i++) {
+ destination[i] = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.; // Report percent differences
+ destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.; // Report percent differences
+ }
+
+}
+
+void MPU9250::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
+{
+ float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
+ float norm;
+ float hx, hy, _2bx, _2bz;
+ float s1, s2, s3, s4;
+ float qDot1, qDot2, qDot3, qDot4;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ float _2q1mx;
+ float _2q1my;
+ float _2q1mz;
+ float _2q2mx;
+ float _4bx;
+ float _4bz;
+ float _2q1 = 2.0f * q1;
+ float _2q2 = 2.0f * q2;
+ float _2q3 = 2.0f * q3;
+ float _2q4 = 2.0f * q4;
+ float _2q1q3 = 2.0f * q1 * q3;
+ float _2q3q4 = 2.0f * q3 * q4;
+ float q1q1 = q1 * q1;
+ float q1q2 = q1 * q2;
+ float q1q3 = q1 * q3;
+ float q1q4 = q1 * q4;
+ float q2q2 = q2 * q2;
+ float q2q3 = q2 * q3;
+ float q2q4 = q2 * q4;
+ float q3q3 = q3 * q3;
+ float q3q4 = q3 * q4;
+ float q4q4 = q4 * q4;
+
+ // Normalise accelerometer measurement
+ norm = sqrt(ax * ax + ay * ay + az * az);
+ if (norm == 0.0f) return; // handle NaN
+ norm = 1.0f/norm;
+ ax *= norm;
+ ay *= norm;
+ az *= norm;
+
+ // Normalise magnetometer measurement
+ norm = sqrt(mx * mx + my * my + mz * mz);
+ if (norm == 0.0f) return; // handle NaN
+ norm = 1.0f/norm;
+ mx *= norm;
+ my *= norm;
+ mz *= norm;
+
+ // Reference direction of Earth's magnetic field
+ _2q1mx = 2.0f * q1 * mx;
+ _2q1my = 2.0f * q1 * my;
+ _2q1mz = 2.0f * q1 * mz;
+ _2q2mx = 2.0f * q2 * mx;
+ hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
+ hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
+ _2bx = sqrt(hx * hx + hy * hy);
+ _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
+ _4bx = 2.0f * _2bx;
+ _4bz = 2.0f * _2bz;
+
+ // Gradient decent algorithm corrective step
+ s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
+ norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
+ norm = 1.0f/norm;
+ s1 *= norm;
+ s2 *= norm;
+ s3 *= norm;
+ s4 *= norm;
+
+ // Compute rate of change of quaternion
+ qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
+ qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
+ qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
+ qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
+
+ // Integrate to yield quaternion
+ q1 += qDot1 * deltat;
+ q2 += qDot2 * deltat;
+ q3 += qDot3 * deltat;
+ q4 += qDot4 * deltat;
+ norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
+ norm = 1.0f/norm;
+ q[0] = q1 * norm;
+ q[1] = q2 * norm;
+ q[2] = q3 * norm;
+ q[3] = q4 * norm;
+
+}
+
+ // Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
+ // measured ones.
+void MPU9250::MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
+{
+ float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
+ float norm;
+ float hx, hy, bx, bz;
+ float vx, vy, vz, wx, wy, wz;
+ float ex, ey, ez;
+ float pa, pb, pc;
+
+ // Auxiliary variables to avoid repeated arithmetic
+ float q1q1 = q1 * q1;
+ float q1q2 = q1 * q2;
+ float q1q3 = q1 * q3;
+ float q1q4 = q1 * q4;
+ float q2q2 = q2 * q2;
+ float q2q3 = q2 * q3;
+ float q2q4 = q2 * q4;
+ float q3q3 = q3 * q3;
+ float q3q4 = q3 * q4;
+ float q4q4 = q4 * q4;
+
+ // Normalise accelerometer measurement
+ norm = sqrt(ax * ax + ay * ay + az * az);
+ if (norm == 0.0f) return; // handle NaN
+ norm = 1.0f / norm; // use reciprocal for division
+ ax *= norm;
+ ay *= norm;
+ az *= norm;
+
+ // Normalise magnetometer measurement
+ norm = sqrt(mx * mx + my * my + mz * mz);
+ if (norm == 0.0f) return; // handle NaN
+ norm = 1.0f / norm; // use reciprocal for division
+ mx *= norm;
+ my *= norm;
+ mz *= norm;
+
+ // Reference direction of Earth's magnetic field
+ hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
+ hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
+ bx = sqrt((hx * hx) + (hy * hy));
+ bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);
+
+ // Estimated direction of gravity and magnetic field
+ vx = 2.0f * (q2q4 - q1q3);
+ vy = 2.0f * (q1q2 + q3q4);
+ vz = q1q1 - q2q2 - q3q3 + q4q4;
+ wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
+ wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
+ wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
+
+ // Error is cross product between estimated direction and measured direction of gravity
+ ex = (ay * vz - az * vy) + (my * wz - mz * wy);
+ ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
+ ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
+ if (Ki > 0.0f)
+ {
+ eInt[0] += ex; // accumulate integral error
+ eInt[1] += ey;
+ eInt[2] += ez;
+ }
+ else
+ {
+ eInt[0] = 0.0f; // prevent integral wind up
+ eInt[1] = 0.0f;
+ eInt[2] = 0.0f;
+ }
+
+ // Apply feedback terms
+ gx = gx + Kp * ex + Ki * eInt[0];
+ gy = gy + Kp * ey + Ki * eInt[1];
+ gz = gz + Kp * ez + Ki * eInt[2];
+
+ // Integrate rate of change of quaternion
+ pa = q2;
+ pb = q3;
+ pc = q4;
+ q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
+ q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
+ q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
+ q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);
+
+ // Normalise quaternion
+ norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
+ norm = 1.0f / norm;
+ q[0] = q1 * norm;
+ q[1] = q2 * norm;
+ q[2] = q3 * norm;
+ q[3] = q4 * norm;
+
+}
+
diff -r 000000000000 -r 89cf0851969b MPU9250.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.h Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,269 @@
+
+
+#ifndef MPU9250_H
+#define MPU9250_H
+
+
+/**
+ * Includes
+ */
+
+#include "mbed.h"
+#include "math.h"
+
+#define USE_ISR 1 // poll or data ready interrupt
+
+// See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in
+// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
+//
+//Magnetometer Registers
+#define AK8963_ADDRESS 0x0C<<1
+#define WHO_AM_I_AK8963 0x00 // should return 0x48
+#define INFO 0x01
+#define AK8963_ST1 0x02 // data ready status bit 0
+#define AK8963_XOUT_L 0x03 // data
+#define AK8963_XOUT_H 0x04
+#define AK8963_YOUT_L 0x05
+#define AK8963_YOUT_H 0x06
+#define AK8963_ZOUT_L 0x07
+#define AK8963_ZOUT_H 0x08
+#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
+#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
+#define AK8963_ASTC 0x0C // Self test control
+#define AK8963_I2CDIS 0x0F // I2C disable
+#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
+#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
+#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value
+
+#define SELF_TEST_X_GYRO 0x00
+#define SELF_TEST_Y_GYRO 0x01
+#define SELF_TEST_Z_GYRO 0x02
+
+/*#define X_FINE_GAIN 0x03 // [7:0] fine gain
+#define Y_FINE_GAIN 0x04
+#define Z_FINE_GAIN 0x05
+#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
+#define XA_OFFSET_L_TC 0x07
+#define YA_OFFSET_H 0x08
+#define YA_OFFSET_L_TC 0x09
+#define ZA_OFFSET_H 0x0A
+#define ZA_OFFSET_L_TC 0x0B */
+
+#define SELF_TEST_X_ACCEL 0x0D
+#define SELF_TEST_Y_ACCEL 0x0E
+#define SELF_TEST_Z_ACCEL 0x0F
+
+#define SELF_TEST_A 0x10
+
+#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
+#define XG_OFFSET_L 0x14
+#define YG_OFFSET_H 0x15
+#define YG_OFFSET_L 0x16
+#define ZG_OFFSET_H 0x17
+#define ZG_OFFSET_L 0x18
+#define SMPLRT_DIV 0x19
+#define CONFIG 0x1A
+#define GYRO_CONFIG 0x1B
+#define ACCEL_CONFIG 0x1C
+#define ACCEL_CONFIG2 0x1D
+#define LP_ACCEL_ODR 0x1E
+#define WOM_THR 0x1F
+
+#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
+#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
+#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms
+
+#define FIFO_EN 0x23
+#define I2C_MST_CTRL 0x24
+#define I2C_SLV0_ADDR 0x25
+#define I2C_SLV0_REG 0x26
+#define I2C_SLV0_CTRL 0x27
+#define I2C_SLV1_ADDR 0x28
+#define I2C_SLV1_REG 0x29
+#define I2C_SLV1_CTRL 0x2A
+#define I2C_SLV2_ADDR 0x2B
+#define I2C_SLV2_REG 0x2C
+#define I2C_SLV2_CTRL 0x2D
+#define I2C_SLV3_ADDR 0x2E
+#define I2C_SLV3_REG 0x2F
+#define I2C_SLV3_CTRL 0x30
+#define I2C_SLV4_ADDR 0x31
+#define I2C_SLV4_REG 0x32
+#define I2C_SLV4_DO 0x33
+#define I2C_SLV4_CTRL 0x34
+#define I2C_SLV4_DI 0x35
+#define I2C_MST_STATUS 0x36
+#define INT_PIN_CFG 0x37
+#define INT_ENABLE 0x38
+#define DMP_INT_STATUS 0x39 // Check DMP interrupt
+#define INT_STATUS 0x3A
+#define ACCEL_XOUT_H 0x3B
+#define ACCEL_XOUT_L 0x3C
+#define ACCEL_YOUT_H 0x3D
+#define ACCEL_YOUT_L 0x3E
+#define ACCEL_ZOUT_H 0x3F
+#define ACCEL_ZOUT_L 0x40
+#define TEMP_OUT_H 0x41
+#define TEMP_OUT_L 0x42
+#define GYRO_XOUT_H 0x43
+#define GYRO_XOUT_L 0x44
+#define GYRO_YOUT_H 0x45
+#define GYRO_YOUT_L 0x46
+#define GYRO_ZOUT_H 0x47
+#define GYRO_ZOUT_L 0x48
+#define EXT_SENS_DATA_00 0x49
+#define EXT_SENS_DATA_01 0x4A
+#define EXT_SENS_DATA_02 0x4B
+#define EXT_SENS_DATA_03 0x4C
+#define EXT_SENS_DATA_04 0x4D
+#define EXT_SENS_DATA_05 0x4E
+#define EXT_SENS_DATA_06 0x4F
+#define EXT_SENS_DATA_07 0x50
+#define EXT_SENS_DATA_08 0x51
+#define EXT_SENS_DATA_09 0x52
+#define EXT_SENS_DATA_10 0x53
+#define EXT_SENS_DATA_11 0x54
+#define EXT_SENS_DATA_12 0x55
+#define EXT_SENS_DATA_13 0x56
+#define EXT_SENS_DATA_14 0x57
+#define EXT_SENS_DATA_15 0x58
+#define EXT_SENS_DATA_16 0x59
+#define EXT_SENS_DATA_17 0x5A
+#define EXT_SENS_DATA_18 0x5B
+#define EXT_SENS_DATA_19 0x5C
+#define EXT_SENS_DATA_20 0x5D
+#define EXT_SENS_DATA_21 0x5E
+#define EXT_SENS_DATA_22 0x5F
+#define EXT_SENS_DATA_23 0x60
+#define MOT_DETECT_STATUS 0x61
+#define I2C_SLV0_DO 0x63
+#define I2C_SLV1_DO 0x64
+#define I2C_SLV2_DO 0x65
+#define I2C_SLV3_DO 0x66
+#define I2C_MST_DELAY_CTRL 0x67
+#define SIGNAL_PATH_RESET 0x68
+#define MOT_DETECT_CTRL 0x69
+#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
+#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
+#define PWR_MGMT_2 0x6C
+#define DMP_BANK 0x6D // Activates a specific bank in the DMP
+#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
+#define DMP_REG 0x6F // Register in DMP from which to read or to which to write
+#define DMP_REG_1 0x70
+#define DMP_REG_2 0x71
+#define FIFO_COUNTH 0x72
+#define FIFO_COUNTL 0x73
+#define FIFO_R_W 0x74
+#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
+#define XA_OFFSET_H 0x77
+#define XA_OFFSET_L 0x78
+#define YA_OFFSET_H 0x7A
+#define YA_OFFSET_L 0x7B
+#define ZA_OFFSET_H 0x7D
+#define ZA_OFFSET_L 0x7E
+
+// Using the MSENSR-9250 breakout board, ADO is set to 0
+// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
+//mbed uses the eight-bit device address, so shift seven-bit addresses left by one!
+#define ADO 0
+#if ADO
+#define MPU9250_ADDRESS 0x69<<1 // Device address when ADO = 1
+#else
+#define MPU9250_ADDRESS 0x68<<1 // Device address when ADO = 0
+#endif
+
+extern float aRes, gRes;
+
+extern Serial pc;
+
+
+extern float ax,ay,az;
+extern float gx,gy,gz;
+extern float mx,my,mz;
+extern int16_t accelData[3],gyroData[3],tempData;
+extern float accelBias[3], gyroBias[3];
+extern float magCalibration[3];
+extern float magbias[3], magScale[3];
+extern int lastUpdate, firstUpdate, Now;
+extern float deltat;
+extern int delt_t;
+extern int count;
+extern float PI;
+extern float pitch, yaw, roll;
+extern float deltat; // integration interval for both filter schemes
+extern int lastUpdate; // used to calculate integration interval // used to calculate integration interval
+extern float q[4]; // vector to hold quaternion
+extern float eInt[3];
+extern float GyroMeasError;
+extern float beta;
+extern float GyroMeasDrift;
+extern float zeta;
+extern float SelfTest[6];
+
+extern uint8_t Ascale; // AFS_2G, AFS_4G, AFS_8G, AFS_16G
+extern uint8_t Gscale; // GFS_250DPS, GFS_500DPS, GFS_1000DPS, GFS_2000DPS
+extern uint8_t Mscale; // MFS_14BITS or MFS_16BITS, 14-bit or 16-bit magnetometer resolution
+extern uint8_t Mmode; // Either 8 Hz 0x02) or 100 Hz (0x06) magnetometer data ODR
+extern float aRes, gRes, mRes;
+
+
+/** MPU9250 IMU library.
+ *
+ * Example:
+ * @code
+ * Later, maybe
+ * @endcode
+ */
+class MPU9250 {
+ public:
+ /**
+ * Constructor.
+ *
+ * Sleep mode of MPU6050 is immediatly disabled
+ *
+ * @param sda - mbed pin to use for the SDA I2C line.
+ * @param scl - mbed pin to use for the SCL I2C line.
+ */
+ MPU9250(PinName sda, PinName scl);
+
+
+ /**
+ * Tests the I2C connection by reading the WHO_AM_I register.
+ *
+ * @return True for a working connection, false for an error
+ */
+ void getAres();
+ void getGres();
+ void getMres();
+
+ void writeByte(uint8_t address, uint8_t subAddress, uint8_t data);
+ char readByte(uint8_t address, uint8_t subAddress);
+ void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest);
+
+ void readAccelData(int16_t * destination);
+ void readGyroData(int16_t * destination);
+ void readMagData(int16_t * destination);
+ int16_t readTempData();
+
+ void resetMPU9250();
+ void initAK8963(float * destination);
+ void initMPU9250();
+ void calibrateMPU9250(float * dest1, float * dest2);
+ void MPU9250::magcalMPU9250(float * dest1, float * dest2) ;
+ void MPU9250SelfTest(float * destination);
+ void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
+ void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz);
+
+ private:
+
+ I2C i2c;
+ char currentAcceleroRange;
+ char currentGyroRange;
+
+
+};
+
+
+
+#endif
+
diff -r 000000000000 -r 89cf0851969b MathFuncs.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MathFuncs.h Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,43 @@
+/*
+ * SOURCE FILE : MathFuncs.h
+ *
+ * Definition of class MathFuncs.
+ *
+ */
+
+#ifndef MathFuncsDefined
+
+ #define MathFuncsDefined
+
+ #include "Types.h"
+
+ /** Various useful maths related functions. */
+ class MathFuncs {
+
+ public :
+
+ /** Constrain a number to be between 2 values.
+ *
+ * @param x Number to constrain.
+ * @param min Minimum value.
+ * @param max Maximum value.
+ * @returns A number between min and max.
+ */
+ static Int16 Constrain( Int16 x, Int16 min, Int16 max ) {
+ if( x < min ) {
+ return min;
+ }
+ else if( x > max ) {
+ return max;
+ }
+ else {
+ return x;
+ }
+ }
+
+ };
+
+#endif
+
+/* END of MathFuncs.h */
+
diff -r 000000000000 -r 89cf0851969b Matrix.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Matrix.lib Tue Jun 26 18:24:45 2018 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/Yo_Robot/code/Matrix/#a4014ab0a8cf
diff -r 000000000000 -r 89cf0851969b PID.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Tue Jun 26 18:24:45 2018 +0000 @@ -0,0 +1,1 @@ +http://os.mbed.com/users/porizou/code/PID/#eeb41e25a490
diff -r 000000000000 -r 89cf0851969b Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Tue Jun 26 18:24:45 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r 89cf0851969b kalman.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/kalman.lib Tue Jun 26 18:24:45 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/cdonate/code/kalman/#8a460b0d6f09
diff -r 000000000000 -r 89cf0851969b main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,119 @@
+#include "mbed.h"
+#include "math.h"
+#include "rtos.h"
+#include "Servo.h"
+#include "CID10DOF.h"
+#include <string>
+#include "stdio.h"
+#include "MathFuncs.h"
+
+
+#define double_SIZE sizeof(double)
+
+
+Serial pc(USBTX, USBRX);
+Serial telem(PC_0, PC_1);
+CID10DOF CID10DOF(D14,D15,D2,D4,D5,D7);
+AnalogIn Analog_I(A0);
+DigitalOut led1(D6);
+DigitalOut led2(D3);
+Timer t;
+
+double bias[3] = {0.0,0.0,0.0}, setpoint[3]= {0.0,0.0,0.0}, speed[4], actSpeed[4];
+double ypr[4],M[5],Diff[3];
+
+
+void getData(void const *argument);
+void PID(void const *argument);
+void motors(void const *argument);
+
+
+int main() {
+
+ pc.baud(115200);
+ //telem.baud(57600);
+
+ led2 = !led2;
+
+ CID10DOF.MPU9250init();
+ CID10DOF.PID_Config(setpoint, bias);
+ CID10DOF.Calibrate_Motors();
+
+ for (int i = 0; i < 4; i++){ //initialise speed to be 0.3
+ speed[i] = MIN_TRHUST;
+ }
+
+ led2 = !led2;
+
+ Thread thread3(getData, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE);
+ Thread thread4(PID, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE);
+ Thread thread5(motors, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE);
+
+ t.start();
+
+ while(1) {
+
+ led1 = !led1;
+ Thread::wait(17);
+ //pc.printf("Y:%f,P:%f,R:%f,PID_P:%.5f,PID_R:%.5f,M1:%f,M2:%f \n\r",ypr[0],ypr[1],ypr[2],Diff[1],Diff[2],actSpeed[0],actSpeed[1]);
+ pc.printf("%f, %f, %f, %f, %f, %f\n\r",ypr[1],ypr[2],0.0,t.read(),ypr[1]-setpoint[1],setpoint[1]);
+
+ if(t>10){
+ setpoint[1] = 5.0;
+ }
+
+ if(t>15){
+ setpoint[1] = 0.0;
+ }
+
+ if(t>20){
+ setpoint[1] = -5;
+ }
+
+ if(t>25){
+ setpoint[1] = 0.0;
+ }
+
+ }
+
+}
+
+
+void PID(void const *argument)
+{
+ while(true){
+
+ CID10DOF.PID_Run(ypr, setpoint, Diff);
+
+ actSpeed[0] = MIN_TRHUST - Diff[2] ;
+ actSpeed[1] = MIN_TRHUST + Diff[2] ;
+
+ if(actSpeed[0]< MIN_TRHUST){actSpeed[0] = MIN_TRHUST;}
+ if(actSpeed[0]> 1.0){actSpeed[0] = MAX_TRHUST;}
+ if(actSpeed[1]< MIN_TRHUST){actSpeed[1] = MIN_TRHUST;}
+ if(actSpeed[1]> 1.0){actSpeed[1] = MAX_TRHUST;}
+ Thread::wait(10);
+ }
+}
+
+
+
+void getData(void const *argument)
+{
+ while(true){
+
+ CID10DOF.MPU9250ReadAxis(ypr);
+ Thread::wait(5);
+ }
+}
+
+void motors(void const *argument)
+{
+ while(true){
+
+ CID10DOF.run (actSpeed);
+ Thread::wait(20);
+ }
+}
+
+
diff -r 000000000000 -r 89cf0851969b mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Tue Jun 26 18:24:45 2018 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#bdd541595fc5
diff -r 000000000000 -r 89cf0851969b mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Jun 26 18:24:45 2018 +0000 @@ -0,0 +1,1 @@ +https://mbed.org/users/mbed_official/code/mbed/builds/86740a56073b \ No newline at end of file