void btMultiBody::fillContactJacobian(int link,
const btVector3 &contact_point,
const btVector3 &normal,
btScalar *jac,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m) const
{
// temporary space
int num_links = getNumLinks();
scratch_v.resize(2*num_links + 2);
scratch_m.resize(num_links + 1);
btVector3 * v_ptr = &scratch_v[0];
btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
scratch_r.resize(num_links);
btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
btMatrix3x3 * rot_from_world = &scratch_m[0];
const btVector3 p_minus_com_world = contact_point - base_pos;
rot_from_world[0] = btMatrix3x3(base_quat);
p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
n_local[0] = rot_from_world[0] * normal;
// omega coeffients first.
btVector3 omega_coeffs;
omega_coeffs = p_minus_com_world.cross(normal);
jac[0] = omega_coeffs[0];
jac[1] = omega_coeffs[1];
jac[2] = omega_coeffs[2];
// then v coefficients
jac[3] = normal[0];
jac[4] = normal[1];
jac[5] = normal[2];
// Set remaining jac values to zero for now.
for (int i = 6; i < 6 + num_links; ++i) {
jac[i] = 0;
}
// Qdot coefficients, if necessary.
if (num_links > 0 && link > -1) {
// TODO: speed this up -- don't calculate for links we don't need.
// (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
// which is resulting in repeated work being done...)
// calculate required normals & positions in the local frames.
for (int i = 0; i < num_links; ++i) {
// transform to local frame
const int parent = links[i].parent;
const btMatrix3x3 mtx(links[i].cached_rot_parent_to_this);
rot_from_world[i+1] = mtx * rot_from_world[parent+1];
n_local[i+1] = mtx * n_local[parent+1];
p_minus_com[i+1] = mtx * p_minus_com[parent+1] - links[i].cached_r_vector;
// calculate the jacobian entry
if (links[i].is_revolute) {
results[i] = n_local[i+1].dot( links[i].axis_top.cross(p_minus_com[i+1]) + links[i].axis_bottom );
} else {
results[i] = n_local[i+1].dot( links[i].axis_bottom );
}
}
// Now copy through to output.
while (link != -1) {
jac[6 + link] = results[link];
link = links[link].parent;
}
}
}
int main(int argc, char *argv[])
{
int pri,retVal,firstTime=1,ind=0;
time_t lastRefresh=0;
time_t lastUpdate=0;
time_t currentTime=0;
char *tempString=0;
char *progName=basename(argv[0]);
// last_send.tv_sec = 0;
// last_send.tv_nsec = 0;
//Sort out PID File
retVal=sortOutPidFile(progName);
if(retVal!=0) {
return retVal;
}
//Directory listing tools
// char currentTouchname[FILENAME_MAX];
char currentHeader[FILENAME_MAX];
int numLinks[NUM_PRIORITIES]={0};
int totalEventLinks=0;
int wdEvents[NUM_PRIORITIES]={0};
// int sillyEvNum[NUM_PRIORITIES]={0};
/* Set signal handlers */
signal(SIGUSR1, sigUsr1Handler);
signal(SIGUSR2, sigUsr2Handler);
signal(SIGTERM, handleBadSigs);
signal(SIGINT,handleBadSigs);
signal(SIGSEGV,handleBadSigs);
/* Setup log */
setlogmask(LOG_UPTO(LOG_INFO));
openlog (progName, LOG_PID, ANITA_LOG_FACILITY) ;
// Load Config
retVal=readConfig();
if(!laptopDebug) {
retVal=initDevice();
if(retVal!=0) {
return 1;
}
}
else {
retVal=0;
printf("Running in debug mode not actually trying to talk to device\n");
makeDirectories(fakeOutputDir);
}
makeDirectories(PRIORITIZERD_EVENT_LINK_DIR);
for(ind=0;ind<NUM_HK_TELEM_DIRS;ind++) {
makeDirectories(telemLinkDirs[ind]);
}
//Fill event dir names
for(pri=0;pri<NUM_PRIORITIES;pri++) {
sprintf(eventTelemDirs[pri],"%s/%s%d",BASE_EVENT_TELEM_DIR,
EVENT_PRI_PREFIX,pri);
sprintf(eventTelemLinkDirs[pri],"%s/link",eventTelemDirs[pri]);
makeDirectories(eventTelemLinkDirs[pri]);
if(sendData) {
//And setup inotify watches
if(wdEvents[pri]==0) {
wdEvents[pri]=setupLinkWatchDir(eventTelemLinkDirs[pri]);
if(wdEvents[pri]<=0) {
fprintf(stderr,"Unable to watch %s\n",eventTelemLinkDirs[pri]);
syslog(LOG_ERR,"Unable to watch %s\n",eventTelemLinkDirs[pri]);
handleBadSigs(0);
}
numLinks[pri]=getNumLinks(wdEvents[pri]);
}
}
}
time(&lastRefresh);
pthread_t thread;
retVal = pthread_create(&thread, 0, watchdogThread,0);
if (retVal)
{
syslog(LOG_ERR, "LOSd: create watchdog thread returned %d\n", retVal);
}
do {
if(verbosity) printf("Initializing LOSd\n");
int lastSendData=sendData;
retVal=readConfig();
if(lastSendData!=sendData) {
break; //Lets restart LOSd
}
if(firstTime) {
sendWakeUpBuffer();
firstTime=0;
}
int hkCount=0;
currentState=PROG_STATE_RUN;
while(currentState==PROG_STATE_RUN) {
//This is just if LOSd has been turned off when we're out of range
if(!sendData) {
sleep(1);
continue;
}
//Check to see if we need to refresh the links
time(¤tTime);
if(currentTime>lastRefresh+REFRESH_LINKS_EVERY) {
refreshLinkDirs();
lastRefresh=currentTime;
}
//Work which priority we're doing
currentPri=priorityOrder[orderIndex];
//Will actually change this to almost work in the future
if(hkCount%5==0) fillBufferWithHk();
hkCount++;
//Check the link dirs
if(currentTime>lastUpdate+10 || totalEventLinks<1) {
if(totalEventLinks<1)
checkLinkDirs(1,0);
else
checkLinkDirs(0,1); //Maybe I don't need to do this every time
lastUpdate=currentTime;
}
totalEventLinks=0;
for(pri=0;pri<NUM_PRIORITIES;pri++) {
numLinks[pri]=getNumLinks(wdEvents[pri]);
totalEventLinks+=numLinks[pri];
}
if(printToScreen && verbosity>1) {
printf("Got %d links in %s\n",numLinks[currentPri],
eventTelemLinkDirs[currentPri]);
}
// sillyEvNum[currentPri]=0;
if(numLinks[currentPri]>0) {
//Got an event
tempString=getLastLink(wdEvents[currentPri]);
sprintf(currentHeader,"%s/%s",eventTelemLinkDirs[currentPri],
tempString);
if(printToScreen && verbosity)
printf("Starting %s\n",currentHeader);
readAndSendEventRamdisk(currentHeader); //Also deletes
numLinks[currentPri]--;
totalEventLinks--;
}
//Again I need to improve the bandwidth sharing between data and hk
fillBufferWithHk();
// if(totalEventLinks<1) usleep(100);
// printf("totalEventLinks %d\n",totalEventLinks);
orderIndex++;
if(orderIndex>=numOrders) orderIndex=0;
}
} while(currentState==PROG_STATE_INIT);
unlink(LOSD_PID_FILE);
syslog(LOG_INFO,"LOSd terminating");
// fprintf(stderr, "Bye bye\n");
return 0;
}
void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
{
// Temporary matrices/vectors -- use scratch space from caller
// so that we don't have to keep reallocating every frame
int num_links = getNumLinks();
scratch_r.resize(num_links);
scratch_v.resize(4*num_links + 4);
btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
btVector3 * v_ptr = &scratch_v[0];
// zhat_i^A (scratch space)
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
// rot_from_parent (cached from calcAccelerations)
const btMatrix3x3 * rot_from_parent = &matrix_buf[0];
// hhat (cached), accel (scratch)
const btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
const btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
// Y_i (scratch), D_i (cached)
btScalar * Y = r_ptr; r_ptr += num_links;
const btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
// First 'upward' loop.
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
btVector3 input_force(force[3],force[4],force[5]);
btVector3 input_torque(force[0],force[1],force[2]);
// Fill in zero_acc
// -- set to force/torque on the base, zero otherwise
if (fixed_base)
{
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
} else
{
zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque);
}
for (int i = 0; i < num_links; ++i)
{
zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
}
// 'Downward' loop.
for (int i = num_links - 1; i >= 0; --i)
{
Y[i] = - SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
Y[i] += force[6 + i]; // add joint torque
const int parent = links[i].parent;
// Zp += pXi * (Zi + hi*Yi/Di)
btVector3 in_top, in_bottom, out_top, out_bottom;
const btScalar Y_over_D = Y[i] / D[i];
in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
in_top, in_bottom, out_top, out_bottom);
zero_acc_top_angular[parent+1] += out_top;
zero_acc_bottom_linear[parent+1] += out_bottom;
}
// ptr to the joint accel part of the output
btScalar * joint_accel = output + 6;
// Second 'upward' loop
if (fixed_base)
{
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
} else
{
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
// ECH
//float result[6];
btScalar result[6];
solveImatrix(rhs_top,rhs_bot, result);
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
for (int i = 0; i < 3; ++i) {
accel_top[0][i] = -result[i];
accel_bottom[0][i] = -result[i+3];
}
}
// now do the loop over the links
for (int i = 0; i < num_links; ++i) {
const int parent = links[i].parent;
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
accel_top[parent+1], accel_bottom[parent+1],
accel_top[i+1], accel_bottom[i+1]);
joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
accel_top[i+1] += joint_accel[i] * links[i].axis_top;
accel_bottom[i+1] += joint_accel[i] * links[i].axis_bottom;
}
// transform base accelerations back to the world frame.
btVector3 omegadot_out;
omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
output[0] = omegadot_out[0];
output[1] = omegadot_out[1];
output[2] = omegadot_out[2];
btVector3 vdot_out;
vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
output[3] = vdot_out[0];
output[4] = vdot_out[1];
output[5] = vdot_out[2];
}
void btMultiBody::stepVelocities(btScalar dt,
btAlignedObjectArray<btScalar> &scratch_r,
btAlignedObjectArray<btVector3> &scratch_v,
btAlignedObjectArray<btMatrix3x3> &scratch_m)
{
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
// and the base linear & angular accelerations.
// We apply damping forces in this routine as well as any external forces specified by the
// caller (via addBaseForce etc).
// output should point to an array of 6 + num_links reals.
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
// num_links joint acceleration values.
int num_links = getNumLinks();
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
const btScalar DAMPING_K2_LINEAR = m_linearDamping;
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
btVector3 base_vel = getBaseVel();
btVector3 base_omega = getBaseOmega();
// Temporary matrices/vectors -- use scratch space from caller
// so that we don't have to keep reallocating every frame
scratch_r.resize(2*num_links + 6);
scratch_v.resize(8*num_links + 6);
scratch_m.resize(4*num_links + 4);
btScalar * r_ptr = &scratch_r[0];
btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results
btVector3 * v_ptr = &scratch_v[0];
// vhat_i (top = angular, bottom = linear part)
btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
// zhat_i^A
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
// chat_i (note NOT defined for the base)
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
// top left, top right and bottom left blocks of Ihat_i^A.
// bottom right block = transpose of top left block and is not stored.
// Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
// Cached 3x3 rotation matrices from parent frame to this frame.
btMatrix3x3 * rot_from_parent = &matrix_buf[0];
btMatrix3x3 * rot_from_world = &scratch_m[0];
// hhat_i, ahat_i
// hhat is NOT stored for the base (but ahat is)
btVector3 * h_top = num_links > 0 ? &vector_buf[0] : 0;
btVector3 * h_bottom = num_links > 0 ? &vector_buf[num_links] : 0;
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
// Y_i, D_i
btScalar * Y = r_ptr; r_ptr += num_links;
btScalar * D = num_links > 0 ? &m_real_buf[6 + num_links] : 0;
// ptr to the joint accel part of the output
btScalar * joint_accel = output + 6;
// Start of the algorithm proper.
// First 'upward' loop.
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
rot_from_parent[0] = btMatrix3x3(base_quat);
vel_top_angular[0] = rot_from_parent[0] * base_omega;
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
if (fixed_base) {
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
} else {
zero_acc_top_angular[0] = - (rot_from_parent[0] * (base_force
- base_mass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
zero_acc_bottom_linear[0] =
- (rot_from_parent[0] * base_torque);
if (m_useGyroTerm)
zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( base_inertia * vel_top_angular[0] );
zero_acc_bottom_linear[0] += base_inertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
}
inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[0].setValue(base_mass, 0, 0,
0, base_mass, 0,
0, 0, base_mass);
inertia_bottom_left[0].setValue(base_inertia[0], 0, 0,
0, base_inertia[1], 0,
0, 0, base_inertia[2]);
rot_from_world[0] = rot_from_parent[0];
for (int i = 0; i < num_links; ++i) {
const int parent = links[i].parent;
rot_from_parent[i+1] = btMatrix3x3(links[i].cached_rot_parent_to_this);
rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
// vhat_i = i_xhat_p(i) * vhat_p(i)
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
vel_top_angular[parent+1], vel_bottom_linear[parent+1],
vel_top_angular[i+1], vel_bottom_linear[i+1]);
// we can now calculate chat_i
// remember vhat_i is really vhat_p(i) (but in current frame) at this point
coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(links[i].cached_r_vector))
+ 2 * vel_top_angular[i+1].cross(links[i].axis_bottom) * getJointVel(i);
if (links[i].is_revolute) {
coriolis_top_angular[i] = vel_top_angular[i+1].cross(links[i].axis_top) * getJointVel(i);
coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * links[i].axis_top.cross(links[i].axis_bottom);
} else {
coriolis_top_angular[i] = btVector3(0,0,0);
}
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
vel_top_angular[i+1] += getJointVel(i) * links[i].axis_top;
vel_bottom_linear[i+1] += getJointVel(i) * links[i].axis_bottom;
// calculate zhat_i^A
zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (links[i].applied_force));
zero_acc_top_angular[i+1] += links[i].mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
zero_acc_bottom_linear[i+1] =
- (rot_from_world[i+1] * links[i].applied_torque);
if (m_useGyroTerm)
{
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( links[i].inertia * vel_top_angular[i+1] );
}
zero_acc_bottom_linear[i+1] += links[i].inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
// calculate Ihat_i^A
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
inertia_top_right[i+1].setValue(links[i].mass, 0, 0,
0, links[i].mass, 0,
0, 0, links[i].mass);
inertia_bottom_left[i+1].setValue(links[i].inertia[0], 0, 0,
0, links[i].inertia[1], 0,
0, 0, links[i].inertia[2]);
}
// 'Downward' loop.
// (part of TreeForwardDynamics in Mirtich.)
for (int i = num_links - 1; i >= 0; --i) {
h_top[i] = inertia_top_left[i+1] * links[i].axis_top + inertia_top_right[i+1] * links[i].axis_bottom;
h_bottom[i] = inertia_bottom_left[i+1] * links[i].axis_top + inertia_top_left[i+1].transpose() * links[i].axis_bottom;
btScalar val = SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, h_top[i], h_bottom[i]);
D[i] = val;
Y[i] = links[i].joint_torque
- SpatialDotProduct(links[i].axis_top, links[i].axis_bottom, zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
const int parent = links[i].parent;
// Ip += pXi * (Ii - hi hi' / Di) * iXp
const btScalar one_over_di = 1.0f / D[i];
const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
btMatrix3x3 r_cross;
r_cross.setValue(
0, -links[i].cached_r_vector[2], links[i].cached_r_vector[1],
links[i].cached_r_vector[2], 0, -links[i].cached_r_vector[0],
-links[i].cached_r_vector[1], links[i].cached_r_vector[0], 0);
inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
(r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
// Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
btVector3 in_top, in_bottom, out_top, out_bottom;
const btScalar Y_over_D = Y[i] * one_over_di;
in_top = zero_acc_top_angular[i+1]
+ inertia_top_left[i+1] * coriolis_top_angular[i]
+ inertia_top_right[i+1] * coriolis_bottom_linear[i]
+ Y_over_D * h_top[i];
in_bottom = zero_acc_bottom_linear[i+1]
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
+ Y_over_D * h_bottom[i];
InverseSpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
in_top, in_bottom, out_top, out_bottom);
zero_acc_top_angular[parent+1] += out_top;
zero_acc_bottom_linear[parent+1] += out_bottom;
}
// Second 'upward' loop
// (part of TreeForwardDynamics in Mirtich)
if (fixed_base)
{
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
}
else
{
if (num_links > 0)
{
//Matrix<btScalar, 6, 6> Imatrix;
//Imatrix.block<3,3>(0,0) = inertia_top_left[0];
//Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
//Imatrix.block<3,3>(0,3) = inertia_top_right[0];
//Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
//cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here?
cached_inertia_top_left = inertia_top_left[0];
cached_inertia_top_right = inertia_top_right[0];
cached_inertia_lower_left = inertia_bottom_left[0];
cached_inertia_lower_right= inertia_top_left[0].transpose();
}
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
// ECH
//float result[6];
btScalar result[6];
solveImatrix(rhs_top, rhs_bot, result);
// printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
for (int i = 0; i < 3; ++i) {
accel_top[0][i] = -result[i];
accel_bottom[0][i] = -result[i+3];
}
}
// now do the loop over the links
for (int i = 0; i < num_links; ++i) {
const int parent = links[i].parent;
SpatialTransform(rot_from_parent[i+1], links[i].cached_r_vector,
accel_top[parent+1], accel_bottom[parent+1],
accel_top[i+1], accel_bottom[i+1]);
joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * links[i].axis_top;
accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * links[i].axis_bottom;
}
// transform base accelerations back to the world frame.
btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
output[0] = omegadot_out[0];
output[1] = omegadot_out[1];
output[2] = omegadot_out[2];
btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
output[3] = vdot_out[0];
output[4] = vdot_out[1];
output[5] = vdot_out[2];
// Final step: add the accelerations (times dt) to the velocities.
applyDeltaVee(output, dt);
}
int main(int argc, char *argv[])
{
//Temporary variables
int retVal=0,pri=0;
/* Config file thingies */
// int status=0;
// char* eString ;
sprintf(currentOpenportDir,"%s",OPENPORT_OUTPUT_DIR);
char *progName=basename(argv[0]);
//
retVal=sortOutPidFile(progName);
if(retVal<0)
return -1;
/* Set signal handlers */
signal(SIGUSR1, sigUsr1Handler);
signal(SIGUSR2, sigUsr2Handler);
signal(SIGTERM, handleBadSigs);
signal(SIGINT, handleBadSigs);
signal(SIGSEGV, handleBadSigs);
signal(SIGRTMIN, refreshLinksHandler);
/* Setup log */
setlogmask(LOG_UPTO(LOG_INFO));
openlog (progName, LOG_PID, ANITA_LOG_FACILITY) ;
syslog(LOG_INFO,"Starting Openportd\n");
/* Load Config */
kvpReset () ;
// status = configLoad (GLOBAL_CONF_FILE,"global") ;
// eString = configErrorString (status) ;
//Fill event dir names
for(pri=0;pri<NUM_PRIORITIES;pri++) {
retVal=snprintf(eventTelemDirs[pri],sizeof(eventTelemDirs[pri]),"%s/%s%d",BASE_EVENT_TELEM_DIR,EVENT_PRI_PREFIX,pri);
if(retVal<0) {
syslog(LOG_ERR,"Error using snprintf -- %s",strerror(errno));
}
retVal=snprintf(eventTelemLinkDirs[pri],sizeof(eventTelemLinkDirs[pri]),"%s/link",eventTelemDirs[pri]);
if(retVal<0) {
syslog(LOG_ERR,"Error using snprintf -- %s",strerror(errno));
}
makeDirectories(eventTelemLinkDirs[pri]);
}
makeDirectories(OPENPORT_OUTPUT_DIR);
makeDirectories(OPENPORT_STAGE_DIR);
makeDirectories(OPENPORTD_CMD_ECHO_TELEM_LINK_DIR);
makeDirectories(HEADER_TELEM_LINK_DIR);
makeDirectories(ADU5A_SAT_TELEM_LINK_DIR);
makeDirectories(ADU5A_PAT_TELEM_LINK_DIR);
makeDirectories(ADU5A_VTG_TELEM_LINK_DIR);
makeDirectories(ADU5B_SAT_TELEM_LINK_DIR);
makeDirectories(ADU5B_PAT_TELEM_LINK_DIR);
makeDirectories(ADU5B_VTG_TELEM_LINK_DIR);
makeDirectories(G12_GGA_TELEM_LINK_DIR);
makeDirectories(ADU5A_GGA_TELEM_LINK_DIR);
makeDirectories(ADU5B_GGA_TELEM_LINK_DIR);
makeDirectories(G12_SAT_TELEM_LINK_DIR);
makeDirectories(G12_POS_TELEM_LINK_DIR);
makeDirectories(GPU_TELEM_LINK_DIR);
makeDirectories(PEDESTAL_TELEM_LINK_DIR);
makeDirectories(SURFHK_TELEM_LINK_DIR);
makeDirectories(TURFHK_TELEM_LINK_DIR);
makeDirectories(HK_TELEM_LINK_DIR);
makeDirectories(MONITOR_TELEM_LINK_DIR);
makeDirectories(OTHER_MONITOR_TELEM_LINK_DIR);
makeDirectories(REQUEST_TELEM_LINK_DIR);
makeDirectories(RTL_TELEM_LINK_DIR);
retVal=readConfig();
char currentHeader[FILENAME_MAX];
static int numLinks[NUM_PRIORITIES]={0};
static int numHkLinks[NUM_HK_TELEM_DIRS]={0};
int totalEventLinks=0;
int totalHkLinks=0;
int totalEventsSent=0;
time_t currentTime=0;
time_t lastRefresh=0;
int hkInd=0;
char *tempString=0;
int numSent=0;
//Setup inotify watches
for(pri=0;pri<NUM_PRIORITIES;pri++) {
//Setup inotify watches
if(wdEvents[pri]==0) {
wdEvents[pri]=setupLinkWatchDir(eventTelemLinkDirs[pri]);
if(wdEvents[pri]<=0) {
fprintf(stderr,"Unable to watch %s\n",eventTelemLinkDirs[pri]);
syslog(LOG_ERR,"Unable to watch %s\n",eventTelemLinkDirs[pri]);
handleBadSigs(0);
}
numLinks[pri]=getNumLinks(wdEvents[pri]);
}
}
for(hkInd=0;hkInd<NUM_HK_TELEM_DIRS;hkInd++) {
if(wdHks[hkInd]==0) {
wdHks[hkInd]=setupLinkWatchDir(telemLinkDirs[hkInd]);
if(wdHks[hkInd]<=0) {
fprintf(stderr,"Unable to watch %s\n",telemLinkDirs[hkInd]);
syslog(LOG_ERR,"Unable to watch %s\n",telemLinkDirs[hkInd]);
// handleBadSigs(0);
}
numHkLinks[hkInd]=getNumLinks(wdHks[hkInd]);
}
}
time(&lastRefresh);
int hkCount=0;
printf("Before while loop\n");
startCopyScript();
do {
if(printToScreen) printf("Initalizing Openportd\n");
currentState=PROG_STATE_RUN;
while(currentState==PROG_STATE_RUN) {
printf("Inside while loop\n");
if(!sendData) {
sleep(1);
continue;
}
//The idea is that we only write an output file if there is space for it
int numWaitingToTransfer=countFilesInDir(currentOpenportDir);
if(numWaitingToTransfer>2) {
fprintf(stderr,"There are %d files waiting to transer, will sleep now\n",numWaitingToTransfer);
sleep(1);
continue;
}
if(totalEventsSent%10==0) {
printf("Data sent\nEvents:\t");
for(pri=0;pri<NUM_PRIORITIES;pri++)
printf("%d ",numEventsSent[pri]);
printf("\nHk:\t");
for(hkInd=0;hkInd<NUM_HK_TELEM_DIRS;hkInd++)
printf("%d ",numHksSent[hkInd]);
printf("\n");
}
//Check to see if we need to refresh the links
time(¤tTime);
if(currentTime>lastRefresh+REFRESH_LINKS_EVERY || needToRefreshLinks) {
refreshLinkDirs();
lastRefresh=currentTime;
needToRefreshLinks=0;
}
//Update the link lists
//or some time has passed
if(totalEventLinks==0)
retVal=checkLinkDirs(1,0);
else
retVal=checkLinkDirs(0,1);
totalEventLinks=0;
for(pri=0;pri<NUM_PRIORITIES;pri++) {
numLinks[pri]=getNumLinks(wdEvents[pri]);
totalEventLinks+=numLinks[pri];
}
totalHkLinks=0;
for(hkInd=0;hkInd<NUM_HK_TELEM_DIRS;hkInd++) {
numHkLinks[hkInd]=getNumLinks(wdHks[hkInd]);
totalHkLinks+=numHkLinks[hkInd];
}
if(!retVal && totalHkLinks==0 && totalEventLinks==0) {
//No data
needToRefreshLinks=1;
usleep(1000);
continue;
}
printf("%d %d %d\n",totalEventLinks,totalHkLinks,retVal);
if(totalEventLinks) {
//Which priority are we sending
currentPri=priorityOrder[orderIndex];
int doneEvent=0;
while(!doneEvent && totalEventLinks>0) {
while(numLinks[currentPri]==0) {
orderIndex++;
if(orderIndex>=numOrders) orderIndex=0;
currentPri=priorityOrder[orderIndex];
}
printf("Trying priority %d -- numLinks %d\n",currentPri,numLinks[currentPri]);
if(numLinks[currentPri]) {
//Got an event
tempString=getLastLink(wdEvents[currentPri]);
retVal=snprintf(currentHeader,sizeof(currentHeader),"%s/%s",eventTelemLinkDirs[currentPri],tempString);
if(retVal<0) {
syslog(LOG_ERR,"Error using snprintf -- %s",strerror(errno));
}
// syslog(LOG_INFO,"Trying %s\n",currentHeader);
doneEvent=readAndSendEventRamdisk(currentHeader); //Also deletes
if(doneEvent) {
numEventsSent[currentPri]++;
totalEventsSent++;
}
numLinks[currentPri]--;
totalEventLinks--;
}
}
if(currentTime>lastRefresh+REFRESH_LINKS_EVERY ||
needToRefreshLinks) {
refreshLinkDirs();
lastRefresh=currentTime;
needToRefreshLinks=0;
totalHkLinks=0;
for(hkInd=0;hkInd<NUM_HK_TELEM_DIRS;hkInd++) {
numHkLinks[hkInd]=getNumLinks(wdHks[hkInd]);
totalHkLinks+=numHkLinks[hkInd];
}
}
//Now try to send some stuff I like
numSent=0;
if(numHkLinks[OPENPORT_TELEM_HEADER])
readHkAndOpenport(wdHks[OPENPORT_TELEM_HEADER],
headersPerEvent,HEADER_TELEM_DIR,
HEADER_TELEM_LINK_DIR,sizeof(AnitaEventHeader_t),&numSent);
numHksSent[OPENPORT_TELEM_HEADER]+=numSent;
numSent=0;
if(numHkLinks[OPENPORT_TELEM_CMD_ECHO])
readHkAndOpenport(wdHks[OPENPORT_TELEM_CMD_ECHO],echoesPerEvent,
OPENPORTD_CMD_ECHO_TELEM_DIR,
OPENPORTD_CMD_ECHO_TELEM_LINK_DIR,
sizeof(CommandEcho_t),&numSent);
numHksSent[OPENPORT_TELEM_CMD_ECHO]+=numSent;
numSent=0;
if(numHkLinks[OPENPORT_TELEM_HK])
readHkAndOpenport(wdHks[OPENPORT_TELEM_HK],hkPerEvent,HK_TELEM_DIR,
HK_TELEM_LINK_DIR,sizeof(HkDataStruct_t),&numSent);
numSent=0;
if(numHkLinks[OPENPORT_TELEM_ADU5A_PAT])
readHkAndOpenport(wdHks[OPENPORT_TELEM_ADU5A_PAT],1,
ADU5A_PAT_TELEM_DIR,
ADU5A_PAT_TELEM_LINK_DIR,
sizeof(GpsAdu5PatStruct_t),&numSent);
numHksSent[OPENPORT_TELEM_ADU5A_PAT]+=numSent;
numSent=0;
if(numHkLinks[OPENPORT_TELEM_MONITOR])
readHkAndOpenport(wdHks[OPENPORT_TELEM_MONITOR],monitorPerEvent,
MONITOR_TELEM_DIR,MONITOR_TELEM_LINK_DIR,
sizeof(MonitorStruct_t),&numSent);
numHksSent[OPENPORT_TELEM_MONITOR]+=numSent;
// else if(totalEventLinks==0) {
// usleep(1000);
// }
orderIndex++;
if(orderIndex>=numOrders) orderIndex=0;
}
//Now we try and send some data
//I'm going to try and modify this but for now
if(hkCount%eventBandwidth==0)
sendSomeHk(10000);
hkCount++;
}
} while (currentState == PROG_STATE_INIT);
stopCopyScript();
unlink(OPENPORTD_PID_FILE);
return 0;
}
int readHkAndOpenport(int wd,int maxCopy, char *telemDir, char *linkDir, int fileSize, int *numSent)
/* Looks in the specified directroy and OPENPORT's up to maxCopy bytes of data */
/* fileSize is the maximum size of a packet in the directory */
{
// fprintf(stderr,"readHkAndOpenport %s -- %d\n",linkDir,maxCopy);
char currentFilename[FILENAME_MAX];
char currentTouchname[FILENAME_MAX];
char currentLOSTouchname[FILENAME_MAX];
char currentLinkname[FILENAME_MAX];
int retVal,numLinks,count,numBytes,totalBytes=0;//,checkVal=0;
char *tempString;
GenericHeader_t *gHdr;
*numSent=0;
numLinks=getNumLinks(wd);
if(numLinks<=0) {
return 0;
}
int counter=0;
for(count=numLinks-1;count>=0;count--) {
//Get last link name
tempString=getLastLink(wd);
retVal=snprintf(currentFilename,sizeof(currentFilename),"%s/%s",telemDir,tempString);
if(retVal<0) {
syslog(LOG_ERR,"Error using snprintf -- %s",strerror(errno));
}
retVal=snprintf(currentTouchname,sizeof(currentTouchname),"%s.sipd",currentFilename);
if(retVal<0) {
syslog(LOG_ERR,"Error using snprintf -- %s",strerror(errno));
}
retVal=snprintf(currentLOSTouchname,sizeof(currentLOSTouchname),"%s.losd",currentFilename);
if(retVal<0) {
syslog(LOG_ERR,"Error using snprintf -- %s",strerror(errno));
}
retVal=snprintf(currentLinkname,sizeof(currentLinkname),"%s/%s",linkDir,tempString);
if(retVal<0) {
syslog(LOG_ERR,"Error using snprintf -- %s",strerror(errno));
}
if(!checkFileExists(currentLinkname))
continue;
if(checkFileExists(currentLOSTouchname))
continue;
touchFile(currentTouchname);
if(checkFileExists(currentLOSTouchname)) {
removeFile(currentTouchname);
continue;
}
retVal=genericReadOfFile((unsigned char*)theBuffer,
currentFilename,
MAX_EVENT_SIZE);
syslog(LOG_DEBUG,"Trying %s",currentFilename);
if(retVal<=0) {
// syslog(LOG_ERR,"Error opening file, will delete: %s",
// currentFilename);
// fprintf(stderr,"Error reading file %s -- %d\n",currentFilename,retVal);
removeFile(currentFilename);
removeFile(currentLinkname);
removeFile(currentTouchname);
continue;
}
numBytes=retVal;
if(printToScreen && verbosity>=0) {
printf("Read File: %s -- (%d bytes)\n",currentFilename,numBytes);
}
// printf("Read %d bytes from file\n",numBytes);
// Maybe I'll add a packet check here
gHdr = (GenericHeader_t*)theBuffer;
// checkVal=checkPacket(gHdr);
// if(checkVal!=0 ) {
// printf("Bad packet %s == %d\n",currentFilename,checkVal);
// }
gHdr->packetNumber=getOpenportNumber();
printf("Openport number %d\n",gHdr->packetNumber);
retVal = openportWrite(theBuffer, numBytes,1);
if(retVal<0) {
//Problem sending data
syslog(LOG_ERR,"Problem sending Wake up Packet over OPENPORT\n");
fprintf(stderr,"Problem sending Wake up Packet over OPENPORT\n");
}
totalBytes+=numBytes;
if(!checkFileExists(currentLOSTouchname)) {
removeFile(currentLinkname);
removeFile(currentFilename);
removeFile(currentTouchname);
}
else {
sleep(1);
removeFile(currentLinkname);
removeFile(currentFilename);
removeFile(currentTouchname);
removeFile(currentLOSTouchname);
}
(*numSent)++;
// if((totalBytes+fileSize)>maxCopy) break;
counter++;
if(counter>=maxCopy) break;
// break;
}
// fprintf(stderr,"readHkAndOpenport %s -- %d\n",linkDir,*numSent);
return totalBytes;
}
int main(int argc, char *argv[]){
{
int runInd=0;
for(runInd=0; runInd<NUM_RUNS; runInd++){
lastEvents[runInd] = firstEvents[runInd]+5000 < lastEvents[runInd] ? firstEvents[runInd]+5000 : lastEvents[runInd];
}
}
/* { */
/* int runInd=0; */
/* for(runInd=0; runInd<NUM_RUNS; runInd++){ */
/* lastEvents[runInd] = firstEvents[runInd]+10000; */
/* } */
/* } */
int retVal; //,count;
/* Directory reading things */
int wd=0;
/* char *tempString; */
int numEventLinks;
/* Log stuff */
char *progName=basename(argv[0]);
retVal=sortOutPidFile(progName);
if(retVal<0)
return retVal;
#ifdef WRITE_DEBUG_FILE
FILE *debugFile = fopen("/tmp/priDebugFile.txt","w");
if(!debugFile) {
fprintf(stderr,"Couldn't open debug file\n");
exit(0);
}
#endif
/* Set signal handlers */
signal(SIGUSR1, sigUsr1Handler);
signal(SIGUSR2, sigUsr2Handler);
signal(SIGTERM, handleBadSigs);
signal(SIGINT, handleBadSigs);
signal(SIGSEGV, handleBadSigs);
//Dont' wait for children
//RJN hack will have to fix this somehow
signal(SIGCLD, SIG_IGN);
/* Setup log */
setlogmask(LOG_UPTO(LOG_INFO));
openlog (progName, LOG_PID, ANITA_LOG_FACILITY) ;
makeDirectories(HEADER_TELEM_LINK_DIR);
makeDirectories(HEADER_TELEM_DIR);
makeDirectories(EVENTD_EVENT_LINK_DIR);
makeDirectories(PRIORITIZERD_EVENT_LINK_DIR);
makeDirectories(ACQD_EVENT_LINK_DIR);
makeDirectories(GPU_TELEM_DIR);
makeDirectories(GPU_TELEM_LINK_DIR);
retVal=0;
/* Main event getting loop. */
if(wd==0) {
//First time need to prep the watcher directory
wd=setupLinkWatchDir(EVENTD_EVENT_LINK_DIR);
if(wd<=0) {
fprintf(stderr,"Unable to watch %s\n",EVENTD_EVENT_LINK_DIR);
syslog(LOG_ERR,"Unable to watch %s\n",EVENTD_EVENT_LINK_DIR);
exit(0);
}
numEventLinks=getNumLinks(wd);
}
/*
This function mallocs at some global pointers
so needs to be outside any kind of loop.
*/
prepareGpuThings();
prepareTimingCalibThings();
/* Reset average */
int phi=0;
for(phi=0; phi<NUM_PHI_SECTORS; phi++){
memset(&payloadPowSpec[phi], 0, sizeof(GpuPhiSectorPowerSpectrumStruct_t));
}
prepWriterStructs();
do {
if(printToScreen) printf("Initalizing fakePrioritizerd\n");
retVal=readConfig();
if(retVal<0) {
syslog(LOG_ERR,"Problem reading Prioritizerd.config");
printf("Problem reading Prioritizerd.config\n");
exit(1);
}
currentState=PROG_STATE_RUN;
/* This one is the main while loop */
int runInd=0;
for(runInd=0; runInd<NUM_RUNS; runInd++) {
int run = runs[runInd];
if(run!=selectedRun) continue;
const char* baseDir = "/home/anita/anitaStorage/antarctica14/raw";
int eventFileNum=0;
int firstFile = 100*(firstEvents[runInd]/100) + 100;
int lastFile = 100*(lastEvents[runInd]/100) - 100;
for(eventFileNum=firstFile; eventFileNum<=lastFile; eventFileNum+=100){
// while(currentState==PROG_STATE_RUN) {
// usleep(1);
retVal=checkLinkDirs(1,0);
if(retVal || numEventLinks) numEventLinks=getNumLinks(wd);
if(numEventLinks>=panicQueueLength) {
syslog(LOG_INFO,"Prioritizerd is getting behind (%d events in inbox), will have some prioritity 7 events",numEventLinks);
}
/* if(!numEventLinks){ */
/* usleep(1000); */
/* continue; */
/* } */
/* Read data into program memory */
int eventDir1 = eventFileNum/1000000;
eventDir1 *= 1000000;
int eventDir2 = eventFileNum/10000;
eventDir2 *= 10000;
char dir[FILENAME_MAX];
sprintf(dir, "%s/run%d/event/ev%d/ev%d", baseDir, run, eventDir1, eventDir2);
char fileName1[FILENAME_MAX];
sprintf(fileName1, "%s/psev_%d.dat.gz", dir, eventFileNum);
char fileName2[FILENAME_MAX];
sprintf(fileName2, "%s/hd_%d.dat.gz", dir, eventFileNum);
printf("%s\n", fileName1);
readIn100Events(fileName1, pedSubBody, fileName2, theHeader);
/* readInTextFile(pedSubBody, theHeader); */
int eventsReadIn = 0;
while(eventsReadIn<100 && currentState==PROG_STATE_RUN){
/* numEventLinks=getNumLinks(wd); */
/* tempString=getFirstLink(wd); */
/* if(numEventLinks==0) break; */
/* if(tempString==NULL) continue; */
/* // printf("tempString = %s\n", tempString); */
/* // printf("%s\n",eventLinkList[eventsReadIn]->d_name); */
/* sscanf(tempString,"hd_%d.dat",&doingEvent); */
/* if(lastEventNumber>0 && doingEvent!=lastEventNumber+1) { */
/* syslog(LOG_INFO,"Non-sequential event numbers %d and %d\n", lastEventNumber, doingEvent); */
/* } */
/* lastEventNumber=doingEvent; */
/* sprintf(linkFilename, "%s/%s", EVENTD_EVENT_LINK_DIR, tempString); */
/* sprintf(hdFilename, "%s/hd_%d.dat", EVENTD_EVENT_DIR, doingEvent); */
/* //RJN 4th June 2008 */
/* //Switch to Acqd writing psev files */
/* sprintf(bodyFilename[eventsReadIn],"%s/psev_%d.dat", ACQD_EVENT_DIR, doingEvent); */
/* retVal=fillHeader(&theHeader[eventsReadIn],hdFilename); */
if(numEventLinks < panicQueueLength){
/* Then we add to GPU queue... */
/* retVal=fillPedSubbedBody(&pedSubBody,bodyFilename[eventsReadIn]); */
double* finalVolts[ACTIVE_SURFS*CHANNELS_PER_SURF];
/* printf("fakePrioritizer reads event %d with priority %d\n",theHeader[eventsReadIn].eventNumber, theHeader[eventsReadIn].priority&0xf); */
doTimingCalibration(eventsReadIn, &theHeader[eventsReadIn], pedSubBody[eventsReadIn], finalVolts);
addEventToGpuQueue(eventsReadIn, finalVolts, theHeader[eventsReadIn]);
int chanInd=0;
for(chanInd=0; chanInd<ACTIVE_SURFS*CHANNELS_PER_SURF; chanInd++){
free(finalVolts[chanInd]);
}
eventsReadIn++;
}
else{/* Panic! Write all header files to archived directory with priority 7! */
panicWriteAllLinks(wd, 7, panicQueueLength, priorityPPS1, priorityPPS2);
}
}
/* exit(-1); */
printf("eventsReadIn = %d, max is %d.\n", eventsReadIn, NUM_EVENTS);
/* Now use GPU to determine priority, send in arrays of length eventsReadIn... */
if(eventsReadIn>0){
mainGpuLoop(eventsReadIn, theHeader, payloadPowSpec, writePowSpecPeriodSeconds);
}
if(payloadPowSpec[0].unixTimeLastEvent - payloadPowSpec[0].unixTimeFirstEvent >= writePowSpecPeriodSeconds
|| currentState!=PROG_STATE_RUN){
int phi=0;
for(phi=0; phi<NUM_PHI_SECTORS; phi++){
printf("Trying to write and link for phi sector %d...\n", phi);
writeFileAndLink(&payloadPowSpec[phi], phi);
}
if(currentState==PROG_STATE_RUN){
/* Reset average */
for(phi=0; phi<NUM_PHI_SECTORS; phi++){
memset(&payloadPowSpec[phi], 0, sizeof(GpuPhiSectorPowerSpectrumStruct_t));
}
}
}
int count = 0;
for(count=0;count<eventsReadIn;count++) {
// handle queue forcing of PPS here
int pri=theHeader[count].priority&0xf;
if((theHeader[count].turfio.trigType&0x2) && (priorityPPS1>=0 && priorityPPS1<=9))
pri=priorityPPS1;
if((theHeader[count].turfio.trigType&0x4) && (priorityPPS2>=0 && priorityPPS2<=9))
pri=priorityPPS2;
if(pri<0 || pri>9) pri=9;
theHeader[count].priority=(16*theHeader[count].priority)+pri;
/* printf("eventNumber = %u, priority = %d\n", theHeader[count].eventNumber, theHeader[count].priority&0xf); */
//Now Fill Generic Header and calculate checksum
/* fillGenericHeader(&theHeader[count],theHeader[count].gHdr.code,sizeof(AnitaEventHeader_t)); */
/* //Rename body and write header for Archived */
/* sprintf(archiveBodyFilename,"%s/psev_%u.dat",PRIORITIZERD_EVENT_DIR, */
/* theHeader[count].eventNumber); */
/* if(rename(bodyFilename[count],archiveBodyFilename)==-1) */
/* { */
/* syslog(LOG_ERR,"Error moving file %s -- %s",archiveBodyFilename, */
/* strerror(errno)); */
/* } */
/* sprintf(archiveHdFilename,"%s/hd_%u.dat",PRIORITIZERD_EVENT_DIR, */
/* theHeader[count].eventNumber); */
/* writeStruct(&theHeader[count],archiveHdFilename,sizeof(AnitaEventHeader_t)); */
/* makeLink(archiveHdFilename,PRIORITIZERD_EVENT_LINK_DIR); */
/* //Write Header and make Link for telemetry */
/* sprintf(telemHdFilename,"%s/hd_%d.dat",HEADER_TELEM_DIR, */
/* theHeader[count].eventNumber); */
/* retVal=writeStruct(&theHeader[count],telemHdFilename,sizeof(AnitaEventHeader_t)); */
/* makeLink(telemHdFilename,HEADER_TELEM_LINK_DIR); */
/* /\* Delete input *\/ */
/* sprintf(linkFilename,"%s/hd_%d.dat",EVENTD_EVENT_LINK_DIR, */
/* theHeader[count].eventNumber); */
/* sprintf(hdFilename,"%s/hd_%d.dat",EVENTD_EVENT_DIR, */
/* theHeader[count].eventNumber); */
/* removeFile(linkFilename); */
/* removeFile(hdFilename); */
}
}
}
} while(currentState==PROG_STATE_INIT && 0);
unlink(PRIORITIZERD_PID_FILE);
closeHkFilesAndTidy(&gpuWriter);
tidyUpGpuThings();
tidyUpTimingCalibThings();
/* Close the file which contains the high level output information */
printf("fakePrioritizerd exiting. Reached end of main\n.");
return 0;
}
