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 (file)
--- 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)