fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 21:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

combine sensor_gps + vehicle_gps_position msgs (keeping separate topics)

Browse Source
This commit is contained in:
Daniel Agar
2022-07-07 10:24:11 -04:00
parent 32ae00fd44
commit 15223009d2
29 changed files with 77 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+2 -2
View File
@@ -57,7 +57,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_odometry.h>
@@ -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)) {
src/modules/commander/Commander.cpp
+4 -4
View File
@@ -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<double>(gps.lat) * 1e-7;
const double lon = static_cast<double>(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<double>(gps_pos.lat) * 1e-7;
const double lon = static_cast<double>(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<double>(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)) {
src/modules/commander/Commander.hpp
+1 -1
View File
@@ -81,13 +81,13 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/power_button_state.h>
#include <uORB/topics/rtl_time_estimate.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/system_power.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vtol_vehicle_status.h>
src/modules/commander/mag_calibration.cpp
+4 -4
View File
@@ -63,7 +63,7 @@
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/mag_worker_data.h>
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<vehicle_gps_position_s, 3> gps_subs{ORB_ID::vehicle_gps_position};
uORB::SubscriptionMultiArray<sensor_gps_s, 3> 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)) {
src/modules/ekf2/EKF2.cpp
+1 -1
View File
@@ -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) {
src/modules/ekf2/EKF2.hpp
+1 -1
View File
@@ -83,7 +83,7 @@
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_imu.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
+2 -2
View File
@@ -25,7 +25,7 @@
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/landing_target_pose.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_odometry.h>
@@ -271,7 +271,7 @@ private:
uORB::SubscriptionData<vehicle_attitude_s> _sub_att{ORB_ID(vehicle_attitude)};
uORB::SubscriptionData<vehicle_angular_velocity_s> _sub_angular_velocity{ORB_ID(vehicle_angular_velocity)};
uORB::SubscriptionData<vehicle_optical_flow_s> _sub_flow{ORB_ID(vehicle_optical_flow)};
uORB::SubscriptionData<vehicle_gps_position_s> _sub_gps{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData<sensor_gps_s> _sub_gps{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData<vehicle_odometry_s> _sub_visual_odom{ORB_ID(vehicle_visual_odometry)};
uORB::SubscriptionData<vehicle_odometry_s> _sub_mocap_odom{ORB_ID(vehicle_mocap_odometry)};
uORB::SubscriptionData<distance_sensor_s> _sub_dist0{ORB_ID(distance_sensor), 0};
src/modules/logger/util.cpp
+2 -2
View File
@@ -40,7 +40,7 @@
#include <unistd.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <drivers/drv_hrt.h>
#include <px4_platform_common/events.h>
@@ -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;
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
+2 -2
View File
@@ -51,7 +51,7 @@
#include <uORB/topics/wind.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_status_flags.h>
@@ -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);
src/modules/navigator/geofence.cpp
+1 -1
View File
@@ -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) {
src/modules/navigator/geofence.h
+2 -2
View File
@@ -50,7 +50,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_air_data.h>
#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.
src/modules/navigator/navigator.h
+2 -2
View File
@@ -73,7 +73,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_command_ack.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
@@ -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 */
src/modules/replay/ReplayEkf2.cpp
+1 -1
View File
@@ -42,7 +42,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_magnetometer.h>
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
+9 -38
View File
@@ -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
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp
+1 -3
View File
@@ -45,7 +45,6 @@
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_gps_position.h>
#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_s> _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)};
uORB::Publication<sensor_gps_s> _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
src/modules/sensors/vehicle_gps_position/gps_blending.cpp
+15 -17
View File
@@ -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;