AP_GSOF: Use sparse endian instead of custom code

* This saves flash

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

libraries/AP_GSOF/AP_GSOF.cpp

@@ -83,56 +83,6 @@ AP_GSOF::parse(const uint8_t temp, const uint8_t n_expected)
     return NO_GSOF_DATA;
 }
 
-double
-AP_GSOF::SwapDouble(const uint8_t* src, const uint32_t pos) const
-{
-    union {
-        double d;
-        char bytes[sizeof(double)];
-    } doubleu;
-    doubleu.bytes[0] = src[pos + 7];
-    doubleu.bytes[1] = src[pos + 6];
-    doubleu.bytes[2] = src[pos + 5];
-    doubleu.bytes[3] = src[pos + 4];
-    doubleu.bytes[4] = src[pos + 3];
-    doubleu.bytes[5] = src[pos + 2];
-    doubleu.bytes[6] = src[pos + 1];
-    doubleu.bytes[7] = src[pos + 0];
-
-    return doubleu.d;
-}
-
-float
-AP_GSOF::SwapFloat(const uint8_t* src, const uint32_t pos) const
-{
-    union {
-        float f;
-        char bytes[sizeof(float)];
-    } floatu;
-    floatu.bytes[0] = src[pos + 3];
-    floatu.bytes[1] = src[pos + 2];
-    floatu.bytes[2] = src[pos + 1];
-    floatu.bytes[3] = src[pos + 0];
-
-    return floatu.f;
-}
-
-uint32_t
-AP_GSOF::SwapUint32(const uint8_t* src, const uint32_t pos) const
-{
-    uint32_t u;
-    memcpy(&u, &src[pos], sizeof(u));
-    return be32toh(u);
-}
-
-uint16_t
-AP_GSOF::SwapUint16(const uint8_t* src, const uint32_t pos) const
-{
-    uint16_t u;
-    memcpy(&u, &src[pos], sizeof(u));
-    return be16toh(u);
-}
-
 int
 AP_GSOF::process_message(void)
 {
@@ -196,8 +146,8 @@ AP_GSOF::process_message(void)
 void AP_GSOF::parse_pos_time(uint32_t a)
 {
     // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
-    pos_time.time_week_ms = SwapUint32(msg.data, a);
+    pos_time.time_week_ms = be32toh_ptr(msg.data + a);
-    pos_time.time_week = SwapUint16(msg.data, a + 4);
+    pos_time.time_week = be32toh_ptr(msg.data + a + 4);
     pos_time.num_sats = msg.data[a + 6];
     pos_time.pos_flags1 = msg.data[a + 7];
     pos_time.pos_flags2 = msg.data[a + 8];
@@ -206,9 +156,9 @@ void AP_GSOF::parse_pos_time(uint32_t a)
 void AP_GSOF::parse_pos(uint32_t a)
 {
     // This packet is not documented in Trimble's receiver help as of May 18, 2023
-    position.latitude_rad = SwapDouble(msg.data, a);
+    position.latitude_rad = be64todouble_ptr(msg.data, a);
-    position.longitude_rad = SwapDouble(msg.data, a + 8);
+    position.longitude_rad = be64todouble_ptr(msg.data, a + 8);
-    position.altitude = SwapDouble(msg.data, a + 16);
+    position.altitude = be64todouble_ptr(msg.data, a + 16);
 }
 
 void AP_GSOF::parse_vel(uint32_t a)
@@ -218,13 +168,13 @@ void AP_GSOF::parse_vel(uint32_t a)
 
     constexpr uint8_t BIT_VELOCITY_VALID = 0;
     if (BIT_IS_SET(vel.velocity_flags, BIT_VELOCITY_VALID)) {
-        vel.horizontal_velocity = SwapFloat(msg.data, a + 1);
+        vel.horizontal_velocity = be32tofloat_ptr(msg.data, a + 1);
-        vel.vertical_velocity = SwapFloat(msg.data, a + 9);
+        vel.vertical_velocity = be32tofloat_ptr(msg.data, a + 9);
     }
 
     constexpr uint8_t BIT_HEADING_VALID = 2;
     if (BIT_IS_SET(vel.velocity_flags, BIT_HEADING_VALID)) {
-        vel.heading = SwapFloat(msg.data, a + 5);
+        vel.heading = be32tofloat_ptr(msg.data, a + 5);
     }
 }
 
@@ -232,16 +182,16 @@ void AP_GSOF::parse_dop(uint32_t a)
 {
     // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12
     // Skip pdop.
-    dop.hdop = SwapFloat(msg.data, a + 4);
+    dop.hdop = be32tofloat_ptr(msg.data, a + 4);
 }
 
 void AP_GSOF::parse_pos_sigma(uint32_t a)
 {
     // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_SIGMA.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____24
     // Skip pos_rms
-    pos_sigma.sigma_east = SwapFloat(msg.data, a + 4);
+    pos_sigma.sigma_east = be32tofloat_ptr(msg.data, a + 4);
-    pos_sigma.sigma_north = SwapFloat(msg.data, a + 8);
+    pos_sigma.sigma_north = be32tofloat_ptr(msg.data, a + 8);
-    pos_sigma.sigma_up = SwapFloat(msg.data, a + 16);
+    pos_sigma.sigma_up = be32tofloat_ptr(msg.data, a + 16);
 }
 #endif // AP_GSOF_ENABLED
 

libraries/AP_GSOF/AP_GSOF.h

@@ -94,10 +94,6 @@ private:
     // Parses current data and returns the number of parsed GSOF messages.
     int process_message() WARN_IF_UNUSED;
 
-    double SwapDouble(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
-    float SwapFloat(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
-    uint32_t SwapUint32(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
-    uint16_t SwapUint16(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
     void parse_pos_time(uint32_t a);
     void parse_pos(uint32_t a);
     void parse_vel(uint32_t a);