Esempio n. 1
0
    bool updateModule()
    {
        Vector *imuData=iPort.read();
        if (imuData==NULL)
            return false;

        Stamp stamp;
        iPort.getEnvelope(stamp);

        double t0=Time::now();
        Vector gyro=imuData->subVector(6,8);
        Vector gyro_filt=gyroFilt.filt(gyro);

        gyro-=gyroBias;
        gyro_filt-=gyroBias;

        Vector mag_filt=magFilt.filt(imuData->subVector(9,11));
        double magVel=norm(velEst.estimate(AWPolyElement(mag_filt,stamp.getTime())));

        adaptGyroBias=adaptGyroBias?(magVel<mag_vel_thres_up):(magVel<mag_vel_thres_down);
        gyroBias=biasInt.integrate(adaptGyroBias?gyro_filt:Vector(3,0.0));
        double dt=Time::now()-t0;

        if (oPort.getOutputCount()>0)
        {
            Vector &outData=oPort.prepare();
            outData=*imuData;
            outData.setSubvector(6,gyro);
            oPort.setEnvelope(stamp);
            oPort.write();
        }

        if (bPort.getOutputCount()>0)
        {
            bPort.prepare()=gyroBias;
            bPort.setEnvelope(stamp);
            bPort.write();
        }

        if (verbose)
        {
            yInfo("magVel   = %g => [%s]",magVel,adaptGyroBias?"adapt-gyroBias":"no-adaption");
            yInfo("gyro     = %s",gyro.toString(3,3).c_str());
            yInfo("gyroBias = %s",gyroBias.toString(3,3).c_str());
            yInfo("dt       = %.0f [us]",dt*1e6);
            yInfo("\n");
        }

        return true;
    }
// Function: mdlOutputs =======================================================
// Abstract:
//   In this function, you compute the outputs of your S-function
//   block.
static void mdlOutputs(SimStruct *S, int_T tid)
{
    BufferedPort<Vector> *port = static_cast<BufferedPort<Vector>*>(ssGetPWork(S)[0]);
    int_T blocking = ssGetIWork(S)[0];
    int_T shouldReadTimestamp = ssGetIWork(S)[1];
    int_T isAutoconnect = ssGetIWork(S)[2];
    int timeStampPortIndex = 1;
    int connectionStatusPortIndex = 1;

    Vector *v = port->read(blocking); // Read from the port.  Waits until data arrives.
    if (v)
    {
        if (shouldReadTimestamp) {
            connectionStatusPortIndex++;
            yarp::os::Stamp timestamp;
            port->getEnvelope(timestamp);

            real_T *pY1 = ssGetOutputPortRealSignal(S, timeStampPortIndex);
            pY1[0] = (real_T)(timestamp.getCount());
            pY1[1] = (real_T)(timestamp.getTime());
        }
        real_T *signal = ssGetOutputPortRealSignal(S, 0);
        int_T widthPort = ssGetOutputPortWidth(S, 0);
        for (int i = 0; i < std::min(widthPort, (int_T)v->size()); i++) {
            signal[i] = (*v)[i];
        }

        if (!isAutoconnect) {
            unsigned char *statusPort = (unsigned char*)ssGetOutputPortSignal(S, connectionStatusPortIndex);
            statusPort[0] = 1; //somebody wrote in the port => the port is connected
            //TODO implement a sort of "timeout" paramter
            //At the current state this is equal to timeout = inf
            //Otherwise we can check the timeout and if nobody sent data in the last X secs
            //we set the port to zero again
        }
    }
}
Esempio n. 3
0
int main(int argc, char *argv[]) {

	//Declair yarp
	Network yarp;
	if (!yarp.checkNetwork())
	{
		printf("YARP server not available!\n");
		return -1;
	}

	//Creating a Resiving yarp port
	BufferedPort<Sound> bufferPort;
	bufferPort.open("/receiver");
	if (!Network::exists("/receiver")) {
		printf("receiver not exists \n");
		return -1;
	}

	//Connecting the resiving audio port with the sender on the pc104
	Network::connect("/sender", "/receiver");
	if (!Network::isConnected("/sender", "/receiver")) {
		printf("no connection \n");
		return -1;
	}

	double empty[samplingBuffer *4] = {0}; //plus 2 for time and sample stamps

	//Setup for memory mapping
	FILE *fid;
	fid = fopen("/tmp/AudioMemMap.tmp", "w");
	fwrite(empty, sizeof(double), samplingBuffer * 4, fid); 
	fclose(fid);
	int mappedFileID;
	mappedFileID = open("/tmp/AudioMemMap.tmp", O_RDWR);
	double *mappedAudioData;
	mappedAudioData = (double *)mmap(0, memoryMapSize_bytes, PROT_WRITE, MAP_SHARED , mappedFileID, 0);
	

	//Main loop that maps the sound to the memory mapped region delaired above
	Sound* s;
	Stamp ts;
	while (true) {
		//This is a blocking read
		s = bufferPort.read(true);
		bufferPort.getEnvelope(ts);
		printf("count:%d time:%f \n", ts.getCount(), ts.getTime());
		int e0 = ts.getCount();
		double e1 = ts.getTime();
		int row = 0;
		for (int col = 0 ; col < samplingBuffer; col++) {
			NetInt16 temp_c = (NetInt16) s->get(col, 0);
			NetInt16 temp_d = (NetInt16) s->get(col, 1);
			mappedAudioData[row]        	= (double) 	temp_c / normDivid ;
			mappedAudioData[row + 1] 		= (double) 	temp_d / normDivid;
			mappedAudioData[row + 2] 		= (double) 	(e0 * samplingBuffer) + col;
			mappedAudioData[row + 3]		= (double) 	(e1 + col * samplePeriod);
			row += 4;

		}
	}
	return 0;
}