uint32_t millis( void )
{
double t = ((double)SysTickPeriodGet()-(double)SysTickValueGet())/(double)SysTickPeriodGet();
t *= 10;
return get_SystickCount() * 10 + (uint32_t)t;
}
uint32_t micros( void )
{
double t = ((double)SysTickPeriodGet()-(double)SysTickValueGet())/(double)SysTickPeriodGet();
t *= 10;
t *= 1000;
return (get_SystickCount() * 1000 * 10 + (uint32_t)t);
}
//Converts all the gyro acc data into usable data for roll pitch yaw... eventually
void getInclination(float *RwAcc, float *RwEst, float *RwGyro, float *Gyro_ds, float *Awz){
//Long convaluted equation that someone found and i'm borrowing. Returns a unit vector need to convet to degrees
int w = 0;
//int x = 0;
float tmpf = 0.0;
long currentTime, signRzGyro;
float R;
//currentTime = ((SysCtlClockGet() / (1000 * 3))*timeBetweenCalculations)-(SysTickValueGet()*(1000*3)); //So I BELIEVE this will tell me how many ms
currentTime = (SysTickPeriodGet()-SysTickValueGet())/80000; //I think this actually converts to ms between calculations
interval = (currentTime - lastTime); //
interval = (interval < 0) ? interval + SysTickValueGet()/80000 : interval;
lastTime = currentTime;
//We need to fill in an inital RwEst for the math to work. We'll assume that the object is stable on the first start thus accelerometer angles are the angles
//However, if we needed to restart mid flight this would fix itself fairly quickly in theory
if (firstSample) { // the NaN check is used to wait for good data from the Stellaris
for(w=0;w<=2;w++) {
RwEst[w] = RwAcc[w]; //initialize with accelerometer readings
}
}else{
/*float bob = 0.7; //This was here to debug float compares. The below RwEst statment is bugged!
if(bob < 0.1){
UARTSend(0xD4);
}*/
//Rz is too small and because it is used as reference for computing Axz, Ayz it's error fluctuations will amplify leading to bad results
//in this case skip the gyro data and just use previous estimate
//My comment on this, The RzEST is very small which means if me divide by a small number we will get close to infinity which is bad
if((fabs(RwEst[2])) < ((float)0.1)) {
for(w=0;w<=2;w++) {
RwGyro[w] = RwEst[w];
}
}else{
//get angles between projection of R on ZX/ZY plane and Z axis, based on last RwEst
//This calculates the Axz Ayz part. It seems right
for(w=0;w<=1;w++){
tmpf = Gyro_ds[w]; //get current gyro rate in deg/s
tmpf *= interval / ((float)1000.0); //get angle change in deg
Awz[w] = atan2f(RwEst[w],RwEst[2]) * 180 / PI; //get angle and convert to degrees
Awz[w] += tmpf; //get updated angle according to gyro movement //These are in degrees
}
//Awz[0] = 30;
//Awz[1] = 45;
//estimate sign of RzGyro by looking in what qudrant the angle Axz is,
//RzGyro is pozitive if Axz in range -90 ..90 => cos(Awz) >= 0
signRzGyro = ( cosf(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
//reverse calculation of RwGyro from Awz angles, for formulas deductions see http://starlino.com/imu_guide.html
//Effectively RwGyro is unitless. Completly unitless
RwGyro[0] = sinf(Awz[0] * PI / 180);
RwGyro[0] /= floatingSquare( 1 + (cosf(Awz[0] * PI / 180)*cosf(Awz[0] * PI / 180)) * (tanf(Awz[1] * PI / 180)*tanf(Awz[1] * PI / 180)));
RwGyro[1] = sinf(Awz[1] * PI / 180);
RwGyro[1] /= floatingSquare( 1 + (cosf(Awz[1] * PI / 180)*cosf(Awz[1] * PI / 180)) * (tanf(Awz[0] * PI / 180)*tanf(Awz[0] * PI / 180)));
RwGyro[2] = signRzGyro * floatingSquare((float)1.0 - (RwGyro[0]*RwGyro[0]) - (RwGyro[1]*RwGyro[1])); //THIS THIS IS THE DEVIL! //Note this does not ever try to eval sqrt(<0)
/*RwGyro[0] = 0.2;
RwGyro[1] = 0.3;
RwGyro[2] = 0.4;*/
}
//combine Accelerometer and gyro readings
//THIS FOR LOOP CAUSES THE SYSTEM :P
//w = 0; failed attempt
for(w=0;w<=2;w++){
//I believe the units are in g's
//RwEst[w] = (RwAcc[w] + (float)10.0 * RwGyro[w]) / (1 + 10); failed attempt
//RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]) / (1 + wGyro); failed attempt
/*RwEst[w] = (RwAcc[w] + wGyro * RwGyro[w]);
RwEst[w] /= (1+wGyro); failed attempt */
//RwEst[w] = nextafter(fmaf(wGyro,RwGyro[w],RwAcc[w]),100000000); //So when I add RwAcc[2] + RwGyro[2] it crashes invebitably. WTF!!!!!
RwEst[w] = floatingAdd(RwGyro[w],wGyro*RwAcc[w])/(1+wGyro); //DO NOT REMOVE FLOATING ADD it for some reason allows this addition to happen which otherwise fails misserably.
}
/*for(x=0;x<=2;x++){ failed attempt
RwEst[x] = (RwAcc[x] + wGyro * RwGyro[x]) / (1 + wGyro);
}*/
R = floatingSquare(RwEst[0]*RwEst[0]+RwEst[1]*RwEst[1]+RwEst[2]*RwEst[2]);
RwEst[0]/=R;
RwEst[1]/=R; //This is unitless.
RwEst[2]/=R;
}
firstSample = 0;
}
STATIC mp_obj_t time_ticks_cpu(void) {
// We want to "cast" the 32 bit unsigned into a 30-bit small-int
return MP_OBJ_NEW_SMALL_INT((SysTickPeriodGet() - SysTickValueGet()) & MP_SMALL_INT_POSITIVE_MASK);
}
