Line data    Source code

       1             : /*
       2             :  * This file is free software: you can redistribute it and/or modify it
       3             :  * under the terms of the GNU General Public License as published by the
       4             :  * Free Software Foundation, either version 3 of the License, or
       5             :  * (at your option) any later version.
       6             :  *
       7             :  * This file is distributed in the hope that it will be useful, but
       8             :  * WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
      10             :  * See the GNU General Public License for more details.
      11             :  *
      12             :  * You should have received a copy of the GNU General Public License along
      13             :  * with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  *
      15             :  * AP_OLC is based on INAV olc.c implemention, thanks @fiam and other contributors.
      16             :  */
      17             : 
      18             : #include "AP_OLC.h"
      19             : #include <cmath>
      20             : #include <AP_Math/AP_Math.h>
      21             : 
      22             : #if HAL_PLUSCODE_ENABLE
      23             : // This is a port of https://github.com/google/open-location-code/blob/master/c/olc.c
      24             : // to avoid double floating point math and use integer math as much as possible.
      25             : 
      26             : static constexpr char SEPARATOR_CHAR = '+';
      27             : static constexpr uint8_t SEPARATOR_POS = 8U;
      28             : static constexpr char PADDING_CHAR = '0';
      29             : 
      30             : static constexpr uint8_t ENCODING_BASE = 20U;
      31             : static constexpr uint8_t PAIR_CODE_LEN = 10U;
      32             : static constexpr uint8_t CODE_LEN_MAX = 15U;
      33             : 
      34             : static constexpr uint8_t GRID_COLS = 4U;
      35             : static constexpr uint8_t GRID_ROWS = (ENCODING_BASE / GRID_COLS);
      36             : 
      37             : static constexpr uint32_t OLC_DEG_MULTIPLIER = 10000000U; // 1e7
      38             : 
      39             : static constexpr int32_t LAT_MAX = static_cast<int32_t>(90 * OLC_DEG_MULTIPLIER);
      40             : static constexpr int32_t LON_MAX = static_cast<int32_t>(180 * OLC_DEG_MULTIPLIER);
      41             : 
      42             : const int32_t AP_OLC::INITIAL_EXPONENT = floorf(logf(2 * (LON_MAX / OLC_DEG_MULTIPLIER)) / logf(ENCODING_BASE));
      43             : // Work out the enclosing resolution (in degrees) for the grid algorithm.
      44             : const int32_t AP_OLC::GRID_SIZE = (1 / powf(ENCODING_BASE, PAIR_CODE_LEN / 2 - (INITIAL_EXPONENT + 1))) * OLC_DEG_MULTIPLIER;
      45             : // Work out the initial resolution
      46             : const int32_t AP_OLC::INITIAL_RESOLUTION = powf(ENCODING_BASE, INITIAL_EXPONENT) * OLC_DEG_MULTIPLIER;
      47             : 
      48             : constexpr char AP_OLC::olc_alphabet[];
      49             : 
      50             : // Compute the latitude precision value for a given code length.  Lengths <= 10
      51             : // have the same precision for latitude and longitude, but lengths > 10 have
      52             : // different precisions due to the grid method having fewer columns than rows.
      53           4 : float AP_OLC::compute_precision_for_length(uint8_t length)
      54             : {
      55             :     // Magic numbers!
      56           4 :     if (length <= PAIR_CODE_LEN) {
      57           2 :         return powf(ENCODING_BASE, floorf((length / -2) + 2));
      58             :     }
      59             : 
      60           2 :     return powf(ENCODING_BASE, -3) / powf(5, length - PAIR_CODE_LEN);
      61             : }
      62             : 
      63          20 : int32_t AP_OLC::adjust_latitude(int32_t lat, uint8_t code_len)
      64             : {
      65             : 
      66          20 :     lat = constrain_int32(lat, -LAT_MAX, LAT_MAX);
      67             : 
      68          20 :     if (lat >= LAT_MAX) {
      69             :         // Subtract half the code precision to get the latitude into the code area.
      70           4 :         int32_t precision = compute_precision_for_length(code_len) * OLC_DEG_MULTIPLIER;
      71           4 :         lat -= precision / 2;
      72             :     }
      73          20 :     return lat;
      74             : }
      75             : 
      76          20 : int32_t AP_OLC::normalize_longitude(int32_t lon)
      77             : {
      78          22 :     while (lon < -LON_MAX) {
      79           2 :         lon += LON_MAX;
      80           2 :         lon += LON_MAX;
      81             :     }
      82          22 :     while (lon >= LON_MAX) {
      83           2 :         lon -= LON_MAX;
      84           2 :         lon -= LON_MAX;
      85             :     }
      86          20 :     return lon;
      87             : }
      88             : 
      89             : // Encodes positive range lat,lon into a sequence of OLC lat/lon pairs.  This
      90             : // uses pairs of characters (latitude and longitude in that order) to represent
      91             : // each step in a 20x20 grid.  Each code, therefore, has 1/400th the area of
      92             : // the previous code.
      93          20 : uint32_t AP_OLC::encode_pairs(uint32_t lat, uint32_t lon, uint8_t length, char *buf, uint8_t bufsize)
      94             : {
      95          20 :     if ((length + 1) >= bufsize) {
      96           2 :         buf[0] = '\0';
      97           2 :         return 0;
      98             :     }
      99             : 
     100          18 :     uint32_t pos = 0;
     101          18 :     int32_t resolution = INITIAL_RESOLUTION;
     102             :     // Add two digits on each pass.
     103          18 :     for (uint8_t digit_count = 0;
     104         102 :          digit_count < length;
     105          84 :          digit_count += 2, resolution /= ENCODING_BASE) {
     106             : 
     107             :         // Do the latitude - gets the digit for this place and subtracts that
     108             :         // for the next digit.
     109          84 :         auto digit_value = lat / resolution;
     110          84 :         lat -= digit_value * resolution;
     111          84 :         buf[pos++] = olc_alphabet[digit_value];
     112             : 
     113             :         // Do the longitude - gets the digit for this place and subtracts that
     114             :         // for the next digit.
     115          84 :         digit_value = lon / resolution;
     116          84 :         lon -= digit_value * resolution;
     117          84 :         buf[pos++] = olc_alphabet[digit_value];
     118             : 
     119             :         // Should we add a separator here?
     120          84 :         if (pos == SEPARATOR_POS && pos < length) {
     121          14 :             buf[pos++] = SEPARATOR_CHAR;
     122             :         }
     123             :     }
     124          22 :     while (pos < SEPARATOR_POS) {
     125           4 :         buf[pos++] = PADDING_CHAR;
     126             :     }
     127          18 :     if (pos == SEPARATOR_POS) {
     128           4 :         buf[pos++] = SEPARATOR_CHAR;
     129             :     }
     130          18 :     buf[pos] = '\0';
     131          18 :     return pos;
     132             : }
     133             : 
     134             : // Encodes a location using the grid refinement method into an OLC string.  The
     135             : // grid refinement method divides the area into a grid of 4x5, and uses a
     136             : // single character to refine the area.  The grid squares use the OLC
     137             : // characters in order to number the squares as follows:
     138             : //
     139             : //   R V W X
     140             : //   J M P Q
     141             : //   C F G H
     142             : //   6 7 8 9
     143             : //   2 3 4 5
     144             : //
     145             : // This allows default accuracy OLC codes to be refined with just a single
     146             : // character.
     147           8 : int32_t AP_OLC::encode_grid(uint32_t lat, uint32_t lon, uint8_t length,
     148             :                        char *buf, uint8_t bufsize)
     149             : {
     150           8 :     if ((length + 1) >= bufsize) {
     151           6 :         buf[0] = '\0';
     152           6 :         return 0;
     153             :     }
     154             : 
     155           2 :     int pos = 0;
     156             : 
     157           2 :     int32_t lat_grid_size = GRID_SIZE;
     158           2 :     int32_t lon_grid_size = GRID_SIZE;
     159             : 
     160           2 :     lat %= lat_grid_size;
     161           2 :     lon %= lon_grid_size;
     162             : 
     163           4 :     for (uint8_t i = 0; i < length; i++) {
     164           2 :         int32_t lat_div = lat_grid_size / GRID_ROWS;
     165           2 :         int32_t lon_div = lon_grid_size / GRID_COLS;
     166             : 
     167           2 :         if (lat_div == 0 || lon_div == 0) {
     168             :             // This case happens when OLC_DEG_MULTIPLIER doesn't have enough
     169             :             // precision for the requested length.
     170             :             break;
     171             :         }
     172             : 
     173             :         // Work out the row and column.
     174           2 :         auto row = lat / lat_div;
     175           2 :         auto col = lon / lon_div;
     176           2 :         lat_grid_size /= GRID_ROWS;
     177           2 :         lon_grid_size /= GRID_COLS;
     178           2 :         lat -= row * lat_grid_size;
     179           2 :         lon -= col * lon_grid_size;
     180           2 :         buf[pos++] = olc_alphabet[row * GRID_COLS + col];
     181             :     }
     182           2 :     buf[pos] = '\0';
     183           2 :     return pos;
     184             : }
     185             : 
     186          20 : uint32_t AP_OLC::olc_encode(int32_t lat, int32_t lon, uint8_t length, char *buf, uint8_t bufsize)
     187             : {
     188          20 :     uint32_t pos = 0;
     189             : 
     190          20 :     length = MIN(length, CODE_LEN_MAX);
     191             : 
     192             :     // Adjust latitude and longitude so they fall into positive ranges.
     193          20 :     uint32_t alat = adjust_latitude(lat, length) + LAT_MAX;
     194          20 :     uint32_t alon = normalize_longitude(lon) + LON_MAX;
     195             : 
     196          20 :     pos += encode_pairs(alat, alon, MIN(length, PAIR_CODE_LEN), buf + pos, bufsize - pos);
     197             :     // If the requested length indicates we want grid refined codes.
     198          20 :     if (length > PAIR_CODE_LEN) {
     199           8 :         pos += encode_grid(alat, alon, length - PAIR_CODE_LEN, buf + pos, bufsize - pos);
     200             :     }
     201          20 :     buf[pos] = '\0';
     202          20 :     return pos;
     203             : }
     204             : 
     205             : #endif