示例#1
0
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;
        }
    }
}
示例#2
0
 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(&currentTime);
       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;
 } 
示例#3
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];
}
示例#4
0
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);

	
}
示例#5
0
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(&currentTime);
	  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;
}
示例#6
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;
}