fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-03 14:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

simulator: GPS populate all sensor_gps fields (alt_ellipsoid, etc)

Browse Source
This commit is contained in:
Daniel Agar
2021-04-05 11:19:35 -04:00
parent 015849b402
commit 4189eb11f5
3 changed files with 61 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator_mavlink.cpp
+26 -6
View File
@@ -385,28 +385,48 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
if (!_gps_blocked) {
sensor_gps_s gps{};
gps.timestamp = hrt_absolute_time();
gps.time_utc_usec = hil_gps.time_usec;
gps.fix_type = hil_gps.fix_type;
gps.lat = hil_gps.lat;
gps.lon = hil_gps.lon;
gps.alt = hil_gps.alt;
gps.alt_ellipsoid = hil_gps.alt;
gps.s_variance_m_s = 0.25f;
gps.c_variance_rad = 0.5f;
gps.fix_type = hil_gps.fix_type;
gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
gps.hdop = 0; // TODO
gps.vdop = 0; // TODO
gps.noise_per_ms = 0;
gps.automatic_gain_control = 0;
gps.jamming_indicator = 0;
gps.jamming_state = 0;
gps.vel_m_s = (float)(hil_gps.vel) / 100.0f; // cm/s -> m/s
gps.vel_n_m_s = (float)(hil_gps.vn) / 100.0f; // cm/s -> m/s
gps.vel_e_m_s = (float)(hil_gps.ve) / 100.0f; // cm/s -> m/s
gps.vel_d_m_s = (float)(hil_gps.vd) / 100.0f; // cm/s -> m/s
gps.cog_rad = math::radians((float)(hil_gps.cog) / 100.0f); // cdeg -> rad
gps.cog_rad = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad
gps.vel_ned_valid = true;
gps.timestamp_time_relative = 0;
gps.time_utc_usec = hil_gps.time_usec;
gps.satellites_used = hil_gps.satellites_visible;
gps.s_variance_m_s = 0.25f;
gps.heading = NAN;
gps.heading_offset = NAN;
gps.timestamp = hrt_absolute_time();
// New publishers will be created based on the HIL_GPS ID's being different or not
for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) {
if (_sensor_gps_pubs[i] && _gps_ids[i] == hil_gps.id) {
_sensor_gps_pubs[i]->publish(gps);
break;
}
if (_sensor_gps_pubs[i] == nullptr) {