EKF: Make position and velocity reset publish success

Some users of the position and velocity reset functions will need to know if the reset has been successful.

EKF/ekf.h

@@ -249,7 +249,7 @@ private:
 	void fuseVelPosHeight();
 
 	// reset velocity states of the ekf
-	void resetVelocity();
+	bool resetVelocity();
 
 	// fuse optical flow line of sight rate measurements
 	void fuseOptFlow();
@@ -275,7 +275,7 @@ private:
 	void calcMagDeclination();
 
 	// reset position states of the ekf (only vertical position)
-	void resetPosition();
+	bool resetPosition();
 
 	// reset height state of the ekf
 	void resetHeight();

EKF/ekf_helper.cpp

@@ -49,22 +49,24 @@
 
 // Reset the velocity states. If we have a recent and valid
 // gps measurement then use for velocity initialisation
-void Ekf::resetVelocity()
+bool Ekf::resetVelocity()
 {
 	// if we have a valid GPS measurement use it to initialise velocity states
 	gpsSample gps_newest = _gps_buffer.get_newest();
 
 	if (_time_last_imu - gps_newest.time_us < 400000) {
 		_state.vel = gps_newest.vel;
+		return true;
 
 	} else {
-		_state.vel.setZero();
+		// XXX use the value of the last known velocity
+		return false;
 	}
 }
 
 // Reset position states. If we have a recent and valid
 // gps measurement then use for position initialisation
-void Ekf::resetPosition()
+bool Ekf::resetPosition()
 {
 	// if we have a fresh GPS measurement, use it to initialise position states and correct the position for the measurement delay
 	gpsSample gps_newest = _gps_buffer.get_newest();
@@ -74,9 +76,11 @@ void Ekf::resetPosition()
 	if (time_delay < 0.4f) {
 		_state.pos(0) = gps_newest.pos(0) + gps_newest.vel(0) * time_delay;
 		_state.pos(1) = gps_newest.pos(1) + gps_newest.vel(1) * time_delay;
+		return true;
 
 	} else {
 		// XXX use the value of the last known position
+		return false;
 	}
 }