dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

combine sensor_gps + vehicle_gps_position msgs (keeping separate topics)

This commit is contained in:
Daniel Agar 2022-07-05 14:28:09 -04:00
parent 32ae00fd44
commit 15223009d2
29 changed files with 77 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/CMakeLists.txt
View File

@ -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

10
msg/sensor_gps.msg
View File

@ -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

39
msg/vehicle_gps_position.msg
View File

@ -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

2
src/drivers/rc_input/crsf_telemetry.cpp
View File

@ -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;

2
src/drivers/rc_input/crsf_telemetry.h
View File

@ -42,7 +42,7 @@
#include <uORB/Subscription.hpp>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_status.h>
#include <drivers/drv_hrt.h>

4
src/drivers/telemetry/bst/bst.cpp
View File

@ -49,7 +49,7 @@
#include <stdlib.h>
#include <math.h>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/vehicle_attitude.h>
#include <matrix/math.hpp>
@ -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) {

8
src/drivers/telemetry/frsky_telemetry/frsky_data.cpp
View File

@ -57,7 +57,7 @@
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_local_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_status.h>
#include <drivers/drv_hrt.h>
@ -71,7 +71,7 @@ struct frsky_subscription_data_s {
uORB::SubscriptionData<vehicle_air_data_s> vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::SubscriptionData<vehicle_local_position_s> vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<vehicle_global_position_s> vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<vehicle_gps_position_s> vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData<sensor_gps_s> vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData<vehicle_status_s> 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;

6
src/drivers/telemetry/frsky_telemetry/sPort_data.cpp
View File

@ -58,7 +58,7 @@
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <drivers/drv_hrt.h>
@ -69,7 +69,7 @@ struct s_port_subscription_data_s {
uORB::SubscriptionData<vehicle_acceleration_s> vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
uORB::SubscriptionData<vehicle_air_data_s> vehicle_air_data_sub{ORB_ID(vehicle_air_data)};
uORB::SubscriptionData<vehicle_global_position_s> vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
uORB::SubscriptionData<vehicle_gps_position_s> vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData<sensor_gps_s> vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
uORB::SubscriptionData<vehicle_local_position_s> vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
uORB::SubscriptionData<vehicle_status_s> 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);
}

4
src/drivers/telemetry/hott/messages.cpp
View File

@ -51,7 +51,7 @@
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vehicle_air_data.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <drivers/drv_hrt.h>
@ -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);

2
src/examples/fake_magnetometer/FakeMagnetometer.cpp
View File

@ -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) {

2
src/examples/fake_magnetometer/FakeMagnetometer.hpp
View File

@ -50,7 +50,7 @@
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
class FakeMagnetometer : public ModuleBase<FakeMagnetometer>, public ModuleParams, public px4::ScheduledWorkItem
{

2
src/lib/terrain_estimation/terrain_estimator.cpp
View File

@ -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)
{

4
src/lib/terrain_estimation/terrain_estimator.h
View File

@ -40,7 +40,7 @@
#include <lib/mathlib/mathlib.h>
#include <matrix/math.hpp>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/distance_sensor.h>
@ -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);

4
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
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)) {

8
src/modules/commander/Commander.cpp
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)) {

2
src/modules/commander/Commander.hpp
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>

8
src/modules/commander/mag_calibration.cpp
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)) {

2
src/modules/ekf2/EKF2.cpp
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) {

2
src/modules/ekf2/EKF2.hpp
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>

4
src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp
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};

4
src/modules/logger/util.cpp
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;

4
src/modules/mavlink/streams/HIGH_LATENCY2.hpp
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);

2
src/modules/navigator/geofence.cpp
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) {

4
src/modules/navigator/geofence.h
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.

4
src/modules/navigator/navigator.h
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 */

2
src/modules/replay/ReplayEkf2.cpp
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>

47
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
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

4
src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp
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};

32
src/modules/sensors/vehicle_gps_position/gps_blending.cpp
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;