static uint64_t ClockResolutionNs() { uint64_t start = ClockTime(); uint64_t end = ClockTime(); uint64_t minres = (end - start); // 10 total trials is arbitrary: what we're trying to avoid by // looping is getting unlucky and being interrupted by a context // switch or signal, or being bitten by paging/cache effects for (int i = 0; i < 9; ++i) { start = ClockTime(); end = ClockTime(); uint64_t candidate = (start - end); if (candidate < minres) minres = candidate; } if (0 == minres) { // measurable resolution is either incredibly low, ~1ns, or very // high. fall back on NSPR's resolution assumption minres = 1 * kNsPerMs; } return minres; }
void PID_UpdateCentroid(PID * pid) { int P, I, D; float refreshRate; //------Read Encoder and clock uint32 nowClocks = ClockTime(); //------Time since last function call in seconds refreshRate = refresh_rate(nowClocks, pid->lastClockTicks_c); pid->lastClockTicks_c = nowClocks; //------Calculate Error pid->error_c = pid->desiredCentroidPID - pid->currentCentroid; ///Wireless_ControlLog(pid->currentCentroid, pid->desiredCentroidPID); //------Update Derivative //pid->differentiator_c = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator_c) + ((2/((2*pid->Tau)+refreshRate))*(pid->currentCentroid - pid->lastCurrentCentroid)); pid->differentiator_c = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator_c) + ((2/((2*pid->Tau)+refreshRate))*(pid->error_c - pid->lastError_c)); //------Update integrator - AntiWindup(only use the integrator if we are close, but not too close) if( abs(pid->error_c) < 75) pid->integrator_c = pid->integrator_c + ((refreshRate/2)*(pid->error_c + pid->lastError_c))*abs(pid->currentVelocity)/1500; else pid->integrator_c = 0; //------Scale Kp based on current velocity float Kp = pid->Kp_c * 1000 / abs(pid->currentVelocity); //------Output Calculation P = Kp * pid->error_c; I = pid->Ki_c * pid->integrator_c; D = pid->Kd_c * pid->differentiator_c; pid->outputPID_unsat_c = (P) + (I) - (D); pid->outputPID_c = sat(pid->outputPID_unsat_c, 40); //pid->integrator_c = pid->integrator_c + (refreshRate/pid->Ki_c)*(pid->outputPID_c - pid->outputPID_unsat_c); //------Save states and send PWM to motors pid->lastCurrentCentroid = pid->currentCentroid; pid->lastError_c = pid->error_c; //------Condition output on current velocity if (pid->currentVelocity < 0) pid->outputPID_c = -pid->outputPID_c; if (pid->currentVelocity > -20 && pid->currentVelocity < 20) pid->outputPID_c = 0; if (pid->outputPID_c == 0) pid->outputPID_c = 0; SetServo(RC_STR_SERVO, pid->outputPID_c); }
void safe_timer(long long *tm, int period_mcs) { uint64_t time_now; uint64_t period_ns; struct timespec wake_up_time; long long *dummy; dummy = tm; ifperrs(ClockTime(CLOCK_MONOTONIC, NULL, &time_now) == -1, AR_ERR_SYS_RESOURCES, strerror(errno)); period_ns = period_mcs*1000; time_now = time_now - time_now % period_ns + period_ns; wake_up_time.tv_sec = time_now/1000000000LL; wake_up_time.tv_nsec = time_now % 1000000000LL; errp(clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &wake_up_time, NULL)); }
TimeStamp TimeStamp::Now(bool aHighResolution) { return TimeStamp(ClockTime()); }
void PID_UpdateRadius(PID * pid) { int P, I, D; double refreshRate; CPU_MSR msr; //------Read Encoder and clock msr = DISABLE_INTERRUPTS(); float desiredRadius = pid->desiredRadiusPID;//----//get info from camera here uint32 nowClocks = ClockTime(); // Gyro_Calculation(pid->gyro); RESTORE_INTERRUPTS(msr); //------Time since last function call in seconds refreshRate = refresh_rate(nowClocks, pid->lastClockTicks_r); pid->lastClockTicks_r = nowClocks; //------Current Radius at front of car pid->currentRadius = pid->gyro->frontRadius; if (pid->currentRadius > 6000) pid->currentRadius = 6000; else if (pid->currentRadius < -6000) pid->currentRadius = -6000; //------Calculate Error pid->error_r = ((float)(pid->currentRadius - desiredRadius))/(pid->currentRadius * desiredRadius); //------Update Derivative pid->differentiator_r = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator_r) + ((2/((2*pid->Tau)+refreshRate))*(pid->currentRadius - pid->lastCurrentRadius)); //------Update integrator - AntiWindup(only use the integrator if we are close, but not too close) if (abs(((int)(1000000*pid->error_r))) > 200) pid->integrator_r = ((int)((pid->integrator_r))) + ((int)(1000000*((refreshRate/2)*(pid->error_r + pid->lastError_r)))); //------Output Calculation //pid->Ki_r = pid->Ki_r/1000000; P = (pid->Kp_r * pid->error_r); I = (pid->Ki_r * pid->integrator_r); D = (pid->Kd_r * pid->differentiator_r); pid->radiusEqualibrium = (40000.0/desiredRadius); // Wireless_Debug("P: "); // PrintInt(P); // Wireless_Debug("\n\r"); // Wireless_Debug("Integral: "); // PrintFloat(pid->integrator_r); // Wireless_Debug("\n\r"); // Wireless_Debug("error: "); // PrintFloat(pid->error_r); // Wireless_Debug("\n\r"); // Wireless_Debug("lastError: "); // PrintFloat(pid->lastError_r); // Wireless_Debug("\n\r"); // Wireless_Debug("Refresh: "); // PrintFloat(refreshRate); // Wireless_Debug("\n\r"); // Wireless_Debug("I: "); // PrintInt(I); // Wireless_Debug("\n\r"); // Wireless_Debug("velocityBack: "); // PrintFloat(pid->currentVelocityBack); // Wireless_Debug("\n\r"); // Wireless_Debug("velocityFront: "); // PrintFloat(pid->currentVelocity); // Wireless_Debug("\n\r"); // Wireless_Debug("-----------------------------"); // Wireless_Debug("\n\r"); // Wireless_Debug("D: "); // PrintInt(D); // Wireless_Debug("\n\r"); // Wireless_ControlLog_Ext(pid->currentRadius, pid->desiredRadiusPID, pid->error, pid->outputPID_unsat, P); //pid->outputPID_unsat_r = (P + I - D) + pid->radiusEqualibrium; pid->outputPID_unsat_r = pid->radiusEqualibrium; pid->outputPID_r = sat(pid->outputPID_unsat_r, 40); //pid->integrator_r = ((int)pid->integrator_r) + ((int)(1000000*(refreshRate/pid->Ki_r)*(pid->outputPID_r - pid->outputPID_unsat_r))); //------Save Info for graph // Wireless_ControlLog_Ext(pid->currentRadius, desiredRadius, pid->error_r, pid->outputPID_unsat_r, I); //------Save states and send PWM to motors pid->lastCurrentRadius = pid->currentRadius; pid->lastError_r = pid->error_r; pid->lastIntegrator_r = pid->integrator_r; // Wireless_Debug("Last Error: "); // PrintFloat(pid->lastError_r); // Wireless_Debug("\n\r"); // Wireless_Debug("Integral: "); // PrintFloat(pid->integrator_r); // Wireless_Debug("\n\r"); // Wireless_Debug("-----------------------------"); // Wireless_Debug("\n\r"); SetServo(RC_STR_SERVO, pid->outputPID_r); }
TimeStamp TimeStamp::Now() { return TimeStamp(ClockTime()); }
void PID_UpdateVelocity(PID * pid) { int desiredVelocity = pid->desiredVelocityPID; float P, I, D; float refreshRate; CPU_MSR msr; //------Read Encoder and clock msr = DISABLE_INTERRUPTS(); pid->encoderValue = getTicks(); uint32 nowClocks = ClockTime(); Gyro_Calculation(pid->gyro); RESTORE_INTERRUPTS(msr); //------Time since last function call in seconds refreshRate = refresh_rate(nowClocks, pid->lastClockTicks); pid->lastClockTicks = nowClocks; //------Distance since last function call in ticks int encoderDifference = pid->encoderValue - pid->lastEncoderValue; //------Calculate Velocity pid->currentVelocity = (int)((encoderDifference) / (refreshRate)); //pid->currentVelocityBack = (int)((encoderDifference) / (refreshRate)); //------Convert Back Velocity to front //pid->currentVelocity = Gyro_VelocityBackToFront(pid->gyro,pid->currentVelocityBack); //------Calculate Error pid->error = desiredVelocity - (pid->currentVelocity); //------Update Derivative //pid->differentiator = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator) + ((2/((2*pid->Tau)+refreshRate))*(pid->currentVelocity - pid->lastCurrentVelocity)); pid->differentiator = ((((2*pid->Tau)-refreshRate)/((2*pid->Tau)+refreshRate))*pid->differentiator) + ((2/((2*pid->Tau)+refreshRate))*(pid->error - pid->lastError)); //------Update integrator - AntiWindup(only use the integrator if we are close, but not too close) //If we have switched directions, zero the integrator if (abs(pid->lastDesiredVelocity) / pid->lastDesiredVelocity != abs(pid->desiredVelocityPID) / pid->desiredVelocityPID) { pid->integrator = 0; } else { pid->integrator = pid->integrator + ((refreshRate/2)*(pid->error + pid->lastError)); } //------Output Calculation P = pid->Kp * pid->error; I = pid->Ki * pid->integrator; D = pid->Kd * pid->differentiator; pid->outputPID_unsat = ((int)P) + ((int)I) - ((int)D); pid->outputPID = sat(pid->outputPID_unsat, 40); //pid->outputPID = downShift(pid->error, -600, 7); pid->integrator = pid->integrator + (refreshRate/pid->Ki)*(pid->outputPID - pid->outputPID_unsat); //Wireless_ControlLog_Ext(pid->currentVelocity, desiredVelocity, pid->error, pid->outputPID_unsat, P); //------Save states and send PWM to motors pid->lastEncoderValue = pid->encoderValue; pid->lastCurrentVelocity = pid->currentVelocity; pid->lastDesiredVelocity = desiredVelocity; SetServo(RC_VEL_SERVO, (int)pid->outputPID); }
void updateDistanceOutput() { int32 desiredDistance = pid.desiredDistancePID; int32 P, I, D; uint32 deltaClocks; CPU_MSR msr; msr = DISABLE_INTERRUPTS(); pid.encoderValue = getTicks(); uint32 nowClocks = ClockTime(); RESTORE_INTERRUPTS(msr); //logData(desiredDistance, pid.encoderValue); //------Time since last function call in seconds uint32 maxClocks = 0xffffffff; if ((nowClocks < pid.lastClockTicks)) deltaClocks = (maxClocks-pid.lastClockTicks)+nowClocks; else deltaClocks = nowClocks - pid.lastClockTicks; float deltaTime = ((float)deltaClocks)/XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ ;//time passed since last function call: sec pid.error_d = desiredDistance - pid.encoderValue; // // xil_printf("Encoder Value: %d\r\n", pid.encoderValue); // xil_printf("Desired Distance: %d\r\n", desiredDistance); // xil_printf("error_d: %d\r\n", pid.error_d); //------Update integrator - AntiWindup(only use the integrator if we are close, but not too close) if ((pid.error_d < 1000 && pid.error_d > -1000) && (pid.error_d > 100 || pid.error_d < -100)){ pid.integrator_d = pid.integrator_d + (deltaTime/2)*(pid.error_d + pid.lastError_d); } else pid.integrator_d = 0; //------only use the integrator if we are close, but not too close // if (pid.error_d < 500 && pid.error_d > -500 && // (pid.error_d > 100 || pid.error_d < -100)) // pid.integrator_d += pid.error_d * deltaTime; // else // pid.integrator_d = 0; //print("integrator: "); //PrintFloat(pid.integrator_d); //print("\n\r"); //------Update Derivative pid.differentiator_d = (2*pid.Tau-deltaTime)/(2*pid.Tau+deltaTime)*pid.differentiator_d + 2/(2*pid.Tau+deltaTime)*(pid.error_d-pid.lastError_d); //print("differentiator: "); //PrintFloat(pid.differentiator_d); P = pid.Kp_d * pid.error_d; I = pid.Ki_d * pid.integrator_d; D = pid.Kd_d * pid.differentiator_d; //print("P: "); //PrintFloat(P); //print("\n\r"); //print("I: "); //PrintFloat(I); //print("\n\r"); //print("D: "); //PrintFloat(D); //print("\n\r"); pid.outputPID_unsat = P + I - D; pid.outputPID = sat(pid.outputPID_unsat, 60); //------if we are really close, don't do anything! if (pid.error_d < 100 && pid.error_d > -100) { pid.outputPID = 0; } //------Integrator Anti-windup // if (pid.Ki_d != 0.0f) { // pid.integrator_d = pid.integrator_d + deltaTime/pid.Ki_d * (pid.outputPID - pid.outputPID_unsat); // } // print("integrator_antiWind: "); // PrintFloat(pid.integrator_d); //print("\n\r"); pid.lastError_d = pid.error_d; pid.lastDesiredDistance = desiredDistance; SetServo(RC_VEL_SERVO, pid.outputPID); //xil_printf("Output PID: %d\r\n", pid.outputPID); //print("--------------------------------------\r\n"); }
void updateVelocityOutput() { float desiredVelocity = pid.desiredVelocityPID; float P, I, D, currentVelocity; uint32 deltaClocks; CPU_MSR msr; //------Read Encoder and clock msr = DISABLE_INTERRUPTS(); pid.encoderValue = getTicks(); uint32 nowClocks = ClockTime(); RESTORE_INTERRUPTS(msr); //------Time since last function call in seconds uint32 maxClocks = 0xffffffff; if ((nowClocks < pid.lastClockTicks)) deltaClocks = (maxClocks-pid.lastClockTicks)+nowClocks; else deltaClocks = nowClocks - pid.lastClockTicks; float refreshRate = ((float)deltaClocks)/XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ ;//time passed since last function call: sec //uint32 refreshRate = (deltaClocks)/(XPAR_CPU_PPC405_CORE_CLOCK_FREQ_HZ/1000) ; //xil_printf("nowClock: %d\r\n", nowClocks); //xil_printf("pid.lastClockTicks: %d\r\n", pid.lastClockTicks); //xil_printf("Delta Clock: %d\r\n", deltaClocks); //xil_printf("Refresh Rate: "); //PrintFloat(refreshRate); //print("\n\r"); pid.lastClockTicks = nowClocks; //------Distance since last function call in ticks int encoderDifference = pid.encoderValue - pid.lastEncoderValue; //xil_printf("Encoder Value: %d\r\n", pid.encoderValue); //xil_printf("Last Encoder Value: %d\r\n", pid.lastEncoderValue); //xil_printf("Encoder Diff: %d\r\n", encoderDifference); //------Calculate velocity currentVelocity = ((encoderDifference) / (refreshRate)); //xil_printf("Current Velocity: "); PrintFloat(currentVelocity); xil_printf(","); //xil_printf("Desired Velocity: "); PrintFloat(desiredVelocity); xil_printf(","); //print("\n\r"); //------Error pid.error = desiredVelocity - (currentVelocity); // xil_printf("error: "); // PrintFloat(pid.error); // print("\n\r"); //------Update Derivative pid.differentiator = (2*pid.Tau-refreshRate)/(2*pid.Tau+refreshRate)*pid.differentiator + 2/(2*pid.Tau+refreshRate)*(pid.error-pid.lastError); //xil_printf("differentiator: "); //PrintFloat(pid.differentiator); //print("\n\r"); //------Update integrator - AntiWindup(only use the integrator if we are close, but not too close) if ((pid.error < 1500 && pid.error > -1500) && (pid.error > 10 || pid.error < -10)){ pid.integrator = pid.integrator + (refreshRate/2)*(pid.error + pid.lastError); } else { pid.integrator = 0; } // if(pid.error == 0) { // pid.integrator = 0; // } // xil_printf("integrator: "); // PrintFloat(pid.integrator); // print("\n\r"); //------Output Calculation P = pid.Kp * pid.error; I = pid.Ki * pid.integrator; D = pid.Kd * 0;//pid.differentiator; pid.outputPID_unsat = P + I - D; pid.outputPID = sat(pid.outputPID_unsat, 60); //------Save states and send PWM to motors pid.lastEncoderValue = pid.encoderValue; pid.lastCurrentVelocity = currentVelocity; pid.lastDesiredVelocity = desiredVelocity; SetServo(RC_VEL_SERVO, (int)pid.outputPID); // xil_printf("Output PID unsat: "); // PrintFloat(pid.outputPID_unsat); // print("\n\r"); // xil_printf("Output PID: %d", pid.outputPID); // print("--------------------------------------\r\n"); }
static void *syspoll_thread( void *p ) { int pool_id; int ret; uint64_t clk; uint64_t rdata; int timeout; DIR *dir; struct dirent* dir_entry; sha1_ctx_t context; uint8_t digest[20]; char as_path[256]; int fd; int i; debug_thread_t debug_tid; debug_process_t debug_pid; ret = yarrow_add_source( Yarrow, &pool_id ); if( ret != 0 ) { slogf( _SLOGC_CHAR, _SLOG_CRITICAL, "random: Unable to get pool_id for syspoll thread." ); return NULL; } /* Setup a default 1 minute timeout */ timeout = 1000 * 60; while( 1 ) { if( Yarrow ) { yarrow_output( Yarrow, (uint8_t *)&rdata, sizeof( rdata ) ); /* Wait from ~16s to ~17.5 minutes */ timeout = ( ( rdata & 0x3F ) + 1 ) << 14; } delay( timeout ); SHA1Init( &context ); dir = opendir( "/proc" ); if( dir == NULL ) continue; dir_entry = readdir( dir ); while( dir_entry != NULL ) { SHA1Update( &context, (uint8_t *)dir_entry, sizeof( struct dirent ) + dir_entry->d_namelen - 1 ); if( dir_entry->d_name[0] < '0' || dir_entry->d_name[1] > '9' ) { dir_entry = readdir( dir ); continue; } sprintf( as_path, "/proc/%s/as", dir_entry->d_name ); fd = open( as_path, O_RDONLY ); while( fd != -1 ) { memset( &debug_pid, 0, sizeof( debug_pid ) ); ret = devctl( fd, DCMD_PROC_INFO, &debug_pid, sizeof( debug_pid ), NULL ); if( ret == -1 ) break; SHA1Update( &context, (uint8_t *)&debug_pid, sizeof( debug_pid ) ); for( i=0; i<debug_pid.num_threads; i++ ) { memset( &debug_tid, 0, sizeof( debug_tid ) ); debug_tid.tid = i + 1; ret = devctl( fd, DCMD_PROC_TIDSTATUS, &debug_tid, sizeof( debug_tid ), NULL ); if( ret == -1 ) continue; SHA1Update( &context, (uint8_t *)&debug_tid, sizeof( debug_tid ) ); } break; } if( fd != -1 ) close( fd ); dir_entry = readdir( dir ); } closedir( dir ); SHA1Update( &context, (uint8_t *)&rdata, sizeof( rdata ) ); ClockTime( CLOCK_REALTIME, NULL, &clk ); SHA1Update( &context, (uint8_t *)&clk, sizeof( clk ) ); SHA1Final( digest, &context ); if( Yarrow ) { /* Assume 1 bit in every 8 is random */ yarrow_input( Yarrow, digest, sizeof( digest ), pool_id, sizeof( digest ) ); } } return NULL; }
static void *interrupt_thread( void *p ) { int ret; int intr; int intr_id; struct sigevent event; struct _pulse pulse; iov_t iov; int rcvid; int chid; int coid; int count; uint64_t rdata; uint16_t target; uint64_t clk; int pool_id; intr = (int)p; rdata = 0; target = 512; ret = ThreadCtl( _NTO_TCTL_IO, 0 ); if( ret != 0 ) { slogf( _SLOGC_CHAR, _SLOG_CRITICAL, "random: Unable to gain IO privs: %s", strerror( errno ) ); return NULL; } chid = ChannelCreate( 0 ); if( chid == -1 ) { slogf( _SLOGC_CHAR, _SLOG_CRITICAL, "random: ChannelCreate() failed: %s", strerror( errno ) ); return NULL; } coid = ConnectAttach( 0, 0, chid, _NTO_SIDE_CHANNEL, 0 ); if( coid == -1 ) { slogf( _SLOGC_CHAR, _SLOG_CRITICAL, "random: ConnectAttach() failed: %s", strerror( errno ) ); return NULL; } ret = yarrow_add_source( Yarrow, &pool_id ); if( ret != 0 ) { slogf( _SLOGC_CHAR, _SLOG_CRITICAL, "random: Unable to get pool_id for interrupt %d thread.", intr ); return NULL; } event.sigev_notify = SIGEV_PULSE; event.sigev_coid = coid; event.sigev_code = 1; event.sigev_priority = 15; intr_id = InterruptAttachEvent( intr, &event, _NTO_INTR_FLAGS_TRK_MSK ); if( intr_id == -1 ) { slogf( _SLOGC_CHAR, _SLOG_CRITICAL, "random: Unable to attach event to intr %d: %s", intr, strerror( errno ) ); return NULL; } /* This is how many interrupts we are gonna get */ if( Yarrow ) { yarrow_output( Yarrow, (uint8_t *)&rdata, sizeof( rdata ) ); target = rdata & 0x1FF; } count = 0; SETIOV( &iov, &pulse, sizeof( pulse ) ); while( 1 ) { rcvid = MsgReceivev( chid, &iov, 1, NULL ); if( rcvid == -1 ) { if( errno == ESRCH ) return NULL; continue; } switch( pulse.code ) { case 1: InterruptUnmask( intr, intr_id ); count++; if( count >= target ) { ClockTime( CLOCK_REALTIME, NULL, &clk ); clk = clk ^ rdata; if( Yarrow ) { yarrow_input( Yarrow, (uint8_t *)&clk, sizeof( clk ), pool_id, 8 ); yarrow_output( Yarrow, (uint8_t *)&rdata, sizeof( rdata ) ); } target = rdata & 0x1FF; count = 0; } break; default: if( rcvid ) MsgError( rcvid, ENOTSUP ); } } return NULL; }
uint64_t RealClockTime() { UpdateClock(); return ClockTime(); }
void StopWatch::StopTimer() { end = ClockTime(); }
void StopWatch::StartTimer( ) { start = ClockTime(); }