PX4双gps数据混合代码详解

admin

void GpsBlending::update(uint64_t hrt_now_us)
{
        _is_new_output_data_available = false;

        // blend multiple receivers if available
        if (!blend_gps_data(hrt_now_us)) {
                // Only use selected receiver data if it has been updated
                uint8_t gps_select_index = 0;

                // Find the single "best" GPS from the data we have
                // First, find the GPS(s) with the best fix
                uint8_t best_fix = 0;

                for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
                        if (_gps_state[i].fix_type > best_fix) {
                                best_fix = _gps_state[i].fix_type;
                        }
                }

                // Second, compare GPS's with best fix and take the one with most satellites
                uint8_t max_sats = 0;

                for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
                        if (_gps_state[i].fix_type == best_fix && _gps_state[i].satellites_used > max_sats) {
                                max_sats = _gps_state[i].satellites_used;
                                gps_select_index = i;
                        }
                }

                // Check for new data on selected GPS, and clear blend offsets
                for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
                        _NE_pos_offset_m[i].zero();
                        _hgt_offset_m[i] = 0.0;
                }

                // Only use a secondary instance if the fallback is allowed
                if ((_primary_instance > -1)
                    && (gps_select_index != _primary_instance)
                    && _primary_instance_available
                    && (_gps_state[_primary_instance].fix_type >= 3)) {
                        gps_select_index = _primary_instance;
                }

                _selected_gps = gps_select_index;
                _is_new_output_data_available =  _gps_updated[gps_select_index];

                for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
                        _gps_updated[gps_select_index] = false;
                }
        }

        for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) {
                _time_prev_us[i] = _gps_state[i].timestamp;
        }
}

我知道答案 回答被采纳将会获得 10 金钱 已有0人回答