AP_GPS: replace utility functions with compiler builtins

This commit is contained in:
Peter Barker 2023-09-11 19:51:08 +10:00 committed by Andrew Tridgell
parent 10c8af3409
commit a391b14a6d
3 changed files with 14 additions and 44 deletions

View File

@ -18,10 +18,14 @@
// Code by Michael Smith. // Code by Michael Smith.
// //
#include "AP_GPS_SIRF.h" #include "AP_GPS_config.h"
#include <stdint.h>
#if AP_GPS_SIRF_ENABLED #if AP_GPS_SIRF_ENABLED
#include "AP_GPS_SIRF.h"
#include <AP_HAL/utility/sparse-endian.h>
#include <stdint.h>
// Initialisation messages // Initialisation messages
// //
// Turn off all messages except for 0x29. // Turn off all messages except for 0x29.
@ -177,13 +181,13 @@ AP_GPS_SIRF::_parse_gps(void)
}else{ }else{
state.status = AP_GPS::GPS_OK_FIX_2D; state.status = AP_GPS::GPS_OK_FIX_2D;
} }
state.location.lat = swap_int32(_buffer.nav.latitude); state.location.lat = int32_t(be32toh(_buffer.nav.latitude));
state.location.lng = swap_int32(_buffer.nav.longitude); state.location.lng = int32_t(be32toh(_buffer.nav.longitude));
state.location.alt = swap_int32(_buffer.nav.altitude_msl); state.location.alt = int32_t(be32toh(_buffer.nav.altitude_msl));
state.have_undulation = true; state.have_undulation = true;
state.undulation = (state.location.alt - swap_int32(_buffer.nav.altitude_ellipsoid))*0.01; state.undulation = (state.location.alt - int32_t(be32toh(_buffer.nav.altitude_ellipsoid)))*0.01;
state.ground_speed = swap_int32(_buffer.nav.ground_speed)*0.01f; state.ground_speed = int32_t(be32toh(_buffer.nav.ground_speed))*0.01f;
state.ground_course = wrap_360(swap_int16(_buffer.nav.ground_course)*0.01f); state.ground_course = wrap_360(int16_t(be16toh(_buffer.nav.ground_course)*0.01f));
state.num_sats = _buffer.nav.satellites; state.num_sats = _buffer.nav.satellites;
fill_3d_velocity(); fill_3d_velocity();
return true; return true;
@ -248,4 +252,5 @@ bool AP_GPS_SIRF::_detect(struct SIRF_detect_state &state, uint8_t data)
} }
return false; return false;
} }
#endif
#endif // AP_GPS_SIRF_ENABLED

View File

@ -46,37 +46,6 @@ AP_GPS_Backend::AP_GPS_Backend(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::
state.have_vertical_accuracy = false; state.have_vertical_accuracy = false;
} }
int32_t AP_GPS_Backend::swap_int32(int32_t v) const
{
const uint8_t *b = (const uint8_t *)&v;
union {
int32_t v;
uint8_t b[4];
} u;
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
return u.v;
}
int16_t AP_GPS_Backend::swap_int16(int16_t v) const
{
const uint8_t *b = (const uint8_t *)&v;
union {
int16_t v;
uint8_t b[2];
} u;
u.b[0] = b[1];
u.b[1] = b[0];
return u.v;
}
/** /**
fill in time_week_ms and time_week from BCD date and time components fill in time_week_ms and time_week from BCD date and time components
assumes MTK19 millisecond form of bcd_time assumes MTK19 millisecond form of bcd_time

View File

@ -111,10 +111,6 @@ protected:
uint32_t _last_itow_ms; uint32_t _last_itow_ms;
bool _have_itow; bool _have_itow;
// common utility functions
int32_t swap_int32(int32_t v) const;
int16_t swap_int16(int16_t v) const;
/* /*
fill in 3D velocity from 2D components fill in 3D velocity from 2D components
*/ */