ekf2: add test for external wind reset

src/modules/ekf2/test/test_EKF_airspeed.cpp

@@ -296,3 +296,53 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset)
 	EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0));
 	EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(2));
 }
+
+TEST_F(EkfAirspeedTest, testExternalWindResetOnGround)
+{
+	// WHEN: an external wind reset is performed before flight
+	const float wind_speed = 4.5f;
+	const float wind_speed_acc = 2.f;
+	const float wind_direction = math::radians(-90.f);
+	const float wind_direction_acc = math::radians(20.f);
+	_ekf->resetWindToExternalObservation(wind_speed, wind_direction, wind_speed_acc, wind_direction_acc);
+
+	Vector2f vel_wind_earth = _ekf->getWindVelocity();
+	EXPECT_EQ(wind_speed, vel_wind_earth.norm());
+	EXPECT_EQ(wind_direction, atan2f(vel_wind_earth(1), vel_wind_earth(0)));
+
+	_ekf->set_in_air_status(true);
+	_ekf->set_vehicle_at_rest(false);
+	_ekf->set_is_fixed_wing(true);
+	_ekf_wrapper.enableBetaFusion();
+	_sensor_simulator.startAirspeedSensor();
+
+	ResetLoggingChecker reset_logging_checker(_ekf);
+	reset_logging_checker.capturePreResetState();
+
+	const float measured_airspeed = 25.f;
+	_sensor_simulator._airspeed.setData(measured_airspeed, measured_airspeed);
+
+	// Since we are doing inertial dead-reckoning
+	// and that the measurement is inconsistent with the filter
+	EXPECT_FALSE(_ekf->isLocalHorizontalPositionValid());
+
+	_sensor_simulator.runSeconds(1);
+
+	// THEN: the velocity is reset to the airspeed measurement
+	reset_logging_checker.capturePostResetState();
+	EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1));
+
+	const Vector3f vel = _ekf->getVelocity();
+	const Vector2f air_relative_vel = vel.xy() - vel_wind_earth;
+
+	EXPECT_NEAR(air_relative_vel.norm(), measured_airspeed, 1e-3f);
+
+	// AND: wind estimate stayed close to its manually initialized value
+	vel_wind_earth = _ekf->getWindVelocity();
+	EXPECT_NEAR(wind_speed, vel_wind_earth.norm(), 0.1f);
+	EXPECT_NEAR(wind_direction, atan2f(vel_wind_earth(1), vel_wind_earth(0)), 0.1f);
+
+	EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion());
+	EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion());
+	EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid());
+}