diff --git a/lib/main/google/olc/LICENSE b/lib/main/google/olc/LICENSE new file mode 100644 index 0000000000..d645695673 --- /dev/null +++ b/lib/main/google/olc/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/lib/main/google/olc/README.md b/lib/main/google/olc/README.md new file mode 100644 index 0000000000..cb008cc308 --- /dev/null +++ b/lib/main/google/olc/README.md @@ -0,0 +1,96 @@ +Open Location Code +================== + +[![Build Status](https://api.travis-ci.org/google/open-location-code.svg?branch=master)](https://travis-ci.org/google/open-location-code) +[![CDNJS](https://img.shields.io/cdnjs/v/openlocationcode.svg)](https://cdnjs.com/libraries/openlocationcode) + +Open Location Code is a technology that gives a way of encoding location into a form that is +easier to use than latitude and longitude. The codes generated are called plus codes, as their +distinguishing attribute is that they include a "+" character. + +The technology is designed to produce codes that can be used as a replacement for street addresses, especially +in places where buildings aren't numbered or streets aren't named. + +Plus codes represent an area, not a point. As digits are added +to a code, the area shrinks, so a long code is more precise than a short +code. + +Codes that are similar are located closer together than codes that are +different. + +A location can be converted into a code, and a code can be converted back +to a location completely offline. + +There are no data tables to lookup or online services required. The +algorithm is publicly available and can be used without restriction. + +Links +----- + * [Demonstration site](http://plus.codes/) + * [Mailing list](https://groups.google.com/forum/#!forum/open-location-code) + * [Comparison of existing location encoding systems](https://github.com/google/open-location-code/wiki/Evaluation-of-Location-Encoding-Systems) + * [Open Location Code definition](https://github.com/google/open-location-code/blob/master/docs/olc_definition.adoc) + +Description +----------- + +Codes are made up of a sequence of digits chosen from a set of 20. The +digits in the code alternate between latitude and longitude. The first +four digits describe a one degree latitude by one degree longitude +area, aligned on degrees. Adding two further digits to the code, +reduces the area to 1/20th of a degree by 1/20th of a degree within the +previous area. And so on - each pair of digits reduces the area to +1/400th of the previous area. + +As an example, the Parliament Buildings in Nairobi, Kenya are located at +6GCRPR6C+24. 6GCR is the area from 2S 36E to 1S 37E. PR6C+24 is a 14 meter +wide by 14 meter high area within 6GCR. + +A "+" character is used after eight digits, to break the code up into two parts +and to distinguish codes from postal codes. + +There will be locations where a 10 digit code is not sufficiently precise, but +refining it by a factor of 20 is i) unnecessarily precise and ii) requires extending +the code by two digits. Instead, after 10 digits, the area is divided +into a 4x5 grid and a single digit used to identify the grid square. A single +grid refinement step reduces the area to approximately 3.5x2.8 meters. + +Codes can be shortened relative to a location. This reduces the number of digits +that must be remembered, by using a location to identify an approximate area, +and then generating the nearest matching code. Shortening a code, if possible, +will drop four or more digits from the start of the code. The degree to which a +code can be shortened depends on the proximity of the reference location. + +If the reference location is derived from a town or city name, it is dependent +on the accuracy of the geocoding service. Although one service may place +"Zurich" close to the Google office, another may move it by a hundred meters or +more, and this could be enough to prevent the original code being recovered. +Rather than a large city size feature to generate the reference location, it is +better to use smaller, neighbourhood features, that will not have as much +variation in their geocode results. + +Guidelines for shortening codes are in the [wiki](https://github.com/google/open-location-code/wiki). + +Recovering shortened codes works by providing the short code and a reference +location. This does not need to be the same as the location used to shorten the +code, but it does need to be nearby. Shortened codes always include the "+" +character so it is simple to compute the missing component. + + * 8F+GG is missing six leading characters + * 6C8F+GG is missing four leading characters + +Example Code +------------ + +The subdirectories contain sample implementations and tests for different +languages. Each implementation provides the following functions: + + * Test a code to see if it is a valid sequence + * Test a code to see if it is a valid full code + Not all valid sequences are valid full codes + * Encode a latitude and longitude to a standard accuracy + (14 meter by 14 meter) code + * Encode a latitude and longitude to a code of any length + * Decode a code to its coordinates: low, high and center + * Shorten a full code relative to a location + * Extend a short code relative to a location diff --git a/lib/main/google/olc/betaflight.h b/lib/main/google/olc/betaflight.h new file mode 100644 index 0000000000..55f888d77f --- /dev/null +++ b/lib/main/google/olc/betaflight.h @@ -0,0 +1,6 @@ +// Be sure to add this to olc.c + +// Ignore a bunch of warnings in this sloppy code +#pragma GCC diagnostic ignored "-Wdouble-promotion" +#pragma GCC diagnostic ignored "-Wunused-parameter" +#pragma GCC diagnostic ignored "-Wsign-compare" diff --git a/lib/main/google/olc/betaflight_readme.txt b/lib/main/google/olc/betaflight_readme.txt new file mode 100644 index 0000000000..e65fdf16da --- /dev/null +++ b/lib/main/google/olc/betaflight_readme.txt @@ -0,0 +1,7 @@ +Open Location Codes + +Source from: https://github.com/google/open-location-code + +Code use here is unaltered except for adding the following header to olc.c to ignore the numerous compiler warnings for sloppy code: + +#include betaflight.h diff --git a/lib/main/google/olc/olc.c b/lib/main/google/olc/olc.c new file mode 100644 index 0000000000..df12775727 --- /dev/null +++ b/lib/main/google/olc/olc.c @@ -0,0 +1,608 @@ +#include "olc.h" +#include +#include +#include +#include +#include +#include "olc_private.h" + +#include "betaflight.h" + +#define CORRECT_IF_SEPARATOR(var, info) \ + do { \ + (var) += (info)->sep_first >= 0 ? 1 : 0; \ + } while (0) + +// Information about a code, produced by analyse(); +typedef struct CodeInfo { + // Original code. + const char* code; + // Total count of characters in the code including padding and separators. + int size; + // Count of valid digits (not including padding or separators). + int len; + // Index of the first separator in the code. + int sep_first; + // Index of the last separator in the code. (If there is only one, same as + // sep_first.) + int sep_last; + // Index of the first padding character in the code. + int pad_first; + // Index of the last padding character in the code. (If there is only one, + // same as pad_first.) + int pad_last; +} CodeInfo; + +// Helper functions +static int analyse(const char* code, size_t size, CodeInfo* info); +static int is_short(CodeInfo* info); +static int is_full(CodeInfo* info); +static int decode(CodeInfo* info, OLC_CodeArea* decoded); +static size_t code_length(CodeInfo* info); + +static double pow_neg(double base, double exponent); +static double compute_latitude_precision(int length); +static double normalize_longitude(double lon_degrees); +static double adjust_latitude(double lat_degrees, size_t length); + +void OLC_GetCenter(const OLC_CodeArea* area, OLC_LatLon* center) { + center->lat = area->lo.lat + (area->hi.lat - area->lo.lat) / 2.0; + if (center->lat > kLatMaxDegrees) { + center->lat = kLatMaxDegrees; + } + + center->lon = area->lo.lon + (area->hi.lon - area->lo.lon) / 2.0; + if (center->lon > kLonMaxDegrees) { + center->lon = kLonMaxDegrees; + } +} + +size_t OLC_CodeLength(const char* code, size_t size) { + CodeInfo info; + analyse(code, size, &info); + return code_length(&info); +} + +int OLC_IsValid(const char* code, size_t size) { + CodeInfo info; + return analyse(code, size, &info) > 0; +} + +int OLC_IsShort(const char* code, size_t size) { + CodeInfo info; + if (analyse(code, size, &info) <= 0) { + return 0; + } + return is_short(&info); +} + +int OLC_IsFull(const char* code, size_t size) { + CodeInfo info; + if (analyse(code, size, &info) <= 0) { + return 0; + } + return is_full(&info); +} + +int OLC_Encode(const OLC_LatLon* location, size_t length, char* code, + int maxlen) { + // Limit the maximum number of digits in the code. + if (length > kMaximumDigitCount) { + length = kMaximumDigitCount; + } + // Adjust latitude and longitude so they fall into positive ranges. + double latitude = adjust_latitude(location->lat, length); + double longitude = normalize_longitude(location->lon); + + // Build up the code here, then copy it to the passed pointer. + char fullcode[] = "12345678901234567"; + + // Compute the code. + // This approach converts each value to an integer after multiplying it by + // the final precision. This allows us to use only integer operations, so + // avoiding any accumulation of floating point representation errors. + + // Multiply values by their precision and convert to positive without any + // floating point operations. + long long int lat_val = kLatMaxDegrees * 2.5e7; + long long int lng_val = kLonMaxDegrees * 8.192e6; + lat_val += latitude * 2.5e7; + lng_val += longitude * 8.192e6; + + size_t pos = kMaximumDigitCount; + // Compute the grid part of the code if necessary. + if (length > kPairCodeLength) { + for (size_t i = 0; i < kGridCodeLength; i++) { + int lat_digit = lat_val % kGridRows; + int lng_digit = lng_val % kGridCols; + int ndx = lat_digit * kGridCols + lng_digit; + fullcode[pos--] = kAlphabet[ndx]; + // Note! Integer division. + lat_val /= kGridRows; + lng_val /= kGridCols; + } + } else { + lat_val /= pow(kGridRows, kGridCodeLength); + lng_val /= pow(kGridCols, kGridCodeLength); + } + pos = kPairCodeLength; + // Compute the pair section of the code. + for (size_t i = 0; i < kPairCodeLength / 2; i++) { + int lat_ndx = lat_val % kEncodingBase; + int lng_ndx = lng_val % kEncodingBase; + fullcode[pos--] = kAlphabet[lng_ndx]; + fullcode[pos--] = kAlphabet[lat_ndx]; + // Note! Integer division. + lat_val /= kEncodingBase; + lng_val /= kEncodingBase; + if (i == 0) { + fullcode[pos--] = kSeparator; + } + } + // Replace digits with padding if necessary. + if (length < kSeparatorPosition) { + for (size_t i = length; i < kSeparatorPosition; i++) { + fullcode[i] = kPaddingCharacter; + } + fullcode[kSeparatorPosition] = kSeparator; + } + // Now copy the full code digits into the buffer. + size_t char_count = length + 1; + if (kSeparatorPosition + 1 > char_count) { + char_count = kSeparatorPosition + 1; + } + for (size_t i = 0; i < char_count; i++) { + code[i] = fullcode[i]; + } + + // Terminate the buffer. + code[char_count] = '\0'; + + return char_count; +} + +int OLC_EncodeDefault(const OLC_LatLon* location, char* code, int maxlen) { + return OLC_Encode(location, kPairCodeLength, code, maxlen); +} + +int OLC_Decode(const char* code, size_t size, OLC_CodeArea* decoded) { + CodeInfo info; + if (analyse(code, size, &info) <= 0) { + return 0; + } + return decode(&info, decoded); +} + +int OLC_Shorten(const char* code, size_t size, const OLC_LatLon* reference, + char* shortened, int maxlen) { + CodeInfo info; + if (analyse(code, size, &info) <= 0) { + return 0; + } + if (info.pad_first > 0) { + return 0; + } + if (!is_full(&info)) { + return 0; + } + + OLC_CodeArea code_area; + decode(&info, &code_area); + OLC_LatLon center; + OLC_GetCenter(&code_area, ¢er); + + // Ensure that latitude and longitude are valid. + double lat = adjust_latitude(reference->lat, info.len); + double lon = normalize_longitude(reference->lon); + + // How close are the latitude and longitude to the code center. + double alat = fabs(center.lat - lat); + double alon = fabs(center.lon - lon); + double range = alat > alon ? alat : alon; + + // Yes, magic numbers... sob. + int start = 0; + const double safety_factor = 0.3; + const int removal_lengths[3] = {8, 6, 4}; + for (int j = 0; j < sizeof(removal_lengths) / sizeof(removal_lengths[0]); + ++j) { + // Check if we're close enough to shorten. The range must be less than + // 1/2 the resolution to shorten at all, and we want to allow some + // safety, so use 0.3 instead of 0.5 as a multiplier. + int removal_length = removal_lengths[j]; + double area_edge = + compute_latitude_precision(removal_length) * safety_factor; + if (range < area_edge) { + start = removal_length; + break; + } + } + int pos = 0; + for (int j = start; j < info.size && code[j] != '\0'; ++j) { + shortened[pos++] = code[j]; + } + shortened[pos] = '\0'; + return pos; +} + +int OLC_RecoverNearest(const char* short_code, size_t size, + const OLC_LatLon* reference, char* code, int maxlen) { + CodeInfo info; + if (analyse(short_code, size, &info) <= 0) { + return 0; + } + // Check if it is a full code - then we just convert to upper case. + if (is_full(&info)) { + OLC_CodeArea code_area; + decode(&info, &code_area); + OLC_LatLon center; + OLC_GetCenter(&code_area, ¢er); + return OLC_Encode(¢er, code_area.len, code, maxlen); + } + if (!is_short(&info)) { + return 0; + } + int len = code_length(&info); + + // Ensure that latitude and longitude are valid. + double lat = adjust_latitude(reference->lat, len); + double lon = normalize_longitude(reference->lon); + + // Compute the number of digits we need to recover. + size_t padding_length = kSeparatorPosition; + if (info.sep_first >= 0) { + padding_length -= info.sep_first; + } + + // The resolution (height and width) of the padded area in degrees. + double resolution = pow_neg(kEncodingBase, 2.0 - (padding_length / 2.0)); + + // Distance from the center to an edge (in degrees). + double half_res = resolution / 2.0; + + // Use the reference location to pad the supplied short code and decode it. + OLC_LatLon latlon = {lat, lon}; + char encoded[256]; + OLC_EncodeDefault(&latlon, encoded, 256); + + char new_code[256]; + int pos = 0; + for (int j = 0; encoded[j] != '\0'; ++j) { + if (j >= padding_length) { + break; + } + new_code[pos++] = encoded[j]; + } + for (int j = 0; j < info.size && short_code[j] != '\0'; ++j) { + new_code[pos++] = short_code[j]; + } + new_code[pos] = '\0'; + if (analyse(new_code, pos, &info) <= 0) { + return 0; + } + + OLC_CodeArea code_area; + decode(&info, &code_area); + OLC_LatLon center; + OLC_GetCenter(&code_area, ¢er); + + // How many degrees latitude is the code from the reference? + if (lat + half_res < center.lat && + center.lat - resolution > -kLatMaxDegrees) { + // If the proposed code is more than half a cell north of the reference + // location, it's too far, and the best match will be one cell south. + center.lat -= resolution; + } else if (lat - half_res > center.lat && + center.lat + resolution < kLatMaxDegrees) { + // If the proposed code is more than half a cell south of the reference + // location, it's too far, and the best match will be one cell north. + center.lat += resolution; + } + + // How many degrees longitude is the code from the reference? + if (lon + half_res < center.lon) { + center.lon -= resolution; + } else if (lon - half_res > center.lon) { + center.lon += resolution; + } + + return OLC_Encode(¢er, len + padding_length, code, maxlen); +} + +// private functions + +static int analyse(const char* code, size_t size, CodeInfo* info) { + memset(info, 0, sizeof(CodeInfo)); + + // null code is not valid + if (!code) { + return 0; + } + if (!size) { + size = strlen(code); + } + + info->code = code; + info->size = size < kMaximumDigitCount ? size : kMaximumDigitCount; + info->sep_first = -1; + info->sep_last = -1; + info->pad_first = -1; + info->pad_last = -1; + int j = 0; + for (j = 0; j <= size && code[j] != '\0'; ++j) { + int ok = 0; + + // if this is a padding character, remember it + if (!ok && code[j] == kPaddingCharacter) { + if (info->pad_first < 0) { + info->pad_first = j; + } + info->pad_last = j; + ok = 1; + } + + // if this is a separator character, remember it + if (!ok && code[j] == kSeparator) { + if (info->sep_first < 0) { + info->sep_first = j; + } + info->sep_last = j; + ok = 1; + } + + // only accept characters in the valid character set + if (!ok && get_alphabet_position(code[j]) >= 0) { + ok = 1; + } + + // didn't find anything expected => bail out + if (!ok) { + return 0; + } + } + + // so far, code only has valid characters -- good + info->len = j < kMaximumDigitCount ? j : kMaximumDigitCount; + + // Cannot be empty + if (info->len <= 0) { + return 0; + } + + // The separator is required. + if (info->sep_first < 0) { + return 0; + } + + // There can be only one... separator. + if (info->sep_last > info->sep_first) { + return 0; + } + + // separator cannot be the only character + if (info->len == 1) { + return 0; + } + + // Is the separator in an illegal position? + if (info->sep_first > kSeparatorPosition || (info->sep_first % 2)) { + return 0; + } + + // padding cannot be at the initial position + if (info->pad_first == 0) { + return 0; + } + + // We can have an even number of padding characters before the separator, + // but then it must be the final character. + if (info->pad_first > 0) { + // Short codes cannot have padding + if (info->sep_first < kSeparatorPosition) { + return 0; + } + + // The first padding character needs to be in an odd position. + if (info->pad_first % 2) { + return 0; + } + + // With padding, the separator must be the final character + if (info->sep_last < info->len - 1) { + return 0; + } + + // After removing padding characters, we mustn't have anything left. + if (info->pad_last < info->sep_first - 1) { + return 0; + } + } + + // If there are characters after the separator, make sure there isn't just + // one of them (not legal). + if (info->len - info->sep_first - 1 == 1) { + return 0; + } + + return info->len; +} + +static int is_short(CodeInfo* info) { + if (info->len <= 0) { + return 0; + } + + // if there is a separator, it cannot be beyond the valid position + if (info->sep_first >= kSeparatorPosition) { + return 0; + } + + return 1; +} + +// checks that the first character of latitude or longitude is valid +static int valid_first_character(CodeInfo* info, int pos, double kMax) { + if (info->len <= pos) { + return 1; + } + + // Work out what the first character indicates + size_t firstValue = get_alphabet_position(info->code[pos]); + firstValue *= kEncodingBase; + return firstValue < kMax; +} + +static int is_full(CodeInfo* info) { + if (info->len <= 0) { + return 0; + } + + // If there are less characters than expected before the separator. + if (info->sep_first < kSeparatorPosition) { + return 0; + } + + // check first latitude character, if any + if (!valid_first_character(info, 0, kLatMaxDegreesT2)) { + return 0; + } + + // check first longitude character, if any + if (!valid_first_character(info, 1, kLonMaxDegreesT2)) { + return 0; + } + + return 1; +} + +static int decode(CodeInfo* info, OLC_CodeArea* decoded) { + // Create a copy of the code, skipping padding and separators. + char clean_code[256]; + int ci = 0; + for (size_t i = 0; i < info->len + 1; i++) { + if (info->code[i] != kPaddingCharacter && info->code[i] != kSeparator) { + clean_code[ci] = info->code[i]; + ci++; + } + } + clean_code[ci] = '\0'; + + // Initialise the values for each section. We work them out as integers and + // convert them to floats at the end. Using doubles all the way results in + // multiplying small rounding errors until they become significant. + int normal_lat = -kLatMaxDegrees * kPairPrecisionInverse; + int normal_lng = -kLonMaxDegrees * kPairPrecisionInverse; + int extra_lat = 0; + int extra_lng = 0; + + // How many digits do we have to process? + size_t digits = strlen(clean_code) < kPairCodeLength ? strlen(clean_code) + : kPairCodeLength; + // Define the place value for the most significant pair. + int pv = pow(kEncodingBase, kPairCodeLength / 2); + for (size_t i = 0; i < digits - 1; i += 2) { + pv /= kEncodingBase; + normal_lat += get_alphabet_position(clean_code[i]) * pv; + normal_lng += get_alphabet_position(clean_code[i + 1]) * pv; + } + // Convert the place value to a float in degrees. + double lat_precision = (double)pv / kPairPrecisionInverse; + double lng_precision = (double)pv / kPairPrecisionInverse; + // Process any extra precision digits. + if (strlen(clean_code) > kPairCodeLength) { + // How many digits do we have to process? + digits = strlen(clean_code) < kMaximumDigitCount ? strlen(clean_code) + : kMaximumDigitCount; + // Initialise the place values for the grid. + int row_pv = pow(kGridRows, kGridCodeLength); + int col_pv = pow(kGridCols, kGridCodeLength); + for (size_t i = kPairCodeLength; i < digits; i++) { + row_pv /= kGridRows; + col_pv /= kGridCols; + int dval = get_alphabet_position(clean_code[i]); + int row = dval / kGridCols; + int col = dval % kGridCols; + extra_lat += row * row_pv; + extra_lng += col * col_pv; + } + // Adjust the precisions from the integer values to degrees. + lat_precision = (double)row_pv / kGridLatPrecisionInverse; + lng_precision = (double)col_pv / kGridLonPrecisionInverse; + } + // Merge the values from the normal and extra precision parts of the code. + // Everything is ints so they all need to be cast to floats. + double lat = (double)normal_lat / kPairPrecisionInverse + + (double)extra_lat / kGridLatPrecisionInverse; + double lng = (double)normal_lng / kPairPrecisionInverse + + (double)extra_lng / kGridLonPrecisionInverse; + decoded->lo.lat = lat; + decoded->lo.lon = lng; + decoded->hi.lat = lat + lat_precision; + decoded->hi.lon = lng + lng_precision; + decoded->len = strlen(clean_code); + return decoded->len; +} + +static size_t code_length(CodeInfo* info) { + int len = info->len; + if (info->sep_first >= 0) { + --len; + } + if (info->pad_first >= 0) { + len = info->pad_first; + } + return len; +} + +// Raises a number to an exponent, handling negative exponents. +static double pow_neg(double base, double exponent) { + if (exponent == 0) { + return 1; + } + + if (exponent > 0) { + return pow(base, exponent); + } + + return 1 / pow(base, -exponent); +} + +// 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. +static double compute_latitude_precision(int length) { + // Magic numbers! + if (length <= kPairCodeLength) { + return pow_neg(kEncodingBase, floor((length / -2) + 2)); + } + + return pow_neg(kEncodingBase, -3) / pow(kGridRows, length - kPairCodeLength); +} + +// Normalize a longitude into the range -180 to 180, not including 180. +static double normalize_longitude(double lon_degrees) { + while (lon_degrees < -kLonMaxDegrees) { + lon_degrees += kLonMaxDegreesT2; + } + while (lon_degrees >= kLonMaxDegrees) { + lon_degrees -= kLonMaxDegreesT2; + } + return lon_degrees; +} + +// Adjusts 90 degree latitude to be lower so that a legal OLC code can be +// generated. +static double adjust_latitude(double lat_degrees, size_t length) { + if (lat_degrees < -kLatMaxDegrees) { + lat_degrees = -kLatMaxDegrees; + } + if (lat_degrees > kLatMaxDegrees) { + lat_degrees = kLatMaxDegrees; + } + if (lat_degrees < kLatMaxDegrees) { + return lat_degrees; + } + // Subtract half the code precision to get the latitude into the code area. + double precision = compute_latitude_precision(length); + return lat_degrees - precision / 2; +} diff --git a/lib/main/google/olc/olc.h b/lib/main/google/olc/olc.h new file mode 100644 index 0000000000..4b5def160b --- /dev/null +++ b/lib/main/google/olc/olc.h @@ -0,0 +1,75 @@ +#ifndef OLC_OPENLOCATIONCODE_H_ +#define OLC_OPENLOCATIONCODE_H_ + +#include + +#define OLC_VERSION_MAJOR 1 +#define OLC_VERSION_MINOR 0 +#define OLC_VERSION_PATCH 0 + +// OLC version number: 2.3.4 => 2003004 +// Useful for checking against a particular version or above: +// +// #if OLC_VERSION_NUM < OLC_MAKE_VERSION_NUM(1, 0, 2) +// #error UNSUPPORTED OLC VERSION +// #endif +#define OLC_MAKE_VERSION_NUM(major, minor, patch) \ + ((major * 1000 + minor) * 1000 + patch) + +// OLC version string: 2.3.4 => "2.3.4" +#define OLC_MAKE_VERSION_STR_IMPL(major, minor, patch) \ + (#major "." #minor "." #patch) +#define OLC_MAKE_VERSION_STR(major, minor, patch) \ + OLC_MAKE_VERSION_STR_IMPL(major, minor, patch) + +// Current version, as a number and a string +#define OLC_VERSION_NUM \ + OLC_MAKE_VERSION_NUM(OLC_VERSION_MAJOR, OLC_VERSION_MINOR, OLC_VERSION_PATCH) +#define OLC_VERSION_STR \ + OLC_MAKE_VERSION_STR(OLC_VERSION_MAJOR, OLC_VERSION_MINOR, OLC_VERSION_PATCH) + +// A pair of doubles representing latitude / longitude +typedef struct OLC_LatLon { + double lat; + double lon; +} OLC_LatLon; + +// An area defined by two corners (lo and hi) and a code length +typedef struct OLC_CodeArea { + OLC_LatLon lo; + OLC_LatLon hi; + size_t len; +} OLC_CodeArea; + +// Get the center coordinates for an area +void OLC_GetCenter(const OLC_CodeArea* area, OLC_LatLon* center); + +// Get the effective length for a code +size_t OLC_CodeLength(const char* code, size_t size); + +// Check for the three obviously-named conditions +int OLC_IsValid(const char* code, size_t size); +int OLC_IsShort(const char* code, size_t size); +int OLC_IsFull(const char* code, size_t size); + +// Encode location with given code length (indicates precision) into an OLC +// Return the string length of the code +int OLC_Encode(const OLC_LatLon* location, size_t code_length, char* code, + int maxlen); + +// Encode location with default code length into an OLC +// Return the string length of the code +int OLC_EncodeDefault(const OLC_LatLon* location, char* code, int maxlen); + +// Decode OLC into the original location +int OLC_Decode(const char* code, size_t size, OLC_CodeArea* decoded); + +// Compute a (shorter) OLC for a given code and a reference location +int OLC_Shorten(const char* code, size_t size, const OLC_LatLon* reference, + char* buf, int maxlen); + +// Given shorter OLC and reference location, compute original (full length) OLC +int OLC_RecoverNearest(const char* short_code, size_t size, + const OLC_LatLon* reference, char* code, int maxlen); + +#endif diff --git a/lib/main/google/olc/olc_private.h b/lib/main/google/olc/olc_private.h new file mode 100644 index 0000000000..2c00edcdf3 --- /dev/null +++ b/lib/main/google/olc/olc_private.h @@ -0,0 +1,69 @@ +/* + * We place these static definitions on a separate header file so that we can + * include the file in both the library and the tests. + */ + +#include +#include +#include +#include + +#define OLC_kEncodingBase 20 +#define OLC_kGridCols 4 +#define OLC_kLatMaxDegrees 90 +#define OLC_kLonMaxDegrees 180 + +// Separates the first eight digits from the rest of the code. +static const char kSeparator = '+'; +// Used to indicate null values before the separator. +static const char kPaddingCharacter = '0'; +// Digits used in the codes. +static const char kAlphabet[] = "23456789CFGHJMPQRVWX"; +// Number of digits in the alphabet. +static const size_t kEncodingBase = OLC_kEncodingBase; +// The max number of digits returned in a plus code. Roughly 1 x 0.5 cm. +static const size_t kMaximumDigitCount = 15; +// The number of code characters that are lat/lng pairs. +static const size_t kPairCodeLength = 10; +// The number of characters that combine lat and lng into a grid. +// kMaximumDigitCount - kPairCodeLength +static const size_t kGridCodeLength = 5; +// The number of columns in each grid step. +static const size_t kGridCols = OLC_kGridCols; +// The number of rows in each grid step. +static const size_t kGridRows = OLC_kEncodingBase / OLC_kGridCols; +// The number of digits before the separator. +static const size_t kSeparatorPosition = 8; +// Inverse of the precision of the last pair digits (in degrees). +static const size_t kPairPrecisionInverse = 8000; +// Inverse (1/) of the precision of the final grid digits in degrees. +// Latitude is kEncodingBase^3 * kGridRows^kGridCodeLength +static const size_t kGridLatPrecisionInverse = 2.5e7; +// Longitude is kEncodingBase^3 * kGridColumns^kGridCodeLength +static const size_t kGridLonPrecisionInverse = 8.192e6; +// Latitude bounds are -kLatMaxDegrees degrees and +kLatMaxDegrees degrees +// which we transpose to 0 and 180 degrees. +static const double kLatMaxDegrees = OLC_kLatMaxDegrees; +static const double kLatMaxDegreesT2 = 2 * OLC_kLatMaxDegrees; + +// Longitude bounds are -kLonMaxDegrees degrees and +kLonMaxDegrees degrees +// which we transpose to 0 and 360 degrees. +static const double kLonMaxDegrees = OLC_kLonMaxDegrees; +static const double kLonMaxDegreesT2 = 2 * OLC_kLonMaxDegrees; + +// Lookup table of the alphabet positions of characters 'C' through 'X', +// inclusive. A value of -1 means the character isn't part of the alphabet. +static const int kPositionLUT['X' - 'C' + 1] = { + 8, -1, -1, 9, 10, 11, -1, 12, -1, -1, 13, + -1, -1, 14, 15, 16, -1, -1, -1, 17, 18, 19, +}; + +// Returns the position of a char in the encoding alphabet, or -1 if invalid. +static int get_alphabet_position(char c) { + char uc = toupper(c); + // We use a lookup table for performance reasons. + if (uc >= 'C' && uc <= 'X') return kPositionLUT[uc - 'C']; + if (uc >= 'c' && uc <= 'x') return kPositionLUT[uc - 'c']; + if (uc >= '2' && uc <= '9') return uc - '2'; + return -1; +} diff --git a/make/source.mk b/make/source.mk index adae172830..d678d1ec2f 100644 --- a/make/source.mk +++ b/make/source.mk @@ -447,3 +447,12 @@ endif # Search path and source files for the ST stdperiph library VPATH := $(VPATH):$(STDPERIPH_DIR)/src + +# Search path and source files for the Open Location Code library +OLC_DIR = $(ROOT)/lib/main/google/olc + +ifneq ($(OLC_DIR),) +INCLUDE_DIRS += $(OLC_DIR) +SRC += $(OLC_DIR)/olc.c +SIZE_OPTIMISED_SRC += $(OLC_DIR)/olc.c +endif diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 57e6e493a4..c3718a70fa 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1035,15 +1035,15 @@ static void writeGPSFrame(timeUs_t currentTimeUs) } blackboxWriteUnsignedVB(gpsSol.numSat); - blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]); - blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]); + blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[GPS_LATITUDE]); + blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[GPS_LONGITUDE]); blackboxWriteUnsignedVB(gpsSol.llh.altCm / 10); // was originally designed to transport meters in int16, but +-3276.7m is a good compromise blackboxWriteUnsignedVB(gpsSol.groundSpeed); blackboxWriteUnsignedVB(gpsSol.groundCourse); gpsHistory.GPS_numSat = gpsSol.numSat; - gpsHistory.GPS_coord[LAT] = gpsSol.llh.lat; - gpsHistory.GPS_coord[LON] = gpsSol.llh.lon; + gpsHistory.GPS_coord[GPS_LATITUDE] = gpsSol.llh.lat; + gpsHistory.GPS_coord[GPS_LONGITUDE] = gpsSol.llh.lon; } #endif @@ -1637,8 +1637,8 @@ STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs) writeGPSHomeFrame(); writeGPSFrame(currentTimeUs); } else if (gpsSol.numSat != gpsHistory.GPS_numSat - || gpsSol.llh.lat != gpsHistory.GPS_coord[LAT] - || gpsSol.llh.lon != gpsHistory.GPS_coord[LON]) { + || gpsSol.llh.lat != gpsHistory.GPS_coord[GPS_LATITUDE] + || gpsSol.llh.lon != gpsHistory.GPS_coord[GPS_LONGITUDE]) { //We could check for velocity changes as well but I doubt it changes independent of position writeGPSFrame(currentTimeUs); } diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index ff9a557508..895bfde0cd 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -156,3 +156,8 @@ #define SYM_STICK_OVERLAY_CENTER 0x0B #define SYM_STICK_OVERLAY_VERTICAL 0x16 #define SYM_STICK_OVERLAY_HORIZONTAL 0x17 + +// GPS degree/minute/second symbols +#define SYM_GPS_DEGREE SYM_STICK_OVERLAY_SPRITE_HIGH // kind of looks like the degree symbol +#define SYM_GPS_MINUTE 0x27 // ' +#define SYM_GPS_SECOND 0x22 // " diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index c8f4118569..1a41b13153 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -114,6 +114,7 @@ extern uint16_t flightModeFlags; typedef enum { GPS_FIX_HOME = (1 << 0), GPS_FIX = (1 << 1), + GPS_FIX_EVER = (1 << 2), } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 8b982597c8..7d1448cc7c 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -950,11 +950,7 @@ static bool gpsNewFrameNMEA(char c) gps_Msg.longitude *= -1; break; case 6: - if (string[0] > '0') { - ENABLE_STATE(GPS_FIX); - } else { - DISABLE_STATE(GPS_FIX); - } + gpsSetFixState(string[0] > '0'); break; case 7: gps_Msg.numSat = grab_fields(string, 0); @@ -1277,11 +1273,7 @@ static bool UBLOX_parse_gps(void) gpsSol.llh.lon = _buffer.posllh.longitude; gpsSol.llh.lat = _buffer.posllh.latitude; gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10; //alt in cm - if (next_fix) { - ENABLE_STATE(GPS_FIX); - } else { - DISABLE_STATE(GPS_FIX); - } + gpsSetFixState(next_fix); _new_position = true; break; case MSG_STATUS: @@ -1499,7 +1491,7 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize) if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) { uint32_t dist; int32_t dir; - GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir); + GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &dir); if (gpsConfig()->gps_use_3d_speed) { dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f)); } @@ -1509,8 +1501,8 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize) GPS_verticalSpeedInCmS = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis); GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500); } - lastCoord[LON] = gpsSol.llh.lon; - lastCoord[LAT] = gpsSol.llh.lat; + lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon; + lastCoord[GPS_LATITUDE] = gpsSol.llh.lat; lastAlt = gpsSol.llh.altCm; lastMillis = currentMillis; } @@ -1519,8 +1511,8 @@ void GPS_reset_home_position(void) { if (!STATE(GPS_FIX_HOME) || !gpsConfig()->gps_set_home_point_once) { if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { - GPS_home[LAT] = gpsSol.llh.lat; - GPS_home[LON] = gpsSol.llh.lon; + GPS_home[GPS_LATITUDE] = gpsSol.llh.lat; + GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon; GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc // Set ground altitude ENABLE_STATE(GPS_FIX_HOME); @@ -1550,7 +1542,7 @@ void GPS_calculateDistanceAndDirectionToHome(void) if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything uint32_t dist; int32_t dir; - GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir); + GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[GPS_LATITUDE], &GPS_home[GPS_LONGITUDE], &dist, &dir); GPS_distanceToHome = dist / 100; GPS_directionToHome = dir / 100; } else { @@ -1585,4 +1577,13 @@ void onGpsNewData(void) #endif } +void gpsSetFixState(bool state) +{ + if (state) { + ENABLE_STATE(GPS_FIX); + ENABLE_STATE(GPS_FIX_EVER); + } else { + DISABLE_STATE(GPS_FIX); + } +} #endif diff --git a/src/main/io/gps.h b/src/main/io/gps.h index ff1430ce95..7db419b0ac 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -25,13 +25,15 @@ #include "pg/pg.h" -#define LAT 0 -#define LON 1 - #define GPS_DEGREES_DIVIDER 10000000L #define GPS_X 1 #define GPS_Y 0 +typedef enum { + GPS_LATITUDE, + GPS_LONGITUDE +} gpsCoordinateType_e; + typedef enum { GPS_NMEA = 0, GPS_UBLOX, @@ -188,4 +190,4 @@ void onGpsNewData(void); void GPS_reset_home_position(void); void GPS_calc_longitude_scaling(int32_t lat); void GPS_distance_cm_bearing(int32_t *currentLat1, int32_t *currentLon1, int32_t *destinationLat2, int32_t *destinationLon2, uint32_t *dist, int32_t *bearing); - +void gpsSetFixState(bool state); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 88b9ccafbe..6a659cfd2b 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -3181,11 +3181,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, #ifdef USE_GPS case MSP_SET_RAW_GPS: - if (sbufReadU8(src)) { - ENABLE_STATE(GPS_FIX); - } else { - DISABLE_STATE(GPS_FIX); - } + gpsSetFixState(sbufReadU8(src)); gpsSol.numSat = sbufReadU8(src); gpsSol.llh.lat = sbufReadU32(src); gpsSol.llh.lon = sbufReadU32(src); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index d562026b6f..5c2da49533 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -59,6 +59,45 @@ CLI parameters should be added before line #endif // end of #ifdef USE_OSD */ +/* + ********************* + OSD element variants: + ********************* + + Each element can have up to 4 display variants. "Type 1" is always the default and every + every element has an implicit type 1 variant even if no additional options exist. The + purpose is to allow the user to choose a different element display or rendering style to + fit their needs. Like displaying GPS coordinates in a different format, displaying a voltage + with a different number of decimal places, etc. The purpose is NOT to display unrelated + information in different variants of the element. For example it would be inappropriate + to use variants to display RSSI for one type and link quality for another. In this case + they should be separate elements. Remember that element variants are mutually exclusive + and only one type can be displayed at a time. So they shouldn't be used in cases where + the user would want to display different types at the same time - like in the above example + where the user might want to display both RSSI and link quality at the same time. + + As variants are added to the firmware, support must also be included in the Configurator. + + The following lists the variants implemented so far (please update this as variants are added): + + OSD_ALTITUDE + type 1: Altitude with one decimal place + type 2: Altitude with no decimal (whole number only) + + OSD_GPS_LON + OSD_GPS_LAT + type 1: Decimal representation with 7 digits + type 2: Decimal representation with 4 digits + type 3: Degrees, minutes, seconds + type 4: Open location code (Google Plus Code) + + OSD_MAIN_BATT_USAGE + type 1: Graphical bar showing remaining battery (shrinks as used) + type 2: Graphical bar showing battery used (grows as used) + type 3: Numeric % of remaining battery + type 4: Numeric % or used battery +*/ + #include #include #include @@ -122,7 +161,10 @@ #include "sensors/esc_sensor.h" #include "sensors/sensors.h" -#include "common/maths.h" +#ifdef USE_GPS_PLUS_CODES +// located in lib/main/google/olc +#include "olc.h" +#endif #define AH_SYMBOL_COUNT 9 #define AH_SIDEBAR_WIDTH_POS 7 @@ -273,22 +315,71 @@ static void osdFormatAltitudeString(char * buff, int32_t altitudeCm, osdElementT } #ifdef USE_GPS -static void osdFormatCoordinate(char *buff, char sym, int32_t val) +static void osdFormatCoordinate(char *buff, gpsCoordinateType_e coordinateType, osdElementType_e variantType) { - // latitude maximum integer width is 3 (-90). - // longitude maximum integer width is 4 (-180). - // We show 7 decimals, so we need to use 12 characters: - // eg: s-180.1234567z s=symbol, z=zero terminator, decimal separator between 0 and 1 + int32_t gpsValue = 0; + const char leadingSymbol = (coordinateType == GPS_LONGITUDE) ? SYM_LON : SYM_LAT; - // NOTE: Don't use osdPrintFloat() for this. There are too many decimal places and float math doesn't have enough precision - - int pos = 0; - buff[pos++] = sym; - if (val < 0) { - buff[pos++] = '-'; - val = -val; + if (STATE(GPS_FIX_EVER)) { // don't display interim coordinates until we get the first position fix + gpsValue = (coordinateType == GPS_LONGITUDE) ? gpsSol.llh.lon : gpsSol.llh.lat; + } + + const int degreesPart = ABS(gpsValue) / GPS_DEGREES_DIVIDER; + int fractionalPart = ABS(gpsValue) % GPS_DEGREES_DIVIDER; + + switch (variantType) { +#ifdef USE_GPS_PLUS_CODES +#define PLUS_CODE_DIGITS 11 + case OSD_ELEMENT_TYPE_4: // Open Location Code + { + *buff++ = SYM_SAT_L; + *buff++ = SYM_SAT_R; + if (STATE(GPS_FIX_EVER)) { + OLC_LatLon location; + location.lat = (double)gpsSol.llh.lat / GPS_DEGREES_DIVIDER; + location.lon = (double)gpsSol.llh.lon / GPS_DEGREES_DIVIDER; + OLC_Encode(&location, PLUS_CODE_DIGITS, buff, OSD_ELEMENT_BUFFER_LENGTH - 3); + } else { + memset(buff, SYM_HYPHEN, PLUS_CODE_DIGITS + 1); + buff[8] = '+'; + buff[PLUS_CODE_DIGITS + 1] = '\0'; + } + break; + } +#endif // USE_GPS_PLUS_CODES + + case OSD_ELEMENT_TYPE_3: // degree, minutes, seconds style. ddd^mm'ss.00"W + { + char trailingSymbol; + *buff++ = leadingSymbol; + + const int minutes = fractionalPart * 60 / GPS_DEGREES_DIVIDER; + const int fractionalMinutes = fractionalPart * 60 % GPS_DEGREES_DIVIDER; + const int seconds = fractionalMinutes * 60 / GPS_DEGREES_DIVIDER; + const int tenthSeconds = (fractionalMinutes * 60 % GPS_DEGREES_DIVIDER) * 10 / GPS_DEGREES_DIVIDER; + + if (coordinateType == GPS_LONGITUDE) { + trailingSymbol = (gpsValue < 0) ? 'W' : 'E'; + } else { + trailingSymbol = (gpsValue < 0) ? 'S' : 'N'; + } + tfp_sprintf(buff, "%u%c%02u%c%02u.%u%c%c", degreesPart, SYM_GPS_DEGREE, minutes, SYM_GPS_MINUTE, seconds, tenthSeconds, SYM_GPS_SECOND, trailingSymbol); + break; + } + + case OSD_ELEMENT_TYPE_2: + fractionalPart /= 1000; + FALLTHROUGH; + + case OSD_ELEMENT_TYPE_1: + default: + *buff++ = leadingSymbol; + if (gpsValue < 0) { + *buff++ = SYM_HYPHEN; + } + tfp_sprintf(buff, (variantType == OSD_ELEMENT_TYPE_1 ? "%u.%07u" : "%u.%04u"), degreesPart, fractionalPart); + break; } - tfp_sprintf(buff + pos, "%d.%07d", val / GPS_DEGREES_DIVIDER, val % GPS_DEGREES_DIVIDER); } #endif // USE_GPS @@ -953,14 +1044,15 @@ static void osdElementGpsHomeDistance(osdElementParms_t *element) } } -static void osdElementGpsLatitude(osdElementParms_t *element) +static void osdElementGpsCoordinate(osdElementParms_t *element) { - osdFormatCoordinate(element->buff, SYM_LAT, gpsSol.llh.lat); -} - -static void osdElementGpsLongitude(osdElementParms_t *element) -{ - osdFormatCoordinate(element->buff, SYM_LON, gpsSol.llh.lon); + const gpsCoordinateType_e coordinateType = (element->item == OSD_GPS_LON) ? GPS_LONGITUDE : GPS_LATITUDE; + osdFormatCoordinate(element->buff, coordinateType, element->type); + if (STATE(GPS_FIX_EVER) && !STATE(GPS_FIX)) { + SET_BLINK(element->item); // blink if we had a fix but have since lost it + } else { + CLR_BLINK(element->item); + } } static void osdElementGpsSats(osdElementParms_t *element) @@ -1493,8 +1585,8 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = { [OSD_WARNINGS] = osdElementWarnings, [OSD_AVG_CELL_VOLTAGE] = osdElementAverageCellVoltage, #ifdef USE_GPS - [OSD_GPS_LON] = osdElementGpsLongitude, - [OSD_GPS_LAT] = osdElementGpsLatitude, + [OSD_GPS_LON] = osdElementGpsCoordinate, + [OSD_GPS_LAT] = osdElementGpsCoordinate, #endif [OSD_DEBUG] = osdElementDebug, #ifdef USE_ACC diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index da671c4201..2c7d551179 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -409,3 +409,7 @@ extern uint8_t __config_end; #if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2) #define USE_RX_BIND #endif + +#ifndef USE_GPS +#undef USE_GPS_PLUS_CODES +#endif diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index ddbf61b13e..2d7f25e7eb 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -398,4 +398,5 @@ #define USE_RX_MSP_OVERRIDE #define USE_SIMPLIFIED_TUNING #define USE_RX_LINK_UPLINK_POWER +#define USE_GPS_PLUS_CODES #endif diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c index 6949aa52de..b5b3ed3150 100644 --- a/src/main/telemetry/frsky_hub.c +++ b/src/main/telemetry/frsky_hub.c @@ -257,15 +257,15 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result) static void sendLatLong(int32_t coord[2]) { gpsCoordinateDDDMMmmmm_t coordinate; - GPStoDDDMM_MMMM(coord[LAT], &coordinate); + GPStoDDDMM_MMMM(coord[GPS_LATITUDE], &coordinate); frSkyHubWriteFrame(ID_LATITUDE_BP, coordinate.dddmm); frSkyHubWriteFrame(ID_LATITUDE_AP, coordinate.mmmm); - frSkyHubWriteFrame(ID_N_S, coord[LAT] < 0 ? 'S' : 'N'); + frSkyHubWriteFrame(ID_N_S, coord[GPS_LATITUDE] < 0 ? 'S' : 'N'); - GPStoDDDMM_MMMM(coord[LON], &coordinate); + GPStoDDDMM_MMMM(coord[GPS_LONGITUDE], &coordinate); frSkyHubWriteFrame(ID_LONGITUDE_BP, coordinate.dddmm); frSkyHubWriteFrame(ID_LONGITUDE_AP, coordinate.mmmm); - frSkyHubWriteFrame(ID_E_W, coord[LON] < 0 ? 'W' : 'E'); + frSkyHubWriteFrame(ID_E_W, coord[GPS_LONGITUDE] < 0 ? 'W' : 'E'); } #if defined(USE_GPS) @@ -316,8 +316,8 @@ static void sendFakeLatLong(void) // Heading is only displayed on OpenTX if non-zero lat/long is also sent int32_t coord[2] = {0,0}; - coord[LAT] = ((0.01f * telemetryConfig()->gpsNoFixLatitude) * GPS_DEGREES_DIVIDER); - coord[LON] = ((0.01f * telemetryConfig()->gpsNoFixLongitude) * GPS_DEGREES_DIVIDER); + coord[GPS_LATITUDE] = ((0.01f * telemetryConfig()->gpsNoFixLatitude) * GPS_DEGREES_DIVIDER); + coord[GPS_LONGITUDE] = ((0.01f * telemetryConfig()->gpsNoFixLongitude) * GPS_DEGREES_DIVIDER); sendLatLong(coord); } @@ -330,8 +330,8 @@ static void sendGPSLatLong(void) if (STATE(GPS_FIX) || gpsFixOccured == 1) { // If we have ever had a fix, send the last known lat/long gpsFixOccured = 1; - coord[LAT] = gpsSol.llh.lat; - coord[LON] = gpsSol.llh.lon; + coord[GPS_LATITUDE] = gpsSol.llh.lat; + coord[GPS_LONGITUDE] = gpsSol.llh.lon; sendLatLong(coord); } else { // otherwise send fake lat/long in order to display compass value diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index aa8f88560f..894a6092a5 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -213,8 +213,8 @@ static void ltm_oframe(void) { ltm_initialise_packet('O'); #if defined(USE_GPS) - ltm_serialise_32(GPS_home[LAT]); - ltm_serialise_32(GPS_home[LON]); + ltm_serialise_32(GPS_home[GPS_LATITUDE]); + ltm_serialise_32(GPS_home[GPS_LONGITUDE]); #else ltm_serialise_32(0); ltm_serialise_32(0); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 3a2d3d62ae..78940a3f17 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -367,9 +367,9 @@ void mavlinkSendPosition(void) mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg, // latitude Latitude (WGS84), expressed as * 1E7 - GPS_home[LAT], + GPS_home[GPS_LATITUDE], // longitude Longitude (WGS84), expressed as * 1E7 - GPS_home[LON], + GPS_home[GPS_LONGITUDE], // altitude Altitude(WGS84), expressed as * 1000 0); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);