mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 03:04:07 +08:00
7f4fedde6a
When the antennas are not parallel to the x body axis, the GPS message contains the angular offset but the data is already corrected in the driver. EKF2 should then not add this offset during the initialisation.
126 lines
2.4 KiB
C++
126 lines
2.4 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::setYawOffset(float yaw_offset) {
|
|
_gps_data.yaw_offset = yaw_offset;
|
|
}
|
|
|
|
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
|