diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 3451651961fc..a60f3af886d8 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -57,7 +57,6 @@ set(msg_files distance_sensor.msg ekf2_timestamps.msg ekf_gps_drift.msg - ekf_gps_position.msg esc_report.msg esc_status.msg estimator_innovations.msg @@ -116,6 +115,7 @@ set(msg_files sensor_baro.msg sensor_combined.msg sensor_correction.msg + sensor_gps.msg sensor_gyro.msg sensor_gyro_fifo.msg sensor_mag.msg diff --git a/msg/ekf_gps_position.msg b/msg/sensor_gps.msg similarity index 62% rename from msg/ekf_gps_position.msg rename to msg/sensor_gps.msg index bfd312f7524a..6d954aa634c0 100644 --- a/msg/ekf_gps_position.msg +++ b/msg/sensor_gps.msg @@ -1,19 +1,36 @@ -# EKF blended position in WGS84 coordinates. +# GPS position in WGS84 coordinates. +# the field 'timestamp' is for the position & velocity (microseconds) uint64 timestamp # time since system start (microseconds) + int32 lat # Latitude in 1E-7 degrees int32 lon # Longitude in 1E-7 degrees int32 alt # Altitude in 1E-3 meters above MSL, (millimetres) int32 alt_ellipsoid # Altitude in 1E-3 meters bove Ellipsoid, (millimetres) + float32 s_variance_m_s # GPS speed accuracy estimate, (metres/sec) +float32 c_variance_rad # GPS course accuracy estimate, (radians) uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + float32 eph # GPS horizontal position accuracy (metres) float32 epv # GPS vertical position accuracy (metres) + +float32 hdop # Horizontal dilution of precision +float32 vdop # Vertical dilution of precision + +int32 noise_per_ms # GPS noise per millisecond +int32 jamming_indicator # indicates jamming is occurring + float32 vel_m_s # GPS ground speed, (metres/sec) float32 vel_n_m_s # GPS North velocity, (metres/sec) float32 vel_e_m_s # GPS East velocity, (metres/sec) float32 vel_d_m_s # GPS Down velocity, (metres/sec) +float32 cog_rad # Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) bool vel_ned_valid # True if NED velocity is valid + +int32 timestamp_time_relative # timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds) +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + uint8 satellites_used # Number of satellites used + float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI]) float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI]) -uint8 selected # GPS selection: 0: GPS1, 1: GPS2. 2: GPS1+GPS2 blend diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index c529fc895d1c..7024f567179e 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -51,7 +51,7 @@ rtps: id: 19 - msg: ekf_gps_drift id: 20 - - msg: ekf_gps_position + - msg: sensor_gps id: 21 - msg: esc_report id: 22 diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg index 6d954aa634c0..818e53448d3c 100644 --- a/msg/vehicle_gps_position.msg +++ b/msg/vehicle_gps_position.msg @@ -34,3 +34,5 @@ uint8 satellites_used # Number of satellites used float32 heading # heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI]) float32 heading_offset # heading offset of dual antenna array in body frame. Set to NaN if not applicable. (rad, [-PI, PI]) + +uint8 selected # GPS selection: 0: GPS1, 1: GPS2. 2: GPS3. 3. Blending multiple receivers \ No newline at end of file diff --git a/src/drivers/gps/definitions.h b/src/drivers/gps/definitions.h index eac75d7324d5..51dc284eda4d 100644 --- a/src/drivers/gps/definitions.h +++ b/src/drivers/gps/definitions.h @@ -41,7 +41,7 @@ #include #include -#include +#include #include #define GPS_INFO(...) PX4_INFO(__VA_ARGS__) diff --git a/src/drivers/gps/devices b/src/drivers/gps/devices index bcd57ae2396f..4d51e5378ec7 160000 --- a/src/drivers/gps/devices +++ b/src/drivers/gps/devices @@ -1 +1 @@ -Subproject commit bcd57ae2396fe0fd88b725d81a686e072ef0ecc9 +Subproject commit 4d51e5378ec74e8948981cfb9e47ed84191e2fd2 diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 01b394437909..e09901764a30 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -58,6 +58,7 @@ #include #include #include +#include #include "devices/src/ashtech.h" #include "devices/src/emlid_reach.h" @@ -159,10 +160,10 @@ class GPS : public ModuleBase GPS_Sat_Info *_sat_info{nullptr}; ///< instance of GPS sat info data object - vehicle_gps_position_s _report_gps_pos{}; ///< uORB topic for gps position + sensor_gps_s _report_gps_pos{}; ///< uORB topic for gps position satellite_info_s *_p_report_sat_info{nullptr}; ///< pointer to uORB topic for satellite info - uORB::PublicationMulti _report_gps_pos_pub{ORB_ID(vehicle_gps_position)}; ///< uORB pub for gps position + uORB::PublicationMulti _report_gps_pos_pub{ORB_ID(sensor_gps)}; ///< uORB pub for gps position uORB::PublicationMulti _report_sat_info_pub{ORB_ID(satellite_info)}; ///< uORB pub for satellite info float _rate{0.0f}; ///< position update rate diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index f9a153022b92..7380cb89484d 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -52,13 +52,11 @@ using namespace time_literals; const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : - UavcanCDevSensorBridgeBase("uavcan_gnss", "/dev/uavcan/gnss", "/dev/gnss", ORB_ID(vehicle_gps_position)), + UavcanCDevSensorBridgeBase("uavcan_gnss", "/dev/uavcan/gnss", "/dev/gnss", ORB_ID(sensor_gps)), _node(node), _sub_auxiliary(node), _sub_fix(node), _sub_fix2(node), - _pub_fix2(node), - _orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb)), _report_pub(nullptr), _channel_using_fix2(new bool[_max_channels]) { @@ -81,13 +79,6 @@ UavcanGnssBridge::init() return res; } - res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower); - - if (res < 0) { - PX4_WARN("GNSS fix2 pub failed %i", res); - return res; - } - res = _sub_auxiliary.start(AuxiliaryCbBinder(this, &UavcanGnssBridge::gnss_auxiliary_sub_cb)); if (res < 0) { @@ -109,8 +100,6 @@ UavcanGnssBridge::init() return res; } - _orb_to_uavcan_pub_timer.startPeriodic(uavcan::MonotonicDuration::fromUSec(1000000U / ORB_TO_UAVCAN_FREQUENCY_HZ)); - return res; } @@ -268,7 +257,18 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure const float (&pos_cov)[9], const float (&vel_cov)[9], const bool valid_pos_cov, const bool valid_vel_cov) { - auto report = ::vehicle_gps_position_s(); + // This bridge does not support redundant GNSS receivers yet. + if (_receiver_node_id < 0) { + _receiver_node_id = msg.getSrcNodeID().get(); + PX4_INFO("GNSS receiver node ID: %d", _receiver_node_id); + + } else { + if (_receiver_node_id != msg.getSrcNodeID().get()) { + return; // This GNSS receiver is the redundant one, ignore it. + } + } + + sensor_gps_s report{}; /* * FIXME HACK @@ -396,44 +396,3 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure publish(msg.getSrcNodeID().get(), &report); } - -void -UavcanGnssBridge::broadcast_from_orb(const uavcan::TimerEvent &) -{ - vehicle_gps_position_s orb_msg{}; - - if (!_orb_sub_gnss.update(&orb_msg)) { - return; - } - - // Convert to UAVCAN - using uavcan::equipment::gnss::Fix2; - Fix2 msg; - - msg.gnss_timestamp = uavcan::UtcTime::fromUSec(orb_msg.time_utc_usec); - msg.gnss_time_standard = Fix2::GNSS_TIME_STANDARD_UTC; - - msg.longitude_deg_1e8 = std::int64_t(orb_msg.lon) * 10LL; - msg.latitude_deg_1e8 = std::int64_t(orb_msg.lat) * 10LL; - msg.height_ellipsoid_mm = orb_msg.alt_ellipsoid; - msg.height_msl_mm = orb_msg.alt; - - msg.ned_velocity[0] = orb_msg.vel_n_m_s; - msg.ned_velocity[1] = orb_msg.vel_e_m_s; - msg.ned_velocity[2] = orb_msg.vel_d_m_s; - - msg.sats_used = orb_msg.satellites_used; - msg.status = orb_msg.fix_type; - // mode skipped - // sub mode skipped - - // diagonal covariance matrix - msg.covariance.resize(2, orb_msg.eph * orb_msg.eph); - msg.covariance.resize(3, orb_msg.epv * orb_msg.epv); - msg.covariance.resize(6, orb_msg.s_variance_m_s * orb_msg.s_variance_m_s); - - msg.pdop = (orb_msg.hdop > orb_msg.vdop) ? orb_msg.hdop : orb_msg.vdop; // this is a hack :( - - // Publishing now - _pub_fix2.broadcast(msg); -} diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 9bbed9449822..a159f0926038 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -46,7 +46,7 @@ #include #include -#include +#include #include #include @@ -82,8 +82,6 @@ class UavcanGnssBridge : public UavcanCDevSensorBridgeBase const float (&pos_cov)[9], const float (&vel_cov)[9], const bool valid_pos_cov, const bool valid_vel_cov); - void broadcast_from_orb(const uavcan::TimerEvent &); - typedef uavcan::MethodBinder < UavcanGnssBridge *, void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure &) > AuxiliaryCbBinder; @@ -106,16 +104,11 @@ class UavcanGnssBridge : public UavcanCDevSensorBridgeBase uavcan::Subscriber _sub_fix; uavcan::Subscriber _sub_fix2; - uavcan::Publisher _pub_fix2; - - uavcan::TimerEventForwarder _orb_to_uavcan_pub_timer; - uint64_t _last_gnss_auxiliary_timestamp{0}; float _last_gnss_auxiliary_hdop{0.0f}; float _last_gnss_auxiliary_vdop{0.0f}; - uORB::PublicationMulti _gps_pub{ORB_ID(vehicle_gps_position)}; - uORB::Subscription _orb_sub_gnss{ORB_ID(vehicle_gps_position)}; + uORB::PublicationMulti _gps_pub{ORB_ID(sensor_gps)}; int _receiver_node_id{-1}; bool _old_fix_subscriber_active{true}; diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 79d648cdb1b2..23cbf8fbeafd 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -98,6 +98,18 @@ bool param_modify_on_import(bson_node_t node) } } + // 2020-08-23 (v1.12 alpha): translate GPS blending parameters from EKF2 -> SENS + { + if (strcmp("EKF2_GPS_MASK", node->name) == 0) { + strcpy(node->name, "SENS_GPS_MASK"); + PX4_INFO("copying %s -> %s", "EKF2_GPS_MASK", "SENS_GPS_MASK"); + } + + if (strcmp("EKF2_GPS_TAU", node->name) == 0) { + strcpy(node->name, "SENS_GPS_TAU"); + PX4_INFO("copying %s -> %s", "EKF2_GPS_TAU", "SENS_GPS_TAU"); + } + } // translate (SPI) calibration ID parameters. This can be removed after the next release (current release=1.10) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 3bf620d78cfb..e45f061cd729 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -58,7 +58,6 @@ #include #include #include -#include #include #include #include @@ -82,15 +81,6 @@ #include "Utility/PreFlightChecker.hpp" -// defines used to specify the mask position for use of different accuracy metrics in the GPS blending algorithm -#define BLEND_MASK_USE_SPD_ACC 1 -#define BLEND_MASK_USE_HPOS_ACC 2 -#define BLEND_MASK_USE_VPOS_ACC 4 - -// define max number of GPS receivers supported and 0 base instance used to access virtual 'blended' GPS solution -#define GPS_MAX_RECEIVERS 2 -#define GPS_BLENDED_INSTANCE 2 - using math::constrain; using namespace time_literals; @@ -135,38 +125,6 @@ class Ekf2 final : public ModuleBase, public ModuleParams, public px4::Sch void publish_wind_estimate(const hrt_abstime ×tamp); void publish_yaw_estimator_status(const hrt_abstime ×tamp); - /* - * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical - * receiver solutions. This internal state cannot be used directly by estimators because if physical receivers - * have significant position differences, variation in receiver estimated accuracy will cause undesirable - * variation in the position solution. - */ - bool blend_gps_data(); - - /* - * 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(); - - /* - * 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(); - - /* - Calculate GPS output that is a blend of the offset corrected physical receiver data - */ - void calc_gps_blend_output(); - /* * Calculate filtered WGS84 height from estimated AMSL height */ @@ -203,28 +161,11 @@ class Ekf2 final : public ModuleBase, public ModuleParams, public px4::Sch static constexpr float eo_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated orientation //static constexpr float ev_max_std_dev = 100.0f; ///< Maximum permissible standard deviation for estimated velocity - // GPS blending and switching - gps_message _gps_state[GPS_MAX_RECEIVERS] {}; ///< internal state data for the physical GPS - gps_message _gps_blended_state{}; ///< internal state data for the blended GPS - gps_message _gps_output[GPS_MAX_RECEIVERS + 1] {}; ///< output state data for the physical and blended GPS - 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) - 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 - bool _had_valid_terrain = false; ///< true if at any time there was a valid terrain estimate - - int32_t _gps_alttitude_ellipsoid[GPS_MAX_RECEIVERS] {}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid - uint64_t _gps_alttitude_ellipsoid_previous_timestamp[GPS_MAX_RECEIVERS] {}; ///< storage for previous timestamp to compute dt + bool _had_valid_terrain{false}; ///< true if at any time there was a valid terrain estimate + + uint64_t _gps_time_usec{0}; + int32_t _gps_alttitude_ellipsoid{0}; ///< altitude in 1E-3 meters (millimeters) above ellipsoid + uint64_t _gps_alttitude_ellipsoid_previous_timestamp{0}; ///< storage for previous timestamp to compute dt float _wgs84_hgt_offset = 0; ///< height offset between AMSL and WGS84 bool _imu_bias_reset_request{false}; @@ -242,6 +183,7 @@ class Ekf2 final : public ModuleBase, public ModuleParams, public px4::Sch uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; uORB::Subscription _status_sub{ORB_ID(vehicle_status)}; + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)}; @@ -260,16 +202,12 @@ class Ekf2 final : public ModuleBase, public ModuleParams, public px4::Sch uORB::Subscription _range_finder_subs[MAX_RNG_SENSOR_COUNT] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; int _range_finder_sub_index = -1; // index for downward-facing range finder subscription - // because we can have multiple GPS instances - uORB::Subscription _gps_subs[GPS_MAX_RECEIVERS] {{ORB_ID(vehicle_gps_position), 0}, {ORB_ID(vehicle_gps_position), 1}}; - sensor_selection_s _sensor_selection{}; vehicle_land_detected_s _vehicle_land_detected{}; vehicle_status_s _vehicle_status{}; uORB::Publication _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::Publication _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; - uORB::Publication _blended_gps_pub{ORB_ID(ekf_gps_position)}; uORB::Publication _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::Publication _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; uORB::Publication _estimator_innovations_pub{ORB_ID(estimator_innovations)}; @@ -520,12 +458,6 @@ class Ekf2 final : public ModuleBase, public ModuleParams, public px4::Sch (ParamExtFloat) _param_ekf2_pcoef_z, ///< static pressure position error coefficient along the Z body axis - // GPS blending - (ParamInt) - _param_ekf2_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio - (ParamFloat) - _param_ekf2_gps_tau, ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec) - // Test used to determine if the vehicle is static or moving (ParamExtFloat) _param_ekf2_move_test, ///< scaling applied to IMU data thresholds used to determine if the vehicle is static or moving. @@ -941,87 +873,18 @@ void Ekf2::Run() } } - // read gps1 data if available - bool gps1_updated = _gps_subs[0].updated(); - - if (gps1_updated) { + if (_vehicle_gps_position_sub.updated()) { vehicle_gps_position_s gps; - if (_gps_subs[0].copy(&gps)) { - fillGpsMsgWithVehicleGpsPosData(_gps_state[0], gps); - _gps_alttitude_ellipsoid[0] = gps.alt_ellipsoid; - } - } + if (_vehicle_gps_position_sub.copy(&gps)) { + gps_message gps_msg{}; - // check for second GPS receiver data - bool gps2_updated = _gps_subs[1].updated(); + fillGpsMsgWithVehicleGpsPosData(gps_msg, gps); - if (gps2_updated) { - vehicle_gps_position_s gps; + _ekf.setGpsData(gps_msg); - if (_gps_subs[1].copy(&gps)) { - fillGpsMsgWithVehicleGpsPosData(_gps_state[1], gps); - _gps_alttitude_ellipsoid[1] = gps.alt_ellipsoid; - } - } - - if ((_param_ekf2_gps_mask.get() == 0) && gps1_updated) { - // When GPS blending is disabled we always use the first receiver instance - _ekf.setGpsData(_gps_state[0]); - - } else if ((_param_ekf2_gps_mask.get() > 0) && (gps1_updated || gps2_updated)) { - // blend dual receivers if available - - // calculate blending weights - if (!blend_gps_data()) { - // handle case where the blended states cannot be updated - // Only use selected receiver data if it has been updated - _gps_new_output_data = (gps1_updated && _gps_select_index == 0) || - (gps2_updated && _gps_select_index == 1); - - // Reset relative position offsets to zero - _NE_pos_offset_m[0].zero(); - _NE_pos_offset_m[1].zero(); - _hgt_offset_mm[0] = _hgt_offset_mm[1] = 0.0f; - } - - if (_gps_new_output_data) { - // correct the _gps_state data for steady state offsets and write to _gps_output - apply_gps_offsets(); - - // calculate a blended output from the offset corrected receiver data - if (_gps_select_index == 2) { - calc_gps_blend_output(); - } - - // write selected GPS to EKF - _ekf.setGpsData(_gps_output[_gps_select_index]); - - // log blended solution as a third GPS instance - ekf_gps_position_s gps; - gps.timestamp = _gps_output[_gps_select_index].time_usec; - gps.lat = _gps_output[_gps_select_index].lat; - gps.lon = _gps_output[_gps_select_index].lon; - gps.alt = _gps_output[_gps_select_index].alt; - gps.fix_type = _gps_output[_gps_select_index].fix_type; - gps.eph = _gps_output[_gps_select_index].eph; - gps.epv = _gps_output[_gps_select_index].epv; - gps.s_variance_m_s = _gps_output[_gps_select_index].sacc; - gps.vel_m_s = _gps_output[_gps_select_index].vel_m_s; - gps.vel_n_m_s = _gps_output[_gps_select_index].vel_ned(0); - gps.vel_e_m_s = _gps_output[_gps_select_index].vel_ned(1); - gps.vel_d_m_s = _gps_output[_gps_select_index].vel_ned(2); - gps.vel_ned_valid = _gps_output[_gps_select_index].vel_ned_valid; - gps.satellites_used = _gps_output[_gps_select_index].nsats; - gps.heading = _gps_output[_gps_select_index].yaw; - gps.heading_offset = _gps_output[_gps_select_index].yaw_offset; - gps.selected = _gps_select_index; - - // Publish to the EKF blended GPS topic - _blended_gps_pub.publish(gps); - - // clear flag to avoid re-use of the same data - _gps_new_output_data = false; + _gps_time_usec = gps_msg.time_usec; + _gps_alttitude_ellipsoid = gps.alt_ellipsoid; } } @@ -1866,574 +1729,24 @@ void Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) } } -bool Ekf2::blend_gps_data() -{ - // zero the blend weights - memset(&_blend_weights, 0, sizeof(_blend_weights)); - - /* - * If both receivers have the same update rate, use the oldest non-zero time. - * If two receivers with different update rates are used, use the slowest. - * If time difference is excessive, use newest to prevent a disconnected receiver - * from blocking updates. - */ - - // Calculate the time step for each receiver with some filtering to reduce the effects of jitter - // Find the largest and smallest time step. - float dt_max = 0.0f; - float dt_min = 0.3f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - float raw_dt = 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]); - - if (raw_dt > 0.0f && raw_dt < 0.3f) { - _gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i]; - } - - if (_gps_dt[i] > dt_max) { - dt_max = _gps_dt[i]; - _gps_slowest_index = i; - } - - if (_gps_dt[i] < dt_min) { - dt_min = _gps_dt[i]; - } - } - - // Find the receiver that is last be updated - uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message - uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - // Find largest and smallest times - if (_gps_state[i].time_usec > max_us) { - max_us = _gps_state[i].time_usec; - _gps_newest_index = i; - } - - if ((_gps_state[i].time_usec < min_us) && (_gps_state[i].time_usec > 0)) { - min_us = _gps_state[i].time_usec; - _gps_oldest_index = i; - } - } - - if ((max_us - min_us) > 300000) { - // A receiver has timed out so fall out of blending - if (_gps_state[0].time_usec > _gps_state[1].time_usec) { - _gps_select_index = 0; - - } else { - _gps_select_index = 1; - } - - return false; - } - - // One receiver has lost 3D fix, fall out of blending - if (_gps_state[0].fix_type > 2 && _gps_state[1].fix_type < 3) { - _gps_select_index = 0; - return false; - - } else if (_gps_state[1].fix_type > 2 && _gps_state[0].fix_type < 3) { - _gps_select_index = 1; - return false; - } - - /* - * If the largest dt is less than 20% greater than the smallest, then we have receivers - * running at the same rate then we wait until we have two messages with an arrival time - * difference that is less than 50% of the smallest time step and use the time stamp from - * the newest data. - * Else we have two receivers at different update rates and use the slowest receiver - * as the timing reference. - */ - - if ((dt_max - dt_min) < 0.2f * dt_min) { - // both receivers assumed to be running at the same rate - if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) { - // data arrival within a short time window enables the two measurements to be blended - _gps_time_ref_index = _gps_newest_index; - _gps_new_output_data = true; - } - - } else { - // both receivers running at different rates - _gps_time_ref_index = _gps_slowest_index; - - if (_gps_state[_gps_time_ref_index].time_usec > _time_prev_us[_gps_time_ref_index]) { - // blend data at the rate of the slower receiver - _gps_new_output_data = true; - } - } - - if (_gps_new_output_data) { - _gps_blended_state.time_usec = _gps_state[_gps_time_ref_index].time_usec; - - // calculate the sum squared speed accuracy across all GPS sensors - float speed_accuracy_sum_sq = 0.0f; - - if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_SPD_ACC) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_gps_state[i].fix_type >= 3 && _gps_state[i].sacc > 0.0f) { - speed_accuracy_sum_sq += _gps_state[i].sacc * _gps_state[i].sacc; - - } else { - // not all receivers support this metric so set it to zero and don't use it - speed_accuracy_sum_sq = 0.0f; - break; - } - } - } - - // calculate the sum squared horizontal position accuracy across all GPS sensors - float horizontal_accuracy_sum_sq = 0.0f; - - if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) { - horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph; - - } else { - // not all receivers support this metric so set it to zero and don't use it - horizontal_accuracy_sum_sq = 0.0f; - break; - } - } - } - - // calculate the sum squared vertical position accuracy across all GPS sensors - float vertical_accuracy_sum_sq = 0.0f; - - if (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) { - vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv; - - } else { - // not all receivers support this metric so set it to zero and don't use it - vertical_accuracy_sum_sq = 0.0f; - break; - } - } - } - - // Check if we can do blending using reported accuracy - bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f - || speed_accuracy_sum_sq > 0.0f); - - // if we can't do blending using reported accuracy, return false and hard switch logic will be used instead - if (!can_do_blending) { - return false; - } - - float sum_of_all_weights = 0.0f; - - // calculate a weighting using the reported speed accuracy - float spd_blend_weights[GPS_MAX_RECEIVERS] = {}; - - if (speed_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_SPD_ACC)) { - // calculate the weights using the inverse of the variances - float sum_of_spd_weights = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_gps_state[i].fix_type >= 3 && _gps_state[i].sacc >= 0.001f) { - spd_blend_weights[i] = 1.0f / (_gps_state[i].sacc * _gps_state[i].sacc); - sum_of_spd_weights += spd_blend_weights[i]; - } - } - - // normalise the weights - if (sum_of_spd_weights > 0.0f) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights; - } - - sum_of_all_weights += 1.0f; - } - } - - // calculate a weighting using the reported horizontal position - float hpos_blend_weights[GPS_MAX_RECEIVERS] = {}; - - if (horizontal_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC)) { - // calculate the weights using the inverse of the variances - float sum_of_hpos_weights = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph >= 0.001f) { - hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (_gps_state[i].eph * _gps_state[i].eph); - sum_of_hpos_weights += hpos_blend_weights[i]; - } - } - - // normalise the weights - if (sum_of_hpos_weights > 0.0f) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights; - } - - sum_of_all_weights += 1.0f; - } - } - - // calculate a weighting using the reported vertical position accuracy - float vpos_blend_weights[GPS_MAX_RECEIVERS] = {}; - - if (vertical_accuracy_sum_sq > 0.0f && (_param_ekf2_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC)) { - // calculate the weights using the inverse of the variances - float sum_of_vpos_weights = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv >= 0.001f) { - vpos_blend_weights[i] = vertical_accuracy_sum_sq / (_gps_state[i].epv * _gps_state[i].epv); - sum_of_vpos_weights += vpos_blend_weights[i]; - } - } - - // normalise the weights - if (sum_of_vpos_weights > 0.0f) { - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights; - } - - sum_of_all_weights += 1.0f; - }; - } - - // calculate an overall weight - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - _blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights; - } - - // With updated weights we can calculate a blended GPS solution and - // offsets for each physical receiver - update_gps_blend_states(); - update_gps_offsets(); - _gps_select_index = 2; - - } - - return true; -} - -/* - * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical receiver solutions - * with weights are calculated in calc_gps_blend_weights(). This internal state cannot be used directly by estimators - * because if physical receivers have significant position differences, variation in receiver estimated accuracy will - * cause undesirable variation in the position solution. -*/ -void Ekf2::update_gps_blend_states() -{ - // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver. - _gps_blended_state.time_usec = 0; - _gps_blended_state.lat = 0; - _gps_blended_state.lon = 0; - _gps_blended_state.alt = 0; - _gps_blended_state.fix_type = 0; - _gps_blended_state.eph = FLT_MAX; - _gps_blended_state.epv = FLT_MAX; - _gps_blended_state.sacc = FLT_MAX; - _gps_blended_state.vel_m_s = 0.0f; - _gps_blended_state.vel_ned.setZero(); - _gps_blended_state.vel_ned_valid = true; - _gps_blended_state.nsats = 0; - _gps_blended_state.pdop = FLT_MAX; - - _blended_antenna_offset.zero(); - - // combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights() - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - // blend the timing data - _gps_blended_state.time_usec += (uint64_t)((double)_gps_state[i].time_usec * (double)_blend_weights[i]); - - // use the highest status - if (_gps_state[i].fix_type > _gps_blended_state.fix_type) { - _gps_blended_state.fix_type = _gps_state[i].fix_type; - } - - // calculate a blended average speed and velocity vector - _gps_blended_state.vel_m_s += _gps_state[i].vel_m_s * _blend_weights[i]; - _gps_blended_state.vel_ned += _gps_state[i].vel_ned * _blend_weights[i]; - - // Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers - // If any receiver contributing has an invalid velocity, then report blended velocity as invalid - if (_blend_weights[i] > 0.0f) { - - if (_gps_state[i].eph > 0.0f - && _gps_state[i].eph < _gps_blended_state.eph) { - _gps_blended_state.eph = _gps_state[i].eph; - } - - if (_gps_state[i].epv > 0.0f - && _gps_state[i].epv < _gps_blended_state.epv) { - _gps_blended_state.epv = _gps_state[i].epv; - } - - if (_gps_state[i].sacc > 0.0f - && _gps_state[i].sacc < _gps_blended_state.sacc) { - _gps_blended_state.sacc = _gps_state[i].sacc; - } - - if (_gps_state[i].pdop > 0 - && _gps_state[i].pdop < _gps_blended_state.pdop) { - _gps_blended_state.pdop = _gps_state[i].pdop; - } - - if (_gps_state[i].nsats > 0 - && _gps_state[i].nsats > _gps_blended_state.nsats) { - _gps_blended_state.nsats = _gps_state[i].nsats; - } - - if (!_gps_state[i].vel_ned_valid) { - _gps_blended_state.vel_ned_valid = false; - } - - } - - // TODO read parameters for individual GPS antenna positions and blend - // Vector3f temp_antenna_offset = _antenna_offset[i]; - // temp_antenna_offset *= _blend_weights[i]; - // _blended_antenna_offset += temp_antenna_offset; - - } - - /* - * Calculate the instantaneous weighted average location using available GPS instances and store in _gps_state. - * This is statistically the most likely location, but may not be stable enough for direct use by the EKF. - */ - - // Use the GPS with the highest weighting as the reference position - float best_weight = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_blend_weights[i] > best_weight) { - best_weight = _blend_weights[i]; - _gps_best_index = i; - _gps_blended_state.lat = _gps_state[i].lat; - _gps_blended_state.lon = _gps_state[i].lon; - _gps_blended_state.alt = _gps_state[i].alt; - } - } - - // Convert each GPS position to a local NEU offset relative to the reference position - Vector2f blended_NE_offset_m; - blended_NE_offset_m.zero(); - float blended_alt_offset_mm = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if ((_blend_weights[i] > 0.0f) && (i != _gps_best_index)) { - // calculate the horizontal offset - Vector2f horiz_offset{}; - get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7), - (_gps_blended_state.lon / 1.0e7), (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), - &horiz_offset(0), &horiz_offset(1)); - - // sum weighted offsets - blended_NE_offset_m += horiz_offset * _blend_weights[i]; - - // calculate vertical offset - float vert_offset = (float)(_gps_state[i].alt - _gps_blended_state.alt); - - // sum weighted offsets - blended_alt_offset_mm += vert_offset * _blend_weights[i]; - } - } - - // Add the sum of weighted offsets to the reference position to obtain the blended position - double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7; - double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7; - double lat_deg_res, lon_deg_res; - add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, - &lon_deg_res); - _gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); - _gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); - _gps_blended_state.alt += (int32_t)blended_alt_offset_mm; - - // Take GPS heading from the highest weighted receiver that is publishing a valid .heading value - uint8_t gps_best_yaw_index = 0; - best_weight = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (PX4_ISFINITE(_gps_state[i].yaw) && (_blend_weights[i] > best_weight)) { - best_weight = _blend_weights[i]; - gps_best_yaw_index = i; - } - } - - _gps_blended_state.yaw = _gps_state[gps_best_yaw_index].yaw; - _gps_blended_state.yaw_offset = _gps_state[gps_best_yaw_index].yaw_offset; -} - -/* - * 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 Ekf2::update_gps_offsets() -{ - - // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset - // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering - float alpha[GPS_MAX_RECEIVERS] = {}; - float omega_lpf = 1.0f / fmaxf(_param_ekf2_gps_tau.get(), 1.0f); - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_gps_state[i].time_usec - _time_prev_us[i] > 0) { - // calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter - alpha[i] = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].time_usec - _time_prev_us[i]), - 0.0f, 1.0f); - - _time_prev_us[i] = _gps_state[i].time_usec; - } - } - - // Calculate a filtered position delta for each GPS relative to the blended solution state - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - Vector2f offset; - get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), - (_gps_blended_state.lat / 1.0e7), (_gps_blended_state.lon / 1.0e7), &offset(0), &offset(1)); - _NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]); - _hgt_offset_mm[i] = (float)(_gps_blended_state.alt - _gps_state[i].alt) * alpha[i] + - _hgt_offset_mm[i] * (1.0f - alpha[i]); - } - - // calculate offset limits from the largest difference between receivers - Vector2f max_ne_offset{}; - float max_alt_offset = 0; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - for (uint8_t j = i; j < GPS_MAX_RECEIVERS; j++) { - if (i != j) { - Vector2f offset; - get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), - (_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7), &offset(0), &offset(1)); - max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0))); - max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1))); - max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt))); - } - } - } - - // apply offset limits - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - _NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0)); - _NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1)); - _hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset); - } - -} - - -/* - * Apply the steady state physical receiver offsets calculated by update_gps_offsets(). -*/ -void Ekf2::apply_gps_offsets() -{ - // calculate offset corrected output for each physical GPS. - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - // Add the sum of weighted offsets to the reference position to obtain the blended position - double lat_deg_now = (double)_gps_state[i].lat * 1.0e-7; - double lon_deg_now = (double)_gps_state[i].lon * 1.0e-7; - double lat_deg_res, lon_deg_res; - add_vector_to_global_position(lat_deg_now, lon_deg_now, _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), &lat_deg_res, - &lon_deg_res); - _gps_output[i].lat = (int32_t)(1.0E7 * lat_deg_res); - _gps_output[i].lon = (int32_t)(1.0E7 * lon_deg_res); - _gps_output[i].alt = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i]; - - // other receiver data is used uncorrected - _gps_output[i].time_usec = _gps_state[i].time_usec; - _gps_output[i].fix_type = _gps_state[i].fix_type; - _gps_output[i].vel_m_s = _gps_state[i].vel_m_s; - _gps_output[i].vel_ned = _gps_state[i].vel_ned; - _gps_output[i].eph = _gps_state[i].eph; - _gps_output[i].epv = _gps_state[i].epv; - _gps_output[i].sacc = _gps_state[i].sacc; - _gps_output[i].pdop = _gps_state[i].pdop; - _gps_output[i].nsats = _gps_state[i].nsats; - _gps_output[i].vel_ned_valid = _gps_state[i].vel_ned_valid; - _gps_output[i].yaw = _gps_state[i].yaw; - _gps_output[i].yaw_offset = _gps_state[i].yaw_offset; - - } -} - -/* - Calculate GPS output that is a blend of the offset corrected physical receiver data -*/ -void Ekf2::calc_gps_blend_output() -{ - // Convert each GPS position to a local NEU offset relative to the reference position - // which is defined as the positon of the blended solution calculated from non offset corrected data - Vector2f blended_NE_offset_m; - blended_NE_offset_m.zero(); - float blended_alt_offset_mm = 0.0f; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { - if (_blend_weights[i] > 0.0f) { - // calculate the horizontal offset - Vector2f horiz_offset{}; - get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7), - (_gps_blended_state.lon / 1.0e7), - (_gps_output[i].lat / 1.0e7), - (_gps_output[i].lon / 1.0e7), - &horiz_offset(0), - &horiz_offset(1)); - - // sum weighted offsets - blended_NE_offset_m += horiz_offset * _blend_weights[i]; - - // calculate vertical offset - float vert_offset = (float)(_gps_output[i].alt - _gps_blended_state.alt); - - // sum weighted offsets - blended_alt_offset_mm += vert_offset * _blend_weights[i]; - } - } - - // Add the sum of weighted offsets to the reference position to obtain the blended position - double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7; - double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7; - double lat_deg_res, lon_deg_res; - add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, - &lon_deg_res); - _gps_output[GPS_BLENDED_INSTANCE].lat = (int32_t)(1.0E7 * lat_deg_res); - _gps_output[GPS_BLENDED_INSTANCE].lon = (int32_t)(1.0E7 * lon_deg_res); - _gps_output[GPS_BLENDED_INSTANCE].alt = _gps_blended_state.alt + (int32_t)blended_alt_offset_mm; - - // Copy remaining data from internal states to output - _gps_output[GPS_BLENDED_INSTANCE].time_usec = _gps_blended_state.time_usec; - _gps_output[GPS_BLENDED_INSTANCE].fix_type = _gps_blended_state.fix_type; - _gps_output[GPS_BLENDED_INSTANCE].vel_m_s = _gps_blended_state.vel_m_s; - _gps_output[GPS_BLENDED_INSTANCE].vel_ned = _gps_blended_state.vel_ned; - _gps_output[GPS_BLENDED_INSTANCE].eph = _gps_blended_state.eph; - _gps_output[GPS_BLENDED_INSTANCE].epv = _gps_blended_state.epv; - _gps_output[GPS_BLENDED_INSTANCE].sacc = _gps_blended_state.sacc; - _gps_output[GPS_BLENDED_INSTANCE].pdop = _gps_blended_state.pdop; - _gps_output[GPS_BLENDED_INSTANCE].nsats = _gps_blended_state.nsats; - _gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid = _gps_blended_state.vel_ned_valid; - _gps_output[GPS_BLENDED_INSTANCE].yaw = _gps_blended_state.yaw; - _gps_output[GPS_BLENDED_INSTANCE].yaw_offset = _gps_blended_state.yaw_offset; - -} - float Ekf2::filter_altitude_ellipsoid(float amsl_hgt) { - float height_diff = static_cast(_gps_alttitude_ellipsoid[0]) * 1e-3f - amsl_hgt; + float height_diff = static_cast(_gps_alttitude_ellipsoid) * 1e-3f - amsl_hgt; - if (_gps_alttitude_ellipsoid_previous_timestamp[0] == 0) { + if (_gps_alttitude_ellipsoid_previous_timestamp == 0) { _wgs84_hgt_offset = height_diff; - _gps_alttitude_ellipsoid_previous_timestamp[0] = _gps_state[0].time_usec; + _gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec; - } else if (_gps_state[0].time_usec != _gps_alttitude_ellipsoid_previous_timestamp[0]) { + } else if (_gps_time_usec != _gps_alttitude_ellipsoid_previous_timestamp) { // apply a 10 second first order low pass filter to baro offset - float dt = 1e-6f * static_cast(_gps_state[0].time_usec - _gps_alttitude_ellipsoid_previous_timestamp[0]); - _gps_alttitude_ellipsoid_previous_timestamp[0] = _gps_state[0].time_usec; + float dt = 1e-6f * (_gps_time_usec - _gps_alttitude_ellipsoid_previous_timestamp); float offset_rate_correction = 0.1f * (height_diff - _wgs84_hgt_offset); _wgs84_hgt_offset += dt * math::constrain(offset_rate_correction, -0.1f, 0.1f); + + _gps_alttitude_ellipsoid_previous_timestamp = _gps_time_usec; } return amsl_hgt + _wgs84_hgt_offset; diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 78b9681c1fee..11867eb35d67 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -1375,37 +1375,6 @@ PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f); */ PARAM_DEFINE_FLOAT(EKF2_ABL_TAU, 0.5f); -/** - * Multi GPS Blending Control Mask. - * - * Set bits in the following positions to set which GPS accuracy metrics will be used to calculate the blending weight. Set to zero to disable and always used first GPS instance. - * 0 : Set to true to use speed accuracy - * 1 : Set to true to use horizontal position accuracy - * 2 : Set to true to use vertical position accuracy - * - * @group EKF2 - * @min 0 - * @max 7 - * @bit 0 use speed accuracy - * @bit 1 use hpos accuracy - * @bit 2 use vpos accuracy - */ -PARAM_DEFINE_INT32(EKF2_GPS_MASK, 0); - -/** - * Multi GPS Blending Time Constant - * - * Sets the longest time constant that will be applied to the calculation of GPS position and height offsets used to correct data from multiple GPS data for steady state position differences. - * - * - * @group EKF2 - * @min 1.0 - * @max 100.0 - * @unit s - * @decimal 1 - */ -PARAM_DEFINE_FLOAT(EKF2_GPS_TAU, 10.0f); - /** * Vehicle movement test threshold * diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 00dcbaff6350..00e33807967d 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -94,6 +94,7 @@ void LoggedTopics::add_default_topics() add_topic("vehicle_command"); add_topic("vehicle_control_mode"); add_topic("vehicle_global_position", 200); + add_topic("vehicle_gps_position", 500); add_topic("vehicle_land_detected"); add_topic("vehicle_local_position", 100); add_topic("vehicle_local_position_setpoint", 100); @@ -119,9 +120,9 @@ void LoggedTopics::add_default_topics() add_topic_multi("optical_flow", 1000); add_topic_multi("sensor_accel", 1000); add_topic_multi("sensor_baro", 1000); + add_topic_multi("sensor_gps", 1000); add_topic_multi("sensor_gyro", 1000); add_topic_multi("sensor_mag", 1000); - add_topic_multi("vehicle_gps_position", 1000); add_topic_multi("vehicle_imu", 500); add_topic_multi("vehicle_imu_status", 1000); @@ -166,7 +167,6 @@ void LoggedTopics::add_estimator_replay_topics() { // for estimator replay (need to be at full rate) add_topic("ekf2_timestamps"); - add_topic("ekf_gps_position"); // current EKF2 subscriptions add_topic("airspeed"); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 030cbb01157b..358b85dd9efe 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2020 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -91,6 +91,7 @@ #include #include #include +#include #include #include #include @@ -103,7 +104,6 @@ #include #include #include -#include #include #include #include @@ -1731,7 +1731,7 @@ class MavlinkStreamGPSRawInt : public MavlinkStream } private: - uORB::Subscription _gps_sub{ORB_ID(vehicle_gps_position)}; + uORB::Subscription _gps_sub{ORB_ID(sensor_gps), 0}; /* do not allow top copying this class */ MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &) = delete; @@ -1743,7 +1743,7 @@ class MavlinkStreamGPSRawInt : public MavlinkStream bool send(const hrt_abstime t) override { - vehicle_gps_position_s gps; + sensor_gps_s gps; if (_gps_sub.update(&gps)) { mavlink_gps_raw_int_t msg{}; @@ -1807,7 +1807,7 @@ class MavlinkStreamGPS2Raw : public MavlinkStream } private: - uORB::Subscription _gps2_sub{ORB_ID(vehicle_gps_position), 1}; + uORB::Subscription _gps2_sub{ORB_ID(sensor_gps), 1}; /* do not allow top copying this class */ MavlinkStreamGPS2Raw(MavlinkStreamGPS2Raw &) = delete; @@ -1819,7 +1819,7 @@ class MavlinkStreamGPS2Raw : public MavlinkStream bool send(const hrt_abstime t) override { - vehicle_gps_position_s gps; + sensor_gps_s gps; if (_gps2_sub.update(&gps)) { mavlink_gps2_raw_t msg = {}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f2d0f56475c1..d7b3548eaea7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2266,7 +2266,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) const uint64_t timestamp = hrt_absolute_time(); - struct vehicle_gps_position_s hil_gps = {}; + sensor_gps_s hil_gps{}; hil_gps.timestamp_time_relative = 0; hil_gps.time_utc_usec = gps.time_usec; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a3909a05f008..3fbb129a375e 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -84,6 +84,7 @@ #include #include #include +#include #include #include #include @@ -93,7 +94,6 @@ #include #include #include -#include #include #include #include @@ -247,12 +247,12 @@ class MavlinkReceiver : public ModuleParams uORB::Publication _onboard_computer_status_pub{ORB_ID(onboard_computer_status)}; uORB::Publication _flow_pub{ORB_ID(optical_flow)}; uORB::Publication _pos_sp_triplet_pub{ORB_ID(position_setpoint_triplet)}; + uORB::Publication _gps_pub{ORB_ID(sensor_gps)}; uORB::Publication _attitude_pub{ORB_ID(vehicle_attitude)}; uORB::Publication _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)}; uORB::Publication _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)}; uORB::Publication _fw_virtual_att_sp_pub{ORB_ID(fw_virtual_attitude_setpoint)}; uORB::Publication _global_pos_pub{ORB_ID(vehicle_global_position)}; - uORB::Publication _gps_pub{ORB_ID(vehicle_gps_position)}; uORB::Publication _land_detector_pub{ORB_ID(vehicle_land_detected)}; uORB::Publication _local_pos_pub{ORB_ID(vehicle_local_position)}; uORB::Publication _mocap_odometry_pub{ORB_ID(vehicle_mocap_odometry)}; diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 5b8398b14d1b..e2f605325a7e 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -37,6 +37,7 @@ include_directories(${CMAKE_CURRENT_SOURCE_DIR}) add_subdirectory(vehicle_acceleration) add_subdirectory(vehicle_angular_velocity) add_subdirectory(vehicle_air_data) +add_subdirectory(vehicle_gps_position) add_subdirectory(vehicle_imu) add_subdirectory(vehicle_magnetometer) @@ -57,6 +58,7 @@ px4_add_module( vehicle_acceleration vehicle_angular_velocity vehicle_air_data + vehicle_gps_position vehicle_imu vehicle_magnetometer ) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d28b1f968cfb..69cb4dff0705 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -73,6 +73,7 @@ #include "vehicle_acceleration/VehicleAcceleration.hpp" #include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" #include "vehicle_air_data/VehicleAirData.hpp" +#include "vehicle_gps_position/VehicleGPSPosition.hpp" #include "vehicle_imu/VehicleIMU.hpp" #include "vehicle_magnetometer/VehicleMagnetometer.hpp" @@ -174,6 +175,7 @@ class Sensors : public ModuleBase, public ModuleParams, public px4::Sch VehicleAngularVelocity _vehicle_angular_velocity; VehicleAirData *_vehicle_air_data{nullptr}; VehicleMagnetometer *_vehicle_magnetometer{nullptr}; + VehicleGPSPosition *_vehicle_gps_position{nullptr}; static constexpr int MAX_SENSOR_COUNT = 3; VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; @@ -206,6 +208,7 @@ class Sensors : public ModuleBase, public ModuleParams, public px4::Sch void adc_poll(); void InitializeVehicleAirData(); + void InitializeVehicleGPSPosition(); void InitializeVehicleIMU(); void InitializeVehicleMagnetometer(); @@ -276,6 +279,11 @@ Sensors::~Sensors() delete _vehicle_air_data; } + if (_vehicle_gps_position) { + _vehicle_gps_position->Stop(); + delete _vehicle_gps_position; + } + if (_vehicle_magnetometer) { _vehicle_magnetometer->Stop(); delete _vehicle_magnetometer; @@ -487,6 +495,19 @@ void Sensors::InitializeVehicleAirData() } } +void Sensors::InitializeVehicleGPSPosition() +{ + if (_vehicle_gps_position == nullptr) { + if (orb_exists(ORB_ID(sensor_gps), 0) == PX4_OK) { + _vehicle_gps_position = new VehicleGPSPosition(); + + if (_vehicle_gps_position) { + _vehicle_gps_position->Start(); + } + } + } +} + void Sensors::InitializeVehicleIMU() { // create a VehicleIMU instance for each accel/gyro pair @@ -553,6 +574,7 @@ void Sensors::Run() if (_last_config_update == 0) { InitializeVehicleAirData(); InitializeVehicleIMU(); + InitializeVehicleGPSPosition(); InitializeVehicleMagnetometer(); _voted_sensors_update.init(_sensor_combined); parameter_update_poll(true); @@ -602,6 +624,7 @@ void Sensors::Run() _voted_sensors_update.initializeSensors(); InitializeVehicleAirData(); InitializeVehicleIMU(); + InitializeVehicleGPSPosition(); InitializeVehicleMagnetometer(); _last_config_update = hrt_absolute_time(); @@ -688,6 +711,11 @@ int Sensors::print_status() PX4_INFO_RAW("\n"); _vehicle_angular_velocity.PrintStatus(); + if (_vehicle_gps_position) { + PX4_INFO_RAW("\n"); + _vehicle_gps_position->PrintStatus(); + } + PX4_INFO_RAW("\n"); for (auto &i : _vehicle_imu_list) { diff --git a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt new file mode 100644 index 000000000000..cdc562d02237 --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt @@ -0,0 +1,38 @@ +############################################################################ +# +# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +px4_add_library(vehicle_gps_position + VehicleGPSPosition.cpp + VehicleGPSPosition.hpp +) +target_link_libraries(vehicle_gps_position PRIVATE ecl_geo px4_work_queue) diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp new file mode 100644 index 000000000000..5720f6e52f3f --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -0,0 +1,742 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "VehicleGPSPosition.hpp" + +#include +#include +#include + +namespace sensors +{ +using namespace matrix; +using math::constrain; + +VehicleGPSPosition::VehicleGPSPosition() : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers) +{ +} + +VehicleGPSPosition::~VehicleGPSPosition() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool VehicleGPSPosition::Start() +{ + // force initial updates + ParametersUpdate(true); + + // needed to change the active sensor if the primary stops updating + bool anyGpsRegistered = false; + bool registered[GPS_MAX_RECEIVERS]; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + registered[i] = _sensor_gps_sub[i].registerCallback(); + anyGpsRegistered |= registered[i]; + } + + return anyGpsRegistered; +} + +void VehicleGPSPosition::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto &sub : _sensor_gps_sub) { + sub.unregisterCallback(); + } +} + +void VehicleGPSPosition::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_params_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + } +} + +void VehicleGPSPosition::Run() +{ + perf_begin(_cycle_perf); + ParametersUpdate(); + + // Check all GPS instance + bool any_gps_updated = false; + bool gps_updated[GPS_MAX_RECEIVERS]; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + + gps_updated[i] = _sensor_gps_sub[i].updated(); + + if (gps_updated[i]) { + any_gps_updated = true; + _sensor_gps_sub[i].copy(&_gps_state[i]); + } + } + + + if ((_param_sens_gps_mask.get() == 0) && gps_updated[0]) { + // When GPS blending is disabled we always use the first receiver instance + Publish(_gps_state[0]); + + } else if ((_param_sens_gps_mask.get() > 0) && any_gps_updated) { + // blend multiple receivers if available + + // calculate blending weights + if (!blend_gps_data()) { + // Only use selected receiver data if it has been updated + _gps_new_output_data = false; + _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; 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; 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; i++) { + if (gps_updated[i] && _gps_select_index == i) { + _gps_new_output_data = true; + } + + _NE_pos_offset_m[i].zero(); + _hgt_offset_mm[i] = 0.0f; + } + } + + if (_gps_new_output_data) { + // correct the _gps_state data for steady state offsets and write to _gps_output + apply_gps_offsets(); + + // calculate a blended output from the offset corrected receiver data + if (_gps_select_index == GPS_MAX_RECEIVERS) { + calc_gps_blend_output(); + } + + // write selected GPS to EKF + Publish(_gps_output[_gps_select_index]); + + // clear flag to avoid re-use of the same data + _gps_new_output_data = false; + } + } + + perf_end(_cycle_perf); +} + +bool VehicleGPSPosition::blend_gps_data() +{ + // zero the blend weights + memset(&_blend_weights, 0, sizeof(_blend_weights)); + + /* + * If both receivers have the same update rate, use the oldest non-zero time. + * If two receivers with different update rates are used, use the slowest. + * If time difference is excessive, use newest to prevent a disconnected receiver + * from blocking updates. + */ + + // Calculate the time step for each receiver with some filtering to reduce the effects of jitter + // Find the largest and smallest time step. + float dt_max = 0.0f; + float dt_min = GPS_TIMEOUT_S; + uint8_t gps_count = 0; // Count of receivers which have an active >=2D fix + const hrt_abstime hrt_now = hrt_absolute_time(); + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + const float raw_dt = 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]); + const float present_dt = 1e-6f * (float)(hrt_now - _gps_state[i].timestamp); + + if (raw_dt > 0.0f && raw_dt < GPS_TIMEOUT_S) { + _gps_dt[i] = 0.1f * raw_dt + 0.9f * _gps_dt[i]; + + } else if (present_dt >= GPS_TIMEOUT_S) { + // Timed out - kill the stored fix for this receiver and don't track its (stale) gps_dt + _gps_state[i].timestamp = 0; + _gps_state[i].fix_type = 0; + _gps_state[i].satellites_used = 0; + _gps_state[i].vel_ned_valid = 0; + + continue; + } + + // Only count GPSs with at least a 2D fix for blending purposes + if (_gps_state[i].fix_type < 2) { + continue; + } + + if (_gps_dt[i] > dt_max) { + dt_max = _gps_dt[i]; + _gps_slowest_index = i; + } + + if (_gps_dt[i] < dt_min) { + dt_min = _gps_dt[i]; + } + + gps_count++; + } + + // Find the receiver that is last be updated + uint64_t max_us = 0; // newest non-zero system time of arrival of a GPS message + uint64_t min_us = -1; // oldest non-zero system time of arrival of a GPS message + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + // Find largest and smallest times + if (_gps_state[i].timestamp > max_us) { + max_us = _gps_state[i].timestamp; + _gps_newest_index = i; + } + + if ((_gps_state[i].timestamp < min_us) && (_gps_state[i].timestamp > 0)) { + min_us = _gps_state[i].timestamp; + _gps_oldest_index = i; + } + } + + if (gps_count < 2) { + // Less than 2 receivers left, so fall out of blending + return false; + } + + /* + * If the largest dt is less than 20% greater than the smallest, then we have receivers + * running at the same rate then we wait until we have two messages with an arrival time + * difference that is less than 50% of the smallest time step and use the time stamp from + * the newest data. + * Else we have two receivers at different update rates and use the slowest receiver + * as the timing reference. + */ + + if ((dt_max - dt_min) < 0.2f * dt_min) { + // both receivers assumed to be running at the same rate + if ((max_us - min_us) < (uint64_t)(5e5f * dt_min)) { + // data arrival within a short time window enables the two measurements to be blended + _gps_time_ref_index = _gps_newest_index; + _gps_new_output_data = true; + } + + } else { + // both receivers running at different rates + _gps_time_ref_index = _gps_slowest_index; + + if (_gps_state[_gps_time_ref_index].timestamp > _time_prev_us[_gps_time_ref_index]) { + // blend data at the rate of the slower receiver + _gps_new_output_data = true; + } + } + + if (_gps_new_output_data) { + _gps_blended_state.timestamp = _gps_state[_gps_time_ref_index].timestamp; + + // calculate the sum squared speed accuracy across all GPS sensors + float speed_accuracy_sum_sq = 0.0f; + + if (_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s > 0.0f) { + speed_accuracy_sum_sq += _gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s; + } + } + } + + // calculate the sum squared horizontal position accuracy across all GPS sensors + float horizontal_accuracy_sum_sq = 0.0f; + + if (_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph > 0.0f) { + horizontal_accuracy_sum_sq += _gps_state[i].eph * _gps_state[i].eph; + } + } + } + + // calculate the sum squared vertical position accuracy across all GPS sensors + float vertical_accuracy_sum_sq = 0.0f; + + if (_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv > 0.0f) { + vertical_accuracy_sum_sq += _gps_state[i].epv * _gps_state[i].epv; + } + } + } + + // Check if we can do blending using reported accuracy + bool can_do_blending = (horizontal_accuracy_sum_sq > 0.0f || vertical_accuracy_sum_sq > 0.0f + || speed_accuracy_sum_sq > 0.0f); + + // if we can't do blending using reported accuracy, return false and hard switch logic will be used instead + if (!can_do_blending) { + return false; + } + + float sum_of_all_weights = 0.0f; + + // calculate a weighting using the reported speed accuracy + float spd_blend_weights[GPS_MAX_RECEIVERS] = {}; + + if (speed_accuracy_sum_sq > 0.0f && (_param_sens_gps_mask.get() & BLEND_MASK_USE_SPD_ACC)) { + // calculate the weights using the inverse of the variances + float sum_of_spd_weights = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].s_variance_m_s >= 0.001f) { + spd_blend_weights[i] = 1.0f / (_gps_state[i].s_variance_m_s * _gps_state[i].s_variance_m_s); + sum_of_spd_weights += spd_blend_weights[i]; + } + } + + // normalise the weights + if (sum_of_spd_weights > 0.0f) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + spd_blend_weights[i] = spd_blend_weights[i] / sum_of_spd_weights; + } + + sum_of_all_weights += 1.0f; + } + } + + // calculate a weighting using the reported horizontal position + float hpos_blend_weights[GPS_MAX_RECEIVERS] = {}; + + if (horizontal_accuracy_sum_sq > 0.0f && (_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC)) { + // calculate the weights using the inverse of the variances + float sum_of_hpos_weights = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 2 && _gps_state[i].eph >= 0.001f) { + hpos_blend_weights[i] = horizontal_accuracy_sum_sq / (_gps_state[i].eph * _gps_state[i].eph); + sum_of_hpos_weights += hpos_blend_weights[i]; + } + } + + // normalise the weights + if (sum_of_hpos_weights > 0.0f) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + hpos_blend_weights[i] = hpos_blend_weights[i] / sum_of_hpos_weights; + } + + sum_of_all_weights += 1.0f; + } + } + + // calculate a weighting using the reported vertical position accuracy + float vpos_blend_weights[GPS_MAX_RECEIVERS] = {}; + + if (vertical_accuracy_sum_sq > 0.0f && (_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC)) { + // calculate the weights using the inverse of the variances + float sum_of_vpos_weights = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].fix_type >= 3 && _gps_state[i].epv >= 0.001f) { + vpos_blend_weights[i] = vertical_accuracy_sum_sq / (_gps_state[i].epv * _gps_state[i].epv); + sum_of_vpos_weights += vpos_blend_weights[i]; + } + } + + // normalise the weights + if (sum_of_vpos_weights > 0.0f) { + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + vpos_blend_weights[i] = vpos_blend_weights[i] / sum_of_vpos_weights; + } + + sum_of_all_weights += 1.0f; + }; + } + + // calculate an overall weight + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + _blend_weights[i] = (hpos_blend_weights[i] + vpos_blend_weights[i] + spd_blend_weights[i]) / sum_of_all_weights; + } + + // With updated weights we can calculate a blended GPS solution and + // offsets for each physical receiver + update_gps_blend_states(); + update_gps_offsets(); + _gps_select_index = GPS_MAX_RECEIVERS; + } + + return true; +} + +void VehicleGPSPosition::update_gps_blend_states() +{ + // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver. + _gps_blended_state = sensor_gps_s{}; + _gps_blended_state.eph = FLT_MAX; + _gps_blended_state.epv = FLT_MAX; + _gps_blended_state.s_variance_m_s = FLT_MAX; + _gps_blended_state.vel_ned_valid = true; + //_gps_blended_state.gdop = FLT_MAX; // TODO: add gdop + + _blended_antenna_offset.zero(); + + // combine the the GPS states into a blended solution using the weights calculated in calc_blend_weights() + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + // blend the timing data + _gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)_blend_weights[i]); + + // use the highest status + if (_gps_state[i].fix_type > _gps_blended_state.fix_type) { + _gps_blended_state.fix_type = _gps_state[i].fix_type; + } + + // calculate a blended average speed and velocity vector + _gps_blended_state.vel_m_s += _gps_state[i].vel_m_s * _blend_weights[i]; + _gps_blended_state.vel_n_m_s += _gps_state[i].vel_n_m_s * _blend_weights[i]; + _gps_blended_state.vel_e_m_s += _gps_state[i].vel_e_m_s * _blend_weights[i]; + _gps_blended_state.vel_d_m_s += _gps_state[i].vel_d_m_s * _blend_weights[i]; + + // Assume blended error magnitude, DOP and sat count is equal to the best value from contributing receivers + // If any receiver contributing has an invalid velocity, then report blended velocity as invalid + if (_blend_weights[i] > 0.0f) { + + if (_gps_state[i].eph > 0.0f + && _gps_state[i].eph < _gps_blended_state.eph) { + _gps_blended_state.eph = _gps_state[i].eph; + } + + if (_gps_state[i].epv > 0.0f + && _gps_state[i].epv < _gps_blended_state.epv) { + _gps_blended_state.epv = _gps_state[i].epv; + } + + if (_gps_state[i].s_variance_m_s > 0.0f + && _gps_state[i].s_variance_m_s < _gps_blended_state.s_variance_m_s) { + _gps_blended_state.s_variance_m_s = _gps_state[i].s_variance_m_s; + } + + // TODO: add gdop + // if (_gps_state[i].gdop > 0 + // && _gps_state[i].gdop < _gps_blended_state.gdop) { + // _gps_blended_state.gdop = _gps_state[i].gdop; + // } + + if (_gps_state[i].satellites_used > 0 + && _gps_state[i].satellites_used > _gps_blended_state.satellites_used) { + _gps_blended_state.satellites_used = _gps_state[i].satellites_used; + } + + if (!_gps_state[i].vel_ned_valid) { + _gps_blended_state.vel_ned_valid = false; + } + } + + // TODO read parameters for individual GPS antenna positions and blend + // Vector3f temp_antenna_offset = _antenna_offset[i]; + // temp_antenna_offset *= _blend_weights[i]; + // _blended_antenna_offset += temp_antenna_offset; + } + + /* + * Calculate the instantaneous weighted average location using available GPS instances and store in _gps_state. + * This is statistically the most likely location, but may not be stable enough for direct use by the EKF. + */ + + // Use the GPS with the highest weighting as the reference position + float best_weight = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_blend_weights[i] > best_weight) { + best_weight = _blend_weights[i]; + _gps_best_index = i; + _gps_blended_state.lat = _gps_state[i].lat; + _gps_blended_state.lon = _gps_state[i].lon; + _gps_blended_state.alt = _gps_state[i].alt; + } + } + + // Convert each GPS position to a local NEU offset relative to the reference position + Vector2f blended_NE_offset_m; + blended_NE_offset_m.zero(); + float blended_alt_offset_mm = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if ((_blend_weights[i] > 0.0f) && (i != _gps_best_index)) { + // calculate the horizontal offset + Vector2f horiz_offset{}; + get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7), + (_gps_blended_state.lon / 1.0e7), (_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + &horiz_offset(0), &horiz_offset(1)); + + // sum weighted offsets + blended_NE_offset_m += horiz_offset * _blend_weights[i]; + + // calculate vertical offset + float vert_offset = (float)(_gps_state[i].alt - _gps_blended_state.alt); + + // sum weighted offsets + blended_alt_offset_mm += vert_offset * _blend_weights[i]; + } + } + + // Add the sum of weighted offsets to the reference position to obtain the blended position + double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7; + double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7; + double lat_deg_res, lon_deg_res; + add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, + &lon_deg_res); + _gps_blended_state.lat = (int32_t)(1.0E7 * lat_deg_res); + _gps_blended_state.lon = (int32_t)(1.0E7 * lon_deg_res); + _gps_blended_state.alt += (int32_t)blended_alt_offset_mm; + + // Take GPS heading from the highest weighted receiver that is publishing a valid .heading value + uint8_t gps_best_yaw_index = 0; + best_weight = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (PX4_ISFINITE(_gps_state[i].heading) && (_blend_weights[i] > best_weight)) { + best_weight = _blend_weights[i]; + gps_best_yaw_index = i; + } + } + + _gps_blended_state.heading = _gps_state[gps_best_yaw_index].heading; + _gps_blended_state.heading_offset = _gps_state[gps_best_yaw_index].heading_offset; +} + +void VehicleGPSPosition::update_gps_offsets() +{ + // Calculate filter coefficients to be applied to the offsets for each GPS position and height offset + // Increase the filter time constant proportional to the inverse of the weighting + // A weighting of 1 will make the offset adjust the slowest, a weighting of 0 will make it adjust with zero filtering + float alpha[GPS_MAX_RECEIVERS] = {}; + float omega_lpf = 1.0f / fmaxf(_param_sens_gps_tau.get(), 1.0f); + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_gps_state[i].timestamp - _time_prev_us[i] > 0) { + // calculate the filter coefficient that achieves the time constant specified by the user adjustable parameter + float min_alpha = constrain(omega_lpf * 1e-6f * (float)(_gps_state[i].timestamp - _time_prev_us[i]), + 0.0f, 1.0f); + + // scale the filter coefficient so that time constant is inversely proprtional to weighting + if (_blend_weights[i] > min_alpha) { + alpha[i] = min_alpha / _blend_weights[i]; + + } else { + alpha[i] = 1.0f; + } + + _time_prev_us[i] = _gps_state[i].timestamp; + } + } + + // Calculate a filtered position delta for each GPS relative to the blended solution state + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + Vector2f offset; + get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + (_gps_blended_state.lat / 1.0e7), (_gps_blended_state.lon / 1.0e7), &offset(0), &offset(1)); + _NE_pos_offset_m[i] = offset * alpha[i] + _NE_pos_offset_m[i] * (1.0f - alpha[i]); + _hgt_offset_mm[i] = (float)(_gps_blended_state.alt - _gps_state[i].alt) * alpha[i] + + _hgt_offset_mm[i] * (1.0f - alpha[i]); + } + + // calculate offset limits from the largest difference between receivers + Vector2f max_ne_offset{}; + float max_alt_offset = 0; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + for (uint8_t j = i; j < GPS_MAX_RECEIVERS; j++) { + if (i != j) { + Vector2f offset; + get_vector_to_next_waypoint((_gps_state[i].lat / 1.0e7), (_gps_state[i].lon / 1.0e7), + (_gps_state[j].lat / 1.0e7), (_gps_state[j].lon / 1.0e7), &offset(0), &offset(1)); + max_ne_offset(0) = fmaxf(max_ne_offset(0), fabsf(offset(0))); + max_ne_offset(1) = fmaxf(max_ne_offset(1), fabsf(offset(1))); + max_alt_offset = fmaxf(max_alt_offset, fabsf((float)(_gps_state[i].alt - _gps_state[j].alt))); + } + } + } + + // apply offset limits + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + _NE_pos_offset_m[i](0) = constrain(_NE_pos_offset_m[i](0), -max_ne_offset(0), max_ne_offset(0)); + _NE_pos_offset_m[i](1) = constrain(_NE_pos_offset_m[i](1), -max_ne_offset(1), max_ne_offset(1)); + _hgt_offset_mm[i] = constrain(_hgt_offset_mm[i], -max_alt_offset, max_alt_offset); + } +} + +void VehicleGPSPosition::apply_gps_offsets() +{ + // calculate offset corrected output for each physical GPS. + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + // Add the sum of weighted offsets to the reference position to obtain the blended position + double lat_deg_now = (double)_gps_state[i].lat * 1.0e-7; + double lon_deg_now = (double)_gps_state[i].lon * 1.0e-7; + double lat_deg_res, lon_deg_res; + add_vector_to_global_position(lat_deg_now, lon_deg_now, _NE_pos_offset_m[i](0), _NE_pos_offset_m[i](1), &lat_deg_res, + &lon_deg_res); + _gps_output[i].lat = (int32_t)(1.0E7 * lat_deg_res); + _gps_output[i].lon = (int32_t)(1.0E7 * lon_deg_res); + _gps_output[i].alt = _gps_state[i].alt + (int32_t)_hgt_offset_mm[i]; + + // other receiver data is used uncorrected + _gps_output[i].timestamp = _gps_state[i].timestamp; + _gps_output[i].fix_type = _gps_state[i].fix_type; + _gps_output[i].vel_m_s = _gps_state[i].vel_m_s; + _gps_output[i].vel_n_m_s = _gps_state[i].vel_n_m_s; + _gps_output[i].vel_e_m_s = _gps_state[i].vel_e_m_s; + _gps_output[i].vel_d_m_s = _gps_state[i].vel_d_m_s; + _gps_output[i].eph = _gps_state[i].eph; + _gps_output[i].epv = _gps_state[i].epv; + _gps_output[i].s_variance_m_s = _gps_state[i].s_variance_m_s; + //_gps_output[i].gdop = _gps_state[i].gdop; // TODO: add gdop + _gps_output[i].satellites_used = _gps_state[i].satellites_used; + _gps_output[i].vel_ned_valid = _gps_state[i].vel_ned_valid; + _gps_output[i].heading = _gps_state[i].heading; + _gps_output[i].heading_offset = _gps_state[i].heading_offset; + } +} + +void VehicleGPSPosition::calc_gps_blend_output() +{ + // Convert each GPS position to a local NEU offset relative to the reference position + // which is defined as the positon of the blended solution calculated from non offset corrected data + Vector2f blended_NE_offset_m; + blended_NE_offset_m.zero(); + float blended_alt_offset_mm = 0.0f; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { + if (_blend_weights[i] > 0.0f) { + // calculate the horizontal offset + Vector2f horiz_offset{}; + get_vector_to_next_waypoint((_gps_blended_state.lat / 1.0e7), + (_gps_blended_state.lon / 1.0e7), + (_gps_output[i].lat / 1.0e7), + (_gps_output[i].lon / 1.0e7), + &horiz_offset(0), + &horiz_offset(1)); + + // sum weighted offsets + blended_NE_offset_m += horiz_offset * _blend_weights[i]; + + // calculate vertical offset + float vert_offset = (float)(_gps_output[i].alt - _gps_blended_state.alt); + + // sum weighted offsets + blended_alt_offset_mm += vert_offset * _blend_weights[i]; + } + } + + // Add the sum of weighted offsets to the reference position to obtain the blended position + double lat_deg_now = (double)_gps_blended_state.lat * 1.0e-7; + double lon_deg_now = (double)_gps_blended_state.lon * 1.0e-7; + double lat_deg_res, lon_deg_res; + add_vector_to_global_position(lat_deg_now, lon_deg_now, blended_NE_offset_m(0), blended_NE_offset_m(1), &lat_deg_res, + &lon_deg_res); + _gps_output[GPS_BLENDED_INSTANCE].lat = (int32_t)(1.0E7 * lat_deg_res); + _gps_output[GPS_BLENDED_INSTANCE].lon = (int32_t)(1.0E7 * lon_deg_res); + _gps_output[GPS_BLENDED_INSTANCE].alt = _gps_blended_state.alt + (int32_t)blended_alt_offset_mm; + + // Copy remaining data from internal states to output + _gps_output[GPS_BLENDED_INSTANCE].timestamp = _gps_blended_state.timestamp; + _gps_output[GPS_BLENDED_INSTANCE].fix_type = _gps_blended_state.fix_type; + _gps_output[GPS_BLENDED_INSTANCE].vel_m_s = _gps_blended_state.vel_m_s; + _gps_output[GPS_BLENDED_INSTANCE].vel_n_m_s = _gps_blended_state.vel_n_m_s; + _gps_output[GPS_BLENDED_INSTANCE].vel_e_m_s = _gps_blended_state.vel_e_m_s; + _gps_output[GPS_BLENDED_INSTANCE].vel_d_m_s = _gps_blended_state.vel_d_m_s; + _gps_output[GPS_BLENDED_INSTANCE].eph = _gps_blended_state.eph; + _gps_output[GPS_BLENDED_INSTANCE].epv = _gps_blended_state.epv; + _gps_output[GPS_BLENDED_INSTANCE].s_variance_m_s = _gps_blended_state.s_variance_m_s; + //_gps_output[GPS_BLENDED_INSTANCE].gdop = _gps_blended_state.gdop; // TODO: add gdop + _gps_output[GPS_BLENDED_INSTANCE].satellites_used = _gps_blended_state.satellites_used; + _gps_output[GPS_BLENDED_INSTANCE].vel_ned_valid = _gps_blended_state.vel_ned_valid; + _gps_output[GPS_BLENDED_INSTANCE].heading = _gps_blended_state.heading; + _gps_output[GPS_BLENDED_INSTANCE].heading_offset = _gps_blended_state.heading_offset; +} + +void VehicleGPSPosition::Publish(const sensor_gps_s &gps) +{ + vehicle_gps_position_s gps_output{}; + + gps_output.timestamp = gps.timestamp; + gps_output.time_utc_usec = gps.time_utc_usec; + gps_output.lat = gps.lat; + gps_output.lon = gps.lon; + gps_output.alt = gps.alt; + gps_output.alt_ellipsoid = gps.alt_ellipsoid; + gps_output.s_variance_m_s = gps.s_variance_m_s; + gps_output.c_variance_rad = gps.c_variance_rad; + gps_output.eph = gps.eph; + gps_output.epv = gps.epv; + gps_output.hdop = gps.hdop; + gps_output.vdop = gps.vdop; + gps_output.noise_per_ms = gps.noise_per_ms; + gps_output.jamming_indicator = gps.jamming_indicator; + gps_output.vel_m_s = gps.vel_m_s; + gps_output.vel_n_m_s = gps.vel_n_m_s; + gps_output.vel_e_m_s = gps.vel_e_m_s; + gps_output.vel_d_m_s = gps.vel_d_m_s; + gps_output.cog_rad = gps.cog_rad; + gps_output.timestamp_time_relative = gps.timestamp_time_relative; + gps_output.heading = gps.heading; + gps_output.heading_offset = gps.heading_offset; + gps_output.fix_type = gps.fix_type; + gps_output.vel_ned_valid = gps.vel_ned_valid; + gps_output.satellites_used = gps.satellites_used; + + gps_output.selected = _gps_select_index; + + _vehicle_gps_position_pub.publish(gps_output); +} + +void VehicleGPSPosition::PrintStatus() +{ + PX4_INFO("selected GPS: %d", _gps_select_index); +} + +}; // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp new file mode 100644 index 000000000000..7374799785f2 --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace sensors +{ +class VehicleGPSPosition : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + + VehicleGPSPosition(); + ~VehicleGPSPosition() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + void Run() override; + + void ParametersUpdate(bool force = false); + void Publish(const sensor_gps_s &gps); + + /* + * Update the internal state estimate for a blended GPS solution that is a weighted average of the phsyical + * receiver solutions. This internal state cannot be used directly by estimators because if physical receivers + * have significant position differences, variation in receiver estimated accuracy will cause undesirable + * variation in the position solution. + */ + bool blend_gps_data(); + + /* + * 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(); + + /* + * 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(); + + /* + Calculate GPS output that is a blend of the offset corrected physical receiver data + */ + void calc_gps_blend_output(); + + // 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); + + uORB::Publication _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)}; + + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + + uORB::SubscriptionCallbackWorkItem _sensor_gps_sub[GPS_MAX_RECEIVERS] { /**< sensor data subscription */ + {this, ORB_ID(sensor_gps), 0}, + {this, ORB_ID(sensor_gps), 1}, + }; + + 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) + _param_sens_gps_mask, ///< mask defining when GPS accuracy metrics are used to calculate the blend ratio + (ParamFloat) + _param_sens_gps_tau ///< time constant controlling how rapidly the offset used to bring GPS solutions together is allowed to change (sec) + ) +}; +}; // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/params.c b/src/modules/sensors/vehicle_gps_position/params.c new file mode 100644 index 000000000000..9ea75132776e --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/params.c @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Multi GPS Blending Control Mask. + * + * Set bits in the following positions to set which GPS accuracy metrics will be used to calculate the blending weight. Set to zero to disable and always used first GPS instance. + * 0 : Set to true to use speed accuracy + * 1 : Set to true to use horizontal position accuracy + * 2 : Set to true to use vertical position accuracy + * + * @group Sensors + * @min 0 + * @max 7 + * @bit 0 use speed accuracy + * @bit 1 use hpos accuracy + * @bit 2 use vpos accuracy + */ +PARAM_DEFINE_INT32(SENS_GPS_MASK, 0); + +/** + * Multi GPS Blending Time Constant + * + * Sets the longest time constant that will be applied to the calculation of GPS position and height offsets used to correct data from multiple GPS data for steady state position differences. + * + * + * @group Sensors + * @min 1.0 + * @max 100.0 + * @unit s + * @decimal 1 + */ +PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f); diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 16e257f50140..3d8082fd7edb 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -229,16 +229,16 @@ void Sih::init_variables() void Sih::init_sensors() { - _vehicle_gps_pos.fix_type = 3; // 3D fix - _vehicle_gps_pos.satellites_used = 8; - _vehicle_gps_pos.heading = NAN; - _vehicle_gps_pos.heading_offset = NAN; - _vehicle_gps_pos.s_variance_m_s = 0.5f; - _vehicle_gps_pos.c_variance_rad = 0.1f; - _vehicle_gps_pos.eph = 0.9f; - _vehicle_gps_pos.epv = 1.78f; - _vehicle_gps_pos.hdop = 0.7f; - _vehicle_gps_pos.vdop = 1.1f; + _sensor_gps.fix_type = 3; // 3D fix + _sensor_gps.satellites_used = 8; + _sensor_gps.heading = NAN; + _sensor_gps.heading_offset = NAN; + _sensor_gps.s_variance_m_s = 0.5f; + _sensor_gps.c_variance_rad = 0.1f; + _sensor_gps.eph = 0.9f; + _sensor_gps.epv = 1.78f; + _sensor_gps.hdop = 0.7f; + _sensor_gps.vdop = 1.1f; } // read the motor signals outputted from the mixer @@ -351,21 +351,21 @@ void Sih::send_IMU() void Sih::send_gps() { - _vehicle_gps_pos.timestamp = _now; - _vehicle_gps_pos.lat = (int32_t)(_gps_lat * 1e7); // Latitude in 1E-7 degrees - _vehicle_gps_pos.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees - _vehicle_gps_pos.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres) - _vehicle_gps_pos.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres) - _vehicle_gps_pos.vel_ned_valid = true; // True if NED velocity is valid - _vehicle_gps_pos.vel_m_s = sqrtf(_gps_vel(0) * _gps_vel(0) + _gps_vel(1) * _gps_vel( - 1)); // GPS ground speed, (metres/sec) - _vehicle_gps_pos.vel_n_m_s = _gps_vel(0); // GPS North velocity, (metres/sec) - _vehicle_gps_pos.vel_e_m_s = _gps_vel(1); // GPS East velocity, (metres/sec) - _vehicle_gps_pos.vel_d_m_s = _gps_vel(2); // GPS Down velocity, (metres/sec) - _vehicle_gps_pos.cog_rad = atan2(_gps_vel(1), - _gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) - - _vehicle_gps_pos_pub.publish(_vehicle_gps_pos); + _sensor_gps.timestamp = _now; + _sensor_gps.lat = (int32_t)(_gps_lat * 1e7); // Latitude in 1E-7 degrees + _sensor_gps.lon = (int32_t)(_gps_lon * 1e7); // Longitude in 1E-7 degrees + _sensor_gps.alt = (int32_t)(_gps_alt * 1000.0f); // Altitude in 1E-3 meters above MSL, (millimetres) + _sensor_gps.alt_ellipsoid = (int32_t)(_gps_alt * 1000); // Altitude in 1E-3 meters bove Ellipsoid, (millimetres) + _sensor_gps.vel_ned_valid = true; // True if NED velocity is valid + _sensor_gps.vel_m_s = sqrtf(_gps_vel(0) * _gps_vel(0) + _gps_vel(1) * _gps_vel( + 1)); // GPS ground speed, (metres/sec) + _sensor_gps.vel_n_m_s = _gps_vel(0); // GPS North velocity, (metres/sec) + _sensor_gps.vel_e_m_s = _gps_vel(1); // GPS East velocity, (metres/sec) + _sensor_gps.vel_d_m_s = _gps_vel(2); // GPS Down velocity, (metres/sec) + _sensor_gps.cog_rad = atan2(_gps_vel(1), + _gps_vel(0)); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) + + _sensor_gps_pub.publish(_sensor_gps); } void Sih::publish_sih() diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index d27e23261263..a0a40143d184 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -50,10 +50,10 @@ #include #include #include +#include #include // to publish groundtruth #include // to publish groundtruth #include // to publish groundtruth -#include extern "C" __EXPORT int sih_main(int argc, char *argv[]); @@ -104,8 +104,8 @@ class Sih : public ModuleBase, public ModuleParams PX4Barometer _px4_baro{6620172}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION // to publish the gps position - vehicle_gps_position_s _vehicle_gps_pos{}; - uORB::Publication _vehicle_gps_pos_pub{ORB_ID(vehicle_gps_position)}; + sensor_gps_s _sensor_gps{}; + uORB::Publication _sensor_gps_pub{ORB_ID(sensor_gps)}; // angular velocity groundtruth vehicle_angular_velocity_s _vehicle_angular_velocity_gt{}; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index d0b450c726d9..1dab01091790 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -65,10 +65,10 @@ #include #include #include +#include #include #include #include -#include #include #include #include @@ -218,7 +218,7 @@ class Simulator : public ModuleParams uORB::Publication _input_rc_pub{ORB_ID(input_rc)}; // HIL GPS - uORB::PublicationMulti *_vehicle_gps_position_pubs[ORB_MULTI_MAX_INSTANCES] {}; + uORB::PublicationMulti *_sensor_gps_pubs[ORB_MULTI_MAX_INSTANCES] {}; uint8_t _gps_ids[ORB_MULTI_MAX_INSTANCES] {}; std::default_random_engine _gen{}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 271ff78971d2..ff1e5e801a8d 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -312,7 +312,7 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg) mavlink_msg_hil_gps_decode(msg, &hil_gps); if (!_param_sim_gps_block.get()) { - vehicle_gps_position_s gps{}; + sensor_gps_s gps{}; gps.timestamp = hrt_absolute_time(); gps.time_utc_usec = hil_gps.time_usec; @@ -332,16 +332,16 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg) // New publishers will be created based on the HIL_GPS ID's being different or not for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) { - if (_vehicle_gps_position_pubs[i] && _gps_ids[i] == hil_gps.id) { - _vehicle_gps_position_pubs[i]->publish(gps); + if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) { + _sensor_gps_pubs[i]->publish(gps); break; } - if (_vehicle_gps_position_pubs[i] == nullptr) { - _vehicle_gps_position_pubs[i] = new uORB::PublicationMulti {ORB_ID(vehicle_gps_position)}; + if (_sensor_gps_pubs[i] == nullptr) { + _sensor_gps_pubs[i] = new uORB::PublicationMulti {ORB_ID(sensor_gps)}; _gps_ids[i] = hil_gps.id; - _vehicle_gps_position_pubs[i]->publish(gps); + _sensor_gps_pubs[i]->publish(gps); break; } }