diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index 5641706345..b7fb16a227 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -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); diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp index fac4e5201e..6bfb06b581 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp @@ -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_pub{ORB_ID(sensor_gps)};