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.
|
// 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
|
||||||
|
@ -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
|
||||||
|
@ -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
|
||||||
*/
|
*/
|
||||||
|
Loading…
Reference in New Issue
Block a user