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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

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

31
libraries/AP_GPS/GPS_Backend.cpp
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;
}
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
assumes MTK19 millisecond form of bcd_time

4
libraries/AP_GPS/GPS_Backend.h
View File

@ -111,10 +111,6 @@ protected:
uint32_t _last_itow_ms;
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
*/