dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -71,8 +71,6 @@
#define MAVLINK_RECEIVER_NET_ADDED_STACK 0
#endif
using matrix::wrap_2pi;
const uint8_t MavlinkReceiver::supported_component_map[COMP_ID_MAX] = {
[COMP_ID_ALL] = MAV_COMP_ID_ALL,
[COMP_ID_AUTOPILOT1] = MAV_COMP_ID_AUTOPILOT1,
@ -2229,39 +2227,49 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
void
MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
{
mavlink_hil_gps_t gps;
mavlink_msg_hil_gps_decode(msg, &gps);
mavlink_hil_gps_t hil_gps;
mavlink_msg_hil_gps_decode(msg, &hil_gps);
const uint64_t timestamp = hrt_absolute_time();
sensor_gps_s gps{};
sensor_gps_s hil_gps{};
gps.lat = hil_gps.lat;
gps.lon = hil_gps.lon;
gps.alt = hil_gps.alt;
gps.alt_ellipsoid = hil_gps.alt;
hil_gps.timestamp_time_relative = 0;
hil_gps.time_utc_usec = gps.time_usec;
gps.s_variance_m_s = 0.25f;
gps.c_variance_rad = 0.5f;
gps.fix_type = hil_gps.fix_type;
hil_gps.timestamp = timestamp;
hil_gps.lat = gps.lat;
hil_gps.lon = gps.lon;
hil_gps.alt = gps.alt;
hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m
hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m
gps.eph = (float)hil_gps.eph * 1e-2f; // cm -> m
gps.epv = (float)hil_gps.epv * 1e-2f; // cm -> m
hil_gps.s_variance_m_s = 0.1f;
gps.hdop = 0; // TODO
gps.vdop = 0; // TODO
hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
hil_gps.vel_ned_valid = true;
hil_gps.cog_rad = ((gps.cog == 65535) ? NAN : wrap_2pi(math::radians(gps.cog * 1e-2f)));
gps.noise_per_ms = 0;
gps.automatic_gain_control = 0;
gps.jamming_indicator = 0;
gps.jamming_state = 0;
hil_gps.fix_type = gps.fix_type;
hil_gps.satellites_used = gps.satellites_visible; //TODO: rename mavlink_hil_gps_t sats visible to used?
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 = ((hil_gps.cog == 65535) ? NAN : matrix::wrap_2pi(math::radians(hil_gps.cog * 1e-2f))); // cdeg -> rad
gps.vel_ned_valid = true;
hil_gps.heading = NAN;
hil_gps.heading_offset = NAN;
gps.timestamp_time_relative = 0;
gps.time_utc_usec = hil_gps.time_usec;
_gps_pub.publish(hil_gps);
gps.satellites_used = hil_gps.satellites_visible;
gps.heading = NAN;
gps.heading_offset = NAN;
gps.timestamp = hrt_absolute_time();
_sensor_gps_pub.publish(gps);
}
void

2
src/modules/mavlink/mavlink_receiver.h
View File

@ -331,7 +331,7 @@ private:
uORB::Publication<onboard_computer_status_s> _onboard_computer_status_pub{ORB_ID(onboard_computer_status)};
uORB::Publication<generator_status_s> _generator_status_pub{ORB_ID(generator_status)};
uORB::Publication<optical_flow_s> _flow_pub{ORB_ID(optical_flow)};
uORB::Publication<sensor_gps_s> _gps_pub{ORB_ID(sensor_gps)};
uORB::Publication<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
uORB::Publication<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
uORB::Publication<vehicle_attitude_setpoint_s> _att_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _mc_virtual_att_sp_pub{ORB_ID(mc_virtual_attitude_setpoint)};

32
src/modules/simulator/simulator_mavlink.cpp
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) {