hackity hack hack

src/modules/ekf2/EKF/common.h

@@ -236,6 +236,7 @@ struct flowSample {
 	Vector3f    gyro_xyz{};    ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive
 	float       dt{};          ///< amount of integration time (sec)
 	uint8_t     quality{};     ///< quality indicator between 0 and 255
+	float       ground_distance_m{NAN}; ///< optical range finder measurement (m) if available
 };
 
 struct extVisionSample {

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -200,6 +200,8 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v
 	_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z);
 	_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z);
 
+	_terrain_vpos -= delta_z;
+
 	// Reset the timout timer
 	_time_last_hgt_fuse = _time_delayed_us;
 }

src/modules/ekf2/EKF/optflow_fusion.cpp

@@ -77,24 +77,12 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
 	const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
 	aid_src.observation_variance[0] = R_LOS;
 	aid_src.observation_variance[1] = R_LOS;
-
-
-	// compute innovation variance to update test ratios
-	const Vector24f state_vector = getStateAtFusionHorizonAsVector();
-	Vector2f innov_var;
-	Vector24f H;
-	sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
-	innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
-
-	// run the innovation consistency check and record result
-	setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
-
-	_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
-	_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
 }
 
 void Ekf::fuseOptFlow()
 {
+	_aid_src_optical_flow.fusion_enabled = true;
+
 	const float R_LOS = _aid_src_optical_flow.observation_variance[0];
 
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED

src/modules/ekf2/EKF/optical_flow_control.cpp

@@ -111,7 +111,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 			// compensate for body motion to give a LOS rate
 			_flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy();
 
-		} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
+		} else if (!_control_status.flags.in_air) {
 
 			if (!is_delta_time_good) {
 				// handle special case of SITL and PX4Flow where dt is forced to
@@ -129,8 +129,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 			_flow_compensated_XY_rad.setZero();
 		}
 
-		updateOptFlow(_aid_src_optical_flow);
-
 	} else {
 		_flow_compensated_XY_rad.setZero();
 	}
@@ -173,13 +171,6 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 		    && !inhibit_flow_use
 		    && _range_sensor.isDataHealthy()) {
 
-			if (isHorizontalAidingActive()
-			    && ((_aid_src_optical_flow.test_ratio[0] > 0.1f) || (_aid_src_optical_flow.test_ratio[1] > 0.1f))
-			   ) {
-				// if horizontal aiding already active don't allow optical flow to start unless the test ratio is good
-				return;
-			}
-
 			// set the flag and reset the fusion timeout
 			ECL_INFO("starting optical flow fusion");
 
@@ -202,7 +193,16 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				}
 			}
 
+			updateOptFlow(_aid_src_optical_flow);
+			_aid_src_optical_flow.fusion_enabled = true;
+			_aid_src_optical_flow.test_ratio[0] = 0.f;
+			_aid_src_optical_flow.test_ratio[1] = 0.f;
+			_aid_src_optical_flow.innovation_rejected = false;
 			_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
+
+			_innov_check_fail_status.flags.reject_optflow_X = false;
+			_innov_check_fail_status.flags.reject_optflow_Y = false;
+
 			_control_status.flags.opt_flow = true;
 
 			return;
@@ -214,8 +214,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed)
 				// Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently
 				// but use a relaxed time criteria to enable it to coast through bad range finder data
 				if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) {
-
-					_aid_src_optical_flow.fusion_enabled = true;
+					updateOptFlow(_aid_src_optical_flow);
 					fuseOptFlow();
 					_last_known_pos.xy() = _state.pos.xy();
 				}

src/modules/ekf2/EKF2.cpp

@@ -1972,6 +1972,7 @@ bool EKF2::UpdateFlowSample(ekf2_timestamps_s &ekf2_timestamps)
 			.gyro_xyz = Vector3f{-optical_flow.delta_angle[0], -optical_flow.delta_angle[1], -optical_flow.delta_angle[2]},
 			.dt = 1e-6f * (float)optical_flow.integration_timespan_us,
 			.quality = optical_flow.quality,
+			.ground_distance_m = optical_flow.distance_m,
 		};
 
 		if (Vector2f(optical_flow.pixel_flow).isAllFinite() && flow.dt < 1) {