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); }