diff --git a/libraries/AP_OLC/AP_OLC.cpp b/libraries/AP_OLC/AP_OLC.cpp index 25b4a0228e..8faa5db431 100644 --- a/libraries/AP_OLC/AP_OLC.cpp +++ b/libraries/AP_OLC/AP_OLC.cpp @@ -16,50 +16,51 @@ */ #include "AP_OLC.h" - -#include +#include #include #if HAL_PLUSCODE_ENABLE // This is a port of https://github.com/google/open-location-code/blob/master/c/olc.c // to avoid double floating point math and use integer math as much as possible. -#define SEPARATOR_CHAR '+' -#define SEPARATOR_POS 8U -#define PADDING_CHAR '0' +static constexpr char SEPARATOR_CHAR = '+'; +static constexpr uint8_t SEPARATOR_POS = 8U; +static constexpr char PADDING_CHAR = '0'; -#define ENCODING_BASE 20U -#define PAIR_CODE_LEN 10U -#define CODE_LEN_MAX 15U +static constexpr uint8_t ENCODING_BASE = 20U; +static constexpr uint8_t PAIR_CODE_LEN = 10U; +static constexpr uint8_t CODE_LEN_MAX = 15U; -#define GRID_COLS 4U -#define GRID_ROWS (ENCODING_BASE / GRID_COLS) +static constexpr uint8_t GRID_COLS = 4U; +static constexpr uint8_t GRID_ROWS = (ENCODING_BASE / GRID_COLS); -#define OLC_DEG_MULTIPLIER 10000000U // 1e7 +static constexpr uint32_t OLC_DEG_MULTIPLIER = 10000000U; // 1e7 -#define LAT_MAX int32_t(90 * OLC_DEG_MULTIPLIER) -#define LON_MAX int32_t(180 * OLC_DEG_MULTIPLIER) +static constexpr int32_t LAT_MAX = static_cast(90 * OLC_DEG_MULTIPLIER); +static constexpr int32_t LON_MAX = static_cast(180 * OLC_DEG_MULTIPLIER); -const int32_t AP_OLC::initial_exponent = floorf(logf(2 * (LON_MAX / OLC_DEG_MULTIPLIER)) / logf(ENCODING_BASE)); -const int32_t AP_OLC::grid_size = (1 / powf(ENCODING_BASE, PAIR_CODE_LEN / 2 - (initial_exponent + 1))) * OLC_DEG_MULTIPLIER; -const int32_t AP_OLC::initial_resolution = powf(ENCODING_BASE, initial_exponent) * OLC_DEG_MULTIPLIER; +const int32_t AP_OLC::INITIAL_EXPONENT = floorf(logf(2 * (LON_MAX / OLC_DEG_MULTIPLIER)) / logf(ENCODING_BASE)); +// Work out the enclosing resolution (in degrees) for the grid algorithm. +const int32_t AP_OLC::GRID_SIZE = (1 / powf(ENCODING_BASE, PAIR_CODE_LEN / 2 - (INITIAL_EXPONENT + 1))) * OLC_DEG_MULTIPLIER; +// Work out the initial resolution +const int32_t AP_OLC::INITIAL_RESOLUTION = powf(ENCODING_BASE, INITIAL_EXPONENT) * OLC_DEG_MULTIPLIER; constexpr char AP_OLC::olc_alphabet[]; // Compute the latitude precision value for a given code length. Lengths <= 10 // have the same precision for latitude and longitude, but lengths > 10 have // different precisions due to the grid method having fewer columns than rows. -float AP_OLC::compute_precision_for_length(int length) +float AP_OLC::compute_precision_for_length(uint8_t length) { // Magic numbers! - if (length <= (int)PAIR_CODE_LEN) { + if (length <= PAIR_CODE_LEN) { return powf(ENCODING_BASE, floorf((length / -2) + 2)); } - return powf(ENCODING_BASE, -3) / powf(5, length - (int)PAIR_CODE_LEN); + return powf(ENCODING_BASE, -3) / powf(5, length - PAIR_CODE_LEN); } -int32_t AP_OLC::adjust_latitude(int32_t lat, size_t code_len) +int32_t AP_OLC::adjust_latitude(int32_t lat, uint8_t code_len) { lat = constrain_int32(lat, -LAT_MAX, LAT_MAX); @@ -89,24 +90,23 @@ int32_t AP_OLC::normalize_longitude(int32_t lon) // uses pairs of characters (latitude and longitude in that order) to represent // each step in a 20x20 grid. Each code, therefore, has 1/400th the area of // the previous code. -unsigned AP_OLC::encode_pairs(uint32_t lat, uint32_t lon, size_t length, char *buf, size_t bufsize) +uint32_t AP_OLC::encode_pairs(uint32_t lat, uint32_t lon, uint8_t length, char *buf, uint8_t bufsize) { if ((length + 1) >= bufsize) { buf[0] = '\0'; return 0; } - unsigned pos = 0; - int32_t resolution = initial_resolution; + uint32_t pos = 0; + int32_t resolution = INITIAL_RESOLUTION; // Add two digits on each pass. - for (size_t digit_count = 0; + for (uint8_t digit_count = 0; digit_count < length; digit_count += 2, resolution /= ENCODING_BASE) { - size_t digit_value; // Do the latitude - gets the digit for this place and subtracts that // for the next digit. - digit_value = lat / resolution; + auto digit_value = lat / resolution; lat -= digit_value * resolution; buf[pos++] = olc_alphabet[digit_value]; @@ -144,8 +144,8 @@ unsigned AP_OLC::encode_pairs(uint32_t lat, uint32_t lon, size_t length, char *b // // This allows default accuracy OLC codes to be refined with just a single // character. -int AP_OLC::encode_grid(uint32_t lat, uint32_t lon, size_t length, - char *buf, size_t bufsize) +int32_t AP_OLC::encode_grid(uint32_t lat, uint32_t lon, uint8_t length, + char *buf, uint8_t bufsize) { if ((length + 1) >= bufsize) { buf[0] = '\0'; @@ -154,13 +154,13 @@ int AP_OLC::encode_grid(uint32_t lat, uint32_t lon, size_t length, int pos = 0; - int32_t lat_grid_size = grid_size; - int32_t lon_grid_size = grid_size; + int32_t lat_grid_size = GRID_SIZE; + int32_t lon_grid_size = GRID_SIZE; lat %= lat_grid_size; lon %= lon_grid_size; - for (size_t i = 0; i < length; i++) { + for (uint8_t i = 0; i < length; i++) { int32_t lat_div = lat_grid_size / GRID_ROWS; int32_t lon_div = lon_grid_size / GRID_COLS; @@ -171,8 +171,8 @@ int AP_OLC::encode_grid(uint32_t lat, uint32_t lon, size_t length, } // Work out the row and column. - size_t row = lat / lat_div; - size_t col = lon / lon_div; + auto row = lat / lat_div; + auto col = lon / lon_div; lat_grid_size /= GRID_ROWS; lon_grid_size /= GRID_COLS; lat -= row * lat_grid_size; @@ -183,9 +183,9 @@ int AP_OLC::encode_grid(uint32_t lat, uint32_t lon, size_t length, return pos; } -int AP_OLC::olc_encode(int32_t lat, int32_t lon, size_t length, char *buf, size_t bufsize) +uint32_t AP_OLC::olc_encode(int32_t lat, int32_t lon, uint8_t length, char *buf, uint8_t bufsize) { - int pos = 0; + uint32_t pos = 0; length = MIN(length, CODE_LEN_MAX); diff --git a/libraries/AP_OLC/AP_OLC.h b/libraries/AP_OLC/AP_OLC.h index 061e3918f2..2c34ed7ad7 100644 --- a/libraries/AP_OLC/AP_OLC.h +++ b/libraries/AP_OLC/AP_OLC.h @@ -32,22 +32,22 @@ public: // olc_encodes the given coordinates in lat and lon (deg * OLC_DEG_MULTIPLIER) // as an OLC code of the given length. It returns the number of characters // written to buf. - static int olc_encode(int32_t lat, int32_t lon, size_t length, char *buf, size_t bufsize); + static uint32_t olc_encode(int32_t lat, int32_t lon, uint8_t length, char *buf, uint8_t bufsize); private: - static const int32_t initial_exponent; + static const int32_t INITIAL_EXPONENT; // Work out the enclosing resolution (in degrees) for the grid algorithm. - static const int32_t grid_size; + static const int32_t GRID_SIZE; // Work out the initial resolution - static const int32_t initial_resolution; + static const int32_t INITIAL_RESOLUTION; static constexpr char olc_alphabet[] = "23456789CFGHJMPQRVWX"; - static float compute_precision_for_length(int length); - static int32_t adjust_latitude(int32_t lat, size_t code_len); + static float compute_precision_for_length(uint8_t length); + static int32_t adjust_latitude(int32_t lat, uint8_t code_len); static int32_t normalize_longitude(int32_t lon); - static unsigned encode_pairs(uint32_t lat, uint32_t lon, size_t length, char *buf, size_t bufsize); - static int encode_grid(uint32_t lat, uint32_t lon, size_t length, char *buf, size_t bufsize); + static uint32_t encode_pairs(uint32_t lat, uint32_t lon, uint8_t length, char *buf, uint8_t bufsize); + static int32_t encode_grid(uint32_t lat, uint32_t lon, uint8_t length, char *buf, uint8_t bufsize); };