Merge branch 'master' into gps_task_time
This commit is contained in:
Michael Keller
GitHub
commit
e9b77e9f30
2
Makefile
2
Makefile
|
|
@ -105,7 +105,7 @@ FEATURE_CUT_LEVEL_SUPPLIED := $(FEATURE_CUT_LEVEL)
|
|||
FEATURE_CUT_LEVEL =
|
||||
|
||||
# The list of targets to build for 'pre-push'
|
||||
PRE_PUSH_TARGET_LIST ?= STM32F405 STM32F411 STM32F7X2 STM32F745 NUCLEOH743 SITL STM32F4DISCOVERY_DEBUG test-representative
|
||||
PRE_PUSH_TARGET_LIST ?= $(UNIFIED_TARGETS) NUCLEOH743 SITL STM32F4DISCOVERY_DEBUG test-representative
|
||||
|
||||
include $(ROOT)/make/targets.mk
|
||||
|
||||
|
|
|
|||
|
|
@ -52,9 +52,9 @@ stages:
|
|||
- script: make EXTRA_FLAGS=-Werror test-all
|
||||
displayName: 'Run all unit tests'
|
||||
- script: make EXTRA_FLAGS=-Werror all
|
||||
displayName: 'Build all oficial targets'
|
||||
displayName: 'Build all official targets'
|
||||
- script: mkdir release; cp obj/*.hex release/
|
||||
displayName: 'Copy artefacts'
|
||||
displayName: 'Copy artifacts'
|
||||
- task: PublishPipelineArtifact@1
|
||||
displayName: 'Publish Linux release'
|
||||
inputs:
|
||||
|
|
|
|||
|
|
@ -175,6 +175,16 @@ amperage_meter_scale = (Imax - Imin) * 100000 / (Tmax + (Tmax * Tmax / 50))
|
|||
= 205
|
||||
amperage_meter_offset = Imin * 100 = 280
|
||||
```
|
||||
Measuring Imax requires a battery and an ESC that can both deliver and support max current for the duration of the measurement, so it's prone to big inaccuracies. Alternatively, current can be measured at a much lower throttle position and be taken into account in the calculations.
|
||||
|
||||
Following the previous example, if we measured an Ibench current of 6A at 30% of throttle (1255 in the motors tab because (0.3*(max_throttle-1000))+1000))
|
||||
```
|
||||
Tbench = Tmax * bench_throttle = 850 * 0.3 = 255
|
||||
amperage_meter_scale = (Ibench - Imin) * 100000 / (Tbench + (Tbench * Tbench / 50))
|
||||
= (6 - 2.8) * 100000 / (255 + (255 * 255 / 50))
|
||||
= 205
|
||||
amperage_meter_offset = Imin * 100 = 280
|
||||
```
|
||||
#### Tuning Using Battery Charger Measurement
|
||||
If you cannot measure current draw directly, you can approximate it indirectly using your battery charger.
|
||||
However, note it may be difficult to adjust `amperage_meter_offset` using this method unless you can
|
||||
|
|
|
|||
|
|
@ -83,6 +83,7 @@ Click on a command to jump to the relevant documentation page.
|
|||
| [`color`](LedStrip.md) | configure colors |
|
||||
| `defaults` | reset to defaults and reboot |
|
||||
| `dump` | print configurable settings in a pastable form |
|
||||
| `dma` | configure direct memory access channel |
|
||||
| `exit` | |
|
||||
| `feature` | list or -val or val |
|
||||
| `get` | get variable value |
|
||||
|
|
@ -102,6 +103,7 @@ Click on a command to jump to the relevant documentation page.
|
|||
| `serialpassthrough` | serial passthrough mode, reset board to exit |
|
||||
| `set` | name=value or blank or * for list |
|
||||
| `status` | show system status |
|
||||
| `timer` | configure timer |
|
||||
| `version` | show version |
|
||||
| [`serial`](Serial.md) | configure serial ports |
|
||||
| [`servo`](Mixer.md) | configure servos |
|
||||
|
|
@ -177,9 +179,9 @@ Click on a variable to jump to the relevant documentation page.
|
|||
| `hott_alarm_sound_interval` | Battery alarm delay in seconds for Hott telemetry | 0 | 120 | 5 | Master | UINT8 |
|
||||
| [`bat_capacity`](Battery.md) | Battery capacity in mAH. This value is used in conjunction with the current meter to determine remaining battery capacity. | 0 | 20000 | 0 | Master | UINT16 |
|
||||
| [`vbat_scale`](Battery.md) | Result is Vbatt in 0.1V steps. 3.3V = ADC Vref, 4095 = 12bit adc, 110 = 11:1 voltage divider (10k:1k) x 10 for 0.1V. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | 0 | 255 | 110 | Master | UINT8 |
|
||||
| [`vbat_max_cell_voltage`](Battery.md) | Maximum voltage per cell, used for auto-detecting battery voltage in 0.1V units, default is 43 (4.3V) | 10 | 50 | 43 | Master | UINT8 |
|
||||
| [`vbat_min_cell_voltage`](Battery.md) | Minimum voltage per cell, this triggers battery-critical alarms, in 0.1V units, default is 33 (3.3V) | 10 | 50 | 33 | Master | UINT8 |
|
||||
| [`vbat_warning_cell_voltage`](Battery.md) | Warning voltage per cell, this triggers battery-warning alarms, in 0.1V units, default is 35 (3.5V) | 10 | 50 | 35 | Master | UINT8 |
|
||||
| [`vbat_max_cell_voltage`](Battery.md) | Maximum voltage per cell, used for auto-detecting battery voltage in 0.01V units, default is 430 (4.3V) | 100 | 500 | 430 | Master | UINT16 |
|
||||
| [`vbat_min_cell_voltage`](Battery.md) | Minimum voltage per cell, this triggers battery-critical alarms, in 0.01V units, default is 330 (3.3V) | 100 | 500 | 330 | Master | UINT16 |
|
||||
| [`vbat_warning_cell_voltage`](Battery.md) | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | 100 | 500 | 350 | Master | UINT16 |
|
||||
| [`vbat_hysteresis`](Battery.md) | Sets the hysteresis value for low-battery alarms, in 0.01V units, default is 1 (0.01V) | 10 | 250 | 1 | Master | UINT8 |
|
||||
| [`current_meter_scale`](Battery.md) | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. | -10000 | 10000 | 400 | Master | INT16 |
|
||||
| [`current_meter_offset`](Battery.md) | This sets the output offset voltage of the current sensor in millivolts. | 0 | 3300 | 0 | Master | UINT16 |
|
||||
|
|
|
|||
|
|
@ -0,0 +1,16 @@
|
|||
## Custom Board Configuration Using CLI
|
||||
Warning: This section covers beyond the normal use-case for Betaflight. It's recommended that you use a pre-made target configuration for a flight controller that has been thoroughly tested before. Those configurations can be found in the [unified targets list](https://github.com/betaflight/unified-targets/tree/master/configs/default).
|
||||
|
||||
However, Betaflight can provide support for custom-made flight controllers, assuming it contains supported hardware. Before using Betaflight at all, it's highly recommended that you, first, check to make sure your MCU and your peripherals are supported. Checking the [Supported Sensors](https://github.com/betaflight/betaflight/wiki/Supported-Sensors) and [Hardware Reference](https://github.com/betaflight/betaflight/wiki/Hardware-Reference) wiki pages are good places to look. Betaflight is supported across many STM32 processors. Second, thoroughly test the functionality of your hardware by programming your board in an IDE (i.e. STMCube) or using any other hardware debugging methods/tools.
|
||||
|
||||
Assuming your hardware is supported and you've thoroughly tested your peripherals, flash generic firmware into your board. It's recommended to flash betaflight firmware using the configurator and [USB Flashing](https://github.com/betaflight/betaflight/blob/master/docs/USB%20Flashing.md).
|
||||
|
||||
To create your own configuration, you must first use the `resource` command in the command line interface to map your peripheral pins on your controller. Use the CLI documentation linked above and other wiki pages for command reference. Then, you use the `set` command to set the bus type, i2c address, lowpass filter frequency/type, baro_hardware, gyro_hardware, etc. This will mainly be dependent on your hardware and will involve a lot of trial and error. Use the `save` command to save your configuration and reboot the controller. If your hardware is not detected, then you need to modify or create more settings in the CLI to accurately describe your hardware to Betaflight to detect it. Use your custom board's and respective peripherals' datasheets and schematics to help you.
|
||||
|
||||
Alongside `resource` and `set` commands, there are `timer` and `dma` commands. For certain functions, such as battery voltage monitoring and motor output, you need to associate a timer and/or a Direct Memory Access (DMA) channel to those pins for those to work.
|
||||
- `# timer`
|
||||
- `timer <pin> AF(x) [1-3]`
|
||||
- `# dma`
|
||||
- `dma [SPI_TX|ADC|pin] [index|pin] [0|1]`
|
||||
|
||||
For custom configuration like this outside the normal use-case, it's recommended that you use the pre-made [configs](https://github.com/betaflight/unified-targets/tree/master/configs/default) as a source of reference.
|
||||
|
|
@ -25,11 +25,6 @@
|
|||
#ifdef GENERATOR_I2S_PRESENT
|
||||
#include "stm32h7xx_ll_rcc.h"
|
||||
#endif /* GENERATOR_I2S_PRESENT*/
|
||||
#ifdef USE_FULL_ASSERT
|
||||
#include "stm32_assert.h"
|
||||
#else
|
||||
#define assert_param(expr) ((void)0U)
|
||||
#endif
|
||||
|
||||
/** @addtogroup STM32H7xx_LL_Driver
|
||||
* @{
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
@ -0,0 +1,96 @@
|
|||
Open Location Code
|
||||
==================
|
||||
|
||||
[](https://travis-ci.org/google/open-location-code)
|
||||
[](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
|
||||
|
|
@ -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"
|
||||
|
|
@ -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
|
||||
|
|
@ -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, ¢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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -130,13 +130,16 @@ ARCH_FLAGS = -mthumb -mcpu=cortex-m4 -march=armv7e-m -mfloat-abi=hard -mfpu
|
|||
|
||||
DEVICE_FLAGS = -DUSE_HAL_DRIVER -DUSE_FULL_LL_DRIVER -DUSE_DMA_RAM -DMAX_MPU_REGIONS=16
|
||||
|
||||
# G4X3_TARGETS includes G47{3,4}{RE,CE,CEU}
|
||||
# G47X_TARGETS includes G47{3,4}{RE,CE,CEU}
|
||||
|
||||
ifeq ($(TARGET),$(filter $(TARGET),$(G4X3_TARGETS)))
|
||||
ifeq ($(TARGET),$(filter $(TARGET),$(G47X_TARGETS)))
|
||||
DEVICE_FLAGS += -DSTM32G474xx
|
||||
LD_SCRIPT = $(LINKER_DIR)/stm32_flash_g474.ld
|
||||
STARTUP_SRC = startup_stm32g474xx.s
|
||||
MCU_FLASH_SIZE = 512
|
||||
# Override the OPTIMISE_SPEED compiler setting to save flash space on these 512KB targets.
|
||||
# Performance is only slightly affected but around 50 kB of flash are saved.
|
||||
OPTIMISE_SPEED = -O2
|
||||
else
|
||||
$(error Unknown MCU for G4 target)
|
||||
endif
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -18,7 +18,7 @@ endif
|
|||
|
||||
F4_TARGETS := $(F405_TARGETS) $(F411_TARGETS) $(F446_TARGETS)
|
||||
F7_TARGETS := $(F7X2RE_TARGETS) $(F7X5XE_TARGETS) $(F7X5XG_TARGETS) $(F7X5XI_TARGETS) $(F7X6XG_TARGETS)
|
||||
G4_TARGETS := $(G4X3_TARGETS)
|
||||
G4_TARGETS := $(G47X_TARGETS)
|
||||
H7_TARGETS := $(H743xI_TARGETS) $(H750xB_TARGETS) $(H7A3xI_TARGETS) $(H7A3xIQ_TARGETS) $(H723xG_TARGETS) $(H725xG_TARGETS)
|
||||
|
||||
ifeq ($(filter $(TARGET),$(VALID_TARGETS)),)
|
||||
|
|
@ -26,7 +26,7 @@ $(error Target '$(TARGET)' is not valid, must be one of $(VALID_TARGETS). Have y
|
|||
endif
|
||||
|
||||
ifeq ($(filter $(TARGET),$(F1_TARGETS) $(F3_TARGETS) $(F4_TARGETS) $(F7_TARGETS) $(G4_TARGETS) $(H7_TARGETS) $(SITL_TARGETS)),)
|
||||
$(error Target '$(TARGET)' has not specified a valid STM group, must be one of F1, F3, F405, F411, F446, F7X2RE, F7X5XE, F7X5XG, F7X5XI, F7X6XG, G4X3 or H7X3XI. Have you prepared a valid target.mk?)
|
||||
$(error Target '$(TARGET)' has not specified a valid STM group, must be one of F1, F3, F405, F411, F446, F7X2RE, F7X5XE, F7X5XG, F7X5XI, F7X6XG, G47X or H7X3XI. Have you prepared a valid target.mk?)
|
||||
endif
|
||||
|
||||
ifeq ($(TARGET),$(filter $(TARGET),$(F3_TARGETS)))
|
||||
|
|
|
|||
|
|
@ -82,7 +82,8 @@ UNSUPPORTED_TARGETS := \
|
|||
UNIFIED_TARGETS := STM32F405 \
|
||||
STM32F411 \
|
||||
STM32F7X2 \
|
||||
STM32F745
|
||||
STM32F745 \
|
||||
STM32G47X
|
||||
|
||||
# Legacy targets are targets that have been replaced by Unified Target configurations
|
||||
LEGACY_TARGETS := MATEKF405 \
|
||||
|
|
|
|||
|
|
@ -12,10 +12,12 @@
|
|||
/* Specify the memory areas. */
|
||||
MEMORY
|
||||
{
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 4K
|
||||
FLASH1 (rx) : ORIGIN = 0x08001000, LENGTH = 492K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x0807C000, LENGTH = 8K
|
||||
FLASH_CUSTOM_DEFAULTS (r) : ORIGIN = 0x0807E000, LENGTH = 8K
|
||||
FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 10K
|
||||
FLASH_CUSTOM_DEFAULTS (r) : ORIGIN = 0x08002800, LENGTH = 6K
|
||||
FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 8K
|
||||
FLASH1 (rx) : ORIGIN = 0x08006000, LENGTH = DEFINED(USE_CUSTOM_DEFAULTS_EXTENDED) ? 480K : 488K
|
||||
FLASH_CUSTOM_DEFAULTS_EXTENDED (r): ORIGIN = DEFINED(USE_CUSTOM_DEFAULTS_EXTENDED) ? 0x0807E000 : 0x08080000, LENGTH = DEFINED(USE_CUSTOM_DEFAULTS_EXTENDED) ? 8K : 0K
|
||||
|
||||
|
||||
SYSTEM_MEMORY (r) : ORIGIN = 0x1FFF0000, LENGTH = 64K
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
|
|
@ -1461,8 +1461,6 @@ static bool blackboxWriteSysinfo(void)
|
|||
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_cutoffs", "%d, %d", rcSmoothingData->inputCutoffSetting,
|
||||
rcSmoothingData->derivativeCutoffSetting);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_auto_factor", "%d", rcSmoothingData->autoSmoothnessFactor);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_filter_type", "%d, %d", rcSmoothingData->inputFilterType,
|
||||
rcSmoothingData->derivativeFilterType);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_active_cutoffs", "%d, %d", rcSmoothingData->inputCutoffFrequency,
|
||||
rcSmoothingData->derivativeCutoffFrequency);
|
||||
BLACKBOX_PRINT_HEADER_LINE("rc_smoothing_rx_average", "%d", rcSmoothingData->averageFrameTimeUs);
|
||||
|
|
@ -1639,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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -4967,29 +4967,18 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
|
|||
cliPrintLinef("%d.%03dms", avgRxFrameUs / 1000, avgRxFrameUs % 1000);
|
||||
}
|
||||
}
|
||||
cliPrintLinef("# Input filter type: %s", lookupTables[TABLE_RC_SMOOTHING_INPUT_TYPE].values[rcSmoothingData->inputFilterType]);
|
||||
cliPrintf("# Active input cutoff: %dhz ", rcSmoothingData->inputCutoffFrequency);
|
||||
if (rcSmoothingData->inputCutoffSetting == 0) {
|
||||
cliPrintLine("(auto)");
|
||||
} else {
|
||||
cliPrintLine("(manual)");
|
||||
}
|
||||
cliPrintf("# Derivative filter type: %s", lookupTables[TABLE_RC_SMOOTHING_DERIVATIVE_TYPE].values[rcSmoothingData->derivativeFilterType]);
|
||||
if (rcSmoothingData->derivativeFilterTypeSetting == RC_SMOOTHING_DERIVATIVE_AUTO) {
|
||||
cliPrintLine(" (auto)");
|
||||
} else {
|
||||
cliPrintLinefeed();
|
||||
}
|
||||
cliPrintf("# Active derivative cutoff: %dhz (", rcSmoothingData->derivativeCutoffFrequency);
|
||||
if (rcSmoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_OFF) {
|
||||
cliPrintLine("off)");
|
||||
} else {
|
||||
if (rcSmoothingData->derivativeCutoffSetting == 0) {
|
||||
cliPrintLine("auto)");
|
||||
} else {
|
||||
cliPrintLine("manual)");
|
||||
}
|
||||
}
|
||||
} else {
|
||||
cliPrintLine("INTERPOLATION");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -412,12 +412,6 @@ static const char * const lookupTableRcSmoothingType[] = {
|
|||
static const char * const lookupTableRcSmoothingDebug[] = {
|
||||
"ROLL", "PITCH", "YAW", "THROTTLE"
|
||||
};
|
||||
static const char * const lookupTableRcSmoothingInputType[] = {
|
||||
"PT1", "BIQUAD"
|
||||
};
|
||||
static const char * const lookupTableRcSmoothingDerivativeType[] = {
|
||||
"OFF", "PT1", "BIQUAD", "AUTO"
|
||||
};
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
#ifdef USE_VTX_COMMON
|
||||
|
|
@ -605,8 +599,6 @@ const lookupTableEntry_t lookupTables[] = {
|
|||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingType),
|
||||
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDebug),
|
||||
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingInputType),
|
||||
LOOKUP_TABLE_ENTRY(lookupTableRcSmoothingDerivativeType),
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
#ifdef USE_VTX_COMMON
|
||||
LOOKUP_TABLE_ENTRY(lookupTableVtxLowPowerDisarm),
|
||||
|
|
@ -750,8 +742,6 @@ const clivalue_t valueTable[] = {
|
|||
{ "rc_smoothing_input_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_cutoff) },
|
||||
{ "rc_smoothing_derivative_hz", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_cutoff) },
|
||||
{ "rc_smoothing_debug_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DEBUG }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_debug_axis) },
|
||||
{ "rc_smoothing_input_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_INPUT_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_input_type) },
|
||||
{ "rc_smoothing_derivative_type",VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_SMOOTHING_DERIVATIVE_TYPE }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_derivative_type) },
|
||||
{ "rc_smoothing_auto_smoothness",VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { RC_SMOOTHING_AUTO_FACTOR_MIN, RC_SMOOTHING_AUTO_FACTOR_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rc_smoothing_auto_factor) },
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
||||
|
|
@ -1174,6 +1164,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "ff_interpolate_sp", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_INTERPOLATED_SP}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
|
||||
{ "ff_max_rate_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },
|
||||
{ "ff_smooth_factor", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 75}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_smooth_factor) },
|
||||
{ "ff_jitter_reduction", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 20}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_jitter_factor) },
|
||||
#endif
|
||||
{ "ff_boost", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, ff_boost) },
|
||||
|
||||
|
|
@ -1326,7 +1317,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "osd_link_quality_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_OSD_CONFIG, offsetof(osdConfig_t, link_quality_alarm) },
|
||||
#endif
|
||||
#ifdef USE_RX_RSSI_DBM
|
||||
{ "osd_rssi_dbm_alarm", VAR_INT16 | MASTER_VALUE, .config.minmaxUnsigned = { CRSF_RSSI_MIN, CRSF_SNR_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_dbm_alarm) },
|
||||
{ "osd_rssi_dbm_alarm", VAR_INT16 | MASTER_VALUE, .config.minmax = { CRSF_RSSI_MIN, CRSF_SNR_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_dbm_alarm) },
|
||||
#endif
|
||||
{ "osd_cap_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 20000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, cap_alarm) },
|
||||
{ "osd_alt_alarm", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 10000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, alt_alarm) },
|
||||
|
|
@ -1352,6 +1343,9 @@ const clivalue_t valueTable[] = {
|
|||
#ifdef USE_RX_LINK_QUALITY_INFO
|
||||
{ "osd_link_quality_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_LINK_QUALITY]) },
|
||||
#endif
|
||||
#ifdef USE_RX_LINK_UPLINK_POWER
|
||||
{ "osd_link_tx_power_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_TX_UPLINK_POWER]) },
|
||||
#endif
|
||||
#ifdef USE_RX_RSSI_DBM
|
||||
{ "osd_rssi_dbm_pos", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_RSSI_DBM_VALUE]) },
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -112,8 +112,6 @@ typedef enum {
|
|||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
TABLE_RC_SMOOTHING_TYPE,
|
||||
TABLE_RC_SMOOTHING_DEBUG,
|
||||
TABLE_RC_SMOOTHING_INPUT_TYPE,
|
||||
TABLE_RC_SMOOTHING_DERIVATIVE_TYPE,
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
#ifdef USE_VTX_COMMON
|
||||
TABLE_VTX_LOW_POWER_DISARM,
|
||||
|
|
|
|||
|
|
@ -519,6 +519,7 @@ static uint8_t cmsx_iterm_relax_cutoff;
|
|||
#ifdef USE_INTERPOLATED_SP
|
||||
static uint8_t cmsx_ff_interpolate_sp;
|
||||
static uint8_t cmsx_ff_smooth_factor;
|
||||
static uint8_t cmsx_ff_jitter_factor;
|
||||
#endif
|
||||
|
||||
static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
|
||||
|
|
@ -561,6 +562,7 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
|
|||
#ifdef USE_INTERPOLATED_SP
|
||||
cmsx_ff_interpolate_sp = pidProfile->ff_interpolate_sp;
|
||||
cmsx_ff_smooth_factor = pidProfile->ff_smooth_factor;
|
||||
cmsx_ff_jitter_factor = pidProfile->ff_jitter_factor;
|
||||
#endif
|
||||
|
||||
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
|
||||
|
|
@ -608,6 +610,7 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry
|
|||
#ifdef USE_INTERPOLATED_SP
|
||||
pidProfile->ff_interpolate_sp = cmsx_ff_interpolate_sp;
|
||||
pidProfile->ff_smooth_factor = cmsx_ff_smooth_factor;
|
||||
pidProfile->ff_jitter_factor = cmsx_ff_jitter_factor;
|
||||
#endif
|
||||
|
||||
#ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION
|
||||
|
|
@ -625,6 +628,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = {
|
|||
#ifdef USE_INTERPOLATED_SP
|
||||
{ "FF MODE", OME_TAB, NULL, &(OSD_TAB_t) { &cmsx_ff_interpolate_sp, 4, lookupTableInterpolatedSetpoint}, 0 },
|
||||
{ "FF SMOOTHNESS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_smooth_factor, 0, 75, 1 } , 0 },
|
||||
{ "FF JITTER", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_jitter_factor, 0, 20, 1 } , 0 },
|
||||
#endif
|
||||
{ "FF BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_ff_boost, 0, 50, 1 } , 0 },
|
||||
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 },
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <ctype.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
|
@ -79,6 +80,20 @@ static const void *cmsx_menuRcConfirmBack(displayPort_t *pDisp, const OSD_Entry
|
|||
}
|
||||
}
|
||||
|
||||
static int16_t rcDataInt[AUX4 + 1];
|
||||
|
||||
static const void *cmsx_menuRcOnDisplayUpdate(displayPort_t *pDisp, const OSD_Entry *selected)
|
||||
{
|
||||
UNUSED(pDisp);
|
||||
UNUSED(selected);
|
||||
|
||||
for (int i = 0; i <= AUX4; i++) {
|
||||
rcDataInt[i] = lroundf(rcData[i]);
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
//
|
||||
// RC preview
|
||||
//
|
||||
|
|
@ -86,15 +101,15 @@ static const OSD_Entry cmsx_menuRcEntries[] =
|
|||
{
|
||||
{ "-- RC PREV --", OME_Label, NULL, NULL, 0},
|
||||
|
||||
{ "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[ROLL], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[PITCH], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[THROTTLE], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[YAW], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "ROLL", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[ROLL], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "PITCH", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[PITCH], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "THR", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[THROTTLE], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "YAW", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[YAW], 1, 2500, 0 }, DYNAMIC },
|
||||
|
||||
{ "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX1], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX2], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX3], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcData[AUX4], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "AUX1", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX1], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "AUX2", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX2], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "AUX3", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX3], 1, 2500, 0 }, DYNAMIC },
|
||||
{ "AUX4", OME_INT16, NULL, &(OSD_INT16_t){ &rcDataInt[AUX4], 1, 2500, 0 }, DYNAMIC },
|
||||
|
||||
{ "BACK", OME_Back, NULL, NULL, 0},
|
||||
{NULL, OME_END, NULL, NULL, 0}
|
||||
|
|
@ -107,7 +122,7 @@ CMS_Menu cmsx_menuRcPreview = {
|
|||
#endif
|
||||
.onEnter = cmsx_menuRcOnEnter,
|
||||
.onExit = cmsx_menuRcConfirmBack,
|
||||
.onDisplayUpdate = NULL,
|
||||
.onDisplayUpdate = cmsx_menuRcOnDisplayUpdate,
|
||||
.entries = cmsx_menuRcEntries
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -49,6 +49,8 @@
|
|||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
#include "rx/crsf.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
|
||||
#ifdef USE_EXTENDED_CMS_MENUS
|
||||
|
|
@ -180,7 +182,7 @@ static CMS_Menu menuOsdActiveElems = {
|
|||
|
||||
static uint8_t osdConfig_rssi_alarm;
|
||||
static uint16_t osdConfig_link_quality_alarm;
|
||||
static uint8_t osdConfig_rssi_dbm_alarm;
|
||||
static int16_t osdConfig_rssi_dbm_alarm;
|
||||
static uint16_t osdConfig_cap_alarm;
|
||||
static uint16_t osdConfig_alt_alarm;
|
||||
static uint16_t osdConfig_distance_alarm;
|
||||
|
|
@ -225,7 +227,7 @@ const OSD_Entry menuAlarmsEntries[] =
|
|||
{"--- ALARMS ---", OME_Label, NULL, NULL, 0},
|
||||
{"RSSI", OME_UINT8, NULL, &(OSD_UINT8_t){&osdConfig_rssi_alarm, 5, 90, 5}, 0},
|
||||
{"LINK QUALITY", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_link_quality_alarm, 5, 300, 5}, 0},
|
||||
{"RSSI DBM", OME_UINT8, NULL, &(OSD_UINT8_t){&osdConfig_rssi_dbm_alarm, 5, 130, 5}, 0},
|
||||
{"RSSI DBM", OME_INT16, NULL, &(OSD_INT16_t){&osdConfig_rssi_dbm_alarm, CRSF_RSSI_MIN, CRSF_SNR_MAX, 5}, 0},
|
||||
{"MAIN BAT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_cap_alarm, 50, 30000, 50}, 0},
|
||||
{"MAX ALT", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_alt_alarm, 1, 200, 1}, 0},
|
||||
{"MAX DISTANCE", OME_UINT16, NULL, &(OSD_UINT16_t){&osdConfig_distance_alarm, 0, UINT16_MAX, 10}, 0},
|
||||
|
|
|
|||
|
|
@ -65,6 +65,71 @@ FAST_CODE float pt1FilterApply(pt1Filter_t *filter, float input)
|
|||
return filter->state;
|
||||
}
|
||||
|
||||
// PT2 Low Pass filter
|
||||
|
||||
float pt2FilterGain(float f_cut, float dT)
|
||||
{
|
||||
const float order = 2.0f;
|
||||
const float orderCutoffCorrection = (1 / sqrtf(powf(2, 1.0f / (order)) - 1));
|
||||
float RC = 1 / ( 2 * orderCutoffCorrection * M_PIf * f_cut);
|
||||
//float RC = 1 / ( 2 * 1.553773974f * M_PIf * f_cut);
|
||||
// where 1.553773974 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 2
|
||||
return dT / (RC + dT);
|
||||
}
|
||||
|
||||
void pt2FilterInit(pt2Filter_t *filter, float k)
|
||||
{
|
||||
filter->state = 0.0f;
|
||||
filter->state1 = 0.0f;
|
||||
filter->k = k;
|
||||
}
|
||||
|
||||
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k)
|
||||
{
|
||||
filter->k = k;
|
||||
}
|
||||
|
||||
FAST_CODE float pt2FilterApply(pt2Filter_t *filter, float input)
|
||||
{
|
||||
filter->state1 = filter->state1 + filter->k * (input - filter->state1);
|
||||
filter->state = filter->state + filter->k * (filter->state1 - filter->state);
|
||||
return filter->state;
|
||||
}
|
||||
|
||||
// PT3 Low Pass filter
|
||||
|
||||
float pt3FilterGain(float f_cut, float dT)
|
||||
{
|
||||
const float order = 3.0f;
|
||||
const float orderCutoffCorrection = (1 / sqrtf(powf(2, 1.0f / (order)) - 1));
|
||||
float RC = 1 / ( 2 * orderCutoffCorrection * M_PIf * f_cut);
|
||||
// float RC = 1 / ( 2 * 1.961459177f * M_PIf * f_cut);
|
||||
// where 1.961459177 = 1 / sqrt( (2^(1 / order) - 1) ) and order is 3
|
||||
return dT / (RC + dT);
|
||||
}
|
||||
|
||||
void pt3FilterInit(pt3Filter_t *filter, float k)
|
||||
{
|
||||
filter->state = 0.0f;
|
||||
filter->state1 = 0.0f;
|
||||
filter->state2 = 0.0f;
|
||||
filter->k = k;
|
||||
}
|
||||
|
||||
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k)
|
||||
{
|
||||
filter->k = k;
|
||||
}
|
||||
|
||||
FAST_CODE float pt3FilterApply(pt3Filter_t *filter, float input)
|
||||
{
|
||||
filter->state1 = filter->state1 + filter->k * (input - filter->state1);
|
||||
filter->state2 = filter->state2 + filter->k * (filter->state1 - filter->state2);
|
||||
filter->state = filter->state + filter->k * (filter->state2 - filter->state);
|
||||
return filter->state;
|
||||
}
|
||||
|
||||
|
||||
// Slew filter with limit
|
||||
|
||||
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)
|
||||
|
|
|
|||
|
|
@ -29,6 +29,19 @@ typedef struct pt1Filter_s {
|
|||
float k;
|
||||
} pt1Filter_t;
|
||||
|
||||
typedef struct pt2Filter_s {
|
||||
float state;
|
||||
float state1;
|
||||
float k;
|
||||
} pt2Filter_t;
|
||||
|
||||
typedef struct pt3Filter_s {
|
||||
float state;
|
||||
float state1;
|
||||
float state2;
|
||||
float k;
|
||||
} pt3Filter_t;
|
||||
|
||||
typedef struct slewFilter_s {
|
||||
float state;
|
||||
float slewLimit;
|
||||
|
|
@ -52,6 +65,8 @@ typedef struct laggedMovingAverage_s {
|
|||
typedef enum {
|
||||
FILTER_PT1 = 0,
|
||||
FILTER_BIQUAD,
|
||||
FILTER_PT2,
|
||||
FILTER_PT3,
|
||||
} lowpassFilterType_e;
|
||||
|
||||
typedef enum {
|
||||
|
|
@ -81,5 +96,15 @@ void pt1FilterInit(pt1Filter_t *filter, float k);
|
|||
void pt1FilterUpdateCutoff(pt1Filter_t *filter, float k);
|
||||
float pt1FilterApply(pt1Filter_t *filter, float input);
|
||||
|
||||
float pt2FilterGain(float f_cut, float dT);
|
||||
void pt2FilterInit(pt2Filter_t *filter, float k);
|
||||
void pt2FilterUpdateCutoff(pt2Filter_t *filter, float k);
|
||||
float pt2FilterApply(pt2Filter_t *filter, float input);
|
||||
|
||||
float pt3FilterGain(float f_cut, float dT);
|
||||
void pt3FilterInit(pt3Filter_t *filter, float k);
|
||||
void pt3FilterUpdateCutoff(pt3Filter_t *filter, float k);
|
||||
float pt3FilterApply(pt3Filter_t *filter, float input);
|
||||
|
||||
void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold);
|
||||
float slewFilterApply(slewFilter_t *filter, float input);
|
||||
|
|
|
|||
|
|
@ -115,13 +115,6 @@ int gcd(int num, int denom)
|
|||
return gcd(denom, num % denom);
|
||||
}
|
||||
|
||||
float powerf(float base, int exp) {
|
||||
float result = base;
|
||||
for (int count = 1; count < exp; count++) result *= base;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
int32_t applyDeadband(const int32_t value, const int32_t deadband)
|
||||
{
|
||||
if (ABS(value) < deadband) {
|
||||
|
|
|
|||
|
|
@ -101,7 +101,6 @@ typedef struct fp_rotationMatrix_s {
|
|||
} fp_rotationMatrix_t;
|
||||
|
||||
int gcd(int num, int denom);
|
||||
float powerf(float base, int exp);
|
||||
int32_t applyDeadband(int32_t value, int32_t deadband);
|
||||
float fapplyDeadband(float value, float deadband);
|
||||
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ static void applySqrt(const sdft_t *sdft, float *data);
|
|||
void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const uint8_t numBatches)
|
||||
{
|
||||
if (!isInitialized) {
|
||||
rPowerN = powerf(SDFT_R, SDFT_SAMPLE_SIZE);
|
||||
rPowerN = powf(SDFT_R, SDFT_SAMPLE_SIZE);
|
||||
const float c = 2.0f * M_PIf / (float)SDFT_SAMPLE_SIZE;
|
||||
float phi = 0.0f;
|
||||
for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) {
|
||||
|
|
|
|||
|
|
@ -183,7 +183,7 @@ typedef struct odrEntry_s {
|
|||
uint8_t odr; // See GYRO_ODR in datasheet.
|
||||
} odrEntry_t;
|
||||
|
||||
odrEntry_t khzToSupportedODRMap[] = {
|
||||
static odrEntry_t icm42605PkhzToSupportedODRMap[] = {
|
||||
{ 8, 3 },
|
||||
{ 4, 4 },
|
||||
{ 2, 5 },
|
||||
|
|
@ -205,9 +205,9 @@ void icm42605GyroInit(gyroDev_t *gyro)
|
|||
if (gyro->gyroRateKHz) {
|
||||
uint8_t gyroSyncDenominator = gyro->mpuDividerDrops + 1; // rebuild it in here, see gyro_sync.c
|
||||
uint8_t desiredODRKhz = 8 / gyroSyncDenominator;
|
||||
for (uint32_t i = 0; i < ARRAYLEN(khzToSupportedODRMap); i++) {
|
||||
if (khzToSupportedODRMap[i].khz == desiredODRKhz) {
|
||||
outputDataRate = khzToSupportedODRMap[i].odr;
|
||||
for (uint32_t i = 0; i < ARRAYLEN(icm42605PkhzToSupportedODRMap); i++) {
|
||||
if (icm42605PkhzToSupportedODRMap[i].khz == desiredODRKhz) {
|
||||
outputDataRate = icm42605PkhzToSupportedODRMap[i].odr;
|
||||
supportedODRFound = true;
|
||||
break;
|
||||
}
|
||||
|
|
@ -219,22 +219,14 @@ void icm42605GyroInit(gyroDev_t *gyro)
|
|||
gyro->gyroRateKHz = GYRO_RATE_1_kHz;
|
||||
}
|
||||
|
||||
uint8_t value;
|
||||
|
||||
STATIC_ASSERT(INV_FSR_2000DPS == 3, "INV_FSR_2000DPS must be 3 to generate correct value");
|
||||
spiBusWriteRegister(&gyro->bus, ICM42605_RA_GYRO_CONFIG0, (3 - INV_FSR_2000DPS) << 5 | (outputDataRate & 0x0F));
|
||||
delay(15);
|
||||
|
||||
value = spiBusReadRegister(&gyro->bus, ICM42605_RA_GYRO_CONFIG0);
|
||||
debug[1] = value;
|
||||
|
||||
STATIC_ASSERT(INV_FSR_16G == 3, "INV_FSR_16G must be 3 to generate correct value");
|
||||
spiBusWriteRegister(&gyro->bus, ICM42605_RA_ACCEL_CONFIG0, (3 - INV_FSR_16G) << 5 | (outputDataRate & 0x0F));
|
||||
delay(15);
|
||||
|
||||
value = spiBusReadRegister(&gyro->bus, ICM42605_RA_ACCEL_CONFIG0);
|
||||
debug[2] = value;
|
||||
|
||||
spiBusWriteRegister(&gyro->bus, ICM42605_RA_GYRO_ACCEL_CONFIG0, ICM42605_ACCEL_UI_FILT_BW_LOW_LATENCY | ICM42605_GYRO_UI_FILT_BW_LOW_LATENCY);
|
||||
|
||||
spiBusWriteRegister(&gyro->bus, ICM42605_RA_INT_CONFIG, ICM42605_INT1_MODE_PULSED | ICM42605_INT1_DRIVE_CIRCUIT_PP | ICM42605_INT1_POLARITY_ACTIVE_HIGH);
|
||||
|
|
|
|||
|
|
@ -20,28 +20,26 @@
|
|||
* BMP388 Driver author: Dominic Clifton
|
||||
*/
|
||||
|
||||
#include <math.h>
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#if defined(USE_BARO) && (defined(USE_BARO_BMP388) || defined(USE_BARO_SPI_BMP388))
|
||||
|
||||
#include "build/build_config.h"
|
||||
|
||||
#include "build/debug.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "barometer.h"
|
||||
|
||||
#include "drivers/barometer/barometer.h"
|
||||
#include "drivers/bus.h"
|
||||
#include "drivers/bus_i2c.h"
|
||||
#include "drivers/bus_i2c_busdev.h"
|
||||
#include "drivers/bus_spi.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/time.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "barometer_bmp388.h"
|
||||
|
||||
|
|
@ -303,8 +301,8 @@ bool bmp388Detect(const bmp388Config_t *config, baroDev_t *baro)
|
|||
|
||||
// See datasheet 3.9.2 "Measurement rate in forced mode and normal mode"
|
||||
baro->up_delay = 234 +
|
||||
(392 + (powerf(2, BMP388_PRESSURE_OSR + 1) * 2000)) +
|
||||
(313 + (powerf(2, BMP388_TEMPERATURE_OSR + 1) * 2000));
|
||||
(392 + (powf(2, BMP388_PRESSURE_OSR + 1) * 2000)) +
|
||||
(313 + (powf(2, BMP388_TEMPERATURE_OSR + 1) * 2000));
|
||||
|
||||
baro->calculate = bmp388Calculate;
|
||||
|
||||
|
|
|
|||
|
|
@ -36,6 +36,8 @@
|
|||
#include "nvic.h"
|
||||
#include "rcc.h"
|
||||
|
||||
#define SPI_TIMEOUT_SYS_TICKS (SPI_TIMEOUT_US / 1000)
|
||||
|
||||
void spiInitDevice(SPIDevice device, bool leadingEdge)
|
||||
{
|
||||
spiDevice_t *spi = &(spiDevice[device]);
|
||||
|
|
@ -93,8 +95,7 @@ void spiInitDevice(SPIDevice device, bool leadingEdge)
|
|||
if (spi->leadingEdge) {
|
||||
spi->hspi.Init.CLKPolarity = SPI_POLARITY_LOW;
|
||||
spi->hspi.Init.CLKPhase = SPI_PHASE_1EDGE;
|
||||
}
|
||||
else {
|
||||
} else {
|
||||
spi->hspi.Init.CLKPolarity = SPI_POLARITY_HIGH;
|
||||
spi->hspi.Init.CLKPhase = SPI_PHASE_2EDGE;
|
||||
}
|
||||
|
|
@ -118,28 +119,27 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t out)
|
|||
bool spiIsBusBusy(SPI_TypeDef *instance)
|
||||
{
|
||||
SPIDevice device = spiDeviceByInstance(instance);
|
||||
if(spiDevice[device].hspi.State == HAL_SPI_STATE_BUSY)
|
||||
if (spiDevice[device].hspi.State == HAL_SPI_STATE_BUSY) {
|
||||
return true;
|
||||
else
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool spiTransfer(SPI_TypeDef *instance, const uint8_t *out, uint8_t *in, int len)
|
||||
{
|
||||
SPIDevice device = spiDeviceByInstance(instance);
|
||||
HAL_StatusTypeDef status;
|
||||
|
||||
#define SPI_DEFAULT_TIMEOUT 10
|
||||
|
||||
if (!in) {
|
||||
// Tx only
|
||||
status = HAL_SPI_Transmit(&spiDevice[device].hspi, out, len, SPI_DEFAULT_TIMEOUT);
|
||||
status = HAL_SPI_Transmit(&spiDevice[device].hspi, out, len, SPI_TIMEOUT_SYS_TICKS);
|
||||
} else if (!out) {
|
||||
// Rx only
|
||||
status = HAL_SPI_Receive(&spiDevice[device].hspi, in, len, SPI_DEFAULT_TIMEOUT);
|
||||
status = HAL_SPI_Receive(&spiDevice[device].hspi, in, len, SPI_TIMEOUT_SYS_TICKS);
|
||||
} else {
|
||||
// Tx and Rx
|
||||
status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, out, in, len, SPI_DEFAULT_TIMEOUT);
|
||||
status = HAL_SPI_TransmitReceive(&spiDevice[device].hspi, out, in, len, SPI_TIMEOUT_SYS_TICKS);
|
||||
}
|
||||
|
||||
if (status != HAL_OK) {
|
||||
|
|
@ -210,8 +210,9 @@ void SPI4_IRQHandler(void)
|
|||
void dmaSPIIRQHandler(dmaChannelDescriptor_t* descriptor)
|
||||
{
|
||||
SPIDevice device = descriptor->userParam;
|
||||
if (device != SPIINVALID)
|
||||
if (device != SPIINVALID) {
|
||||
HAL_DMA_IRQHandler(&spiDevice[device].hdma);
|
||||
}
|
||||
}
|
||||
#endif // USE_DMA
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define SPI_TIMEOUT_US 10000
|
||||
|
||||
#if defined(STM32F1) || defined(STM32F3) || defined(STM32F4) || defined(STM32G4)
|
||||
#define MAX_SPI_PIN_SEL 2
|
||||
#elif defined(STM32F7)
|
||||
|
|
|
|||
|
|
@ -36,6 +36,7 @@
|
|||
#include "drivers/io.h"
|
||||
#include "drivers/nvic.h"
|
||||
#include "drivers/rcc.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#ifndef SPI2_SCK_PIN
|
||||
#define SPI2_NSS_PIN PB12
|
||||
|
|
@ -71,8 +72,6 @@
|
|||
#define SPI4_NSS_PIN NONE
|
||||
#endif
|
||||
|
||||
#define SPI_DEFAULT_TIMEOUT 10
|
||||
|
||||
static LL_SPI_InitTypeDef defaultInit =
|
||||
{
|
||||
.TransferDirection = SPI_DIRECTION_2LINES,
|
||||
|
|
@ -107,10 +106,12 @@ void spiInitDevice(SPIDevice device, bool leadingEdge)
|
|||
IOInit(IOGetByTag(spi->miso), OWNER_SPI_MISO, RESOURCE_INDEX(device));
|
||||
IOInit(IOGetByTag(spi->mosi), OWNER_SPI_MOSI, RESOURCE_INDEX(device));
|
||||
|
||||
if (spi->leadingEdge == true)
|
||||
if (spi->leadingEdge == true) {
|
||||
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_LOW, spi->sckAF);
|
||||
else
|
||||
} else {
|
||||
IOConfigGPIOAF(IOGetByTag(spi->sck), SPI_IO_AF_SCK_CFG_HIGH, spi->sckAF);
|
||||
}
|
||||
|
||||
IOConfigGPIOAF(IOGetByTag(spi->miso), SPI_IO_AF_MISO_CFG, spi->misoAF);
|
||||
IOConfigGPIOAF(IOGetByTag(spi->mosi), SPI_IO_AF_CFG, spi->mosiAF);
|
||||
|
||||
|
|
@ -136,18 +137,21 @@ void spiInitDevice(SPIDevice device, bool leadingEdge)
|
|||
|
||||
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte)
|
||||
{
|
||||
uint16_t spiTimeout = 1000;
|
||||
timeUs_t timeoutStartUs = microsISR();
|
||||
|
||||
while (!LL_SPI_IsActiveFlag_TXE(instance))
|
||||
if ((spiTimeout--) == 0)
|
||||
while (!LL_SPI_IsActiveFlag_TXE(instance)) {
|
||||
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
|
||||
return spiTimeoutUserCallback(instance);
|
||||
|
||||
}
|
||||
}
|
||||
LL_SPI_TransmitData8(instance, txByte);
|
||||
|
||||
spiTimeout = 1000;
|
||||
while (!LL_SPI_IsActiveFlag_RXNE(instance))
|
||||
if ((spiTimeout--) == 0)
|
||||
timeoutStartUs = microsISR();
|
||||
while (!LL_SPI_IsActiveFlag_RXNE(instance)) {
|
||||
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
|
||||
return spiTimeoutUserCallback(instance);
|
||||
}
|
||||
}
|
||||
|
||||
return (uint8_t)LL_SPI_ReceiveData8(instance);
|
||||
}
|
||||
|
|
@ -163,12 +167,14 @@ bool spiIsBusBusy(SPI_TypeDef *instance)
|
|||
|
||||
bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
|
||||
{
|
||||
timeUs_t timeoutStartUs;
|
||||
|
||||
// set 16-bit transfer
|
||||
CLEAR_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD);
|
||||
while (len > 1) {
|
||||
int spiTimeout = 1000;
|
||||
timeoutStartUs = microsISR();
|
||||
while (!LL_SPI_IsActiveFlag_TXE(instance)) {
|
||||
if ((spiTimeout--) == 0) {
|
||||
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
|
||||
return spiTimeoutUserCallback(instance);
|
||||
}
|
||||
}
|
||||
|
|
@ -181,9 +187,9 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData,
|
|||
}
|
||||
LL_SPI_TransmitData16(instance, w);
|
||||
|
||||
spiTimeout = 1000;
|
||||
timeoutStartUs = microsISR();
|
||||
while (!LL_SPI_IsActiveFlag_RXNE(instance)) {
|
||||
if ((spiTimeout--) == 0) {
|
||||
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
|
||||
return spiTimeoutUserCallback(instance);
|
||||
}
|
||||
}
|
||||
|
|
@ -197,18 +203,18 @@ bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData,
|
|||
// set 8-bit transfer
|
||||
SET_BIT(instance->CR2, SPI_RXFIFO_THRESHOLD);
|
||||
if (len) {
|
||||
int spiTimeout = 1000;
|
||||
timeoutStartUs = microsISR();
|
||||
while (!LL_SPI_IsActiveFlag_TXE(instance)) {
|
||||
if ((spiTimeout--) == 0) {
|
||||
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
|
||||
return spiTimeoutUserCallback(instance);
|
||||
}
|
||||
}
|
||||
uint8_t b = txData ? *(txData++) : 0xFF;
|
||||
LL_SPI_TransmitData8(instance, b);
|
||||
|
||||
spiTimeout = 1000;
|
||||
timeoutStartUs = microsISR();
|
||||
while (!LL_SPI_IsActiveFlag_RXNE(instance)) {
|
||||
if ((spiTimeout--) == 0) {
|
||||
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
|
||||
return spiTimeoutUserCallback(instance);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@
|
|||
#include "drivers/exti.h"
|
||||
#include "drivers/io.h"
|
||||
#include "drivers/rcc.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
static SPI_InitTypeDef defaultInit = {
|
||||
.SPI_Mode = SPI_Mode_Master,
|
||||
|
|
@ -104,23 +105,27 @@ void spiInitDevice(SPIDevice device, bool leadingEdge)
|
|||
// return uint8_t value or -1 when failure
|
||||
uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t txByte)
|
||||
{
|
||||
uint16_t spiTimeout = 1000;
|
||||
timeUs_t timeoutStartUs = microsISR();
|
||||
|
||||
DISCARD(instance->DR);
|
||||
|
||||
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET)
|
||||
if ((spiTimeout--) == 0)
|
||||
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) {
|
||||
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
|
||||
return spiTimeoutUserCallback(instance);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef STM32F303xC
|
||||
SPI_SendData8(instance, txByte);
|
||||
#else
|
||||
SPI_I2S_SendData(instance, txByte);
|
||||
#endif
|
||||
spiTimeout = 1000;
|
||||
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET)
|
||||
if ((spiTimeout--) == 0)
|
||||
timeoutStartUs = microsISR();
|
||||
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) {
|
||||
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
|
||||
return spiTimeoutUserCallback(instance);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef STM32F303xC
|
||||
return ((uint8_t)SPI_ReceiveData8(instance));
|
||||
|
|
@ -144,36 +149,40 @@ bool spiIsBusBusy(SPI_TypeDef *instance)
|
|||
|
||||
bool spiTransfer(SPI_TypeDef *instance, const uint8_t *txData, uint8_t *rxData, int len)
|
||||
{
|
||||
uint16_t spiTimeout = 1000;
|
||||
timeUs_t timeoutStartUs;
|
||||
|
||||
uint8_t b;
|
||||
DISCARD(instance->DR);
|
||||
while (len--) {
|
||||
b = txData ? *(txData++) : 0xFF;
|
||||
timeoutStartUs = microsISR();
|
||||
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_TXE) == RESET) {
|
||||
if ((spiTimeout--) == 0)
|
||||
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
|
||||
return spiTimeoutUserCallback(instance);
|
||||
}
|
||||
}
|
||||
#ifdef STM32F303xC
|
||||
SPI_SendData8(instance, b);
|
||||
#else
|
||||
SPI_I2S_SendData(instance, b);
|
||||
#endif
|
||||
|
||||
spiTimeout = 1000;
|
||||
timeoutStartUs = microsISR();
|
||||
|
||||
while (SPI_I2S_GetFlagStatus(instance, SPI_I2S_FLAG_RXNE) == RESET) {
|
||||
if ((spiTimeout--) == 0)
|
||||
if (cmpTimeUs(microsISR(), timeoutStartUs) >= SPI_TIMEOUT_US) {
|
||||
return spiTimeoutUserCallback(instance);
|
||||
}
|
||||
}
|
||||
#ifdef STM32F303xC
|
||||
b = SPI_ReceiveData8(instance);
|
||||
#else
|
||||
b = SPI_I2S_ReceiveData(instance);
|
||||
#endif
|
||||
if (rxData)
|
||||
if (rxData) {
|
||||
*(rxData++) = b;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -89,7 +89,8 @@ struct {
|
|||
{ 0xEF4016, 133, 50, 64, 256 },
|
||||
// Winbond W25Q64
|
||||
// Datasheet: https://www.winbond.com/resource-files/w25q64jv%20spi%20%20%20revc%2006032016%20kms.pdf
|
||||
{ 0xEF4017, 133, 50, 128, 256 },
|
||||
{ 0xEF4017, 133, 50, 128, 256 }, // W25Q64JV-IQ/JQ
|
||||
{ 0xEF7017, 133, 50, 128, 256 }, // W25Q64JV-IM/JM*
|
||||
// Winbond W25Q128
|
||||
// Datasheet: https://www.winbond.com/resource-files/w25q128fv%20rev.l%2008242015.pdf
|
||||
{ 0xEF4018, 104, 50, 256, 256 },
|
||||
|
|
|
|||
|
|
@ -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 // "
|
||||
|
|
|
|||
|
|
@ -47,16 +47,16 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
|
|||
.thrExpo8 = 0,
|
||||
.dynThrPID = 65,
|
||||
.tpa_breakpoint = 1350,
|
||||
.rates_type = RATES_TYPE_BETAFLIGHT,
|
||||
.rcRates[FD_ROLL] = 100,
|
||||
.rcRates[FD_PITCH] = 100,
|
||||
.rcRates[FD_YAW] = 100,
|
||||
.rates_type = RATES_TYPE_ACTUAL,
|
||||
.rcRates[FD_ROLL] = 7,
|
||||
.rcRates[FD_PITCH] = 7,
|
||||
.rcRates[FD_YAW] = 7,
|
||||
.rcExpo[FD_ROLL] = 0,
|
||||
.rcExpo[FD_PITCH] = 0,
|
||||
.rcExpo[FD_YAW] = 0,
|
||||
.rates[FD_ROLL] = 70,
|
||||
.rates[FD_PITCH] = 70,
|
||||
.rates[FD_YAW] = 70,
|
||||
.rates[FD_ROLL] = 67,
|
||||
.rates[FD_PITCH] = 67,
|
||||
.rates[FD_YAW] = 67,
|
||||
.throttle_limit_type = THROTTLE_LIMIT_TYPE_OFF,
|
||||
.throttle_limit_percent = 100,
|
||||
.rate_limit[FD_ROLL] = CONTROL_RATE_CONFIG_RATE_LIMIT_MAX,
|
||||
|
|
|
|||
|
|
@ -101,11 +101,9 @@
|
|||
#include "io/asyncfatfs/asyncfatfs.h"
|
||||
#include "io/beeper.h"
|
||||
#include "io/dashboard.h"
|
||||
#include "io/displayport_crsf.h"
|
||||
#include "io/displayport_frsky_osd.h"
|
||||
#include "io/displayport_max7456.h"
|
||||
#include "io/displayport_msp.h"
|
||||
#include "io/displayport_srxl.h"
|
||||
#include "io/flashfs.h"
|
||||
#include "io/gimbal.h"
|
||||
#include "io/gps.h"
|
||||
|
|
@ -1011,15 +1009,6 @@ void init(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
|
||||
// Register the srxl Textgen telemetry sensor as a displayport device
|
||||
cmsDisplayPortRegister(displayPortSrxlInit());
|
||||
#endif
|
||||
|
||||
#if defined(USE_CMS) && defined(USE_CRSF_CMS_TELEMETRY)
|
||||
cmsDisplayPortRegister(displayPortCrsfInit());
|
||||
#endif
|
||||
|
||||
setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
|
||||
|
||||
#ifdef USE_MOTOR
|
||||
|
|
|
|||
105
src/main/fc/rc.c
105
src/main/fc/rc.c
|
|
@ -27,7 +27,6 @@
|
|||
#include "build/debug.h"
|
||||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/config.h"
|
||||
|
|
@ -61,7 +60,11 @@ typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCom
|
|||
#ifdef USE_INTERPOLATED_SP
|
||||
// Setpoint in degrees/sec before RC-Smoothing is applied
|
||||
static float rawSetpoint[XYZ_AXIS_COUNT];
|
||||
static float oldRcCommand[XYZ_AXIS_COUNT];
|
||||
static bool isDuplicate[XYZ_AXIS_COUNT];
|
||||
float rcCommandDelta[XYZ_AXIS_COUNT];
|
||||
#endif
|
||||
|
||||
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
|
||||
static float throttlePIDAttenuation;
|
||||
static bool reverseMotors = false;
|
||||
|
|
@ -82,7 +85,6 @@ enum {
|
|||
};
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
#define RC_SMOOTHING_IDENTITY_FREQUENCY 80 // Used in the formula to convert a BIQUAD cutoff frequency to PT1
|
||||
#define RC_SMOOTHING_FILTER_STARTUP_DELAY_MS 5000 // Time to wait after power to let the PID loop stabilize before starting average frame rate calculation
|
||||
#define RC_SMOOTHING_FILTER_TRAINING_SAMPLES 50 // Number of rx frame rate samples to average during initial training
|
||||
#define RC_SMOOTHING_FILTER_RETRAINING_SAMPLES 20 // Number of rx frame rate samples to average during frame rate changes
|
||||
|
|
@ -91,7 +93,7 @@ enum {
|
|||
#define RC_SMOOTHING_RX_RATE_CHANGE_PERCENT 20 // Look for samples varying this much from the current detected frame rate to initiate retraining
|
||||
#define RC_SMOOTHING_RX_RATE_MIN_US 1000 // 1ms
|
||||
#define RC_SMOOTHING_RX_RATE_MAX_US 50000 // 50ms or 20hz
|
||||
#define RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_DERIVATIVE_PT1_HZ 100 // The value to use for "auto" when interpolated feedforward is enabled
|
||||
#define RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ 100 // Initial value for "auto" when interpolated feedforward is enabled
|
||||
|
||||
static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
|
||||
#endif // USE_RC_SMOOTHING_FILTER
|
||||
|
|
@ -133,6 +135,11 @@ float getRawSetpoint(int axis)
|
|||
return rawSetpoint[axis];
|
||||
}
|
||||
|
||||
float getRcCommandDelta(int axis)
|
||||
{
|
||||
return rcCommandDelta[axis];
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#define THROTTLE_LOOKUP_LENGTH 12
|
||||
|
|
@ -195,7 +202,7 @@ float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs
|
|||
float applyActualRates(const int axis, float rcCommandf, const float rcCommandfAbs)
|
||||
{
|
||||
float expof = currentControlRateProfile->rcExpo[axis] / 100.0f;
|
||||
expof = rcCommandfAbs * (powerf(rcCommandf, 5) * expof + rcCommandf * (1 - expof));
|
||||
expof = rcCommandfAbs * (powf(rcCommandf, 5) * expof + rcCommandf * (1 - expof));
|
||||
|
||||
const float centerSensitivity = currentControlRateProfile->rcRates[axis] * 10.0f;
|
||||
const float stickMovement = MAX(0, currentControlRateProfile->rates[axis] * 10.0f - centerSensitivity);
|
||||
|
|
@ -395,16 +402,12 @@ uint16_t getCurrentRxRefreshRate(void)
|
|||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
// Determine a cutoff frequency based on filter type and the calculated
|
||||
// average rx frame time
|
||||
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, bool pt1, uint8_t autoSmoothnessFactor)
|
||||
FAST_CODE_NOINLINE int calcRcSmoothingCutoff(int avgRxFrameTimeUs, uint8_t autoSmoothnessFactor)
|
||||
{
|
||||
if (avgRxFrameTimeUs > 0) {
|
||||
const float cutoffFactor = (100 - autoSmoothnessFactor) / 100.0f;
|
||||
float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)) / 2; // calculate the nyquist frequency
|
||||
const float cutoffFactor = 1.5f / (1.0f + (autoSmoothnessFactor / 10.0f));
|
||||
float cutoff = (1 / (avgRxFrameTimeUs * 1e-6f)); // link frequency
|
||||
cutoff = cutoff * cutoffFactor;
|
||||
|
||||
if (pt1) {
|
||||
cutoff = sq(cutoff) / RC_SMOOTHING_IDENTITY_FREQUENCY; // convert to a cutoff for pt1 that has similar characteristics
|
||||
}
|
||||
return lrintf(cutoff);
|
||||
} else {
|
||||
return 0;
|
||||
|
|
@ -426,31 +429,17 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
|
|||
uint16_t oldCutoff = smoothingData->inputCutoffFrequency;
|
||||
|
||||
if (smoothingData->inputCutoffSetting == 0) {
|
||||
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->inputFilterType == RC_SMOOTHING_INPUT_PT1), smoothingData->autoSmoothnessFactor);
|
||||
smoothingData->inputCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor);
|
||||
}
|
||||
|
||||
// initialize or update the input filter
|
||||
if ((smoothingData->inputCutoffFrequency != oldCutoff) || !smoothingData->filterInitialized) {
|
||||
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
|
||||
if ((1 << i) & interpolationChannels) { // only update channels specified by rc_interp_ch
|
||||
switch (smoothingData->inputFilterType) {
|
||||
|
||||
case RC_SMOOTHING_INPUT_PT1:
|
||||
if (!smoothingData->filterInitialized) {
|
||||
pt1FilterInit((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT));
|
||||
pt3FilterInit((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT));
|
||||
} else {
|
||||
pt1FilterUpdateCutoff((pt1Filter_t*) &smoothingData->filter[i], pt1FilterGain(smoothingData->inputCutoffFrequency, dT));
|
||||
}
|
||||
break;
|
||||
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
default:
|
||||
if (!smoothingData->filterInitialized) {
|
||||
biquadFilterInitLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime);
|
||||
} else {
|
||||
biquadFilterUpdateLPF((biquadFilter_t*) &smoothingData->filter[i], smoothingData->inputCutoffFrequency, targetPidLooptime);
|
||||
}
|
||||
break;
|
||||
pt3FilterUpdateCutoff((pt3Filter_t*) &smoothingData->filter[i], pt3FilterGain(smoothingData->inputCutoffFrequency, dT));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -458,15 +447,12 @@ FAST_CODE_NOINLINE void rcSmoothingSetFilterCutoffs(rcSmoothingFilter_t *smoothi
|
|||
|
||||
// update or initialize the derivative filter
|
||||
oldCutoff = smoothingData->derivativeCutoffFrequency;
|
||||
if ((rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)
|
||||
&& (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF)
|
||||
&& (rcSmoothingData.derivativeCutoffSetting == 0)) {
|
||||
|
||||
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, (smoothingData->derivativeFilterType == RC_SMOOTHING_DERIVATIVE_PT1), smoothingData->autoSmoothnessFactor);
|
||||
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
|
||||
smoothingData->derivativeCutoffFrequency = calcRcSmoothingCutoff(smoothingData->averageFrameTimeUs, smoothingData->autoSmoothnessFactor);
|
||||
}
|
||||
|
||||
if (!smoothingData->filterInitialized) {
|
||||
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis, smoothingData->derivativeFilterType);
|
||||
pidInitSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency, smoothingData->debugAxis);
|
||||
} else if (smoothingData->derivativeCutoffFrequency != oldCutoff) {
|
||||
pidUpdateSetpointDerivativeLpf(smoothingData->derivativeCutoffFrequency);
|
||||
}
|
||||
|
|
@ -505,14 +491,7 @@ static FAST_CODE bool rcSmoothingAccumulateSample(rcSmoothingFilter_t *smoothing
|
|||
FAST_CODE_NOINLINE bool rcSmoothingAutoCalculate(void)
|
||||
{
|
||||
// if the input cutoff is 0 (auto) then we need to calculate cutoffs
|
||||
if (rcSmoothingData.inputCutoffSetting == 0) {
|
||||
return true;
|
||||
}
|
||||
|
||||
// if the derivative type isn't OFF, and the cutoff is 0, and interpolated feedforward is not enabled then we need to calculate
|
||||
if ((rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF)
|
||||
&& (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF)
|
||||
&& (rcSmoothingData.derivativeCutoffSetting == 0)) {
|
||||
if ((rcSmoothingData.inputCutoffSetting == 0) || (rcSmoothingData.derivativeCutoffSetting == 0)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
|
@ -533,40 +512,20 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
|
|||
rcSmoothingData.averageFrameTimeUs = 0;
|
||||
rcSmoothingData.autoSmoothnessFactor = rxConfig()->rc_smoothing_auto_factor;
|
||||
rcSmoothingData.debugAxis = rxConfig()->rc_smoothing_debug_axis;
|
||||
rcSmoothingData.inputFilterType = rxConfig()->rc_smoothing_input_type;
|
||||
rcSmoothingData.inputCutoffSetting = rxConfig()->rc_smoothing_input_cutoff;
|
||||
|
||||
rcSmoothingData.derivativeFilterTypeSetting = rxConfig()->rc_smoothing_derivative_type;
|
||||
if (rxConfig()->rc_smoothing_derivative_type == RC_SMOOTHING_DERIVATIVE_AUTO) {
|
||||
// for derivative filter type "AUTO" set to BIQUAD for classic FF and PT1 for interpolated FF
|
||||
if (currentPidProfile->ff_interpolate_sp == FF_INTERPOLATE_OFF) {
|
||||
rcSmoothingData.derivativeFilterType = RC_SMOOTHING_DERIVATIVE_BIQUAD;
|
||||
} else {
|
||||
rcSmoothingData.derivativeFilterType = RC_SMOOTHING_DERIVATIVE_PT1;
|
||||
}
|
||||
} else {
|
||||
rcSmoothingData.derivativeFilterType = rxConfig()->rc_smoothing_derivative_type;
|
||||
}
|
||||
|
||||
rcSmoothingData.derivativeCutoffSetting = rxConfig()->rc_smoothing_derivative_cutoff;
|
||||
rcSmoothingResetAccumulation(&rcSmoothingData);
|
||||
|
||||
rcSmoothingData.inputCutoffFrequency = rcSmoothingData.inputCutoffSetting;
|
||||
|
||||
if (rcSmoothingData.derivativeFilterType != RC_SMOOTHING_DERIVATIVE_OFF) {
|
||||
if ((currentPidProfile->ff_interpolate_sp != FF_INTERPOLATE_OFF) && (rcSmoothingData.derivativeCutoffSetting == 0)) {
|
||||
// calculate the fixed derivative cutoff used for interpolated feedforward
|
||||
const float cutoffFactor = (100 - rcSmoothingData.autoSmoothnessFactor) / 100.0f;
|
||||
float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_DERIVATIVE_PT1_HZ * cutoffFactor; // PT1 cutoff frequency
|
||||
if (rcSmoothingData.derivativeFilterType == RC_SMOOTHING_DERIVATIVE_BIQUAD) {
|
||||
// convert to an equivalent BIQUAD cutoff
|
||||
derivativeCutoff = sqrt(derivativeCutoff * RC_SMOOTHING_IDENTITY_FREQUENCY);
|
||||
}
|
||||
// calculate the initial derivative cutoff used for interpolated feedforward until RC interval is known
|
||||
const float cutoffFactor = 1.5f / (1.0f + (rcSmoothingData.autoSmoothnessFactor / 10.0f));
|
||||
float derivativeCutoff = RC_SMOOTHING_INTERPOLATED_FEEDFORWARD_INITIAL_HZ * cutoffFactor; // PT1 cutoff frequency
|
||||
rcSmoothingData.derivativeCutoffFrequency = lrintf(derivativeCutoff);
|
||||
} else {
|
||||
rcSmoothingData.derivativeCutoffFrequency = rcSmoothingData.derivativeCutoffSetting;
|
||||
}
|
||||
}
|
||||
|
||||
calculateCutoffs = rcSmoothingAutoCalculate();
|
||||
|
||||
|
|
@ -658,16 +617,7 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
|
|||
for (updatedChannel = 0; updatedChannel < PRIMARY_CHANNEL_COUNT; updatedChannel++) {
|
||||
if ((1 << updatedChannel) & interpolationChannels) { // only smooth selected channels base on the rc_interp_ch value
|
||||
if (rcSmoothingData.filterInitialized) {
|
||||
switch (rcSmoothingData.inputFilterType) {
|
||||
case RC_SMOOTHING_INPUT_PT1:
|
||||
rcCommand[updatedChannel] = pt1FilterApply((pt1Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
|
||||
break;
|
||||
|
||||
case RC_SMOOTHING_INPUT_BIQUAD:
|
||||
default:
|
||||
rcCommand[updatedChannel] = biquadFilterApplyDF1((biquadFilter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
|
||||
break;
|
||||
}
|
||||
rcCommand[updatedChannel] = pt3FilterApply((pt3Filter_t*) &rcSmoothingData.filter[updatedChannel], lastRxData[updatedChannel]);
|
||||
} else {
|
||||
// If filter isn't initialized yet then use the actual unsmoothed rx channel data
|
||||
rcCommand[updatedChannel] = lastRxData[updatedChannel];
|
||||
|
|
@ -694,6 +644,9 @@ FAST_CODE void processRcCommand(void)
|
|||
#ifdef USE_INTERPOLATED_SP
|
||||
if (isRxDataNew) {
|
||||
for (int i = FD_ROLL; i <= FD_YAW; i++) {
|
||||
isDuplicate[i] = (oldRcCommand[i] == rcCommand[i]);
|
||||
rcCommandDelta[i] = fabsf(rcCommand[i] - oldRcCommand[i]);
|
||||
oldRcCommand[i] = rcCommand[i];
|
||||
float rcCommandf;
|
||||
if (i == FD_YAW) {
|
||||
rcCommandf = rcCommand[i] / rcCommandYawDivider;
|
||||
|
|
@ -763,7 +716,7 @@ FAST_CODE_NOINLINE void updateRcCommands(void)
|
|||
for (int axis = 0; axis < 3; axis++) {
|
||||
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2.
|
||||
|
||||
int32_t tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
|
||||
float tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500);
|
||||
if (axis == ROLL || axis == PITCH) {
|
||||
if (tmp > rcControlsConfig()->deadband) {
|
||||
tmp -= rcControlsConfig()->deadband;
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ typedef enum {
|
|||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
#define RC_SMOOTHING_AUTO_FACTOR_MIN 0
|
||||
#define RC_SMOOTHING_AUTO_FACTOR_MAX 50
|
||||
#define RC_SMOOTHING_AUTO_FACTOR_MAX 250
|
||||
#endif
|
||||
|
||||
void processRcCommand(void);
|
||||
|
|
@ -51,6 +51,7 @@ rcSmoothingFilter_t *getRcSmoothingData(void);
|
|||
bool rcSmoothingAutoCalculate(void);
|
||||
bool rcSmoothingInitializationComplete(void);
|
||||
float getRawSetpoint(int axis);
|
||||
float getRcCommandDelta(int axis);
|
||||
float applyCurve(int axis, float deflection);
|
||||
bool getShouldUpdateFf();
|
||||
void updateRcRefreshRate(timeUs_t currentTimeUs);
|
||||
|
|
|
|||
|
|
@ -70,18 +70,6 @@ typedef enum {
|
|||
RC_SMOOTHING_TYPE_FILTER
|
||||
} rcSmoothingType_e;
|
||||
|
||||
typedef enum {
|
||||
RC_SMOOTHING_INPUT_PT1,
|
||||
RC_SMOOTHING_INPUT_BIQUAD
|
||||
} rcSmoothingInputFilter_e;
|
||||
|
||||
typedef enum {
|
||||
RC_SMOOTHING_DERIVATIVE_OFF,
|
||||
RC_SMOOTHING_DERIVATIVE_PT1,
|
||||
RC_SMOOTHING_DERIVATIVE_BIQUAD,
|
||||
RC_SMOOTHING_DERIVATIVE_AUTO,
|
||||
} rcSmoothingDerivativeFilter_e;
|
||||
|
||||
#define ROL_LO (1 << (2 * ROLL))
|
||||
#define ROL_CE (3 << (2 * ROLL))
|
||||
#define ROL_HI (2 << (2 * ROLL))
|
||||
|
|
@ -116,19 +104,11 @@ typedef struct rcSmoothingFilterTraining_s {
|
|||
uint16_t max;
|
||||
} rcSmoothingFilterTraining_t;
|
||||
|
||||
typedef union rcSmoothingFilterTypes_u {
|
||||
pt1Filter_t pt1Filter;
|
||||
biquadFilter_t biquadFilter;
|
||||
} rcSmoothingFilterTypes_t;
|
||||
|
||||
typedef struct rcSmoothingFilter_s {
|
||||
bool filterInitialized;
|
||||
rcSmoothingFilterTypes_t filter[4];
|
||||
rcSmoothingInputFilter_e inputFilterType;
|
||||
pt3Filter_t filter[4];
|
||||
uint8_t inputCutoffSetting;
|
||||
uint16_t inputCutoffFrequency;
|
||||
rcSmoothingDerivativeFilter_e derivativeFilterTypeSetting;
|
||||
rcSmoothingDerivativeFilter_e derivativeFilterType;
|
||||
uint8_t derivativeCutoffSetting;
|
||||
uint16_t derivativeCutoffFrequency;
|
||||
int averageFrameTimeUs;
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -179,6 +179,8 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
|
|||
case PROCESS:
|
||||
ignoreTaskTime();
|
||||
if (!processRx(currentTimeUs)) {
|
||||
rxState = CHECK;
|
||||
|
||||
break;
|
||||
}
|
||||
rxState = MODES;
|
||||
|
|
|
|||
|
|
@ -43,15 +43,12 @@ typedef struct laggedMovingAverageCombined_s {
|
|||
|
||||
laggedMovingAverageCombined_t setpointDeltaAvg[XYZ_AXIS_COUNT];
|
||||
|
||||
static float prevSetpointSpeed[XYZ_AXIS_COUNT];
|
||||
static float prevAcceleration[XYZ_AXIS_COUNT];
|
||||
static float prevRawSetpoint[XYZ_AXIS_COUNT];
|
||||
//for smoothing
|
||||
static float prevDeltaImpl[XYZ_AXIS_COUNT];
|
||||
static float prevBoostAmount[XYZ_AXIS_COUNT];
|
||||
static float prevSetpoint[XYZ_AXIS_COUNT]; // equals raw unless interpolated
|
||||
static float prevSetpointSpeed[XYZ_AXIS_COUNT]; // equals raw unless interpolated
|
||||
static float prevAcceleration[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation
|
||||
static float prevRcCommandDelta[XYZ_AXIS_COUNT]; // for accurate duplicate interpolation
|
||||
|
||||
static uint8_t ffStatus[XYZ_AXIS_COUNT];
|
||||
static bool bigStep[XYZ_AXIS_COUNT];
|
||||
static bool prevDuplicatePacket[XYZ_AXIS_COUNT]; // to identify multiple identical packets
|
||||
static uint8_t averagingCount;
|
||||
|
||||
static float ffMaxRateLimit[XYZ_AXIS_COUNT];
|
||||
|
|
@ -70,112 +67,86 @@ void interpolatedSpInit(const pidProfile_t *pidProfile) {
|
|||
FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterpolationType_t type) {
|
||||
|
||||
if (newRcFrame) {
|
||||
float rawSetpoint = getRawSetpoint(axis);
|
||||
float absRawSetpoint = fabsf(rawSetpoint);
|
||||
float rcCommandDelta = getRcCommandDelta(axis);
|
||||
float setpoint = getRawSetpoint(axis);
|
||||
const float rxInterval = getCurrentRxRefreshRate() * 1e-6f;
|
||||
const float rxRate = 1.0f / rxInterval;
|
||||
float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
|
||||
float absSetpointSpeed = fabsf(setpointSpeed);
|
||||
float setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
|
||||
float absPrevSetpointSpeed = fabsf(prevSetpointSpeed[axis]);
|
||||
float setpointAccelerationModifier = 1.0f;
|
||||
float setpointAcceleration = 0.0f;
|
||||
const float ffSmoothFactor = pidGetFfSmoothFactor();
|
||||
const float ffJitterFactor = pidGetFfJitterFactor();
|
||||
|
||||
if (setpointSpeed == 0 && absRawSetpoint < 0.98f * ffMaxRate[axis]) {
|
||||
// no movement, or sticks at max; ffStatus set
|
||||
// the max stick check is needed to prevent interpolation when arriving at max sticks
|
||||
if (prevSetpointSpeed[axis] == 0) {
|
||||
// no movement on two packets in a row
|
||||
// do nothing now, but may use status = 3 to smooth following packet
|
||||
ffStatus[axis] = 3;
|
||||
// calculate an attenuator from average of two most recent rcCommand deltas vs jitter threshold
|
||||
float ffAttenuator = 1.0f;
|
||||
if (ffJitterFactor) {
|
||||
if (rcCommandDelta < ffJitterFactor) {
|
||||
ffAttenuator = MAX(1.0f - ((rcCommandDelta + prevRcCommandDelta[axis]) / 2.0f) / ffJitterFactor, 0.0f);
|
||||
ffAttenuator = 1.0f - ffAttenuator * ffAttenuator;
|
||||
}
|
||||
}
|
||||
|
||||
// interpolate setpoint if necessary
|
||||
if (rcCommandDelta == 0.0f) {
|
||||
if (prevDuplicatePacket[axis] == false && fabsf(setpoint) < 0.98f * ffMaxRate[axis]) {
|
||||
// first duplicate after movement
|
||||
// interpolate rawSetpoint by adding (speed + acceleration) * attenuator to previous setpoint
|
||||
setpoint = prevSetpoint[axis] + (prevSetpointSpeed[axis] + prevAcceleration[axis]) * ffAttenuator * rxInterval;
|
||||
// recalculate setpointSpeed and (later) acceleration from this new setpoint value
|
||||
setpointSpeed = (setpoint - prevSetpoint[axis]) * rxRate;
|
||||
}
|
||||
prevDuplicatePacket[axis] = true;
|
||||
} else {
|
||||
// there was movement on previous packet, now none
|
||||
if (bigStep[axis] == true) {
|
||||
// previous movement was big; likely an early FrSky packet
|
||||
// don't project these forward or we get a sustained large spike
|
||||
ffStatus[axis] = 2;
|
||||
} else {
|
||||
// likely a dropped packet
|
||||
// interpolate forward using previous setpoint speed and acceleration
|
||||
setpointSpeed = prevSetpointSpeed[axis] + prevAcceleration[axis];
|
||||
// use status = 1 to halve the step for the next packet
|
||||
ffStatus[axis] = 1;
|
||||
// movement!
|
||||
if (prevDuplicatePacket[axis] == true) {
|
||||
// don't boost the packet after a duplicate, the FF alone is enough, usually
|
||||
// in part because after a duplicate, the raw up-step is large, so the jitter attenuator is less active
|
||||
ffAttenuator = 0.0f;
|
||||
}
|
||||
prevDuplicatePacket[axis] = false;
|
||||
}
|
||||
} else {
|
||||
// we have movement; let's consider what happened on previous packets, using ffStatus
|
||||
if (ffStatus[axis] != 0) {
|
||||
if (ffStatus[axis] == 1) {
|
||||
// was interpolated forward after previous dropped packet after small step
|
||||
// this step is likely twice as tall as it should be
|
||||
setpointSpeed = setpointSpeed / 2.0f;
|
||||
} else if (ffStatus[axis] == 2) {
|
||||
// we are doing nothing for these to avoid exaggerating the FrSky early packet problem
|
||||
} else if (ffStatus[axis] == 3) {
|
||||
// movement after nothing on previous two packets
|
||||
// reduce boost when higher averaging is used to improve slow stick smoothness
|
||||
setpointAccelerationModifier /= (averagingCount + 1);
|
||||
}
|
||||
ffStatus[axis] = 0;
|
||||
// all is normal
|
||||
}
|
||||
prevSetpoint[axis] = setpoint;
|
||||
|
||||
if (axis == FD_ROLL) {
|
||||
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(setpoint)); // setpoint after interpolations
|
||||
}
|
||||
|
||||
float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
|
||||
float absSetpointSpeed = fabsf(setpointSpeed); // unsmoothed for kick prevention
|
||||
|
||||
// determine if this step was a relatively large one, to use when evaluating next packet
|
||||
// calculate acceleration, smooth and attenuate it
|
||||
setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];
|
||||
setpointAcceleration = prevAcceleration[axis] + ffSmoothFactor * (setpointAcceleration - prevAcceleration[axis]);
|
||||
setpointAcceleration *= ffAttenuator;
|
||||
|
||||
if (absSetpointSpeed > 1.5f * absPrevSetpointSpeed || absPrevSetpointSpeed > 1.5f * absSetpointSpeed){
|
||||
bigStep[axis] = true;
|
||||
} else {
|
||||
bigStep[axis] = false;
|
||||
}
|
||||
|
||||
// smooth deadband type suppression of FF jitter when sticks are at or returning to centre
|
||||
// only when ff_averaging is 3 or more, for HD or cinematic flying
|
||||
if (averagingCount > 2) {
|
||||
const float rawSetpointCentred = absRawSetpoint / averagingCount;
|
||||
if (rawSetpointCentred < 1.0f) {
|
||||
setpointSpeed *= rawSetpointCentred;
|
||||
setpointAcceleration *= rawSetpointCentred;
|
||||
}
|
||||
}
|
||||
// smooth setpointSpeed but don't attenuate
|
||||
setpointSpeed = prevSetpointSpeed[axis] + ffSmoothFactor * (setpointSpeed - prevSetpointSpeed[axis]);
|
||||
|
||||
prevSetpointSpeed[axis] = setpointSpeed;
|
||||
prevAcceleration[axis] = setpointAcceleration;
|
||||
prevRcCommandDelta[axis] = rcCommandDelta;
|
||||
|
||||
// all values afterwards are small numbers
|
||||
setpointAcceleration *= pidGetDT();
|
||||
setpointDeltaImpl[axis] = setpointSpeed * pidGetDT();
|
||||
|
||||
|
||||
// calculate boost and prevent kick-back spike at max deflection
|
||||
const float ffBoostFactor = pidGetFfBoostFactor();
|
||||
float boostAmount = 0.0f;
|
||||
if (ffBoostFactor != 0.0f) {
|
||||
// calculate boost and prevent kick-back spike at max deflection
|
||||
if (fabsf(rawSetpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
|
||||
boostAmount = ffBoostFactor * setpointAcceleration * setpointAccelerationModifier;
|
||||
if (ffBoostFactor) {
|
||||
if (fabsf(setpoint) < 0.95f * ffMaxRate[axis] || absSetpointSpeed > 3.0f * absPrevSetpointSpeed) {
|
||||
boostAmount = ffBoostFactor * setpointAcceleration;
|
||||
}
|
||||
}
|
||||
|
||||
prevSetpointSpeed[axis] = setpointSpeed;
|
||||
prevRawSetpoint[axis] = rawSetpoint;
|
||||
|
||||
if (axis == FD_ROLL) {
|
||||
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100));
|
||||
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(setpointAcceleration * 100));
|
||||
DEBUG_SET(DEBUG_FF_INTERPOLATED, 2, lrintf(((setpointDeltaImpl[axis] + boostAmount) * 100)));
|
||||
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, ffStatus[axis]);
|
||||
DEBUG_SET(DEBUG_FF_INTERPOLATED, 0, lrintf(setpointDeltaImpl[axis] * 100.0f)); // base FF
|
||||
DEBUG_SET(DEBUG_FF_INTERPOLATED, 1, lrintf(boostAmount * 100.0f)); // boost amount
|
||||
// debug 2 is interpolated setpoint, above
|
||||
DEBUG_SET(DEBUG_FF_INTERPOLATED, 3, lrintf(rcCommandDelta * 100.0f)); // rcCommand packet difference
|
||||
}
|
||||
|
||||
// first order smoothing of boost to reduce jitter
|
||||
const float ffSmoothFactor = pidGetFfSmoothFactor();
|
||||
boostAmount = prevBoostAmount[axis] + ffSmoothFactor * (boostAmount - prevBoostAmount[axis]);
|
||||
prevBoostAmount[axis] = boostAmount;
|
||||
|
||||
// add boost to base feed forward
|
||||
setpointDeltaImpl[axis] += boostAmount;
|
||||
|
||||
// first order smoothing of FF (second order boost filtering since boost filtered twice)
|
||||
setpointDeltaImpl[axis] = prevDeltaImpl[axis] + ffSmoothFactor * (setpointDeltaImpl[axis] - prevDeltaImpl[axis]);
|
||||
prevDeltaImpl[axis] = setpointDeltaImpl[axis];
|
||||
|
||||
// apply averaging
|
||||
if (type == FF_INTERPOLATE_ON) {
|
||||
setpointDelta[axis] = setpointDeltaImpl[axis];
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "config/config_reset.h"
|
||||
#include "config/simplified_tuning.h"
|
||||
|
|
@ -206,6 +205,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
.ff_interpolate_sp = FF_INTERPOLATE_ON,
|
||||
.ff_max_rate_limit = 100,
|
||||
.ff_smooth_factor = 37,
|
||||
.ff_jitter_factor = 7,
|
||||
.ff_boost = 15,
|
||||
.dyn_lpf_curve_expo = 5,
|
||||
.level_race_mode = false,
|
||||
|
|
@ -266,6 +266,11 @@ float pidGetFfSmoothFactor()
|
|||
{
|
||||
return pidRuntime.ffSmoothFactor;
|
||||
}
|
||||
|
||||
float pidGetFfJitterFactor()
|
||||
{
|
||||
return pidRuntime.ffJitterFactor;
|
||||
}
|
||||
#endif
|
||||
|
||||
void pidResetIterm(void)
|
||||
|
|
@ -315,7 +320,7 @@ float pidCompensateThrustLinearization(float throttle)
|
|||
if (pidRuntime.thrustLinearization != 0.0f) {
|
||||
// for whoops where a lot of TL is needed, allow more throttle boost
|
||||
const float throttleReversed = (1.0f - throttle);
|
||||
throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powerf(throttleReversed, 2);
|
||||
throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powf(throttleReversed, 2);
|
||||
}
|
||||
return throttle;
|
||||
}
|
||||
|
|
@ -325,7 +330,7 @@ float pidApplyThrustLinearization(float motorOutput)
|
|||
if (pidRuntime.thrustLinearization != 0.0f) {
|
||||
if (motorOutput > 0.0f) {
|
||||
const float motorOutputReversed = (1.0f - motorOutput);
|
||||
motorOutput *= 1.0f + powerf(motorOutputReversed, 2) * pidRuntime.thrustLinearization;
|
||||
motorOutput *= 1.0f + powf(motorOutputReversed, 2) * pidRuntime.thrustLinearization;
|
||||
}
|
||||
}
|
||||
return motorOutput;
|
||||
|
|
@ -638,14 +643,7 @@ float FAST_CODE applyRcSmoothingDerivativeFilter(int axis, float pidSetpointDelt
|
|||
DEBUG_SET(DEBUG_RC_SMOOTHING, 1, lrintf(pidSetpointDelta * 100.0f));
|
||||
}
|
||||
if (pidRuntime.setpointDerivativeLpfInitialized) {
|
||||
switch (pidRuntime.rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
ret = pt1FilterApply(&pidRuntime.setpointDerivativePt1[axis], pidSetpointDelta);
|
||||
break;
|
||||
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
|
||||
ret = biquadFilterApplyDF1(&pidRuntime.setpointDerivativeBiquad[axis], pidSetpointDelta);
|
||||
break;
|
||||
}
|
||||
ret = pt3FilterApply(&pidRuntime.setpointDerivativePt3[axis], pidSetpointDelta);
|
||||
if (axis == pidRuntime.rcSmoothingDebugAxis) {
|
||||
DEBUG_SET(DEBUG_RC_SMOOTHING, 2, lrintf(ret * 100.0f));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -210,6 +210,7 @@ typedef struct pidProfile_s {
|
|||
uint8_t ff_interpolate_sp; // Calculate FF from interpolated setpoint
|
||||
uint8_t ff_max_rate_limit; // Maximum setpoint rate percentage for FF
|
||||
uint8_t ff_smooth_factor; // Amount of smoothing for interpolated FF steps
|
||||
uint8_t ff_jitter_factor; // Number of RC steps below which to attenuate FF
|
||||
uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter
|
||||
uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode
|
||||
uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount
|
||||
|
|
@ -340,8 +341,7 @@ typedef struct pidRuntime_s {
|
|||
#endif
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
pt1Filter_t setpointDerivativePt1[XYZ_AXIS_COUNT];
|
||||
biquadFilter_t setpointDerivativeBiquad[XYZ_AXIS_COUNT];
|
||||
pt3Filter_t setpointDerivativePt3[XYZ_AXIS_COUNT];
|
||||
bool setpointDerivativeLpfInitialized;
|
||||
uint8_t rcSmoothingDebugAxis;
|
||||
uint8_t rcSmoothingFilterType;
|
||||
|
|
@ -386,6 +386,7 @@ typedef struct pidRuntime_s {
|
|||
#ifdef USE_INTERPOLATED_SP
|
||||
ffInterpolationType_t ffFromInterpolatedSetpoint;
|
||||
float ffSmoothFactor;
|
||||
float ffJitterFactor;
|
||||
#endif
|
||||
} pidRuntime_t;
|
||||
|
||||
|
|
@ -440,4 +441,5 @@ float pidGetDT();
|
|||
float pidGetPidFrequency();
|
||||
float pidGetFfBoostFactor();
|
||||
float pidGetFfSmoothFactor();
|
||||
float pidGetFfJitterFactor();
|
||||
float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo);
|
||||
|
|
|
|||
|
|
@ -30,7 +30,6 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
#include "drivers/dshot_command.h"
|
||||
|
||||
|
|
@ -222,37 +221,22 @@ void pidInit(const pidProfile_t *pidProfile)
|
|||
}
|
||||
|
||||
#ifdef USE_RC_SMOOTHING_FILTER
|
||||
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType)
|
||||
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis)
|
||||
{
|
||||
pidRuntime.rcSmoothingDebugAxis = debugAxis;
|
||||
pidRuntime.rcSmoothingFilterType = filterType;
|
||||
if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
||||
if (filterCutoff > 0) {
|
||||
pidRuntime.setpointDerivativeLpfInitialized = true;
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
switch (pidRuntime.rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
pt1FilterInit(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT));
|
||||
break;
|
||||
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
|
||||
biquadFilterInitLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
|
||||
break;
|
||||
}
|
||||
pt3FilterInit(&pidRuntime.setpointDerivativePt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff)
|
||||
{
|
||||
if ((filterCutoff > 0) && (pidRuntime.rcSmoothingFilterType != RC_SMOOTHING_DERIVATIVE_OFF)) {
|
||||
if (filterCutoff > 0) {
|
||||
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
|
||||
switch (pidRuntime.rcSmoothingFilterType) {
|
||||
case RC_SMOOTHING_DERIVATIVE_PT1:
|
||||
pt1FilterUpdateCutoff(&pidRuntime.setpointDerivativePt1[axis], pt1FilterGain(filterCutoff, pidRuntime.dT));
|
||||
break;
|
||||
case RC_SMOOTHING_DERIVATIVE_BIQUAD:
|
||||
biquadFilterUpdateLPF(&pidRuntime.setpointDerivativeBiquad[axis], filterCutoff, targetPidLooptime);
|
||||
break;
|
||||
}
|
||||
pt3FilterUpdateCutoff(&pidRuntime.setpointDerivativePt3[axis], pt3FilterGain(filterCutoff, pidRuntime.dT));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -378,7 +362,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
|
||||
#ifdef USE_THRUST_LINEARIZATION
|
||||
pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f;
|
||||
pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powerf(pidRuntime.thrustLinearization, 2);
|
||||
pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powf(pidRuntime.thrustLinearization, 2);
|
||||
#endif
|
||||
|
||||
#if defined(USE_D_MIN)
|
||||
|
|
@ -401,7 +385,13 @@ void pidInitConfig(const pidProfile_t *pidProfile)
|
|||
|
||||
#ifdef USE_INTERPOLATED_SP
|
||||
pidRuntime.ffFromInterpolatedSetpoint = pidProfile->ff_interpolate_sp;
|
||||
if (pidProfile->ff_smooth_factor) {
|
||||
pidRuntime.ffSmoothFactor = 1.0f - ((float)pidProfile->ff_smooth_factor) / 100.0f;
|
||||
} else {
|
||||
// set automatically according to boost amount, limit to 0.5 for auto
|
||||
pidRuntime.ffSmoothFactor = MAX(0.5f, 1.0f - ((float)pidProfile->ff_boost) * 2.0f / 100.0f);
|
||||
}
|
||||
pidRuntime.ffJitterFactor = pidProfile->ff_jitter_factor;
|
||||
interpolatedSpInit(pidProfile);
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -24,6 +24,6 @@ void pidInit(const pidProfile_t *pidProfile);
|
|||
void pidInitFilters(const pidProfile_t *pidProfile);
|
||||
void pidInitConfig(const pidProfile_t *pidProfile);
|
||||
void pidSetItermAccelerator(float newItermAccelerator);
|
||||
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
|
||||
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis);
|
||||
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
|
||||
void pidCopyProfile(uint8_t dstPidProfileIndex, uint8_t srcPidProfileIndex);
|
||||
|
|
|
|||
|
|
@ -31,14 +31,10 @@
|
|||
#include "common/printf.h"
|
||||
#include "common/time.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
|
||||
#include "drivers/display.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "io/displayport_crsf.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "displayport_crsf.h"
|
||||
|
||||
#define CRSF_DISPLAY_PORT_OPEN_DELAY_MS 400
|
||||
#define CRSF_DISPLAY_PORT_CLEAR_DELAY_MS 45
|
||||
|
|
@ -203,18 +199,16 @@ bool crsfDisplayPortIsReady(void)
|
|||
return (bool)(delayExpired && cmsReady);
|
||||
}
|
||||
|
||||
displayPort_t *displayPortCrsfInit()
|
||||
static displayPort_t *displayPortCrsfInit()
|
||||
{
|
||||
if (featureIsEnabled(FEATURE_TELEMETRY)
|
||||
&& featureIsEnabled(FEATURE_RX_SERIAL)
|
||||
&& (rxConfig()->serialrx_provider == SERIALRX_CRSF)) {
|
||||
|
||||
crsfDisplayPortSetDimensions(CRSF_DISPLAY_PORT_ROWS_MAX, CRSF_DISPLAY_PORT_COLS_MAX);
|
||||
displayInit(&crsfDisplayPort, &crsfDisplayPortVTable, DISPLAYPORT_DEVICE_TYPE_CRSF);
|
||||
|
||||
return &crsfDisplayPort;
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
void crsfDisplayportRegister(void)
|
||||
{
|
||||
cmsDisplayPortRegister(displayPortCrsfInit());
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ typedef struct crsfDisplayPortScreen_s {
|
|||
bool reset;
|
||||
} crsfDisplayPortScreen_t;
|
||||
|
||||
displayPort_t *displayPortCrsfInit(void);
|
||||
void crsfDisplayportRegister(void);
|
||||
crsfDisplayPortScreen_t *crsfDisplayPortScreen(void);
|
||||
void crsfDisplayPortMenuOpen(void);
|
||||
void crsfDisplayPortMenuExit(void);
|
||||
|
|
|
|||
|
|
@ -128,7 +128,7 @@ static const displayPortVTable_t hottVTable = {
|
|||
.layerCopy = NULL,
|
||||
};
|
||||
|
||||
displayPort_t *displayPortHottInit()
|
||||
static displayPort_t *displayPortHottInit()
|
||||
{
|
||||
hottDisplayPort.device = NULL;
|
||||
displayInit(&hottDisplayPort, &hottVTable, DISPLAYPORT_DEVICE_TYPE_HOTT);
|
||||
|
|
|
|||
|
|
@ -21,10 +21,6 @@
|
|||
#pragma once
|
||||
#include "drivers/display.h"
|
||||
|
||||
displayPort_t *displayPortHottInit();
|
||||
|
||||
extern displayPort_t hottDisplayPort;
|
||||
|
||||
void hottDisplayportRegister();
|
||||
void hottCmsOpen();
|
||||
void hottSetCmsKey(uint8_t hottKey, bool esc);
|
||||
|
|
|
|||
|
|
@ -23,20 +23,19 @@
|
|||
#include <string.h>
|
||||
|
||||
#include "platform.h"
|
||||
#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) && defined(USE_TELEMETRY_SRXL)
|
||||
|
||||
#if defined(USE_SPEKTRUM_CMS_TELEMETRY)
|
||||
|
||||
#include "cms/cms.h"
|
||||
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
|
||||
#include "drivers/display.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
|
||||
#include "telemetry/srxl.h"
|
||||
|
||||
#include "displayport_srxl.h"
|
||||
|
||||
displayPort_t srxlDisplayPort;
|
||||
|
||||
static int srxlDrawScreen(displayPort_t *displayPort)
|
||||
|
|
@ -143,20 +142,18 @@ static const displayPortVTable_t srxlVTable = {
|
|||
.layerCopy = NULL,
|
||||
};
|
||||
|
||||
displayPort_t *displayPortSrxlInit()
|
||||
static displayPort_t *displayPortSrxlInit()
|
||||
{
|
||||
if (featureIsEnabled(FEATURE_TELEMETRY)
|
||||
&& featureIsEnabled(FEATURE_RX_SERIAL)
|
||||
&& ((rxConfig()->serialrx_provider == SERIALRX_SRXL) || (rxConfig()->serialrx_provider == SERIALRX_SRXL2))) {
|
||||
|
||||
srxlDisplayPort.device = NULL;
|
||||
displayInit(&srxlDisplayPort, &srxlVTable, DISPLAYPORT_DEVICE_TYPE_SRXL);
|
||||
srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS;
|
||||
srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS;
|
||||
|
||||
return &srxlDisplayPort;
|
||||
} else {
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
void srxlDisplayportRegister(void)
|
||||
{
|
||||
cmsDisplayPortRegister(displayPortSrxlInit());
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -20,6 +20,6 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
displayPort_t *displayPortSrxlInit();
|
||||
|
||||
extern displayPort_t srxlDisplayPort;
|
||||
|
||||
void srxlDisplayportRegister(void);
|
||||
|
|
|
|||
|
|
@ -968,11 +968,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);
|
||||
|
|
@ -1295,11 +1291,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:
|
||||
|
|
@ -1517,7 +1509,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));
|
||||
}
|
||||
|
|
@ -1527,8 +1519,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;
|
||||
}
|
||||
|
|
@ -1537,8 +1529,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);
|
||||
|
|
@ -1568,7 +1560,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 {
|
||||
|
|
@ -1603,4 +1595,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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -1451,6 +1451,8 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst)
|
|||
sbufWriteU16(dst, gpsRescueConfig()->descendRate);
|
||||
sbufWriteU8(dst, gpsRescueConfig()->allowArmingWithoutFix);
|
||||
sbufWriteU8(dst, gpsRescueConfig()->altitudeMode);
|
||||
// Added in API version 1.44
|
||||
sbufWriteU16(dst, gpsRescueConfig()->minRescueDth);
|
||||
break;
|
||||
|
||||
case MSP_GPS_RESCUE_PIDS:
|
||||
|
|
@ -2451,6 +2453,10 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
gpsRescueConfigMutable()->allowArmingWithoutFix = sbufReadU8(src);
|
||||
gpsRescueConfigMutable()->altitudeMode = sbufReadU8(src);
|
||||
}
|
||||
if (sbufBytesRemaining(src) >= 2) {
|
||||
// Added in API version 1.44
|
||||
gpsRescueConfigMutable()->minRescueDth = sbufReadU16(src);
|
||||
}
|
||||
break;
|
||||
|
||||
case MSP_SET_GPS_RESCUE_PIDS:
|
||||
|
|
@ -2660,7 +2666,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
sbufReadU8(src);
|
||||
#endif
|
||||
}
|
||||
if (sbufBytesRemaining(src) >= 1) {
|
||||
if (sbufBytesRemaining(src) >= 2) {
|
||||
#if defined(USE_GYRO_DATA_ANALYSE)
|
||||
// Added in MSP API 1.43
|
||||
gyroConfigMutable()->dyn_notch_max_hz = sbufReadU16(src);
|
||||
|
|
@ -3175,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);
|
||||
|
|
|
|||
|
|
@ -160,6 +160,7 @@ typedef enum {
|
|||
OSD_EFFICIENCY,
|
||||
OSD_TOTAL_FLIGHTS,
|
||||
OSD_UP_DOWN_REFERENCE,
|
||||
OSD_TX_UPLINK_POWER,
|
||||
OSD_ITEM_COUNT // MUST BE LAST
|
||||
} osd_items_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -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 <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
|
|
@ -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);
|
||||
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)
|
||||
|
|
@ -1036,6 +1128,18 @@ static void osdElementLinkQuality(osdElementParms_t *element)
|
|||
}
|
||||
#endif // USE_RX_LINK_QUALITY_INFO
|
||||
|
||||
#ifdef USE_RX_LINK_UPLINK_POWER
|
||||
static void osdElementTxUplinkPower(osdElementParms_t *element)
|
||||
{
|
||||
const uint16_t osdUplinkTxPowerMw = rxGetUplinkTxPwrMw();
|
||||
if (osdUplinkTxPowerMw < 1000) {
|
||||
tfp_sprintf(element->buff, "%c%3dMW", SYM_RSSI, osdUplinkTxPowerMw);
|
||||
} else {
|
||||
osdPrintFloat(element->buff, SYM_RSSI, osdUplinkTxPowerMw / 1000.0f, "", 1, false, 'W');
|
||||
}
|
||||
}
|
||||
#endif // USE_RX_LINK_UPLINK_POWER
|
||||
|
||||
#ifdef USE_BLACKBOX
|
||||
static void osdElementLogStatus(osdElementParms_t *element)
|
||||
{
|
||||
|
|
@ -1422,6 +1526,9 @@ static const uint8_t osdElementDisplayOrder[] = {
|
|||
#ifdef USE_RX_LINK_QUALITY_INFO
|
||||
OSD_LINK_QUALITY,
|
||||
#endif
|
||||
#ifdef USE_RX_LINK_UPLINK_POWER
|
||||
OSD_TX_UPLINK_POWER,
|
||||
#endif
|
||||
#ifdef USE_RX_RSSI_DBM
|
||||
OSD_RSSI_DBM_VALUE,
|
||||
#endif
|
||||
|
|
@ -1478,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
|
||||
|
|
@ -1527,6 +1634,9 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
|
|||
#ifdef USE_RX_LINK_QUALITY_INFO
|
||||
[OSD_LINK_QUALITY] = osdElementLinkQuality,
|
||||
#endif
|
||||
#ifdef USE_RX_LINK_UPLINK_POWER
|
||||
[OSD_TX_UPLINK_POWER] = osdElementTxUplinkPower,
|
||||
#endif
|
||||
#ifdef USE_GPS
|
||||
[OSD_FLIGHT_DIST] = osdElementGpsFlightDistance,
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -66,9 +66,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
|
|||
.rc_smoothing_input_cutoff = 0, // automatically calculate the cutoff by default
|
||||
.rc_smoothing_derivative_cutoff = 0, // automatically calculate the cutoff by default
|
||||
.rc_smoothing_debug_axis = ROLL, // default to debug logging for the roll axis
|
||||
.rc_smoothing_input_type = RC_SMOOTHING_INPUT_BIQUAD,
|
||||
.rc_smoothing_derivative_type = RC_SMOOTHING_DERIVATIVE_AUTO, // automatically choose type based on feedforward method
|
||||
.rc_smoothing_auto_factor = 10,
|
||||
.rc_smoothing_auto_factor = 30,
|
||||
.srxl2_unit_id = 1,
|
||||
.srxl2_baud_fast = true,
|
||||
.sbus_baud_fast = false,
|
||||
|
|
|
|||
|
|
@ -45,7 +45,17 @@ void pgResetFn_sdcardConfig(sdcardConfig_t *config)
|
|||
// On generic targets, SPI has precedence over SDIO; SDIO must be post-flash configured.
|
||||
config->useDma = false;
|
||||
config->device = SPI_DEV_TO_CFG(SPIINVALID);
|
||||
|
||||
#ifdef CONFIG_IN_SDCARD
|
||||
// CONFIG_ID_SDDCARD requires a default mode.
|
||||
#if defined(USE_SDCARD_SDIO)
|
||||
config->mode = SDCARD_MODE_SDIO;
|
||||
#elif defined(USE_SDCARD_SPI)
|
||||
config->mode = SDCARD_MODE_SPI;
|
||||
#endif
|
||||
#else
|
||||
config->mode = SDCARD_MODE_NONE;
|
||||
#endif
|
||||
|
||||
#ifdef USE_SDCARD_SPI
|
||||
// These settings do not work for Unified Targets
|
||||
|
|
|
|||
|
|
@ -70,6 +70,12 @@ static uint8_t telemetryBufLen = 0;
|
|||
|
||||
static timeUs_t lastRcFrameTimeUs = 0;
|
||||
|
||||
#ifdef USE_RX_LINK_UPLINK_POWER
|
||||
#define CRSF_UPLINK_POWER_LEVEL_MW_ITEMS_COUNT 8
|
||||
// Uplink power levels by uplinkTXPower expressed in mW (250 mW is from ver >=4.00)
|
||||
const uint16_t uplinkTXPowerStatesMw[CRSF_UPLINK_POWER_LEVEL_MW_ITEMS_COUNT] = {0, 10, 25, 100, 500, 1000, 2000, 250};
|
||||
#endif
|
||||
|
||||
/*
|
||||
* CRSF protocol
|
||||
*
|
||||
|
|
@ -130,7 +136,7 @@ typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t;
|
|||
* int8_t Uplink SNR ( db )
|
||||
* uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
|
||||
* uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz)
|
||||
* uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
|
||||
* uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW, 250mW )
|
||||
* uint8_t Downlink RSSI ( dBm * -1 )
|
||||
* uint8_t Downlink package success rate / Link quality ( % )
|
||||
* int8_t Downlink SNR ( db )
|
||||
|
|
@ -175,6 +181,11 @@ static void handleCrsfLinkStatisticsFrame(const crsfLinkStatistics_t* statsPtr,
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_RX_LINK_UPLINK_POWER
|
||||
const uint8_t crsfUplinkPowerStatesItemIndex = (stats.uplink_TX_Power < CRSF_UPLINK_POWER_LEVEL_MW_ITEMS_COUNT) ? stats.uplink_TX_Power : 0;
|
||||
rxSetUplinkTxPwrMw(uplinkTXPowerStatesMw[crsfUplinkPowerStatesItemIndex]);
|
||||
#endif
|
||||
|
||||
switch (debugMode) {
|
||||
case DEBUG_CRSF_LINK_STATISTICS_UPLINK:
|
||||
DEBUG_SET(DEBUG_CRSF_LINK_STATISTICS_UPLINK, 0, stats.uplink_RSSI_1);
|
||||
|
|
@ -345,7 +356,7 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
|||
return RX_FRAME_PENDING;
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
STATIC_UNIT_TESTED float crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
UNUSED(rxRuntimeState);
|
||||
/* conversion from RC value to PWM
|
||||
|
|
@ -356,7 +367,7 @@ STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeState_t *rxRuntimeState
|
|||
* scale factor = (2012-988) / (1811-172) = 0.62477120195241
|
||||
* offset = 988 - 172 * 0.62477120195241 = 880.53935326418548
|
||||
*/
|
||||
return (0.62477120195241f * crsfChannelData[chan]) + 881;
|
||||
return (0.62477120195241f * (float)crsfChannelData[chan]) + 881;
|
||||
}
|
||||
|
||||
void crsfRxWriteTelemetryData(const void *data, int len)
|
||||
|
|
|
|||
|
|
@ -289,7 +289,7 @@ static bool ghstProcessFrame(const rxRuntimeState_t *rxRuntimeState)
|
|||
return true;
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED uint16_t ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
STATIC_UNIT_TESTED float ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
UNUSED(rxRuntimeState);
|
||||
|
||||
|
|
@ -302,7 +302,7 @@ STATIC_UNIT_TESTED uint16_t ghstReadRawRC(const rxRuntimeState_t *rxRuntimeState
|
|||
// max 1024 1811 2012us
|
||||
//
|
||||
|
||||
return (5 * (ghstChannelData[chan]+1) / 8) + 880;
|
||||
return (5 * ((float)ghstChannelData[chan] + 1) / 8) + 880;
|
||||
}
|
||||
|
||||
static timeUs_t ghstFrameTimeUs(void)
|
||||
|
|
|
|||
|
|
@ -196,7 +196,7 @@ static uint8_t ibusFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
|||
}
|
||||
|
||||
|
||||
static uint16_t ibusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
static float ibusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
UNUSED(rxRuntimeState);
|
||||
return ibusChannelData[chan];
|
||||
|
|
|
|||
|
|
@ -237,7 +237,7 @@ static uint8_t jetiExBusFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
|||
return frameStatus;
|
||||
}
|
||||
|
||||
static uint16_t jetiExBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
static float jetiExBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
if (chan >= rxRuntimeState->channelCount)
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -36,7 +36,7 @@
|
|||
static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
static bool rxMspFrameDone = false;
|
||||
|
||||
uint16_t rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
float rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
UNUSED(rxRuntimeState);
|
||||
return mspFrame[chan];
|
||||
|
|
|
|||
|
|
@ -22,6 +22,6 @@
|
|||
|
||||
struct rxConfig_s;
|
||||
struct rxRuntimeState_s;
|
||||
uint16_t rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan);
|
||||
float rxMspReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan);
|
||||
void rxMspInit(const struct rxConfig_s *rxConfig, struct rxRuntimeState_s *rxRuntimeState);
|
||||
void rxMspFrameReceive(uint16_t *frame, int channelCount);
|
||||
|
|
|
|||
|
|
@ -41,13 +41,13 @@
|
|||
#include "rx/rx.h"
|
||||
#include "rx/pwm.h"
|
||||
|
||||
static uint16_t pwmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||
static float pwmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||
{
|
||||
UNUSED(rxRuntimeState);
|
||||
return pwmRead(channel);
|
||||
}
|
||||
|
||||
static uint16_t ppmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||
static float ppmReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||
{
|
||||
UNUSED(rxRuntimeState);
|
||||
return ppmRead(channel);
|
||||
|
|
|
|||
|
|
@ -85,6 +85,10 @@ static uint16_t linkQuality = 0;
|
|||
static uint8_t rfMode = 0;
|
||||
#endif
|
||||
|
||||
#ifdef USE_RX_LINK_UPLINK_POWER
|
||||
static uint16_t uplinkTxPwrMw = 0; //Uplink Tx power in mW
|
||||
#endif
|
||||
|
||||
#define MSP_RSSI_TIMEOUT_US 1500000 // 1.5 sec
|
||||
|
||||
#define RSSI_ADC_DIVISOR (4096 / 1024)
|
||||
|
|
@ -107,8 +111,8 @@ static uint32_t needRxSignalMaxDelayUs;
|
|||
static uint32_t suspendRxSignalUntil = 0;
|
||||
static uint8_t skipRxSamples = 0;
|
||||
|
||||
static int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||
static float rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||
uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
|
||||
#define MAX_INVALID_PULS_TIME 300
|
||||
|
|
@ -154,7 +158,7 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRange
|
|||
}
|
||||
}
|
||||
|
||||
static uint16_t nullReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||
static float nullReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||
{
|
||||
UNUSED(rxRuntimeState);
|
||||
UNUSED(channel);
|
||||
|
|
@ -455,6 +459,13 @@ void setLinkQualityDirect(uint16_t linkqualityValue)
|
|||
#endif
|
||||
}
|
||||
|
||||
#ifdef USE_RX_LINK_UPLINK_POWER
|
||||
void rxSetUplinkTxPwrMw(uint16_t uplinkTxPwrMwValue)
|
||||
{
|
||||
uplinkTxPwrMw = uplinkTxPwrMwValue;
|
||||
}
|
||||
#endif
|
||||
|
||||
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
|
||||
{
|
||||
bool signalReceived = false;
|
||||
|
|
@ -584,15 +595,15 @@ static uint16_t getRxfailValue(uint8_t channel)
|
|||
}
|
||||
}
|
||||
|
||||
STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range)
|
||||
STATIC_UNIT_TESTED float applyRxChannelRangeConfiguraton(float sample, const rxChannelRangeConfig_t *range)
|
||||
{
|
||||
// Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT
|
||||
if (sample == PPM_RCVR_TIMEOUT) {
|
||||
return PPM_RCVR_TIMEOUT;
|
||||
}
|
||||
|
||||
sample = scaleRange(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||
sample = constrain(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
|
||||
sample = scaleRangef(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||
sample = constrainf(sample, PWM_PULSE_MIN, PWM_PULSE_MAX);
|
||||
|
||||
return sample;
|
||||
}
|
||||
|
|
@ -604,7 +615,7 @@ static void readRxChannelsApplyRanges(void)
|
|||
const uint8_t rawChannel = channel < RX_MAPPABLE_CHANNEL_COUNT ? rxConfig()->rcmap[channel] : channel;
|
||||
|
||||
// sample the channel
|
||||
uint16_t sample;
|
||||
float sample;
|
||||
#if defined(USE_RX_MSP_OVERRIDE)
|
||||
if (rxConfig()->msp_override_channels_mask) {
|
||||
sample = rxMspOverrideReadRawRc(&rxRuntimeState, rxConfig(), rawChannel);
|
||||
|
|
@ -634,7 +645,7 @@ static void detectAndApplySignalLossBehaviour(void)
|
|||
|
||||
rxFlightChannelsValid = true;
|
||||
for (int channel = 0; channel < rxChannelCount; channel++) {
|
||||
uint16_t sample = rcRaw[channel];
|
||||
float sample = rcRaw[channel];
|
||||
|
||||
const bool validPulse = useValueFromRx && isPulseValid(sample);
|
||||
|
||||
|
|
@ -881,6 +892,13 @@ uint16_t rxGetLinkQualityPercent(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
#ifdef USE_RX_LINK_UPLINK_POWER
|
||||
uint16_t rxGetUplinkTxPwrMw(void)
|
||||
{
|
||||
return uplinkTxPwrMw;
|
||||
}
|
||||
#endif
|
||||
|
||||
uint16_t rxGetRefreshRate(void)
|
||||
{
|
||||
return rxRuntimeState.rxRefreshRate;
|
||||
|
|
|
|||
|
|
@ -86,7 +86,7 @@ typedef enum {
|
|||
|
||||
extern const char rcChannelLetters[];
|
||||
|
||||
extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||
extern float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||
|
||||
#define RSSI_SCALE_MIN 1
|
||||
#define RSSI_SCALE_MAX 255
|
||||
|
|
@ -124,7 +124,7 @@ typedef struct rxChannelRangeConfig_s {
|
|||
PG_DECLARE_ARRAY(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeConfigs);
|
||||
|
||||
struct rxRuntimeState_s;
|
||||
typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeState_s *rxRuntimeState, uint8_t chan); // used by receiver driver to return channel data
|
||||
typedef float (*rcReadRawDataFnPtr)(const struct rxRuntimeState_s *rxRuntimeState, uint8_t chan); // used by receiver driver to return channel data
|
||||
typedef uint8_t (*rcFrameStatusFnPtr)(struct rxRuntimeState_s *rxRuntimeState);
|
||||
typedef bool (*rcProcessFrameFnPtr)(const struct rxRuntimeState_s *rxRuntimeState);
|
||||
typedef timeUs_t rcGetFrameTimeUsFn(void); // used to retrieve the timestamp in microseconds for the last channel data frame
|
||||
|
|
@ -206,6 +206,9 @@ void setRssiDbmDirect(int16_t newRssiDbm, rssiSource_e source);
|
|||
void rxSetRfMode(uint8_t rfModeValue);
|
||||
uint8_t rxGetRfMode(void);
|
||||
|
||||
void rxSetUplinkTxPwrMw(uint16_t uplinkTxPwrMwValue);
|
||||
uint16_t rxGetUplinkTxPwrMw(void);
|
||||
|
||||
void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig);
|
||||
|
||||
void suspendRxPwmPpmSignal(void);
|
||||
|
|
|
|||
|
|
@ -67,7 +67,7 @@ static protocolDataReceivedFnPtr protocolDataReceived;
|
|||
static protocolProcessFrameFnPtr protocolProcessFrame;
|
||||
static protocolSetRcDataFromPayloadFnPtr protocolSetRcDataFromPayload;
|
||||
|
||||
STATIC_UNIT_TESTED uint16_t rxSpiReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||
STATIC_UNIT_TESTED float rxSpiReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channel)
|
||||
{
|
||||
STATIC_ASSERT(NRF24L01_MAX_PAYLOAD_SIZE <= RX_SPI_MAX_PAYLOAD_SIZE, NRF24L01_MAX_PAYLOAD_SIZE_larger_than_RX_SPI_MAX_PAYLOAD_SIZE);
|
||||
|
||||
|
|
|
|||
|
|
@ -87,11 +87,11 @@ uint8_t sbusChannelsDecode(rxRuntimeState_t *rxRuntimeState, const sbusChannels_
|
|||
return RX_FRAME_COMPLETE;
|
||||
}
|
||||
|
||||
static uint16_t sbusChannelsReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
static float sbusChannelsReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
// Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
|
||||
// http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
|
||||
return (5 * rxRuntimeState->channelData[chan] / 8) + 880;
|
||||
return (5 * (float)rxRuntimeState->channelData[chan] / 8) + 880;
|
||||
}
|
||||
|
||||
void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
|
||||
|
|
|
|||
|
|
@ -174,18 +174,19 @@ static uint8_t spektrumFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
|||
return result;
|
||||
}
|
||||
|
||||
static uint16_t spektrumReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
static float spektrumReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
uint16_t data;
|
||||
float data;
|
||||
|
||||
if (chan >= rxRuntimeState->channelCount) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (spekHiRes)
|
||||
data = 988 + (spekChannelData[chan] >> 1); // 2048 mode
|
||||
else
|
||||
data = 988 + spekChannelData[chan]; // 1024 mode
|
||||
if (spekHiRes) {
|
||||
data = 0.5f * (float)spekChannelData[chan] + 988; // 2048 mode
|
||||
} else {
|
||||
data = spekChannelData[chan] + 988; // 1024 mode
|
||||
}
|
||||
|
||||
return data;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -55,7 +55,7 @@
|
|||
|
||||
#define SRXL2_MAX_CHANNELS 32
|
||||
#define SRXL2_FRAME_PERIOD_US 11000 // 5500 for DSMR
|
||||
#define SRXL2_CHANNEL_SHIFT 5
|
||||
#define SRXL2_CHANNEL_SHIFT 2
|
||||
#define SRXL2_CHANNEL_CENTER 0x8000
|
||||
|
||||
#define SRXL2_PORT_BAUDRATE_DEFAULT 115200
|
||||
|
|
@ -458,13 +458,13 @@ static bool srxl2ProcessFrame(const rxRuntimeState_t *rxRuntimeState)
|
|||
return true;
|
||||
}
|
||||
|
||||
static uint16_t srxl2ReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channelIdx)
|
||||
static float srxl2ReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t channelIdx)
|
||||
{
|
||||
if (channelIdx >= rxRuntimeState->channelCount) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return SPEKTRUM_PULSE_OFFSET + ((rxRuntimeState->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) >> 1);
|
||||
return ((float)(rxRuntimeState->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) / 16) + SPEKTRUM_PULSE_OFFSET;
|
||||
}
|
||||
|
||||
void srxl2RxWriteData(const void *data, int len)
|
||||
|
|
|
|||
|
|
@ -162,10 +162,10 @@ static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
|||
return frameStatus;
|
||||
}
|
||||
|
||||
static uint16_t sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
static float sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
UNUSED(rxRuntimeState);
|
||||
return sumdChannels[chan] / 8;
|
||||
return (float)sumdChannels[chan] / 8;
|
||||
}
|
||||
|
||||
static timeUs_t sumdFrameTimeUsFn(void)
|
||||
|
|
|
|||
|
|
@ -108,7 +108,7 @@ static uint8_t sumhFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
|||
return RX_FRAME_COMPLETE;
|
||||
}
|
||||
|
||||
static uint16_t sumhReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
static float sumhReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
UNUSED(rxRuntimeState);
|
||||
|
||||
|
|
|
|||
|
|
@ -248,7 +248,7 @@ static uint8_t xBusFrameStatus(rxRuntimeState_t *rxRuntimeState)
|
|||
return RX_FRAME_COMPLETE;
|
||||
}
|
||||
|
||||
static uint16_t xBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
static float xBusReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
uint16_t data;
|
||||
|
||||
|
|
|
|||
|
|
@ -191,7 +191,7 @@ void currentMeterVirtualRefresh(int32_t lastUpdateAt, bool armed, bool throttleL
|
|||
throttleOffset = 0;
|
||||
}
|
||||
|
||||
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); // FIXME magic number 50, 50hz?
|
||||
int throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); // FIXME magic number 50. Possibly use thrustLinearization if configured.
|
||||
currentMeterVirtualState.amperage += throttleFactor * (int32_t)currentSensorVirtualConfig()->scale / 1000;
|
||||
}
|
||||
updateCurrentmAhDrawnState(¤tMeterVirtualState.mahDrawnState, currentMeterVirtualState.amperage, lastUpdateAt);
|
||||
|
|
|
|||
|
|
@ -82,12 +82,12 @@ PG_DECLARE(currentSensorADCConfig_t, currentSensorADCConfig);
|
|||
|
||||
typedef struct currentMeterVirtualState_s {
|
||||
currentMeterMAhDrawnState_t mahDrawnState;
|
||||
int32_t amperage; // current read by current sensor in centiampere (1/100th A)
|
||||
int32_t amperage; // current read by current sensor in centiamperes (1/100th A)
|
||||
} currentSensorVirtualState_t;
|
||||
|
||||
typedef struct currentSensorVirtualConfig_s {
|
||||
int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A
|
||||
uint16_t offset; // offset of the current sensor in millivolt steps
|
||||
int16_t scale; // scale the throttle to centiamperes, using a hardcoded thrust linearization function (see Battery.md)
|
||||
uint16_t offset; // offset of the current sensor in centiamperes (1/100th A)
|
||||
} currentSensorVirtualConfig_t;
|
||||
|
||||
PG_DECLARE(currentSensorVirtualConfig_t, currentSensorVirtualConfig);
|
||||
|
|
|
|||
|
|
@ -505,7 +505,7 @@ static bool gyroDetectSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t
|
|||
{
|
||||
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \
|
||||
|| defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \
|
||||
|| defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO)
|
||||
|| defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_L3GD20) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) || defined(USE_ACCGYRO_LSM6DSO) || defined(USE_GYRO_SPI_ICM42605)
|
||||
|
||||
bool gyroFound = mpuDetect(&gyroSensor->gyroDev, config);
|
||||
|
||||
|
|
|
|||
|
|
@ -201,7 +201,7 @@ static uint8_t frameStatus(rxRuntimeState_t *rxRuntimeState)
|
|||
return RX_FRAME_COMPLETE;
|
||||
}
|
||||
|
||||
static uint16_t readRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
static float readRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t chan)
|
||||
{
|
||||
if (chan >= rxRuntimeState->channelCount) {
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -139,6 +139,42 @@
|
|||
#define TARGET_IO_PORTD 0xffff
|
||||
#define TARGET_IO_PORTE 0xffff
|
||||
#define TARGET_IO_PORTF 0xffff
|
||||
|
||||
#elif defined(STM32G47X)
|
||||
#define TARGET_BOARD_IDENTIFIER "SG47"
|
||||
|
||||
#define USBD_PRODUCT_STRING "Betaflight STM32G47x"
|
||||
|
||||
#define USE_I2C_DEVICE_1
|
||||
#define USE_I2C_DEVICE_2
|
||||
#define USE_I2C_DEVICE_3
|
||||
#define USE_I2C_DEVICE_4
|
||||
|
||||
#define USE_UART1
|
||||
#define USE_UART2
|
||||
#define USE_UART3
|
||||
#define USE_UART4
|
||||
#define USE_UART5
|
||||
#define USE_UART9
|
||||
|
||||
#define SERIAL_PORT_COUNT (UNIFIED_SERIAL_PORT_COUNT + 6)
|
||||
|
||||
#define USE_SPI_DEVICE_1
|
||||
#define USE_SPI_DEVICE_2
|
||||
#define USE_SPI_DEVICE_3
|
||||
#define USE_SPI_DEVICE_4
|
||||
|
||||
#define TARGET_IO_PORTA 0xffff
|
||||
#define TARGET_IO_PORTB 0xffff
|
||||
#define TARGET_IO_PORTC 0xffff
|
||||
#define TARGET_IO_PORTD 0xffff
|
||||
#define TARGET_IO_PORTE 0xffff
|
||||
#define TARGET_IO_PORTF 0xffff
|
||||
|
||||
#elif !defined(UNIT_TEST)
|
||||
|
||||
#error "No resources defined for this Unified Target."
|
||||
|
||||
#endif
|
||||
|
||||
// Treat the target as unified, and expect manufacturer id / board name
|
||||
|
|
@ -190,9 +226,12 @@
|
|||
#define USE_BARO_DPS310
|
||||
#define USE_BARO_SPI_DPS310
|
||||
|
||||
#if !defined(STM32G4)
|
||||
// G4 support needs fixing
|
||||
#define USE_SDCARD
|
||||
#define USE_SDCARD_SPI
|
||||
#define USE_SDCARD_SDIO
|
||||
#endif
|
||||
|
||||
#define USE_FLASHFS
|
||||
#define USE_FLASH_TOOLS
|
||||
|
|
@ -207,11 +246,14 @@
|
|||
#define USE_VTX_RTC6705
|
||||
#define USE_VTX_RTC6705_SOFTSPI
|
||||
|
||||
#if !defined(STM32G4)
|
||||
// To make it fit RAM
|
||||
#define USE_TRANSPONDER
|
||||
|
||||
#define USE_RANGEFINDER
|
||||
#define USE_RANGEFINDER_HCSR04
|
||||
#define USE_RANGEFINDER_TF
|
||||
#endif
|
||||
|
||||
#define USE_SPI
|
||||
#define SPI_FULL_RECONFIGURABILITY
|
||||
|
|
|
|||
|
|
@ -9,12 +9,17 @@ else
|
|||
ifeq ($(TARGET), STM32F7X2)
|
||||
F7X2RE_TARGETS += $(TARGET)
|
||||
|
||||
else
|
||||
ifeq ($(TARGET), STM32G47X)
|
||||
G47X_TARGETS += $(TARGET)
|
||||
|
||||
else # STM32F745
|
||||
F7X5XG_TARGETS += $(TARGET)
|
||||
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
endif
|
||||
|
||||
ifeq ($(TARGET), $(filter $(TARGET), STM32F405 STM32F745))
|
||||
# Use a full block (16 kB) of flash for custom defaults - with 1 MB flash we have more than we know how to use anyway
|
||||
|
|
@ -22,8 +27,11 @@ ifeq ($(TARGET), $(filter $(TARGET), STM32F405 STM32F745))
|
|||
CUSTOM_DEFAULTS_EXTENDED = yes
|
||||
endif
|
||||
|
||||
|
||||
ifeq ($(TARGET), STM32G47X)
|
||||
FEATURES += VCP ONBOARDFLASH
|
||||
else
|
||||
FEATURES += VCP SDCARD_SPI SDCARD_SDIO ONBOARDFLASH
|
||||
endif
|
||||
|
||||
TARGET_SRC = \
|
||||
$(addprefix drivers/accgyro/,$(notdir $(wildcard $(SRC_DIR)/drivers/accgyro/*.c))) \
|
||||
|
|
|
|||
|
|
@ -90,7 +90,6 @@
|
|||
#endif
|
||||
|
||||
#if !defined(USE_TELEMETRY)
|
||||
#undef USE_CRSF_CMS_TELEMETRY
|
||||
#undef USE_TELEMETRY_CRSF
|
||||
#undef USE_TELEMETRY_GHST
|
||||
#undef USE_TELEMETRY_FRSKY_HUB
|
||||
|
|
@ -136,10 +135,13 @@
|
|||
#undef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
|
||||
#undef USE_SPEKTRUM_VTX_CONTROL
|
||||
#undef USE_SPEKTRUM_VTX_TELEMETRY
|
||||
#undef USE_SPEKTRUM_CMS_TELEMETRY
|
||||
#undef USE_TELEMETRY_SRXL
|
||||
#endif
|
||||
|
||||
#if !defined(USE_CMS) || !defined(USE_TELEMETRY_SRXL)
|
||||
#undef USE_SPEKTRUM_CMS_TELEMETRY
|
||||
#endif
|
||||
|
||||
#if defined(USE_SERIALRX_SBUS) || defined(USE_SERIALRX_FPORT)
|
||||
#define USE_SBUS_CHANNELS
|
||||
#endif
|
||||
|
|
@ -229,6 +231,7 @@
|
|||
#undef USE_RX_LINK_QUALITY_INFO
|
||||
#undef USE_OSD_PROFILES
|
||||
#undef USE_OSD_STICK_OVERLAY
|
||||
#undef USE_RX_LINK_UPLINK_POWER
|
||||
#endif
|
||||
|
||||
#if defined(USE_GPS_RESCUE)
|
||||
|
|
@ -240,7 +243,7 @@
|
|||
#define USE_I2C_GYRO
|
||||
#endif
|
||||
|
||||
#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_L3GD20)
|
||||
#if defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_L3GD20) || defined(USE_GYRO_SPI_ICM42605)
|
||||
#define USE_SPI_GYRO
|
||||
#endif
|
||||
|
||||
|
|
@ -406,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
|
||||
|
|
|
|||
|
|
@ -397,4 +397,6 @@
|
|||
#define USE_BATTERY_VOLTAGE_SAG_COMPENSATION
|
||||
#define USE_RX_MSP_OVERRIDE
|
||||
#define USE_SIMPLIFIED_TUNING
|
||||
#define USE_RX_LINK_UPLINK_POWER
|
||||
#define USE_GPS_PLUS_CODES
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -30,21 +30,19 @@
|
|||
#include "build/build_config.h"
|
||||
#include "build/version.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
#include "cms/cms.h"
|
||||
|
||||
#include "config/feature.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "common/crc.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/printf.h"
|
||||
#include "common/streambuf.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "cms/cms.h"
|
||||
|
||||
#include "drivers/nvic.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
|
|
@ -55,6 +53,9 @@
|
|||
#include "io/gps.h"
|
||||
#include "io/serial.h"
|
||||
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
|
||||
#include "rx/crsf.h"
|
||||
#include "rx/crsf_protocol.h"
|
||||
|
||||
|
|
@ -64,7 +65,7 @@
|
|||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/msp_shared.h"
|
||||
|
||||
#include "telemetry/crsf.h"
|
||||
#include "crsf.h"
|
||||
|
||||
|
||||
#define CRSF_CYCLETIME_US 100000 // 100ms, 10 Hz
|
||||
|
|
@ -525,6 +526,10 @@ void initCrsfTelemetry(void)
|
|||
}
|
||||
#endif
|
||||
crsfScheduleCount = (uint8_t)index;
|
||||
|
||||
#if defined(USE_CRSF_CMS_TELEMETRY)
|
||||
crsfDisplayportRegister();
|
||||
#endif
|
||||
}
|
||||
|
||||
bool checkCrsfTelemetryState(void)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -29,46 +29,44 @@
|
|||
#include "build/version.h"
|
||||
|
||||
#include "cms/cms.h"
|
||||
#include "io/displayport_srxl.h"
|
||||
|
||||
#include "common/crc.h"
|
||||
#include "common/streambuf.h"
|
||||
#include "common/utils.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "config/feature.h"
|
||||
|
||||
#include "io/gps.h"
|
||||
#include "io/serial.h"
|
||||
#include "drivers/dshot.h"
|
||||
#include "drivers/vtx_common.h"
|
||||
|
||||
#include "config/config.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
|
||||
#include "io/displayport_srxl.h"
|
||||
#include "io/gps.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/vtx_smartaudio.h"
|
||||
#include "io/vtx_tramp.h"
|
||||
|
||||
#include "pg/rx.h"
|
||||
#include "pg/motor.h"
|
||||
|
||||
#include "rx/rx.h"
|
||||
#include "rx/spektrum.h"
|
||||
#include "rx/srxl2.h"
|
||||
#include "io/spektrum_vtx_control.h"
|
||||
#include "rx/srxl2.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/adcinternal.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/esc_sensor.h"
|
||||
|
||||
#include "telemetry/telemetry.h"
|
||||
#include "telemetry/srxl.h"
|
||||
|
||||
#include "drivers/vtx_common.h"
|
||||
#include "drivers/dshot.h"
|
||||
|
||||
#include "io/vtx_tramp.h"
|
||||
#include "io/vtx_smartaudio.h"
|
||||
#include "srxl.h"
|
||||
|
||||
#define SRXL_ADDRESS_FIRST 0xA5
|
||||
#define SRXL_ADDRESS_SECOND 0x80
|
||||
|
|
@ -786,6 +784,12 @@ void initSrxlTelemetry(void)
|
|||
srxlTelemetryEnabled = false;
|
||||
srxl2 = false;
|
||||
}
|
||||
|
||||
#if defined(USE_SPEKTRUM_CMS_TELEMETRY)
|
||||
if (srxlTelemetryEnabled) {
|
||||
srxlDisplayportRegister();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
bool checkSrxlTelemetryState(void)
|
||||
|
|
|
|||
|
|
@ -59,7 +59,7 @@ extern "C" {
|
|||
PG_REGISTER(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 0);
|
||||
|
||||
float rcCommand[4];
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
uint16_t averageSystemLoadPercent = 0;
|
||||
uint8_t cliMode = 0;
|
||||
uint8_t debugMode = 0;
|
||||
|
|
|
|||
|
|
@ -543,7 +543,7 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
|
|||
// STUBS
|
||||
|
||||
extern "C" {
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
float rcCommand[4];
|
||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||
bool isUsingSticksToArm = true;
|
||||
|
|
|
|||
|
|
@ -204,7 +204,7 @@ TEST(FlightImuTest, TestSmallAngle)
|
|||
extern "C" {
|
||||
boxBitmask_t rcModeActivationMask;
|
||||
float rcCommand[4];
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
|
||||
gyro_t gyro;
|
||||
acc_t acc;
|
||||
|
|
|
|||
|
|
@ -299,7 +299,7 @@ uint8_t armingFlags = 0;
|
|||
uint8_t stateFlags = 0;
|
||||
uint16_t flightModeFlags = 0;
|
||||
float rcCommand[4];
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
boxBitmask_t rcModeActivationMask;
|
||||
gpsSolutionData_t gpsSol;
|
||||
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ extern "C" {
|
|||
float rMat[3][3];
|
||||
|
||||
pidProfile_t *currentPidProfile;
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
uint8_t GPS_numSat;
|
||||
uint16_t GPS_distanceToHome;
|
||||
int16_t GPS_directionToHome;
|
||||
|
|
|
|||
|
|
@ -73,7 +73,7 @@ extern "C" {
|
|||
|
||||
pidProfile_t *currentPidProfile;
|
||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
uint8_t GPS_numSat;
|
||||
uint16_t GPS_distanceToHome;
|
||||
int16_t GPS_directionToHome;
|
||||
|
|
|
|||
|
|
@ -640,7 +640,7 @@ uint8_t armingFlags = 0;
|
|||
uint16_t flightModeFlags = 0;
|
||||
int16_t heading;
|
||||
uint8_t stateFlags = 0;
|
||||
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
pidProfile_t *currentPidProfile;
|
||||
rxRuntimeState_t rxRuntimeState;
|
||||
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
|
||||
|
|
|
|||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue