Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

Merge pull request #10566 from etracer65/osd_gps_coordinates_variants

Browse source
This commit is contained in:
Michael Keller 2021-04-27 00:18:01 +12:00 • committed by GitHub
commit 87955d9734
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
20 changed files with 1240 additions and 66 deletions
Download patch file Download diff file Expand all files Collapse all files

202
lib/main/google/olc/LICENSE Normal file
View file

@ -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.

96
lib/main/google/olc/README.md Normal file
View file

@ -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

6
lib/main/google/olc/betaflight.h Normal file
View file

@ -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"

7
lib/main/google/olc/betaflight_readme.txt Normal file
View file

@ -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

608
lib/main/google/olc/olc.c Normal file
View file

@ -0,0 +1,608 @@
#include "olc.h"
#include <ctype.h>
#include <float.h>
#include <math.h>
#include <memory.h>
#include <stdio.h>
#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, &center);
// 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, &center);
return OLC_Encode(&center, 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, &center);
// 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(&center, 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;
}

75
lib/main/google/olc/olc.h Normal file
View file

@ -0,0 +1,75 @@
#ifndef OLC_OPENLOCATIONCODE_H_
#define OLC_OPENLOCATIONCODE_H_
#include <stdlib.h>
#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

69
lib/main/google/olc/olc_private.h Normal file
View file

@ -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 <ctype.h>
#include <float.h>
#include <math.h>
#include <memory.h>
#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;
}

9
make/source.mk
View file

@ -447,3 +447,12 @@ endif
# Search path and source files for the ST stdperiph library # Search path and source files for the ST stdperiph library
VPATH := $(VPATH):$(STDPERIPH_DIR)/src 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

12
src/main/blackbox/blackbox.c
View file

@ -1035,15 +1035,15 @@ static void writeGPSFrame(timeUs_t currentTimeUs)
} }
blackboxWriteUnsignedVB(gpsSol.numSat); blackboxWriteUnsignedVB(gpsSol.numSat);
blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]); blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[GPS_LATITUDE]);
blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]); 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.llh.altCm / 10); // was originally designed to transport meters in int16, but +-3276.7m is a good compromise
blackboxWriteUnsignedVB(gpsSol.groundSpeed); blackboxWriteUnsignedVB(gpsSol.groundSpeed);
blackboxWriteUnsignedVB(gpsSol.groundCourse); blackboxWriteUnsignedVB(gpsSol.groundCourse);
gpsHistory.GPS_numSat = gpsSol.numSat; gpsHistory.GPS_numSat = gpsSol.numSat;
gpsHistory.GPS_coord[LAT] = gpsSol.llh.lat; gpsHistory.GPS_coord[GPS_LATITUDE] = gpsSol.llh.lat;
gpsHistory.GPS_coord[LON] = gpsSol.llh.lon; gpsHistory.GPS_coord[GPS_LONGITUDE] = gpsSol.llh.lon;
} }
#endif #endif
@ -1637,8 +1637,8 @@ STATIC_UNIT_TESTED void blackboxLogIteration(timeUs_t currentTimeUs)
writeGPSHomeFrame(); writeGPSHomeFrame();
writeGPSFrame(currentTimeUs); writeGPSFrame(currentTimeUs);
} else if (gpsSol.numSat != gpsHistory.GPS_numSat } else if (gpsSol.numSat != gpsHistory.GPS_numSat
|| gpsSol.llh.lat != gpsHistory.GPS_coord[LAT] || gpsSol.llh.lat != gpsHistory.GPS_coord[GPS_LATITUDE]
|| gpsSol.llh.lon != gpsHistory.GPS_coord[LON]) { || gpsSol.llh.lon != gpsHistory.GPS_coord[GPS_LONGITUDE]) {
//We could check for velocity changes as well but I doubt it changes independent of position //We could check for velocity changes as well but I doubt it changes independent of position
writeGPSFrame(currentTimeUs); writeGPSFrame(currentTimeUs);
} }

5
src/main/drivers/osd_symbols.h
View file

@ -156,3 +156,8 @@
#define SYM_STICK_OVERLAY_CENTER 0x0B #define SYM_STICK_OVERLAY_CENTER 0x0B
#define SYM_STICK_OVERLAY_VERTICAL 0x16 #define SYM_STICK_OVERLAY_VERTICAL 0x16
#define SYM_STICK_OVERLAY_HORIZONTAL 0x17 #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 // "

1
src/main/fc/runtime_config.h
View file

@ -114,6 +114,7 @@ extern uint16_t flightModeFlags;
typedef enum { typedef enum {
GPS_FIX_HOME = (1 << 0), GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1), GPS_FIX = (1 << 1),
GPS_FIX_EVER = (1 << 2),
} stateFlags_t; } stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask)) #define DISABLE_STATE(mask) (stateFlags &= ~(mask))

33
src/main/io/gps.c
View file

@ -950,11 +950,7 @@ static bool gpsNewFrameNMEA(char c)
gps_Msg.longitude *= -1; gps_Msg.longitude *= -1;
break; break;
case 6: case 6:
if (string[0] > '0') { gpsSetFixState(string[0] > '0');
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
break; break;
case 7: case 7:
gps_Msg.numSat = grab_fields(string, 0); 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.lon = _buffer.posllh.longitude;
gpsSol.llh.lat = _buffer.posllh.latitude; gpsSol.llh.lat = _buffer.posllh.latitude;
gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10; //alt in cm gpsSol.llh.altCm = _buffer.posllh.altitudeMslMm / 10; //alt in cm
if (next_fix) { gpsSetFixState(next_fix);
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
_new_position = true; _new_position = true;
break; break;
case MSG_STATUS: case MSG_STATUS:
@ -1499,7 +1491,7 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize)
if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) { if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) {
uint32_t dist; uint32_t dist;
int32_t dir; 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) { if (gpsConfig()->gps_use_3d_speed) {
dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f)); 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 = (gpsSol.llh.altCm - lastAlt) * 1000 / (currentMillis - lastMillis);
GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500); GPS_verticalSpeedInCmS = constrain(GPS_verticalSpeedInCmS, -1500, 1500);
} }
lastCoord[LON] = gpsSol.llh.lon; lastCoord[GPS_LONGITUDE] = gpsSol.llh.lon;
lastCoord[LAT] = gpsSol.llh.lat; lastCoord[GPS_LATITUDE] = gpsSol.llh.lat;
lastAlt = gpsSol.llh.altCm; lastAlt = gpsSol.llh.altCm;
lastMillis = currentMillis; 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_HOME) || !gpsConfig()->gps_set_home_point_once) {
if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { if (STATE(GPS_FIX) && gpsSol.numSat >= 5) {
GPS_home[LAT] = gpsSol.llh.lat; GPS_home[GPS_LATITUDE] = gpsSol.llh.lat;
GPS_home[LON] = gpsSol.llh.lon; GPS_home[GPS_LONGITUDE] = gpsSol.llh.lon;
GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc
// Set ground altitude // Set ground altitude
ENABLE_STATE(GPS_FIX_HOME); 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 if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything
uint32_t dist; uint32_t dist;
int32_t dir; 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_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100; GPS_directionToHome = dir / 100;
} else { } else {
@ -1585,4 +1577,13 @@ void onGpsNewData(void)
#endif #endif
} }
void gpsSetFixState(bool state)
{
if (state) {
ENABLE_STATE(GPS_FIX);
ENABLE_STATE(GPS_FIX_EVER);
} else {
DISABLE_STATE(GPS_FIX);
}
}
#endif #endif

10
src/main/io/gps.h
View file

@ -25,13 +25,15 @@
#include "pg/pg.h" #include "pg/pg.h"
#define LAT 0
#define LON 1
#define GPS_DEGREES_DIVIDER 10000000L #define GPS_DEGREES_DIVIDER 10000000L
#define GPS_X 1 #define GPS_X 1
#define GPS_Y 0 #define GPS_Y 0
typedef enum {
GPS_LATITUDE,
GPS_LONGITUDE
} gpsCoordinateType_e;
typedef enum { typedef enum {
GPS_NMEA = 0, GPS_NMEA = 0,
GPS_UBLOX, GPS_UBLOX,
@ -188,4 +190,4 @@ void onGpsNewData(void);
void GPS_reset_home_position(void); void GPS_reset_home_position(void);
void GPS_calc_longitude_scaling(int32_t lat); 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 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);

6
src/main/msp/msp.c
View file

@ -3181,11 +3181,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
#ifdef USE_GPS #ifdef USE_GPS
case MSP_SET_RAW_GPS: case MSP_SET_RAW_GPS:
if (sbufReadU8(src)) { gpsSetFixState(sbufReadU8(src));
ENABLE_STATE(GPS_FIX);
} else {
DISABLE_STATE(GPS_FIX);
}
gpsSol.numSat = sbufReadU8(src); gpsSol.numSat = sbufReadU8(src);
gpsSol.llh.lat = sbufReadU32(src); gpsSol.llh.lat = sbufReadU32(src);
gpsSol.llh.lon = sbufReadU32(src); gpsSol.llh.lon = sbufReadU32(src);

136
src/main/osd/osd_elements.c
View file

@ -59,6 +59,45 @@
CLI parameters should be added before line #endif // end of #ifdef USE_OSD 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 <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <stdlib.h> #include <stdlib.h>
@ -122,7 +161,10 @@
#include "sensors/esc_sensor.h" #include "sensors/esc_sensor.h"
#include "sensors/sensors.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_SYMBOL_COUNT 9
#define AH_SIDEBAR_WIDTH_POS 7 #define AH_SIDEBAR_WIDTH_POS 7
@ -273,22 +315,71 @@ static void osdFormatAltitudeString(char * buff, int32_t altitudeCm, osdElementT
} }
#ifdef USE_GPS #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). int32_t gpsValue = 0;
// longitude maximum integer width is 4 (-180). const char leadingSymbol = (coordinateType == GPS_LONGITUDE) ? SYM_LON : SYM_LAT;
// 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
// NOTE: Don't use osdPrintFloat() for this. There are too many decimal places and float math doesn't have enough precision 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;
int pos = 0; }
buff[pos++] = sym;
if (val < 0) { const int degreesPart = ABS(gpsValue) / GPS_DEGREES_DIVIDER;
buff[pos++] = '-'; int fractionalPart = ABS(gpsValue) % GPS_DEGREES_DIVIDER;
val = -val;
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 #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); 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 osdElementGpsLongitude(osdElementParms_t *element)
{
osdFormatCoordinate(element->buff, SYM_LON, gpsSol.llh.lon);
} }
static void osdElementGpsSats(osdElementParms_t *element) static void osdElementGpsSats(osdElementParms_t *element)
@ -1493,8 +1585,8 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
[OSD_WARNINGS] = osdElementWarnings, [OSD_WARNINGS] = osdElementWarnings,
[OSD_AVG_CELL_VOLTAGE] = osdElementAverageCellVoltage, [OSD_AVG_CELL_VOLTAGE] = osdElementAverageCellVoltage,
#ifdef USE_GPS #ifdef USE_GPS
[OSD_GPS_LON] = osdElementGpsLongitude, [OSD_GPS_LON] = osdElementGpsCoordinate,
[OSD_GPS_LAT] = osdElementGpsLatitude, [OSD_GPS_LAT] = osdElementGpsCoordinate,
#endif #endif
[OSD_DEBUG] = osdElementDebug, [OSD_DEBUG] = osdElementDebug,
#ifdef USE_ACC #ifdef USE_ACC

