dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

sensor_simulator: add possibility to set GPS position rate 

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
test/sensor_simulator/gps.cpp
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{};

2
test/sensor_simulator/gps.h
View File

@ -53,6 +53,7 @@ public:
void setData(const gps_message& gps);
void stepHeightByMeters(float hgt_change);
void stepHorizontalPositionByMeters(Vector2f hpos_change);
void setPositionRateNED(const Vector3f& rate) { _gps_pos_rate = rate; }
void setAltitude(int32_t alt);
void setLatitude(int32_t lat);
void setLongitude(int32_t lon);
@ -66,6 +67,7 @@ public:
private:
gps_message _gps_data;
Vector3f _gps_pos_rate{};
void send(uint64_t time) override;