You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
121 lines
2.3 KiB
121 lines
2.3 KiB
#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
|
|
|