ImuBuffer::ImuBuffer(std::string name)
: TaskContext(name)
{
// Add properties
properties()->addProperty( "prop",_prop ).doc("The description of the property");
// Add ports
addEventPort( "imuData",_imuData, boost::bind(&ImuBuffer::addMeasurement,this) ).doc("The IMU data: omegax, omegay, omegaz, ax, ay, az");
addPort( "imuMeanCov",_imuMeanCov ).doc("The mean and covariance of last and second to last imu data: mean_last, 1/cov_last, mean_prev, 1/cov_prev");
addPort("imuCameraRatio" , _imuCameraRatio).doc("The ratio of imu freq. on camera freq.");
imuMeanCov.resize(24,0.0);
imuMean_last.resize(6,0.0);
imuMean_prev.resize(6,0.0);
imucov_last.resize(6,0.0);
imucov_prev.resize(6,0.0);
halfBufferIndex = 0;
fullBufferIndex = 0;
sigma_omega = 1.0;
sigma_acc = 0.1;
addProperty("sigma_omega",sigma_omega).doc("The standard deviation of the angular velocity measurements. Default = 1.0");
addProperty("sigma_acc",sigma_acc).doc("The standard deviation of the acceleration measurements. Default = 0.1");
_imuMeanCov.setDataSample( imuMeanCov );
_imuMeanCov.write( imuMeanCov );
}
MeasurementsRecorder::MeasurementsRecorder(std::string name) : TaskContext(name) {
addPort("portMeasurementsMarkers",portMeasurementsMarkers);
addPort("portMeasurementsIMU",portMeasurementsIMU);
addEventPort("portMeasurementsEncoder",portMeasurementsEncoder);
addPort("portMeasurementsCtrl",portMeasurementsCtrl);
addPort("portMeasurementsCtrlRates",portMeasurementsCtrlRates);
addPort("portMeasurementsPose",portMeasurementsPose);
addPort("portStateEstimate",portStateEstimate);
}
planeSimulatorRframe::planeSimulatorRframe(std::string name)
: TaskContext(name)
{
// Add properties
properties()->addProperty( "prop",_prop )
.doc("The description of the property");
// Add ports
addEventPort( "trigger",_trigger);
addPort( "controlInputPort",_controlInputPort )
.doc("Input to the system: dur1,dur2,dup");
addPort( "stateOutputPort",_stateOutputPort )
.doc("State output: x,y,z,dx,dy,dz,e11,e12,e13,e21,e22,e23,e31,e32,e33,w1,w2,w3,delta,ddelta,ua,ue");
addPort( "triggerTimeStampOut",_triggerTimeStampOut )
.doc("triggerTimeStampOut");
addPort( "imuData",_IMUOutputPort )
.doc("IMU output: wx,wy,wz,ax,ay,az");
addPort( "deltaOmega",_deltaOmega )
.doc("encoder output: delta, ddelta");
deltaOmega.resize(2);
addPort( "controlOutputPort",_controlOutputPort )
.doc("controlOutputPort: ur1, ur2, up");
controlOutputPort.resize(3);
addProperty( "integrationStep",h ).doc("The time step to integrate");
h = 1.0/500.0;
// Add operations
provides()->addOperation("initialiseState",&planeSimulatorRframe::initialiseState,this)
.doc("Description of the method");
X.resize(NSTATES);
X1.resize(NSTATES);
IMU.resize(NIMU);
U.resize(NCONTROLS);
rk4Wrapper_input[0] = rk4Wrapper_input_X;
rk4Wrapper_input[1] = rk4Wrapper_input_U;
rk4Wrapper_input[2] = rk4Wrapper_input_h;
rk4Wrapper_output[0] = rk4Wrapper_output_X1;
rk4Wrapper_output[1] = rk4Wrapper_output_IMU;
}
TestEventService(TaskContext* owner = 0) : Service("portservice", owner)
{
addEventPort("ip",ip).doc("ip");
addEventPort("ip",ip2).doc("ip"); // overrides ip
addPort("op",op).doc("op");
}
