mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 15:33:57 -04:00
AP_OLC: clean namespace and use constexpr instead of init method
This commit is contained in:
Michel Pastor
Peter Barker
parent
4eeccb61cd
commit
8465588467
@ -21,46 +21,15 @@
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#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.
|
||||
|
||||
static constexpr uint8_t SEPARATOR_CHAR = '+';
|
||||
static constexpr uint8_t SEPARATOR_POS = 8;
|
||||
static constexpr uint8_t PADDING_CHAR = '0';
|
||||
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;
|
||||
|
||||
static constexpr uint8_t ENCODING_BASE = 20;
|
||||
static constexpr uint8_t PAIR_CODE_LEN = 10;
|
||||
static constexpr uint8_t CODE_LEN_MAX = 15;
|
||||
|
||||
static constexpr uint8_t GRID_COLS = 4;
|
||||
static constexpr uint8_t GRID_ROWS = ENCODING_BASE / GRID_COLS;
|
||||
|
||||
|
||||
static constexpr int32_t LAT_MAX = 90 * OLC_DEG_MULTIPLIER;
|
||||
static constexpr int32_t LON_MAX = 180 * OLC_DEG_MULTIPLIER;
|
||||
|
||||
int32_t AP_OLC::grid_size;
|
||||
int32_t AP_OLC::initial_resolution;
|
||||
const char alphabet[] = "23456789CFGHJMPQRVWX";
|
||||
|
||||
bool AP_OLC::inited = false;
|
||||
|
||||
void AP_OLC::init_constants(void)
|
||||
{
|
||||
if (inited) {
|
||||
return;
|
||||
}
|
||||
inited = true;
|
||||
|
||||
// Work out the encoding base exponent necessary to represent 360 degrees.
|
||||
int32_t initial_exponent = floorf(logf(2 * (LON_MAX / OLC_DEG_MULTIPLIER)) / logf(ENCODING_BASE));
|
||||
|
||||
// Work out the enclosing resolution (in degrees) for the grid algorithm.
|
||||
AP_OLC::grid_size = (1 / powf(ENCODING_BASE, PAIR_CODE_LEN / 2 - (initial_exponent + 1))) * OLC_DEG_MULTIPLIER;
|
||||
|
||||
// Work out the initial resolution
|
||||
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
|
||||
@ -77,12 +46,9 @@ float AP_OLC::compute_precision_for_length(int length)
|
||||
|
||||
int32_t AP_OLC::adjust_latitude(int32_t lat, size_t code_len)
|
||||
{
|
||||
if (lat < -LAT_MAX) {
|
||||
lat = -LAT_MAX;
|
||||
}
|
||||
if (lat > LAT_MAX) {
|
||||
lat = LAT_MAX;
|
||||
}
|
||||
|
||||
lat = constrain_int32(lat, -LAT_MAX, LAT_MAX);
|
||||
|
||||
if (lat >= LAT_MAX) {
|
||||
// Subtract half the code precision to get the latitude into the code area.
|
||||
int32_t precision = compute_precision_for_length(code_len) * OLC_DEG_MULTIPLIER;
|
||||
@ -116,7 +82,7 @@ unsigned AP_OLC::encode_pairs(uint32_t lat, uint32_t lon, size_t length, char *b
|
||||
}
|
||||
|
||||
unsigned pos = 0;
|
||||
int32_t resolution = AP_OLC::initial_resolution;
|
||||
int32_t resolution = initial_resolution;
|
||||
// Add two digits on each pass.
|
||||
for (size_t digit_count = 0;
|
||||
digit_count < length;
|
||||
@ -127,13 +93,13 @@ unsigned AP_OLC::encode_pairs(uint32_t lat, uint32_t lon, size_t length, char *b
|
||||
// for the next digit.
|
||||
digit_value = lat / resolution;
|
||||
lat -= digit_value * resolution;
|
||||
buf[pos++] = alphabet[digit_value];
|
||||
buf[pos++] = olc_alphabet[digit_value];
|
||||
|
||||
// Do the longitude - gets the digit for this place and subtracts that
|
||||
// for the next digit.
|
||||
digit_value = lon / resolution;
|
||||
lon -= digit_value * resolution;
|
||||
buf[pos++] = alphabet[digit_value];
|
||||
buf[pos++] = olc_alphabet[digit_value];
|
||||
|
||||
// Should we add a separator here?
|
||||
if (pos == SEPARATOR_POS && pos < length) {
|
||||
@ -173,8 +139,8 @@ int AP_OLC::encode_grid(uint32_t lat, uint32_t lon, size_t length,
|
||||
|
||||
int pos = 0;
|
||||
|
||||
int32_t lat_grid_size = AP_OLC::grid_size;
|
||||
int32_t lon_grid_size = AP_OLC::grid_size;
|
||||
int32_t lat_grid_size = grid_size;
|
||||
int32_t lon_grid_size = grid_size;
|
||||
|
||||
lat %= lat_grid_size;
|
||||
lon %= lon_grid_size;
|
||||
@ -196,7 +162,7 @@ int AP_OLC::encode_grid(uint32_t lat, uint32_t lon, size_t length,
|
||||
lon_grid_size /= GRID_COLS;
|
||||
lat -= row * lat_grid_size;
|
||||
lon -= col * lon_grid_size;
|
||||
buf[pos++] = alphabet[row * GRID_COLS + col];
|
||||
buf[pos++] = olc_alphabet[row * GRID_COLS + col];
|
||||
}
|
||||
buf[pos] = '\0';
|
||||
return pos;
|
||||
@ -212,8 +178,6 @@ int AP_OLC::olc_encode(int32_t lat, int32_t lon, size_t length, char *buf, size_
|
||||
uint32_t alat = adjust_latitude(lat, length) + LAT_MAX;
|
||||
uint32_t alon = normalize_longitude(lon) + LON_MAX;
|
||||
|
||||
init_constants();
|
||||
|
||||
pos += encode_pairs(alat, alon, MIN(length, PAIR_CODE_LEN), buf + pos, bufsize - pos);
|
||||
// If the requested length indicates we want grid refined codes.
|
||||
if (length > PAIR_CODE_LEN) {
|
||||
@ -222,3 +186,5 @@ int AP_OLC::olc_encode(int32_t lat, int32_t lon, size_t length, char *buf, size_
|
||||
buf[pos] = '\0';
|
||||
return pos;
|
||||
}
|
||||
|
||||
#endif
|
||||
@ -17,12 +17,15 @@
|
||||
|
||||
#pragma once
|
||||
#include <stdint.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
|
||||
typedef int32_t int32_t;
|
||||
#ifndef HAL_PLUSCODE_ENABLE
|
||||
#define HAL_PLUSCODE_ENABLE !HAL_MINIMIZE_FEATURES && BOARD_FLASH_SIZE > 1024
|
||||
#endif
|
||||
|
||||
|
||||
static constexpr int32_t OLC_DEG_MULTIPLIER = 10000000; // 1e7
|
||||
|
||||
#if HAL_PLUSCODE_ENABLE
|
||||
class AP_OLC
|
||||
{
|
||||
public:
|
||||
@ -34,16 +37,36 @@ public:
|
||||
|
||||
private:
|
||||
|
||||
static bool inited;
|
||||
// Initialized via init_constants()
|
||||
static int32_t grid_size;
|
||||
static int32_t initial_resolution;
|
||||
static constexpr uint8_t SEPARATOR_CHAR = '+';
|
||||
static constexpr uint8_t SEPARATOR_POS = 8;
|
||||
static constexpr uint8_t PADDING_CHAR = '0';
|
||||
|
||||
static constexpr uint8_t ENCODING_BASE = 20;
|
||||
static constexpr uint8_t PAIR_CODE_LEN = 10;
|
||||
static constexpr uint8_t CODE_LEN_MAX = 15;
|
||||
|
||||
static constexpr uint8_t GRID_COLS = 4;
|
||||
static constexpr uint8_t GRID_ROWS = ENCODING_BASE / GRID_COLS;
|
||||
|
||||
static constexpr int32_t OLC_DEG_MULTIPLIER = 10000000; // 1e7
|
||||
|
||||
static constexpr int32_t LAT_MAX = 90 * OLC_DEG_MULTIPLIER;
|
||||
static constexpr int32_t LON_MAX = 180 * OLC_DEG_MULTIPLIER;
|
||||
|
||||
static const int32_t initial_exponent;
|
||||
// Work out the enclosing resolution (in degrees) for the grid algorithm.
|
||||
static const int32_t grid_size;
|
||||
// Work out the initial resolution
|
||||
static const int32_t initial_resolution;
|
||||
|
||||
static constexpr char olc_alphabet[] = "23456789CFGHJMPQRVWX";
|
||||
|
||||
static void init_constants(void);
|
||||
static float compute_precision_for_length(int length);
|
||||
static int32_t adjust_latitude(int32_t lat, size_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 int encode_grid(uint32_t lat, uint32_t lon, size_t length, char *buf, size_t bufsize);
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
@ -23,6 +23,7 @@
|
||||
#include <RC_Channel/RC_Channel.h>
|
||||
#include <AP_Param/AP_Param.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_OLC/AP_OLC.h>
|
||||
|
||||
#ifndef OSD_ENABLED
|
||||
#define OSD_ENABLED 0
|
||||
@ -175,7 +176,9 @@ private:
|
||||
AP_OSD_Setting bat2_vlt{false, 0, 0};
|
||||
AP_OSD_Setting bat2used{false, 0, 0};
|
||||
AP_OSD_Setting clk{false, 0, 0};
|
||||
#if HAL_PLUSCODE_ENABLE
|
||||
AP_OSD_Setting pluscode{false, 0, 0};
|
||||
#endif
|
||||
|
||||
// MSP OSD only
|
||||
AP_OSD_Setting sidebars{false, 0, 0};
|
||||
@ -207,7 +210,9 @@ private:
|
||||
void draw_aspd1(uint8_t x, uint8_t y);
|
||||
void draw_aspd2(uint8_t x, uint8_t y);
|
||||
void draw_vspeed(uint8_t x, uint8_t y);
|
||||
#if HAL_PLUSCODE_ENABLE
|
||||
void draw_pluscode(uint8_t x, uint8_t y);
|
||||
#endif
|
||||
|
||||
//helper functions
|
||||
void draw_speed_vector(uint8_t x, uint8_t y, Vector2f v, int32_t yaw);
|
||||
|
||||
@ -836,6 +836,7 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
|
||||
AP_SUBGROUPINFO(arming, "ARMING", 51, AP_OSD_Screen, AP_OSD_Setting),
|
||||
#endif //HAL_MSP_ENABLED
|
||||
|
||||
#if HAL_PLUSCODE_ENABLE
|
||||
// @Param: PLUSCODE_EN
|
||||
// @DisplayName: PLUSCODE_EN
|
||||
// @Description: Displays pluscode (OLC) element
|
||||
@ -851,6 +852,7 @@ const AP_Param::GroupInfo AP_OSD_Screen::var_info[] = {
|
||||
// @Description: Vertical position on screen
|
||||
// @Range: 0 15
|
||||
AP_SUBGROUPINFO(pluscode, "PLUSCODE", 52, AP_OSD_Screen, AP_OSD_Setting),
|
||||
#endif
|
||||
AP_GROUPEND
|
||||
};
|
||||
|
||||
@ -1705,6 +1707,7 @@ void AP_OSD_Screen::draw_clk(uint8_t x, uint8_t y)
|
||||
}
|
||||
}
|
||||
|
||||
#if HAL_PLUSCODE_ENABLE
|
||||
void AP_OSD_Screen::draw_pluscode(uint8_t x, uint8_t y)
|
||||
{
|
||||
AP_GPS & gps = AP::gps();
|
||||
@ -1717,6 +1720,7 @@ void AP_OSD_Screen::draw_pluscode(uint8_t x, uint8_t y)
|
||||
backend->write(x, y, false, buff);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#define DRAW_SETTING(n) if (n.enabled) draw_ ## n(n.xpos, n.ypos)
|
||||
|
||||
@ -1770,7 +1774,9 @@ void AP_OSD_Screen::draw(void)
|
||||
|
||||
DRAW_SETTING(gps_latitude);
|
||||
DRAW_SETTING(gps_longitude);
|
||||
#if HAL_PLUSCODE_ENABLE
|
||||
DRAW_SETTING(pluscode);
|
||||
#endif
|
||||
DRAW_SETTING(dist);
|
||||
DRAW_SETTING(stat);
|
||||
DRAW_SETTING(climbeff);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user