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