4
src/main/target/common_post.h
View file

@ -409,3 +409,7 @@ extern uint8_t __config_end;
#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2) #if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
#define USE_RX_BIND #define USE_RX_BIND
#endif #endif
#ifndef USE_GPS
#undef USE_GPS_PLUS_CODES
#endif

1
src/main/target/common_pre.h
View file

@ -398,4 +398,5 @@
#define USE_RX_MSP_OVERRIDE #define USE_RX_MSP_OVERRIDE
#define USE_SIMPLIFIED_TUNING #define USE_SIMPLIFIED_TUNING
#define USE_RX_LINK_UPLINK_POWER #define USE_RX_LINK_UPLINK_POWER
#define USE_GPS_PLUS_CODES
#endif #endif

16
src/main/telemetry/frsky_hub.c
View file

@ -257,15 +257,15 @@ static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
static void sendLatLong(int32_t coord[2]) static void sendLatLong(int32_t coord[2])
{ {
gpsCoordinateDDDMMmmmm_t coordinate; gpsCoordinateDDDMMmmmm_t coordinate;
GPStoDDDMM_MMMM(coord[LAT], &coordinate); GPStoDDDMM_MMMM(coord[GPS_LATITUDE], &coordinate);
frSkyHubWriteFrame(ID_LATITUDE_BP, coordinate.dddmm); frSkyHubWriteFrame(ID_LATITUDE_BP, coordinate.dddmm);
frSkyHubWriteFrame(ID_LATITUDE_AP, coordinate.mmmm); 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_BP, coordinate.dddmm);
frSkyHubWriteFrame(ID_LONGITUDE_AP, coordinate.mmmm); 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) #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 // Heading is only displayed on OpenTX if non-zero lat/long is also sent
int32_t coord[2] = {0,0}; int32_t coord[2] = {0,0};
coord[LAT] = ((0.01f * telemetryConfig()->gpsNoFixLatitude) * GPS_DEGREES_DIVIDER); coord[GPS_LATITUDE] = ((0.01f * telemetryConfig()->gpsNoFixLatitude) * GPS_DEGREES_DIVIDER);
coord[LON] = ((0.01f * telemetryConfig()->gpsNoFixLongitude) * GPS_DEGREES_DIVIDER); coord[GPS_LONGITUDE] = ((0.01f * telemetryConfig()->gpsNoFixLongitude) * GPS_DEGREES_DIVIDER);
sendLatLong(coord); sendLatLong(coord);
} }
@ -330,8 +330,8 @@ static void sendGPSLatLong(void)
if (STATE(GPS_FIX) || gpsFixOccured == 1) { if (STATE(GPS_FIX) || gpsFixOccured == 1) {
// If we have ever had a fix, send the last known lat/long // If we have ever had a fix, send the last known lat/long
gpsFixOccured = 1; gpsFixOccured = 1;
coord[LAT] = gpsSol.llh.lat; coord[GPS_LATITUDE] = gpsSol.llh.lat;
coord[LON] = gpsSol.llh.lon; coord[GPS_LONGITUDE] = gpsSol.llh.lon;
sendLatLong(coord); sendLatLong(coord);
} else { } else {
// otherwise send fake lat/long in order to display compass value // otherwise send fake lat/long in order to display compass value

4
src/main/telemetry/ltm.c
View file

@ -213,8 +213,8 @@ static void ltm_oframe(void)
{ {
ltm_initialise_packet('O'); ltm_initialise_packet('O');
#if defined(USE_GPS) #if defined(USE_GPS)
ltm_serialise_32(GPS_home[LAT]); ltm_serialise_32(GPS_home[GPS_LATITUDE]);
ltm_serialise_32(GPS_home[LON]); ltm_serialise_32(GPS_home[GPS_LONGITUDE]);
#else #else
ltm_serialise_32(0); ltm_serialise_32(0);
ltm_serialise_32(0); ltm_serialise_32(0);

4
src/main/telemetry/mavlink.c
View file

@ -367,9 +367,9 @@ void mavlinkSendPosition(void)
mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg, mavlink_msg_gps_global_origin_pack(0, 200, &mavMsg,
// latitude Latitude (WGS84), expressed as * 1E7 // latitude Latitude (WGS84), expressed as * 1E7
GPS_home[LAT], GPS_home[GPS_LATITUDE],
// longitude Longitude (WGS84), expressed as * 1E7 // longitude Longitude (WGS84), expressed as * 1E7
GPS_home[LON], GPS_home[GPS_LONGITUDE],
// altitude Altitude(WGS84), expressed as * 1000 // altitude Altitude(WGS84), expressed as * 1000
0); 0);
msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg);