Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: Add NMEA THS message.

This commit is contained in:
Philippe Hamelin 2020-07-07 09:17:08 -04:00 committed by Andrew Tridgell
parent 738d000f86
commit 187549b431
3 changed files with 14 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AP_HAL_SITL/sitl_gps.cpp
View File

@ -421,7 +421,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
pvt.headVeh = 0;
memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2));
if (_sitl->gps_hdg_enabled[instance]) {
if (_sitl->gps_hdg_enabled[instance] > SITL::SITL::GPS_HEADING_NONE) {
const Vector3f ant1_pos = _sitl->gps_pos_offset[instance^1].get();
const Vector3f ant2_pos = _sitl->gps_pos_offset[instance].get();
Vector3f rel_antenna_pos = ant2_pos - ant1_pos;
@ -444,7 +444,7 @@ void SITL_State::_update_gps_ubx(const struct gps_data *d, uint8_t instance)
_gps_send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol), instance);
_gps_send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop), instance);
_gps_send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt), instance);
if (_sitl->gps_hdg_enabled[instance]) {
if (_sitl->gps_hdg_enabled[instance] > SITL::SITL::GPS_HEADING_NONE) {
_gps_send_ubx(MSG_RELPOSNED, (uint8_t*)&relposned, sizeof(relposned), instance);
}
@ -738,9 +738,12 @@ void SITL_State::_update_gps_nmea(const struct gps_data *d, uint8_t instance)
heading,
dstring);
if (_sitl->gps_hdg_enabled[instance]) {
if (_sitl->gps_hdg_enabled[instance] == SITL::SITL::GPS_HEADING_HDT) {
_gps_nmea_printf(instance, "$GPHDT,%.2f,T", d->yaw);
}
else if (_sitl->gps_hdg_enabled[instance] == SITL::SITL::GPS_HEADING_THS) {
_gps_nmea_printf(instance, "$GPTHS,%.2f,%c,T", d->yaw, d->have_lock ? 'A' : 'V');
}
}
void SITL_State::_sbp_send_message(uint16_t msg_type, uint16_t sender_id, uint8_t len, uint8_t *payload, uint8_t instance)

4
libraries/SITL/SITL.cpp
View File

@ -294,7 +294,7 @@ const AP_Param::GroupInfo SITL::var_gps[] = {
AP_GROUPINFO("GPS_NOISE", 10, SITL, gps_noise[0], 0),
AP_GROUPINFO("GPS_LOCKTIME", 11, SITL, gps_lock_time[0], 0),
AP_GROUPINFO("GPS_ALT_OFS", 12, SITL, gps_alt_offset[0], 0),
AP_GROUPINFO("GPS_HDG", 13, SITL, gps_hdg_enabled[0], 0),
AP_GROUPINFO("GPS_HDG", 13, SITL, gps_hdg_enabled[0], SITL::GPS_HEADING_NONE),
AP_GROUPINFO("GPS_ACC", 14, SITL, gps_accuracy[0], 0.3),
AP_GROUPINFO("GPS_VERR", 15, SITL, gps_vel_err[0], 0),
@ -310,7 +310,7 @@ const AP_Param::GroupInfo SITL::var_gps[] = {
AP_GROUPINFO("GPS2_NOISE", 39, SITL, gps_noise[1], 0),
AP_GROUPINFO("GPS2_LCKTIME", 40, SITL, gps_lock_time[1], 0),
AP_GROUPINFO("GPS2_ALT_OFS", 41, SITL, gps_alt_offset[1], 0),
AP_GROUPINFO("GPS2_HDG", 42, SITL, gps_hdg_enabled[1], 0),
AP_GROUPINFO("GPS2_HDG", 42, SITL, gps_hdg_enabled[1], SITL::GPS_HEADING_NONE),
AP_GROUPINFO("GPS2_ACC", 43, SITL, gps_accuracy[1], 0.3),
AP_GROUPINFO("GPS2_VERR", 44, SITL, gps_vel_err[1], 0),

6
libraries/SITL/SITL.h
View File

@ -138,6 +138,12 @@ public:
GPS_TYPE_SBP2 = 9,
};
enum GPSHeading {
GPS_HEADING_NONE = 0,
GPS_HEADING_HDT = 1,
GPS_HEADING_THS = 2,
};
struct sitl_fdm state;
// loop update rate in Hz