fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 17:43: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/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)) {