sensors/vehicle_gps_position: untangle and remove unnecessary state

src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp

@@ -64,10 +64,14 @@ public:
 	void PrintStatus();
 
 private:
+	// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution
+	static constexpr int GPS_MAX_RECEIVERS = 2;
+	static constexpr int GPS_BLENDED_INSTANCE = GPS_MAX_RECEIVERS;
+
 	void Run() override;
 
 	void ParametersUpdate(bool force = false);
-	void Publish(const sensor_gps_s &gps);
+	void Publish(const sensor_gps_s &gps, uint8_t selected);
 
 	/*
 	 * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical
@@ -80,36 +84,26 @@ private:
 	/*
 	 * Calculate internal states used to blend GPS data from multiple receivers using weightings calculated
 	 * by calc_blend_weights()
-	 * States are written to _gps_state and _gps_blended_state class variables
 	 */
-	void update_gps_blend_states();
+	sensor_gps_s gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS]) const;
 
 	/*
-	 * The location in _gps_blended_state will move around as the relative accuracy changes.
+	 * The location in gps_blended_state will move around as the relative accuracy changes.
 	 * To mitigate this effect a low-pass filtered offset from each GPS location to the blended location is
 	 * calculated.
 	*/
-	void update_gps_offsets();
+	void update_gps_offsets(const sensor_gps_s &gps_blended_state);
-
-	/*
-	 * Apply the steady state physical receiver offsets calculated by update_gps_offsets().
-	*/
-	void apply_gps_offsets();
 
 	/*
 	 Calculate GPS output that is a blend of the offset corrected physical receiver data
 	*/
-	void calc_gps_blend_output();
+	void calc_gps_blend_output(sensor_gps_s &gps_blended_state, float blend_weights[GPS_MAX_RECEIVERS]) const;
 
 	// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm
 	static constexpr uint8_t BLEND_MASK_USE_SPD_ACC  = 1;
 	static constexpr uint8_t BLEND_MASK_USE_HPOS_ACC = 2;
 	static constexpr uint8_t BLEND_MASK_USE_VPOS_ACC = 4;
 
-	// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution
-	static constexpr int GPS_MAX_RECEIVERS = 2;
-	static constexpr int GPS_BLENDED_INSTANCE = GPS_MAX_RECEIVERS;
-
 	// Set the GPS timeout to 2s, after which a receiver will be ignored
 	static constexpr hrt_abstime GPS_TIMEOUT_US = 2_s;
 	static constexpr float GPS_TIMEOUT_S = (GPS_TIMEOUT_US / 1e6f);
@@ -125,30 +119,20 @@ private:
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
-	// GPS blending and switching
 	sensor_gps_s _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS
-	sensor_gps_s _gps_blended_state{};		///< internal state data for the blended GPS
+
-	sensor_gps_s _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS
 	matrix::Vector2f _NE_pos_offset_m[GPS_MAX_RECEIVERS] {}; ///< Filtered North,East position offset from GPS instance to blended solution in _output_state.location (m)
 	float _hgt_offset_mm[GPS_MAX_RECEIVERS] {};	///< Filtered height offset from GPS instance relative to blended solution in _output_state.location (mm)
-	matrix::Vector3f _blended_antenna_offset{};		///< blended antenna offset
+
-	float _blend_weights[GPS_MAX_RECEIVERS] {};	///< blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
 	uint64_t _time_prev_us[GPS_MAX_RECEIVERS] {};	///< the previous value of time_us for that GPS instance - used to detect new data.
-	uint8_t _gps_best_index{0};			///< index of the physical receiver with the lowest reported error
-	uint8_t _gps_select_index{0};			///< 0 = GPS1, 1 = GPS2, 2 = blended
 	uint8_t _gps_time_ref_index{0};			///< index of the receiver that is used as the timing reference for the blending update
-	uint8_t _gps_oldest_index{0};			///< index of the physical receiver with the oldest data
 	uint8_t _gps_newest_index{0};			///< index of the physical receiver with the newest data
 	uint8_t _gps_slowest_index{0};			///< index of the physical receiver with the slowest update rate
 	float _gps_dt[GPS_MAX_RECEIVERS] {};		///< average time step in seconds.
-	bool  _gps_new_output_data{false};		///< true if there is new output data for the EKF
 
 	DEFINE_PARAMETERS(
-		// GPS blending
+		(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
-		(ParamInt<px4::params::SENS_GPS_MASK>)
+		(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau
-		_param_sens_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio
-		(ParamFloat<px4::params::SENS_GPS_TAU>)
-		_param_sens_gps_tau ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec)
 	)
 };
 }; // namespace sensors