Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-22 06:14:14 +08:00
Code Issues Actions 8 Packages Projects Releases Wiki Activity

sensors/vehicle_gps_position: untangle and remove unnecessary state

Browse Source
This commit is contained in:
Daniel Agar
2020-09-27 10:59:02 -04:00
parent bc8ec5d1f5
commit a0d8d5ac74
2 changed files with 178 additions and 222 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
+165 -193
View File
File diff suppressed because it is too large Load Diff
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp
+13 -29
View File
@@ -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();
/*
* Apply the steady state physical receiver offsets calculated by update_gps_offsets().
*/
void apply_gps_offsets();
void update_gps_offsets(const sensor_gps_s &gps_blended_state);
/*
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, ///< 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)
(ParamInt<px4::params::SENS_GPS_MASK>) _param_sens_gps_mask,
(ParamFloat<px4::params::SENS_GPS_TAU>) _param_sens_gps_tau
)
};
}; // namespace sensors