mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 02:24:06 +08:00
2bafe9df08
This fixes the cases where the yaw message from the GNSS receiver would take more time than the vel/pos. The estimator should wait and not immediately fall back to an other aiding source after 5sec. If it never comes, it will never fall back, but this is ok since the user wants to fly with GPS aiding an not with something else.
122 lines
2.3 KiB
C++
122 lines
2.3 KiB
C++
#include "gps.h"
|
|
|
|
namespace sensor_simulator
|
|
{
|
|
namespace sensor
|
|
{
|
|
|
|
Gps::Gps(std::shared_ptr<Ekf> ekf):Sensor(ekf)
|
|
{
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
void Gps::setData(const gps_message& gps)
|
|
{
|
|
_gps_data = gps;
|
|
}
|
|
|
|
void Gps::setAltitude(int32_t alt)
|
|
{
|
|
_gps_data.alt = alt;
|
|
}
|
|
|
|
void Gps::setLatitude(int32_t lat)
|
|
{
|
|
_gps_data.lat = lat;
|
|
}
|
|
|
|
void Gps::setLongitude(int32_t lon)
|
|
{
|
|
_gps_data.lon = lon;
|
|
}
|
|
|
|
void Gps::setVelocity(const Vector3f& vel)
|
|
{
|
|
_gps_data.vel_ned = vel;
|
|
}
|
|
|
|
void Gps::setYaw(float yaw) {
|
|
_gps_data.yaw = yaw;
|
|
}
|
|
|
|
void Gps::setFixType(int n)
|
|
{
|
|
_gps_data.fix_type = n;
|
|
}
|
|
|
|
void Gps::setNumberOfSatellites(int n)
|
|
{
|
|
_gps_data.nsats = n;
|
|
}
|
|
|
|
void Gps::setPdop(float pdop)
|
|
{
|
|
_gps_data.pdop = pdop;
|
|
}
|
|
|
|
void Gps::stepHeightByMeters(float hgt_change)
|
|
{
|
|
_gps_data.alt += hgt_change * 1e3f;
|
|
}
|
|
|
|
void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change)
|
|
{
|
|
float hposN_curr;
|
|
float hposE_curr;
|
|
map_projection_reference_s origin;
|
|
uint64_t time;
|
|
float alt;
|
|
_ekf->get_ekf_origin(&time, &origin, &alt);
|
|
map_projection_project(&origin, _gps_data.lat * 1e-7, _gps_data.lon * 1e-7, &hposN_curr, &hposE_curr);
|
|
Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change;
|
|
double lat_new;
|
|
double lon_new;
|
|
map_projection_reproject(&origin, hpos_new(0), hpos_new(1), &lat_new, &lon_new);
|
|
_gps_data.lon = static_cast<int32_t>(lon_new * 1e7);
|
|
_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);
|
|
}
|
|
|
|
gps_message Gps::getDefaultGpsData()
|
|
{
|
|
gps_message gps_data{};
|
|
gps_data.time_usec = 0;
|
|
gps_data.lat = 473566094;
|
|
gps_data.lon = 85190237;
|
|
gps_data.alt = 422056;
|
|
gps_data.yaw = NAN;
|
|
gps_data.yaw_offset = 0.0f;
|
|
gps_data.fix_type = 3;
|
|
gps_data.eph = 0.5f;
|
|
gps_data.epv = 0.8f;
|
|
gps_data.sacc = 0.2f;
|
|
gps_data.vel_m_s = 0.0;
|
|
gps_data.vel_ned.setZero();
|
|
gps_data.vel_ned_valid = 1;
|
|
gps_data.nsats = 16;
|
|
gps_data.pdop = 0.0f;
|
|
|
|
return gps_data;
|
|
}
|
|
|
|
} // namespace sensor
|
|
} // namespace sensor_simulator
|