mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 03:54:07 +08:00
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 = 0.0f;
|
|
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
|