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

AP_OLC: use right type and static const variable

This commit is contained in:
Pierre Kancir 2023-05-24 17:29:50 +02:00 committed by Andrew Tridgell
parent 938ec84fa7
commit 02b3da65f0
2 changed files with 43 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
libraries/AP_OLC/AP_OLC.cpp
View File

@ -16,50 +16,51 @@
*/ */
#include "AP_OLC.h" #include "AP_OLC.h"
#include <cmath>
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#if HAL_PLUSCODE_ENABLE #if HAL_PLUSCODE_ENABLE
// This is a port of https://github.com/google/open-location-code/blob/master/c/olc.c // 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. // to avoid double floating point math and use integer math as much as possible.
#define SEPARATOR_CHAR '+' static constexpr char SEPARATOR_CHAR = '+';
#define SEPARATOR_POS 8U static constexpr uint8_t SEPARATOR_POS = 8U;
#define PADDING_CHAR '0' static constexpr char PADDING_CHAR = '0';
#define ENCODING_BASE 20U static constexpr uint8_t ENCODING_BASE = 20U;
#define PAIR_CODE_LEN 10U static constexpr uint8_t PAIR_CODE_LEN = 10U;
#define CODE_LEN_MAX 15U static constexpr uint8_t CODE_LEN_MAX = 15U;
#define GRID_COLS 4U static constexpr uint8_t GRID_COLS = 4U;
#define GRID_ROWS (ENCODING_BASE / GRID_COLS) 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) static constexpr int32_t LAT_MAX = static_cast<int32_t>(90 * OLC_DEG_MULTIPLIER);
#define LON_MAX int32_t(180 * OLC_DEG_MULTIPLIER) static constexpr int32_t LON_MAX = static_cast<int32_t>(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::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; // Work out the enclosing resolution (in degrees) for the grid algorithm.
const int32_t AP_OLC::initial_resolution = powf(ENCODING_BASE, initial_exponent) * OLC_DEG_MULTIPLIER; 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[]; constexpr char AP_OLC::olc_alphabet[];
// Compute the latitude precision value for a given code length. Lengths <= 10 // Compute the latitude precision value for a given code length. Lengths <= 10
// have the same precision for latitude and longitude, but lengths > 10 have // have the same precision for latitude and longitude, but lengths > 10 have
// different precisions due to the grid method having fewer columns than rows. // 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! // 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, 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); 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 // 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 // each step in a 20x20 grid. Each code, therefore, has 1/400th the area of
// the previous code. // 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) { if ((length + 1) >= bufsize) {
buf[0] = '\0'; buf[0] = '\0';
return 0; return 0;
} }
unsigned pos = 0; uint32_t pos = 0;
int32_t resolution = initial_resolution; int32_t resolution = INITIAL_RESOLUTION;
// Add two digits on each pass. // Add two digits on each pass.
for (size_t digit_count = 0; for (uint8_t digit_count = 0;
digit_count < length; digit_count < length;
digit_count += 2, resolution /= ENCODING_BASE) { digit_count += 2, resolution /= ENCODING_BASE) {
size_t digit_value;
// Do the latitude - gets the digit for this place and subtracts that // Do the latitude - gets the digit for this place and subtracts that
// for the next digit. // for the next digit.
digit_value = lat / resolution; auto digit_value = lat / resolution;
lat -= digit_value * resolution; lat -= digit_value * resolution;
buf[pos++] = olc_alphabet[digit_value]; 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 // This allows default accuracy OLC codes to be refined with just a single
// character. // character.
int AP_OLC::encode_grid(uint32_t lat, uint32_t lon, size_t length, int32_t AP_OLC::encode_grid(uint32_t lat, uint32_t lon, uint8_t length,
char *buf, size_t bufsize) char *buf, uint8_t bufsize)
{ {
if ((length + 1) >= bufsize) { if ((length + 1) >= bufsize) {
buf[0] = '\0'; buf[0] = '\0';
@ -154,13 +154,13 @@ int AP_OLC::encode_grid(uint32_t lat, uint32_t lon, size_t length,
int pos = 0; int pos = 0;
int32_t lat_grid_size = grid_size; int32_t lat_grid_size = GRID_SIZE;
int32_t lon_grid_size = grid_size; int32_t lon_grid_size = GRID_SIZE;
lat %= lat_grid_size; lat %= lat_grid_size;
lon %= lon_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 lat_div = lat_grid_size / GRID_ROWS;
int32_t lon_div = lon_grid_size / GRID_COLS; 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. // Work out the row and column.
size_t row = lat / lat_div; auto row = lat / lat_div;
size_t col = lon / lon_div; auto col = lon / lon_div;
lat_grid_size /= GRID_ROWS; lat_grid_size /= GRID_ROWS;
lon_grid_size /= GRID_COLS; lon_grid_size /= GRID_COLS;
lat -= row * lat_grid_size; 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; 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); length = MIN(length, CODE_LEN_MAX);

16
libraries/AP_OLC/AP_OLC.h
View File

@ -32,22 +32,22 @@ public:
// olc_encodes the given coordinates in lat and lon (deg * OLC_DEG_MULTIPLIER) // 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 // as an OLC code of the given length. It returns the number of characters
// written to buf. // 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: private:
static const int32_t initial_exponent; static const int32_t INITIAL_EXPONENT;
// Work out the enclosing resolution (in degrees) for the grid algorithm. // 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 // Work out the initial resolution
static const int32_t initial_resolution; static const int32_t INITIAL_RESOLUTION;
static constexpr char olc_alphabet[] = "23456789CFGHJMPQRVWX"; static constexpr char olc_alphabet[] = "23456789CFGHJMPQRVWX";
static float compute_precision_for_length(int length); static float compute_precision_for_length(uint8_t length);
static int32_t adjust_latitude(int32_t lat, size_t code_len); static int32_t adjust_latitude(int32_t lat, uint8_t code_len);
static int32_t normalize_longitude(int32_t lon); 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 uint32_t encode_pairs(uint32_t lat, uint32_t lon, uint8_t length, char *buf, uint8_t bufsize);
static int encode_grid(uint32_t lat, uint32_t lon, size_t length, char *buf, size_t bufsize); static int32_t encode_grid(uint32_t lat, uint32_t lon, uint8_t length, char *buf, uint8_t bufsize);
}; };