diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp index 67cc25b31d..1b73e48899 100644 --- a/libraries/SITL/SIM_GPS.cpp +++ b/libraries/SITL/SIM_GPS.cpp @@ -1086,9 +1086,9 @@ void GPS_GSOF::update_write(const GPS_Data *d) } pos { GSOF_POS_TYPE, GSOF_POS_LEN, - pack_double_into_gsof_packet(d->latitude * DEG_TO_RAD_DOUBLE), - pack_double_into_gsof_packet(d->longitude * DEG_TO_RAD_DOUBLE), - pack_double_into_gsof_packet(static_cast(d->altitude)) + gsof_pack_double(d->latitude * DEG_TO_RAD_DOUBLE), + gsof_pack_double(d->longitude * DEG_TO_RAD_DOUBLE), + gsof_pack_double(static_cast(d->altitude)) }; static_assert(sizeof(gsof_pos) - (sizeof(gsof_pos::OUTPUT_RECORD_TYPE) + sizeof(gsof_pos::RECORD_LEN)) == GSOF_POS_LEN); @@ -1127,11 +1127,11 @@ void GPS_GSOF::update_write(const GPS_Data *d) GSOF_VEL_TYPE, GSOF_VEL_LEN, vel_flags, - pack_float_into_gsof_packet(d->speed_2d()), - pack_float_into_gsof_packet(d->heading()), + gsof_pack_float(d->speed_2d()), + gsof_pack_float(d->heading()), // Trimble API has ambiguous direction here. // Intentionally narrow from double. - pack_float_into_gsof_packet(static_cast(d->speedD)) + gsof_pack_float(static_cast(d->speedD)) }; static_assert(sizeof(gsof_vel) - (sizeof(gsof_vel::OUTPUT_RECORD_TYPE) + sizeof(gsof_vel::RECORD_LEN)) == GSOF_VEL_LEN); @@ -1219,7 +1219,6 @@ void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) TRANSMISSION_NUMBER, PAGE_INDEX, MAX_PAGE_INDEX, - }; ++TRANSMISSION_NUMBER; @@ -1245,7 +1244,6 @@ void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) }; - // Sum bytes (status + type + length + data bytes) and modulo 256 the summation // Because it's a uint8, use natural overflow uint8_t csum = STATUS + PACKET_TYPE + length; @@ -1273,7 +1271,7 @@ void GPS_GSOF::send_gsof(const uint8_t *buf, const uint16_t size) } } -uint64_t GPS_GSOF::pack_double_into_gsof_packet(const double& src) +uint64_t GPS_GSOF::gsof_pack_double(const double& src) { uint64_t dst; static_assert(sizeof(src) == sizeof(dst)); @@ -1282,7 +1280,7 @@ uint64_t GPS_GSOF::pack_double_into_gsof_packet(const double& src) return dst; } -uint32_t GPS_GSOF::pack_float_into_gsof_packet(const float& src) +uint32_t GPS_GSOF::gsof_pack_float(const float& src) { uint32_t dst; static_assert(sizeof(src) == sizeof(dst)); diff --git a/libraries/SITL/SIM_GPS.h b/libraries/SITL/SIM_GPS.h index 746a5832a7..a4648c6123 100644 --- a/libraries/SITL/SIM_GPS.h +++ b/libraries/SITL/SIM_GPS.h @@ -113,8 +113,10 @@ public: private: void send_gsof(const uint8_t *buf, const uint16_t size); - uint64_t pack_double_into_gsof_packet(const double& src) WARN_IF_UNUSED; - uint32_t pack_float_into_gsof_packet(const float& src) WARN_IF_UNUSED; + // These packing utilities for GSOF perform a type-safe floating point byteswap. + // They return integer types because returning floating points would involve an extra copy. + uint64_t gsof_pack_double(const double& src) WARN_IF_UNUSED; + uint32_t gsof_pack_float(const float& src) WARN_IF_UNUSED; }; class GPS_NMEA : public GPS_Backend {