fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-09 06:38:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

SensorGpsSim: follow naming convention, replace length valculation with vector library call

Browse Source
This commit is contained in:
Matthias Grob
2026-02-03 19:51:16 +13:00
committed by Julian Oes
parent 82e3322e0c
commit f29eb2ee32
2 changed files with 15 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp
+12 -12
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2025 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
@@ -106,19 +106,19 @@ void SensorGpsSim::Run()
updateParams();
}
if (_vehicle_local_position_sub.updated() && _vehicle_global_position_sub.updated()) {
if (_vehicle_local_position_groundtruth_sub.updated() && _vehicle_global_position_groundtruth_sub.updated()) {
vehicle_local_position_s lpos{};
_vehicle_local_position_sub.copy(&lpos);
vehicle_local_position_s vehicle_local_position{};
_vehicle_local_position_groundtruth_sub.copy(&vehicle_local_position);
vehicle_global_position_s gpos{};
_vehicle_global_position_sub.copy(&gpos);
vehicle_global_position_s vehicle_global_position{};
_vehicle_global_position_groundtruth_sub.copy(&vehicle_global_position);
double latitude = gpos.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
double longitude = gpos.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
double altitude = (double)(gpos.alt + (generate_wgn() * 0.5f));
double latitude = vehicle_global_position.lat + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
double longitude = vehicle_global_position.lon + math::degrees((double)generate_wgn() * 0.2 / CONSTANTS_RADIUS_OF_EARTH);
double altitude = (double)(vehicle_global_position.alt + (generate_wgn() * 0.5f));
Vector3f gps_vel = Vector3f{lpos.vx, lpos.vy, lpos.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f);
Vector3f gps_vel = Vector3f{vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz} + noiseGauss3f(0.06f, 0.077f, 0.158f);
// device id
device::Device::DeviceId device_id;
@@ -150,7 +150,7 @@ void SensorGpsSim::Run()
sensor_gps.vdop = 100.f;
}
sensor_gps.timestamp_sample = gpos.timestamp_sample;
sensor_gps.timestamp_sample = vehicle_global_position.timestamp_sample;
sensor_gps.time_utc_usec = 0;
sensor_gps.device_id = device_id.devid;
sensor_gps.latitude_deg = latitude; // Latitude in degrees
@@ -159,7 +159,7 @@ void SensorGpsSim::Run()
sensor_gps.altitude_ellipsoid_m = altitude;
sensor_gps.noise_per_ms = 0;
sensor_gps.jamming_indicator = 0;
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_m_s = Vector2f(gps_vel).length(); // GPS ground speed, (metres/sec)
sensor_gps.vel_n_m_s = gps_vel(0);
sensor_gps.vel_e_m_s = gps_vel(1);
sensor_gps.vel_d_m_s = gps_vel(2);
src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp
+3 -3
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2025 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
@@ -75,8 +75,8 @@ private:
matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz) { return matrix::Vector3f(generate_wgn() * stdx, generate_wgn() * stdy, generate_wgn() * stdz); }
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)};
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)};
uORB::Subscription _vehicle_global_position_groundtruth_sub{ORB_ID(vehicle_global_position_groundtruth)};
uORB::Subscription _vehicle_local_position_groundtruth_sub{ORB_ID(vehicle_local_position_groundtruth)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};