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