only allow VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE during wind dead-reckoning

and increase horizontal velocity variance to allow velocity states to move
towards solution that is aligned with the newly set wind

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/ekf2/EKF/wind.cpp

@@ -60,6 +60,11 @@ void Ekf::resetWindToExternalObservation(float wind_speed, float wind_direction,
 	_information_events.flags.reset_wind_to_ext_obs = true;
 
 	resetWindTo(wind, wind_var);
+
+	// reset the horizontal velocity variances to allow the velocity states to be pulled towards
+	// a solution that is aligned with the newly set wind estimates
+	static constexpr float hor_vel_var = 25.0f;
+	P.uncorrelateCovarianceSetVariance<2>(State::vel.idx, hor_vel_var);
 }
 
 void Ekf::resetWindTo(const Vector2f &wind, const Vector2f &wind_var)

src/modules/ekf2/EKF2.cpp

@@ -536,8 +536,10 @@ void EKF2::Run()
 			}
 
 			if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_EXTERNAL_WIND_ESTIMATE) {
-				_ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2,
+				if (_ekf.control_status_flags().wind_dead_reckoning) {
-								    vehicle_command.param4);
+					_ekf.resetWindToExternalObservation(vehicle_command.param1, vehicle_command.param3, vehicle_command.param2,
+									    vehicle_command.param4);
+				}
 			}
 		}
 	}