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