Refactor velocity resets

EKF/ekf.h

@@ -574,6 +574,14 @@ private:
 	// reset velocity states of the ekf
 	bool resetVelocity();
 
+	void resetVelocityToGps();
+
+	void resetHorizontalVelocityToOpticalFlow();
+
+	void resetVelocityToVision();
+
+	void resetHorizontalVelocityToZero();
+
 	void resetVelocityTo(const Vector3f &vel);
 
 	inline void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);

EKF/ekf_helper.cpp

@@ -50,13 +50,29 @@
 bool Ekf::resetVelocity()
 {
 	if (_control_status.flags.gps && isTimedOut(_last_gps_fail_us, (uint64_t)_min_gps_health_time_us)) {
-		ECL_INFO_TIMESTAMPED("reset velocity to GPS");
 		// this reset is only called if we have new gps data at the fusion time horizon
-		resetVelocityTo(_gps_sample_delayed.vel);
+		resetVelocityToGps();
-
-		P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc));
 
 	} else if (_control_status.flags.opt_flow) {
+		resetHorizontalVelocityToOpticalFlow();
+
+	} else if (_control_status.flags.ev_vel) {
+		resetVelocityToVision();
+
+	} else {
+		resetHorizontalVelocityToZero();
+	}
+
+	return true;
+}
+
+void Ekf::resetVelocityToGps() {
+	ECL_INFO_TIMESTAMPED("reset velocity to GPS");
+	resetVelocityTo(_gps_sample_delayed.vel);
+	P.uncorrelateCovarianceSetVariance<3>(4, sq(_gps_sample_delayed.sacc));
+}
+
+void Ekf::resetHorizontalVelocityToOpticalFlow() {
 	ECL_INFO_TIMESTAMPED("reset velocity to flow");
 	// constrain height above ground to be above minimum possible
 	const float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance);
@@ -82,26 +98,25 @@ bool Ekf::resetVelocity()
 
 	// reset the horizontal velocity variance using the optical flow noise variance
 	P.uncorrelateCovarianceSetVariance<2>(4, sq(range) * calcOptFlowMeasVar());
+}
 
-	} else if (_control_status.flags.ev_vel) {
+void Ekf::resetVelocityToVision() {
-		ECL_INFO_TIMESTAMPED("reset velocity to ev velocity");
+	ECL_INFO_TIMESTAMPED("reset to vision velocity");
 	Vector3f _ev_vel = _ev_sample_delayed.vel;
 	if(_params.fusion_mode & MASK_ROTATE_EV){
 		_ev_vel = _R_ev_to_ekf *_ev_sample_delayed.vel;
 	}
 	resetVelocityTo(_ev_vel);
 	P.uncorrelateCovarianceSetVariance<3>(4, _ev_sample_delayed.velVar);
+}
 
-	} else {
+void Ekf::resetHorizontalVelocityToZero() {
 	ECL_INFO_TIMESTAMPED("reset velocity to zero");
 	// Used when falling back to non-aiding mode of operation
 	resetHorizontalVelocityTo(Vector2f{0.f, 0.f});
 	P.uncorrelateCovarianceSetVariance<2>(4, 25.0f);
 }
 
-	return true;
-}
-
 void Ekf::resetVelocityTo(const Vector3f &new_vel) {
 	resetHorizontalVelocityTo(Vector2f(new_vel));
 	resetVerticalVelocityTo(new_vel(2));