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_Common/AP_Common.h>
#include <cmath>
#include <AP_Math/AP_Math.h>
#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<int32_t>(90 * 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::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);

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)
// 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);
};