mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_GPS: replace utility functions with compiler builtins
This commit is contained in:
parent
10c8af3409
commit
a391b14a6d
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
*/
|
||||
|
Loading…
Reference in New Issue
Block a user