diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index fcd004f85f..cb4b6f35fd 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -189,7 +189,6 @@ set(msg_files vehicle_constraints.msg vehicle_control_mode.msg vehicle_global_position.msg - vehicle_gps_position.msg vehicle_imu.msg vehicle_imu_status.msg vehicle_land_detected.msg diff --git a/msg/sensor_gps.msg b/msg/sensor_gps.msg index a02300df03..7b07893b3c 100644 --- a/msg/sensor_gps.msg +++ b/msg/sensor_gps.msg @@ -1,6 +1,7 @@ # GPS position in WGS84 coordinates. # the field 'timestamp' is for the position & velocity (microseconds) uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles @@ -21,8 +22,13 @@ float32 vdop # Vertical dilution of precision int32 noise_per_ms # GPS noise per millisecond uint16 automatic_gain_control # Automatic gain control monitor -int32 jamming_indicator # indicates jamming is occurring + +uint8 JAMMING_STATE_UNKNOWN = 0 +uint8 JAMMING_STATE_OK = 1 +uint8 JAMMING_STATE_WARNING = 2 +uint8 JAMMING_STATE_CRITICAL = 3 uint8 jamming_state # indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical +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) @@ -39,3 +45,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]) float32 heading_accuracy # heading accuracy (rad, [0, 2PI]) + +# TOPICS sensor_gps vehicle_gps_position diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg deleted file mode 100644 index e5c87169b5..0000000000 --- a/msg/vehicle_gps_position.msg +++ /dev/null @@ -1,39 +0,0 @@ -# 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 -uint8 jamming_state # indicates whether jamming has been detected or suspected by the receivers. O: Unknown, 1: OK, 2: Warning, 3: Critical - -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: GPS3. 3. Blending multiple receivers diff --git a/src/drivers/rc_input/crsf_telemetry.cpp b/src/drivers/rc_input/crsf_telemetry.cpp index 4cec3f8294..e11687b5d8 100644 --- a/src/drivers/rc_input/crsf_telemetry.cpp +++ b/src/drivers/rc_input/crsf_telemetry.cpp @@ -90,7 +90,7 @@ bool CRSFTelemetry::send_battery() bool CRSFTelemetry::send_gps() { - vehicle_gps_position_s vehicle_gps_position; + sensor_gps_s vehicle_gps_position; if (!_vehicle_gps_position_sub.update(&vehicle_gps_position)) { return false; diff --git a/src/drivers/rc_input/crsf_telemetry.h b/src/drivers/rc_input/crsf_telemetry.h index 6a6e3d7807..910a0ec871 100644 --- a/src/drivers/rc_input/crsf_telemetry.h +++ b/src/drivers/rc_input/crsf_telemetry.h @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/drivers/telemetry/bst/bst.cpp b/src/drivers/telemetry/bst/bst.cpp index 7efc260fdf..5eaa83d011 100644 --- a/src/drivers/telemetry/bst/bst.cpp +++ b/src/drivers/telemetry/bst/bst.cpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include #include #include @@ -262,7 +262,7 @@ void BST::RunImpl() } if (_gps_sub.updated()) { - vehicle_gps_position_s gps; + sensor_gps_s gps; _gps_sub.copy(&gps); if (gps.fix_type >= 3 && gps.eph < 50.0f) { diff --git a/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp b/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp index 6cc69d068d..aba2523ebc 100644 --- a/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp +++ b/src/drivers/telemetry/frsky_telemetry/frsky_data.cpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include @@ -71,7 +71,7 @@ struct frsky_subscription_data_s { uORB::SubscriptionData vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uORB::SubscriptionData vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::SubscriptionData vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::SubscriptionData vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::SubscriptionData vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; }; @@ -180,7 +180,7 @@ void frsky_send_frame1(int uart) int16_t telem_flight_mode = get_telemetry_flight_mode(subscription_data->vehicle_status_sub.get().nav_state); frsky_send_data(uart, FRSKY_ID_TEMP1, telem_flight_mode); // send flight mode as TEMP1. This matches with OpenTX & APM - const vehicle_gps_position_s &gps = subscription_data->vehicle_gps_position_sub.get(); + const sensor_gps_s &gps = subscription_data->vehicle_gps_position_sub.get(); frsky_send_data(uart, FRSKY_ID_TEMP2, gps.satellites_used * 10 + gps.fix_type); frsky_send_startstop(uart); @@ -204,7 +204,7 @@ void frsky_send_frame2(int uart) const vehicle_global_position_s &gpos = subscription_data->vehicle_global_position_sub.get(); const vehicle_local_position_s &lpos = subscription_data->vehicle_local_position_sub.get(); const battery_status_s &battery_status = subscription_data->battery_status_sub.get(); - const vehicle_gps_position_s &gps = subscription_data->vehicle_gps_position_sub.get(); + const sensor_gps_s &gps = subscription_data->vehicle_gps_position_sub.get(); /* send formatted frame */ float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; diff --git a/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp b/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp index dc6995f697..238f8949c0 100644 --- a/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp +++ b/src/drivers/telemetry/frsky_telemetry/sPort_data.cpp @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include @@ -69,7 +69,7 @@ struct s_port_subscription_data_s { uORB::SubscriptionData vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; uORB::SubscriptionData vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uORB::SubscriptionData vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - uORB::SubscriptionData vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; + uORB::SubscriptionData vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; uORB::SubscriptionData vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::SubscriptionData vehicle_status_sub{ORB_ID(vehicle_status)}; }; @@ -336,6 +336,6 @@ void sPort_send_flight_mode(int uart) void sPort_send_GPS_info(int uart) { - const vehicle_gps_position_s &gps = s_port_subscription_data->vehicle_gps_position_sub.get(); + const sensor_gps_s &gps = s_port_subscription_data->vehicle_gps_position_sub.get(); sPort_send_data(uart, FRSKY_ID_TEMP2, gps.satellites_used * 10 + gps.fix_type); } diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index f8e9192570..96c2dd3386 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include @@ -213,7 +213,7 @@ void build_gps_response(uint8_t *buffer, size_t *size) { /* get a local copy of the battery data */ - struct vehicle_gps_position_s gps; + struct sensor_gps_s gps; memset(&gps, 0, sizeof(gps)); orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.cpp b/src/examples/fake_magnetometer/FakeMagnetometer.cpp index 1cb55e4ab3..488ed0d407 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.cpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.cpp @@ -61,7 +61,7 @@ void FakeMagnetometer::Run() } if (_vehicle_gps_position_sub.updated()) { - vehicle_gps_position_s gps; + sensor_gps_s gps; if (_vehicle_gps_position_sub.copy(&gps)) { if (gps.eph < 1000) { diff --git a/src/examples/fake_magnetometer/FakeMagnetometer.hpp b/src/examples/fake_magnetometer/FakeMagnetometer.hpp index 55acc8bbad..c13516f037 100644 --- a/src/examples/fake_magnetometer/FakeMagnetometer.hpp +++ b/src/examples/fake_magnetometer/FakeMagnetometer.hpp @@ -50,7 +50,7 @@ #include #include #include -#include +#include class FakeMagnetometer : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 2ec7b87652..cae05bda9b 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -97,7 +97,7 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu B * R * B.transpose() + Q) * dt; } -void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, +void TerrainEstimator::measurement_update(uint64_t time_ref, const struct sensor_gps_s *gps, const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude) { diff --git a/src/lib/terrain_estimation/terrain_estimator.h b/src/lib/terrain_estimation/terrain_estimator.h index ab5db616fa..f1b3ad13e6 100644 --- a/src/lib/terrain_estimation/terrain_estimator.h +++ b/src/lib/terrain_estimation/terrain_estimator.h @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include @@ -68,7 +68,7 @@ public: void predict(float dt, const struct vehicle_attitude_s *attitude, const struct sensor_combined_s *sensor, const struct distance_sensor_s *distance); - void measurement_update(uint64_t time_ref, const struct vehicle_gps_position_s *gps, + void measurement_update(uint64_t time_ref, const struct sensor_gps_s *gps, const struct distance_sensor_s *distance, const struct vehicle_attitude_s *attitude); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 4089b4e583..627bd89121 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include #include @@ -214,7 +214,7 @@ void AttitudeEstimatorQ::Run() void AttitudeEstimatorQ::update_gps_position() { if (_vehicle_gps_position_sub.updated()) { - vehicle_gps_position_s gps; + sensor_gps_s gps; if (_vehicle_gps_position_sub.update(&gps)) { if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) { diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index c5ae9f1562..291e2d2237 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1864,7 +1864,7 @@ Commander::hasMovedFromCurrentHomeLocation() epv = gpos.epv; } else if (_vehicle_status_flags.gps_position_valid) { - vehicle_gps_position_s gps; + sensor_gps_s gps; _vehicle_gps_position_sub.copy(&gps); const double lat = static_cast(gps.lat) * 1e-7; const double lon = static_cast(gps.lon) * 1e-7; @@ -1911,7 +1911,7 @@ Commander::set_home_position() } else if (_vehicle_status_flags.gps_position_valid) { // Set home using GNSS position - vehicle_gps_position_s gps_pos; + sensor_gps_s gps_pos; _vehicle_gps_position_sub.copy(&gps_pos); const double lat = static_cast(gps_pos.lat) * 1e-7; const double lon = static_cast(gps_pos.lon) * 1e-7; @@ -1970,7 +1970,7 @@ Commander::set_in_air_home_position() // Back-compute lon, lat and alt of home position given the local home position // and current positions in local and global (GNSS raw) frames const vehicle_local_position_s &lpos = _local_position_sub.get(); - vehicle_gps_position_s gps; + sensor_gps_s gps; _vehicle_gps_position_sub.copy(&gps); const double lat = static_cast(gps.lat) * 1e-7; @@ -4219,7 +4219,7 @@ void Commander::estimator_check() const bool condition_gps_position_was_valid = _vehicle_status_flags.gps_position_valid; if (_vehicle_gps_position_sub.updated()) { - vehicle_gps_position_s vehicle_gps_position; + sensor_gps_s vehicle_gps_position; if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) { diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index 221307b847..4690c6182f 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -81,13 +81,13 @@ #include #include #include +#include #include #include #include #include #include #include -#include #include #include #include diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7255581ff3..9903990a53 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -63,7 +63,7 @@ #include #include #include -#include +#include #include using namespace matrix; @@ -230,10 +230,10 @@ static calibrate_return check_calibration_result(float offset_x, float offset_y, static float get_sphere_radius() { // if GPS is available use real field intensity from world magnetic model - uORB::SubscriptionMultiArray gps_subs{ORB_ID::vehicle_gps_position}; + uORB::SubscriptionMultiArray gps_subs{ORB_ID::vehicle_gps_position}; for (auto &gps_sub : gps_subs) { - vehicle_gps_position_s gps; + sensor_gps_s gps; if (gps_sub.copy(&gps)) { if (hrt_elapsed_time(&gps.timestamp) < 100_s && (gps.fix_type >= 2) && (gps.eph < 1000)) { @@ -956,7 +956,7 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian } else { uORB::Subscription vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; - vehicle_gps_position_s gps; + sensor_gps_s gps; if (vehicle_gps_position_sub.copy(&gps)) { if ((gps.timestamp != 0) && (gps.eph < 1000)) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 0737095e30..5ee4688169 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1779,7 +1779,7 @@ void EKF2::UpdateGpsSample(ekf2_timestamps_s &ekf2_timestamps) { // EKF GPS message const unsigned last_generation = _vehicle_gps_position_sub.get_last_generation(); - vehicle_gps_position_s vehicle_gps_position; + sensor_gps_s vehicle_gps_position; if (_vehicle_gps_position_sub.update(&vehicle_gps_position)) { if (_msg_missed_gps_perf == nullptr) { diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index bc72f98a73..9df018b3fe 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -83,7 +83,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 12f169360a..b8cf96b00a 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -25,7 +25,7 @@ #include #include #include -#include +#include #include #include #include @@ -271,7 +271,7 @@ private: uORB::SubscriptionData _sub_att{ORB_ID(vehicle_attitude)}; uORB::SubscriptionData _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)}; uORB::SubscriptionData _sub_flow{ORB_ID(vehicle_optical_flow)}; - uORB::SubscriptionData _sub_gps{ORB_ID(vehicle_gps_position)}; + uORB::SubscriptionData _sub_gps{ORB_ID(vehicle_gps_position)}; uORB::SubscriptionData _sub_visual_odom{ORB_ID(vehicle_visual_odometry)}; uORB::SubscriptionData _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)}; uORB::SubscriptionData _sub_dist0{ORB_ID(distance_sensor), 0}; diff --git a/src/modules/logger/util.cpp b/src/modules/logger/util.cpp index ec1e658d07..efff34af6a 100644 --- a/src/modules/logger/util.cpp +++ b/src/modules/logger/util.cpp @@ -40,7 +40,7 @@ #include #include -#include +#include #include #include @@ -80,7 +80,7 @@ bool get_log_time(struct tm *tt, int utc_offset_sec, bool boot_time) bool use_clock_time = true; /* Get the latest GPS publication */ - vehicle_gps_position_s gps_pos; + sensor_gps_s gps_pos; if (vehicle_gps_position_sub.copy(&gps_pos)) { utc_time_sec = gps_pos.time_utc_usec / 1e6; diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index a7312d29a9..ec3bf49923 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include #include @@ -550,7 +550,7 @@ private: void update_gps() { - vehicle_gps_position_s gps; + sensor_gps_s gps; if (_gps_sub.update(&gps)) { _eph.add_value(gps.eph, _update_rate_filtered); diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 968cd48a63..f131130b90 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -213,7 +213,7 @@ bool Geofence::checkAll(double lat, double lon, float altitude) } } -bool Geofence::check(const vehicle_global_position_s &global_position, const vehicle_gps_position_s &gps_position) +bool Geofence::check(const vehicle_global_position_s &global_position, const sensor_gps_s &gps_position) { if (_param_gf_altmode.get() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 2ab449b331..b7386db830 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include #include #define GEOFENCE_FILENAME PX4_STORAGEDIR"/etc/geofence.txt" @@ -88,7 +88,7 @@ public: * * @return true: system is obeying fence, false: system is violating fence */ - bool check(const vehicle_global_position_s &global_position, const vehicle_gps_position_s &gps_position); + bool check(const vehicle_global_position_s &global_position, const sensor_gps_s &gps_position); /** * Return whether a mission item obeys the geofence. diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d4bb945890..a90e124ffc 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -73,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -356,7 +356,7 @@ private: home_position_s _home_pos{}; /**< home position for RTL */ mission_result_s _mission_result{}; vehicle_global_position_s _global_pos{}; /**< global vehicle position */ - vehicle_gps_position_s _gps_pos{}; /**< gps position */ + sensor_gps_s _gps_pos{}; /**< gps position */ vehicle_land_detected_s _land_detected{}; /**< vehicle land_detected */ vehicle_local_position_s _local_pos{}; /**< local vehicle position */ vehicle_status_s _vstatus{}; /**< vehicle status */ diff --git a/src/modules/replay/ReplayEkf2.cpp b/src/modules/replay/ReplayEkf2.cpp index c096da6ab5..823cff189c 100644 --- a/src/modules/replay/ReplayEkf2.cpp +++ b/src/modules/replay/ReplayEkf2.cpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index b722b65624..964b5fdecc 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -129,7 +129,14 @@ void VehicleGPSPosition::Run() _gps_blending.update(hrt_absolute_time()); if (_gps_blending.isNewOutputDataAvailable()) { - Publish(_gps_blending.getOutputGpsData(), _gps_blending.getSelectedGps()); + sensor_gps_s gps_output{_gps_blending.getOutputGpsData()}; + + // clear device_id if blending + if (_gps_blending.getSelectedGps() == GpsBlending::GPS_MAX_RECEIVERS_BLEND) { + gps_output.device_id = 0; + } + + _vehicle_gps_position_pub.publish(gps_output); } } @@ -138,45 +145,9 @@ void VehicleGPSPosition::Run() perf_end(_cycle_perf); } -void VehicleGPSPosition::Publish(const sensor_gps_s &gps, uint8_t selected) -{ - 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.jamming_state = gps.jamming_state; - 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 = selected; - - _vehicle_gps_position_pub.publish(gps_output); -} - void VehicleGPSPosition::PrintStatus() { - //PX4_INFO_RAW("[vehicle_gps_position] selected GPS: %d\n", _gps_select_index); + PX4_INFO_RAW("[vehicle_gps_position] selected GPS: %d\n", _gps_blending.getSelectedGps()); } }; // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index 8126fd5efd..971f24ae4a 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -45,7 +45,6 @@ #include #include #include -#include #include "gps_blending.hpp" @@ -69,7 +68,6 @@ private: void Run() override; void ParametersUpdate(bool force = false); - void Publish(const sensor_gps_s &gps, uint8_t selected); // 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; @@ -81,7 +79,7 @@ private: static_assert(GPS_MAX_RECEIVERS == GpsBlending::GPS_MAX_RECEIVERS_BLEND, "GPS_MAX_RECEIVERS must match to GPS_MAX_RECEIVERS_BLEND"); - uORB::Publication _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)}; + uORB::Publication _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; diff --git a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp index b37d91c6ba..35e88c9964 100644 --- a/src/modules/sensors/vehicle_gps_position/gps_blending.cpp +++ b/src/modules/sensors/vehicle_gps_position/gps_blending.cpp @@ -348,8 +348,21 @@ bool GpsBlending::blend_gps_data(uint64_t hrt_now_us) sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS_BLEND]) const { + // Use the GPS with the highest weighting as the reference position + float best_weight = 0.0f; + + // index of the physical receiver with the lowest reported error + uint8_t gps_best_index = 0; + + for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { + if (blend_weights[i] > best_weight) { + best_weight = blend_weights[i]; + gps_best_index = i; + } + } + // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver. - sensor_gps_s gps_blended_state{}; + sensor_gps_s gps_blended_state{_gps_state[0]}; // start with best GPS for all other misc fields gps_blended_state.eph = FLT_MAX; gps_blended_state.epv = FLT_MAX; gps_blended_state.s_variance_m_s = FLT_MAX; @@ -361,6 +374,7 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; i++) { // blend the timing data gps_blended_state.timestamp += (uint64_t)((double)_gps_state[i].timestamp * (double)blend_weights[i]); + gps_blended_state.timestamp_sample += (uint64_t)((double)_gps_state[i].timestamp_sample * (double)blend_weights[i]); // use the highest status if (_gps_state[i].fix_type > gps_blended_state.fix_type) { @@ -423,22 +437,6 @@ sensor_gps_s GpsBlending::gps_blend_states(float blend_weights[GPS_MAX_RECEIVERS * 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; - - // index of the physical receiver with the lowest reported error - uint8_t gps_best_index = 0; - - for (uint8_t i = 0; i < GPS_MAX_RECEIVERS_BLEND; 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{0, 0}; float blended_alt_offset_mm = 0.0f;