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

AP_GSOF: Use sparse endian instead of custom code 

* This saves flash

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2024-08-20 23:38:53 -07:00 committed by Peter Barker
parent 616d74f1b0
commit 284faf08ee
2 changed files with 12 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
libraries/AP_GSOF/AP_GSOF.cpp
View File

@ -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 = SwapUint16(msg.data, a + 4);
pos_time.time_week_ms = be32toh_ptr(msg.data + a);
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.longitude_rad = SwapDouble(msg.data, a + 8);
position.altitude = SwapDouble(msg.data, a + 16);
position.latitude_rad = be64todouble_ptr(msg.data, a);
position.longitude_rad = be64todouble_ptr(msg.data, a + 8);
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.vertical_velocity = SwapFloat(msg.data, a + 9);
vel.horizontal_velocity = be32tofloat_ptr(msg.data, a + 1);
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_north = SwapFloat(msg.data, a + 8);
pos_sigma.sigma_up = SwapFloat(msg.data, a + 16);
pos_sigma.sigma_east = be32tofloat_ptr(msg.data, a + 4);
pos_sigma.sigma_north = be32tofloat_ptr(msg.data, a + 8);
pos_sigma.sigma_up = be32tofloat_ptr(msg.data, a + 16);
}
#endif // AP_GSOF_ENABLED

4
libraries/AP_GSOF/AP_GSOF.h
View File

@ -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);