From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Mon, 14 Oct 2019 17:18:10 +0000 (-0300)
Subject: Fix update of EKF only when acceleration and magnetic field are available.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=e6ad66e6f66ab3db43c231c540523029b9c2932b;p=imufusion_ros.git

Fix update of EKF only when acceleration and magnetic field are available.
---

diff --git a/src/ekf.cpp b/src/ekf.cpp
index 62d21ba..6cf4c93 100644
--- a/src/ekf.cpp
+++ b/src/ekf.cpp
@@ -53,6 +53,9 @@ class EkfNode
 	
 	imufusion::Ekf *ekf;
 
+	bool imuAvailable;
+	bool magAvailable;
+
 	void imuCB(const sensor_msgs::Imu::ConstPtr &imuMsg);
 	void magCB(const sensor_msgs::MagneticField::ConstPtr &magMsg);
 	
@@ -65,6 +68,8 @@ EkfNode::EkfNode(const ros::NodeHandle &node,const char *frameId)
 {
 	node_=node;
 	frameId_=frameId;
+	imuAvailable=false;
+	magAvailable=false;
 	imuSubscriber_=node_.subscribe("imu/data_raw",10,&EkfNode::imuCB,this);
 	magSubscriber_=node_.subscribe("imu/mag",10,&EkfNode::magCB,this);
 	imuPublisher_=node_.advertise<sensor_msgs::Imu>("imu/data",10);
@@ -105,7 +110,7 @@ EkfNode::EkfNode(const ros::NodeHandle &node,const char *frameId)
 	
 	const double T=0.02;	// 0.01 is too fast for an UART at 115200 bps
 
-	ekf=new imufusion::Ekf(q0,P0,Pw,Pv,T);
+	ekf=new imufusion::Ekf(Pw,Pv,T,100);
 }
 
 EkfNode::~EkfNode(void)
@@ -126,6 +131,7 @@ void EkfNode::imuCB(const sensor_msgs::Imu::ConstPtr &imuMsg)
 		accelCovariance_(i,j)=imuMsg->linear_acceleration_covariance[i*3+j];
 		gyroCovariance_(i,j)=imuMsg->angular_velocity_covariance[i*3+j];
 	}
+	imuAvailable=true;
 }
 
 void EkfNode::magCB(const sensor_msgs::MagneticField::ConstPtr &magMsg)
@@ -133,11 +139,12 @@ void EkfNode::magCB(const sensor_msgs::MagneticField::ConstPtr &magMsg)
 	tf::vectorMsgToEigen(magMsg->magnetic_field,mag_);
 	for(int i=0;i < 3;i++) for(int j=0;j < 3;j++)
 		magCovariance_(i,j)=magMsg->magnetic_field_covariance[i*3+j];
+	magAvailable=true;
 }
 
 void EkfNode::update(void)
 {
-	ekf->update(accel_,mag_,gyro_);
+	if(imuAvailable && magAvailable) ekf->update(accel_,mag_,gyro_);
 }
 
 void EkfNode::publish(void)