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

GPS simulation: Manage delays correctly

Browse Source 
The GPS simulation now mimicks the real driver more closely and should provide even GPS delays. The delays themselves are set by the simulator, and default to 120 ms for Gazebo
This commit is contained in:
Lorenz Meier
2017-08-02 13:06:59 +02:00
parent 7d46858ae4
commit 9d49690f17
4 changed files with 33 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/sitl_gazebo
+1 -1
Submodule Tools/sitl_gazebo updated: 0686806fd9...db9a70eccb
src/modules/simulator/simulator.h
+1
View File
@@ -111,6 +111,7 @@ struct RawAirspeedData {
#pragma pack(push, 1)
struct RawGPSData {
int64_t timestamp;
int32_t lat;
int32_t lon;
int32_t alt;
src/modules/simulator/simulator_mavlink.cpp
+1
View File
@@ -261,6 +261,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
void Simulator::update_gps(mavlink_hil_gps_t *gps_sim)
{
RawGPSData gps = {};
gps.timestamp = gps_sim->time_usec;
gps.lat = gps_sim->lat;
gps.lon = gps_sim->lon;
gps.alt = gps_sim->alt;
src/platforms/posix/drivers/gpssim/gpssim.cpp
+30 -17
View File
@@ -71,6 +71,7 @@ using namespace DriverFramework;
#define GPSSIM_DEVICE_PATH "/dev/gpssim"
#define TIMEOUT_5HZ 500
#define TIMEOUT_10MS 10
#define RATE_MEASUREMENT_PERIOD 5000000
/* class for dynamic allocation of satellite info data */
@@ -274,22 +275,32 @@ GPSSIM::receive(int timeout)
simulator::RawGPSData gps;
sim->getGPSSample((uint8_t *)&gps, sizeof(gps));
_report_gps_pos.timestamp = hrt_absolute_time();
_report_gps_pos.lat = gps.lat;
_report_gps_pos.lon = gps.lon;
_report_gps_pos.alt = gps.alt;
_report_gps_pos.eph = (float)gps.eph * 1e-2f;
_report_gps_pos.epv = (float)gps.epv * 1e-2f;
_report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f;
_report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f;
_report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f;
_report_gps_pos.vel_d_m_s = (float)(gps.vd) / 100.0f;
_report_gps_pos.cog_rad = (float)(gps.cog) * 3.1415f / (100.0f * 180.0f);
_report_gps_pos.fix_type = gps.fix_type;
_report_gps_pos.satellites_used = gps.satellites_visible;
static uint64_t timestamp_last = 0;
usleep(120000);
return 1;
if (gps.timestamp != timestamp_last) {
_report_gps_pos.timestamp = hrt_absolute_time();
_report_gps_pos.lat = gps.lat;
_report_gps_pos.lon = gps.lon;
_report_gps_pos.alt = gps.alt;
_report_gps_pos.eph = (float)gps.eph * 1e-2f;
_report_gps_pos.epv = (float)gps.epv * 1e-2f;
_report_gps_pos.vel_m_s = (float)(gps.vel) / 100.0f;
_report_gps_pos.vel_n_m_s = (float)(gps.vn) / 100.0f;
_report_gps_pos.vel_e_m_s = (float)(gps.ve) / 100.0f;
_report_gps_pos.vel_d_m_s = (float)(gps.vd) / 100.0f;
_report_gps_pos.cog_rad = (float)(gps.cog) * 3.1415f / (100.0f * 180.0f);
_report_gps_pos.fix_type = gps.fix_type;
_report_gps_pos.satellites_used = gps.satellites_visible;
timestamp_last = gps.timestamp;
return 1;
} else {
usleep(timeout);
return 0;
}
}
void
@@ -349,10 +360,12 @@ GPSSIM::task_main()
// GPS is obviously detected successfully, reset statistics
//_Helper->reset_update_rates();
while ((receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) {
int recv_ret = 0;
while ((recv_ret = receive(TIMEOUT_10MS)) >= 0 && !_task_should_exit) {
/* opportunistic publishing - else invalid data would end up on the bus */
if (!(m_pub_blocked)) {
if (recv_ret && !(m_pub_blocked)) {
orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos);
if (_p_report_sat_info) {