AirspeedSelector: use Vector3f

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/airspeed_selector/AirspeedValidator.cpp

@@ -50,12 +50,11 @@ AirspeedValidator::update_airspeed_validator(const airspeed_validator_update_dat
 	// get indicated airspeed from input data (raw airspeed)
 	_IAS = input_data.airspeed_indicated_raw;
 
-	update_CAS_scale_estimated(input_data.lpos_valid, input_data.lpos_vx, input_data.lpos_vy, input_data.lpos_vz);
+	update_CAS_scale_estimated(input_data.lpos_valid, input_data.ground_velocity);
 	update_CAS_scale_applied();
 	update_CAS_TAS(input_data.air_pressure_pa, input_data.air_temperature_celsius);
-	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid, input_data.lpos_vx,
-			      input_data.lpos_vy,
-			      input_data.lpos_vz, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
+	update_wind_estimator(input_data.timestamp, input_data.airspeed_true_raw, input_data.lpos_valid,
+			      input_data.ground_velocity, input_data.lpos_evh, input_data.lpos_evv, input_data.att_q);
 	update_in_fixed_wing_flight(input_data.in_fixed_wing_flight);
 	check_airspeed_data_stuck(input_data.timestamp);
 	check_airspeed_innovation(input_data.timestamp, input_data.vel_test_ratio, input_data.mag_test_ratio);
@@ -72,14 +71,12 @@ AirspeedValidator::reset_airspeed_to_invalid(const uint64_t timestamp)
 
 void
 AirspeedValidator::update_wind_estimator(const uint64_t time_now_usec, float airspeed_true_raw, bool lpos_valid,
-		float lpos_vx, float lpos_vy,
-		float lpos_vz, float lpos_evh, float lpos_evv, const float att_q[4])
+		matrix::Vector3f vI, float lpos_evh, float lpos_evv, const float att_q[4])
 {
 	_wind_estimator.update(time_now_usec);
 
 	if (lpos_valid && _in_fixed_wing_flight) {
 
-		Vector3f vI(lpos_vx, lpos_vy, lpos_vz);
 		Quatf q(att_q);
 
 		// airspeed fusion (with raw TAS)
@@ -117,7 +114,7 @@ AirspeedValidator::get_wind_estimator_states(uint64_t timestamp)
 }
 
 void
-AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, float vx, float vy, float vz)
+AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, matrix::Vector3f vI)
 {
 	if (!_in_fixed_wing_flight) {
 		return;
@@ -128,10 +125,10 @@ AirspeedValidator::update_CAS_scale_estimated(bool lpos_valid, float vx, float v
 		reset_CAS_scale_check();
 	}
 
-	const float course_over_ground_rad = matrix::wrap_2pi(atan2f(vy, vx));
+	const float course_over_ground_rad = matrix::wrap_2pi(atan2f(vI(0), vI(1)));
 	const int segment_index = int(SCALE_CHECK_SAMPLES * course_over_ground_rad / (2.f * M_PI_F));
 
-	_scale_check_groundspeed(segment_index) = sqrt(vx * vx + vy * vy + vz * vz);
+	_scale_check_groundspeed(segment_index) = vI.norm();
 	_scale_check_TAS(segment_index) = _TAS;
 
 	// run check if all segments are filled

src/modules/airspeed_selector/AirspeedValidator.hpp

@@ -55,9 +55,7 @@ struct airspeed_validator_update_data {
 	float airspeed_indicated_raw;
 	float airspeed_true_raw;
 	uint64_t airspeed_timestamp;
-	float lpos_vx;
-	float lpos_vy;
-	float lpos_vz;
+	matrix::Vector3f ground_velocity;
 	bool lpos_valid;
 	float lpos_evh;
 	float lpos_evv;
@@ -173,11 +171,9 @@ private:
 
 	void update_in_fixed_wing_flight(bool in_fixed_wing_flight) { _in_fixed_wing_flight = in_fixed_wing_flight; }
 
-	void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, float lpos_vx,
-				   float lpos_vy,
-				   float lpos_vz,
+	void update_wind_estimator(const uint64_t timestamp, float airspeed_true_raw, bool lpos_valid, matrix::Vector3f vI,
 				   float lpos_evh, float lpos_evv, const float att_q[4]);
-	void update_CAS_scale_estimated(bool lpos_valid, float vx, float vy, float vz);
+	void update_CAS_scale_estimated(bool lpos_valid, matrix::Vector3f vI);
 	void update_CAS_scale_applied();
 	void update_CAS_TAS(float air_pressure_pa, float air_temperature_celsius);
 	void check_airspeed_data_stuck(uint64_t timestamp);

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -329,12 +329,12 @@ AirspeedModule::Run()
 					       _position_setpoint.type != position_setpoint_s::SETPOINT_TYPE_LAND &&
 					       _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
 
+		const matrix::Vector3f vI(_vehicle_local_position.vx, _vehicle_local_position.vy, _vehicle_local_position.vz);
+
 		// Prepare data for airspeed_validator
 		struct airspeed_validator_update_data input_data = {};
 		input_data.timestamp = _time_now_usec;
-		input_data.lpos_vx = _vehicle_local_position.vx;
-		input_data.lpos_vy = _vehicle_local_position.vy;
-		input_data.lpos_vz = _vehicle_local_position.vz;
+		input_data.ground_velocity = vI;
 		input_data.lpos_valid = _vehicle_local_position_valid;
 		input_data.lpos_evh = _vehicle_local_position.evh;
 		input_data.lpos_evv = _vehicle_local_position.evv;