fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-12 16:27:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensor_simulator: add possibility to set GPS position rate

Browse Source 
one can set the position rate = velocity to obtain consistent gps data
This commit is contained in:
bresch
2020-06-03 11:14:47 +02:00
committed by Mathieu Bresciani
parent b3dc06d0cb
commit 5d6a72e383
2 changed files with 12 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/gps.cpp
+10 -1
View File
@@ -15,7 +15,17 @@ Gps::~Gps()
void Gps::send(uint64_t time)
{
const float dt = static_cast<float>(time - _gps_data.time_usec) * 1e-6f;
_gps_data.time_usec = time;
if (fabsf(_gps_pos_rate(0)) > FLT_EPSILON || fabsf(_gps_pos_rate(1)) > FLT_EPSILON) {
stepHorizontalPositionByMeters(Vector2f(_gps_pos_rate) * dt);
}
if (fabsf(_gps_pos_rate(2)) > FLT_EPSILON) {
stepHeightByMeters(-_gps_pos_rate(2) * dt);
}
_ekf->setGpsData(_gps_data);
}
@@ -85,7 +95,6 @@ void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change)
_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);
}
gps_message Gps::getDefaultGpsData()
{
gps_message gps_data{};