From 86afe137baa0577c91b5dcf0a2a88bc7b159900e Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Thu, 14 Dec 2023 12:03:40 +0100 Subject: [PATCH 001/112] fixed: SITL accelerometer/mag data is not processed when using SITL with HITL plugin --- src/main/fc/fc_core.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a61204dee90..3248e4a9bf7 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -905,7 +905,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) } #if defined(SITL_BUILD) - if (lockMainPID()) { + if (ARMING_FLAG(SIMULATOR_MODE_HITL) || lockMainPID()) { #endif gyroFilter(); From a625e83534d42245875c4bee93981e2859d995fb Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 30 Mar 2024 22:01:40 +0100 Subject: [PATCH 002/112] Increase allowed speeds in NAV modes on multirotors --- src/main/fc/settings.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 106a830decc..52c727b846f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2510,7 +2510,7 @@ groups: table: nav_fw_wp_turn_smoothing - name: nav_auto_speed description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]" - default_value: 300 + default_value: 600 field: general.auto_speed min: 10 max: 2000 @@ -2522,13 +2522,13 @@ groups: max: 50 - name: nav_max_auto_speed description: "Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]" - default_value: 1000 + default_value: 1200 field: general.max_auto_speed min: 10 max: 2000 - name: nav_manual_speed description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]" - default_value: 500 + default_value: 1000 field: general.max_manual_speed min: 10 max: 2000 From 890147770019da9f6dd2e2a2c6d49b7fbb548026 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 30 Mar 2024 22:05:58 +0100 Subject: [PATCH 003/112] Increase bank angles --- docs/Settings.md | 12 ++++++------ src/main/fc/settings.yaml | 6 +++--- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index fefcc927f20..40490808c83 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2388,7 +2388,7 @@ Maximum inclination in level (angle) mode (PITCH axis). 100=10° | Default | Min | Max | | --- | --- | --- | -| 300 | 100 | 900 | +| 450 | 100 | 900 | --- @@ -2398,7 +2398,7 @@ Maximum inclination in level (angle) mode (ROLL axis). 100=10° | Default | Min | Max | | --- | --- | --- | -| 300 | 100 | 900 | +| 450 | 100 | 900 | --- @@ -2758,7 +2758,7 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci | Default | Min | Max | | --- | --- | --- | -| 300 | 10 | 2000 | +| 600 | 10 | 2000 | --- @@ -3418,7 +3418,7 @@ Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mod | Default | Min | Max | | --- | --- | --- | -| 500 | 10 | 2000 | +| 1000 | 10 | 2000 | --- @@ -3438,7 +3438,7 @@ Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor onl | Default | Min | Max | | --- | --- | --- | -| 1000 | 10 | 2000 | +| 1200 | 10 | 2000 | --- @@ -3478,7 +3478,7 @@ Maximum banking angle (deg) that multicopter navigation is allowed to set. Machi | Default | Min | Max | | --- | --- | --- | -| 30 | 15 | 45 | +| 40 | 15 | 45 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 52c727b846f..71964891ce9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1913,13 +1913,13 @@ groups: max: RPYL_PID_MAX - name: max_angle_inclination_rll description: "Maximum inclination in level (angle) mode (ROLL axis). 100=10°" - default_value: 300 + default_value: 450 field: max_angle_inclination[FD_ROLL] min: 100 max: 900 - name: max_angle_inclination_pit description: "Maximum inclination in level (angle) mode (PITCH axis). 100=10°" - default_value: 300 + default_value: 450 field: max_angle_inclination[FD_PITCH] min: 100 max: 900 @@ -2704,7 +2704,7 @@ groups: max: 120 - name: nav_mc_bank_angle description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude" - default_value: 30 + default_value: 40 field: mc.max_bank_angle min: 15 max: 45 From 93a636d3f1d8fec4af5c6effdc6aee3de06171dd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 31 Mar 2024 18:21:08 +0200 Subject: [PATCH 004/112] Compute RMS of filtered gyro data --- .vscode/c_cpp_properties.json | 3 +- cmake/at32.cmake | 3 ++ cmake/stm32.cmake | 3 ++ src/main/CMakeLists.txt | 2 + src/main/build/debug.h | 1 + src/main/fc/fc_tasks.c | 13 ++++++ src/main/fc/settings.yaml | 2 +- src/main/flight/adaptive_filter.c | 77 +++++++++++++++++++++++++++++++ src/main/flight/adaptive_filter.h | 32 +++++++++++++ src/main/scheduler/scheduler.h | 3 ++ src/main/sensors/gyro.c | 3 ++ src/main/target/common.h | 6 +++ 12 files changed, 146 insertions(+), 2 deletions(-) create mode 100644 src/main/flight/adaptive_filter.c create mode 100644 src/main/flight/adaptive_filter.h diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 6e7d914b25a..a2c5f582e1c 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -57,7 +57,8 @@ "USE_SDCARD_SDIO", "USE_SDCARD", "USE_Q_TUNE", - "USE_GYRO_FFT_FILTER" + "USE_GYRO_FFT_FILTER", + "USE_ADAPTIVE_FILTER", ], "configurationProvider": "ms-vscode.cmake-tools" } diff --git a/cmake/at32.cmake b/cmake/at32.cmake index 54e178deb7b..79efcfc0f17 100644 --- a/cmake/at32.cmake +++ b/cmake/at32.cmake @@ -18,6 +18,7 @@ set(CMSIS_DSP_SRC BasicMathFunctions/arm_scale_f32.c BasicMathFunctions/arm_sub_f32.c BasicMathFunctions/arm_mult_f32.c + BasicMathFunctions/arm_offset_f32.c TransformFunctions/arm_rfft_fast_f32.c TransformFunctions/arm_cfft_f32.c TransformFunctions/arm_rfft_fast_init_f32.c @@ -26,6 +27,8 @@ set(CMSIS_DSP_SRC CommonTables/arm_common_tables.c ComplexMathFunctions/arm_cmplx_mag_f32.c StatisticsFunctions/arm_max_f32.c + StatisticsFunctions/arm_rms_f32.c + StatisticsFunctions/arm_mean_f32.c ) list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/") diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake index f02185e9aff..256f8d09bef 100644 --- a/cmake/stm32.cmake +++ b/cmake/stm32.cmake @@ -19,6 +19,7 @@ set(CMSIS_DSP_SRC BasicMathFunctions/arm_scale_f32.c BasicMathFunctions/arm_sub_f32.c BasicMathFunctions/arm_mult_f32.c + BasicMathFunctions/arm_offset_f32.c TransformFunctions/arm_rfft_fast_f32.c TransformFunctions/arm_cfft_f32.c TransformFunctions/arm_rfft_fast_init_f32.c @@ -27,6 +28,8 @@ set(CMSIS_DSP_SRC CommonTables/arm_common_tables.c ComplexMathFunctions/arm_cmplx_mag_f32.c StatisticsFunctions/arm_max_f32.c + StatisticsFunctions/arm_rms_f32.c + StatisticsFunctions/arm_mean_f32.c ) list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/") diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 98a43854cf2..7b3a22006f7 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -343,6 +343,8 @@ main_sources(COMMON_SRC flight/dynamic_lpf.h flight/ez_tune.c flight/ez_tune.h + flight/adaptive_filter.c + flight/adaptive_filter.h io/adsb.c io/beeper.c diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 3a09da50daf..60b22df27bc 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -72,5 +72,6 @@ typedef enum { DEBUG_RATE_DYNAMICS, DEBUG_LANDING, DEBUG_POS_EST, + DEBUG_ADAPTIVE_FILTER, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 246d65c0a7e..74be2f42258 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -51,6 +51,7 @@ #include "flight/rpm_filter.h" #include "flight/servos.h" #include "flight/wind_estimator.h" +#include "flight/adaptive_filter.h" #include "navigation/navigation.h" @@ -421,6 +422,10 @@ void fcTasksInit(void) #if defined(USE_SMARTPORT_MASTER) setTaskEnabled(TASK_SMARTPORT_MASTER, true); #endif + +#ifdef USE_ADAPTIVE_FILTER + setTaskEnabled(TASK_ADAPTIVE_FILTER, true); +#endif } cfTask_t cfTasks[TASK_COUNT] = { @@ -672,4 +677,12 @@ cfTask_t cfTasks[TASK_COUNT] = { .desiredPeriod = TASK_PERIOD_HZ(TASK_AUX_RATE_HZ), // 100Hz @10ms .staticPriority = TASK_PRIORITY_HIGH, }, +#ifdef USE_ADAPTIVE_FILTER + [TASK_ADAPTIVE_FILTER] = { + .taskName = "ADAPTIVE_FILTER", + .taskFunc = adaptiveFilterTask, + .desiredPeriod = TASK_PERIOD_HZ(ADAPTIVE_FILTER_RATE_HZ), // 100Hz @10ms + .staticPriority = TASK_PRIORITY_LOW, + }, +#endif }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 106a830decc..8c2ed270d35 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -88,7 +88,7 @@ tables: values: ["NONE", "AGL", "FLOW_RAW", "FLOW", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "NAV_YAW", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "ALTITUDE", - "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST"] + "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS", "LANDING", "POS_EST", "ADAPTIVE_FILTER"] - name: aux_operator values: ["OR", "AND"] enum: modeActivationOperator_e diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c new file mode 100644 index 00000000000..cb057ad5cfb --- /dev/null +++ b/src/main/flight/adaptive_filter.c @@ -0,0 +1,77 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "platform.h" + +#ifdef USE_ADAPTIVE_FILTER + +#include +#include "flight/adaptive_filter.h" +#include "arm_math.h" +#include +#include "common/maths.h" +#include "common/axis.h" +#include "build/debug.h" + +static float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; +static uint8_t adaptiveFilterSampleIndex = 0; + +void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) { + //Push new sample to the buffer so later we can compute RMS and other measures + adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = value; + adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE; +} + +void adaptiveFilterTask(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + + //Compute RMS for each axis + for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + + //Copy axis samples to a temporary buffer + float32_t tempBuffer[ADAPTIVE_FILTER_BUFFER_SIZE]; + //Use memcpy to copy the samples to the temporary buffer + memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis])); + + //Use arm_mean_f32 to compute the mean of the samples + float32_t mean; + arm_mean_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &mean); + + float32_t normalizedBuffer[ADAPTIVE_FILTER_BUFFER_SIZE]; + + //Use arm_offset_f32 to subtract the mean from the samples + arm_offset_f32(tempBuffer, -mean, normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE); + + //Compute RMS from normalizedBuffer using arm_rms_f32 + float32_t rms; + arm_rms_f32(normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms); + + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, rms * 1000.0f); + } + + +} + + +#endif /* USE_ADAPTIVE_FILTER */ \ No newline at end of file diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h new file mode 100644 index 00000000000..9999f17105d --- /dev/null +++ b/src/main/flight/adaptive_filter.h @@ -0,0 +1,32 @@ +/* + * This file is part of INAV Project. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "common/axis.h" +#include "common/time.h" + +#define ADAPTIVE_FILTER_BUFFER_SIZE 32 +#define ADAPTIVE_FILTER_RATE_HZ 100 + +void adaptiveFilterPush(const flight_dynamics_index_t index, const float value); +void adaptiveFilterTask(timeUs_t currentTimeUs); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 0d91876cb72..c1e0ab6bb53 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -121,6 +121,9 @@ typedef enum { #endif #ifdef USE_IRLOCK TASK_IRLOCK, +#endif +#ifdef USE_ADAPTIVE_FILTER + TASK_ADAPTIVE_FILTER, #endif /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index a586ce6df6b..5873c9d7950 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -68,6 +68,7 @@ #include "flight/gyroanalyse.h" #include "flight/rpm_filter.h" #include "flight/kalman.h" +#include "flight/adaptive_filter.h" #ifdef USE_HARDWARE_REVISION_DETECTION #include "hardware_revision.h" @@ -453,6 +454,8 @@ void FAST_CODE NOINLINE gyroFilter(void) gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf); + adaptiveFilterPush(axis, gyroADCf); + #ifdef USE_DYNAMIC_FILTERS if (dynamicGyroNotchState.enabled) { gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf); diff --git a/src/main/target/common.h b/src/main/target/common.h index 8bbaadef718..63a03b59851 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -203,3 +203,9 @@ #define MAX_MIXER_PROFILE_COUNT 1 #endif #define USE_EZ_TUNE + +#ifdef STM32H7 + +#define USE_ADAPTIVE_FILTER + +#endif From e99563deaebb0a02f4f226f2fcf8e4ccdb46a378 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 31 Mar 2024 21:34:59 +0200 Subject: [PATCH 005/112] Add a filter --- src/main/flight/adaptive_filter.c | 20 ++++++++++++++++++-- src/main/flight/adaptive_filter.h | 1 + 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index cb057ad5cfb..31078b95ba9 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -32,10 +32,15 @@ #include #include "common/maths.h" #include "common/axis.h" +#include "common/filter.h" #include "build/debug.h" -static float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; -static uint8_t adaptiveFilterSampleIndex = 0; +STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; +STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0; + +STATIC_FASTRAM pt1Filter_t rmsFilter[XYZ_AXIS_COUNT]; + +STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0; void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) { //Push new sample to the buffer so later we can compute RMS and other measures @@ -46,6 +51,14 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) void adaptiveFilterTask(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); + if (!adaptiveFilterInitialized) { + //Initialize the filter + for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt1FilterInit(&rmsFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ); + } + adaptiveFilterInitialized = 1; + } + //Compute RMS for each axis for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -67,7 +80,10 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { float32_t rms; arm_rms_f32(normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms); + float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, rms * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis + XYZ_AXIS_COUNT, filteredRms * 1000.0f); } diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h index 9999f17105d..1b61c769a97 100644 --- a/src/main/flight/adaptive_filter.h +++ b/src/main/flight/adaptive_filter.h @@ -27,6 +27,7 @@ #define ADAPTIVE_FILTER_BUFFER_SIZE 32 #define ADAPTIVE_FILTER_RATE_HZ 100 +#define ADAPTIVE_FILTER_LPF_HZ 1 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value); void adaptiveFilterTask(timeUs_t currentTimeUs); From 4c46e519c518711f507adf6d076631ccd6bd9c70 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 31 Mar 2024 22:47:06 +0200 Subject: [PATCH 006/112] Change formating --- src/main/flight/adaptive_filter.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index 31078b95ba9..2ec4e9780bc 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -86,7 +86,6 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis + XYZ_AXIS_COUNT, filteredRms * 1000.0f); } - } From 64cfdba5befa792d806aa87bd4e57f62cbefc4bf Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 2 Apr 2024 13:02:09 +0200 Subject: [PATCH 007/112] Use HPF on samples instead of buffer mean value --- src/main/flight/adaptive_filter.c | 30 +++++++++++++++++++----------- src/main/flight/adaptive_filter.h | 1 + 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index 2ec4e9780bc..d402369c910 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -34,17 +34,34 @@ #include "common/axis.h" #include "common/filter.h" #include "build/debug.h" +#include "fc/config.h" STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0; STATIC_FASTRAM pt1Filter_t rmsFilter[XYZ_AXIS_COUNT]; +STATIC_FASTRAM pt1Filter_t hpfFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0; +STATIC_FASTRAM uint8_t hpfFilterInitialized = 0; +/** + * This function is called at pid rate, so has to be initialized at PID loop frequency +*/ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) { + + if (!hpfFilterInitialized) { + //Initialize the filter + for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + pt1FilterInit(&hpfFilter[axis], ADAPTIVE_FILTER_HPF_HZ, US2S(getLooptime())); + } + hpfFilterInitialized = 1; + } + + const float filteredGyro = value - pt1FilterApply(&hpfFilter[index], value); + //Push new sample to the buffer so later we can compute RMS and other measures - adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = value; + adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = filteredGyro; adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE; } @@ -67,18 +84,9 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { //Use memcpy to copy the samples to the temporary buffer memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis])); - //Use arm_mean_f32 to compute the mean of the samples - float32_t mean; - arm_mean_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &mean); - - float32_t normalizedBuffer[ADAPTIVE_FILTER_BUFFER_SIZE]; - - //Use arm_offset_f32 to subtract the mean from the samples - arm_offset_f32(tempBuffer, -mean, normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE); - //Compute RMS from normalizedBuffer using arm_rms_f32 float32_t rms; - arm_rms_f32(normalizedBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms); + arm_rms_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms); float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms); diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h index 1b61c769a97..9a9d4dc88fb 100644 --- a/src/main/flight/adaptive_filter.h +++ b/src/main/flight/adaptive_filter.h @@ -28,6 +28,7 @@ #define ADAPTIVE_FILTER_BUFFER_SIZE 32 #define ADAPTIVE_FILTER_RATE_HZ 100 #define ADAPTIVE_FILTER_LPF_HZ 1 +#define ADAPTIVE_FILTER_HPF_HZ 15 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value); void adaptiveFilterTask(timeUs_t currentTimeUs); From 3585f889296ea882139dd43aa2f4284f37c9f2be Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 2 Apr 2024 14:58:04 +0200 Subject: [PATCH 008/112] Compute combined RMS --- src/main/flight/adaptive_filter.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index d402369c910..0c4767e293f 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -76,6 +76,8 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { adaptiveFilterInitialized = 1; } + float combinedRms = 0.0f; + //Compute RMS for each axis for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -90,10 +92,14 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms); - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, rms * 1000.0f); - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis + XYZ_AXIS_COUNT, filteredRms * 1000.0f); + combinedRms += filteredRms; } + combinedRms /= XYZ_AXIS_COUNT; + + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedRms); + + } From 49aba32044d2a7de3cb373ed0003b91855e5c1ec Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 2 Apr 2024 15:04:10 +0200 Subject: [PATCH 009/112] Compute task dT --- src/main/flight/adaptive_filter.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index 0c4767e293f..52f82ec72bb 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -66,7 +66,8 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) } void adaptiveFilterTask(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); + static timeUs_t previousUpdateTimeUs; + const float dT = US2S(currentTimeUs - previousUpdateTimeUs); if (!adaptiveFilterInitialized) { //Initialize the filter @@ -96,8 +97,8 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { } combinedRms /= XYZ_AXIS_COUNT; - - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedRms); + + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedRms * 1000.0f); } From 9f72d0744af7a91802c18859d6d81f9a1cd8dae5 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 2 Apr 2024 17:36:32 +0200 Subject: [PATCH 010/112] Change filtering rules --- src/main/flight/adaptive_filter.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index 52f82ec72bb..bb35557f317 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -94,11 +94,13 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms); combinedRms += filteredRms; + + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, filteredRms * 1000.0f); } combinedRms /= XYZ_AXIS_COUNT; - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedRms * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, combinedRms * 1000.0f); } From bc8446fd0ebdfc8d3d436d57a74b71e3906de33b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 2 Apr 2024 21:17:03 +0200 Subject: [PATCH 011/112] Compute RMS and STD --- cmake/at32.cmake | 1 + cmake/stm32.cmake | 1 + src/main/flight/adaptive_filter.c | 29 +++++++++++++++++++++++++---- 3 files changed, 27 insertions(+), 4 deletions(-) diff --git a/cmake/at32.cmake b/cmake/at32.cmake index 79efcfc0f17..ef5041a8267 100644 --- a/cmake/at32.cmake +++ b/cmake/at32.cmake @@ -28,6 +28,7 @@ set(CMSIS_DSP_SRC ComplexMathFunctions/arm_cmplx_mag_f32.c StatisticsFunctions/arm_max_f32.c StatisticsFunctions/arm_rms_f32.c + StatisticsFunctions/arm_std_f32.c StatisticsFunctions/arm_mean_f32.c ) list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/") diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake index 256f8d09bef..bbb9c3b8dc9 100644 --- a/cmake/stm32.cmake +++ b/cmake/stm32.cmake @@ -29,6 +29,7 @@ set(CMSIS_DSP_SRC ComplexMathFunctions/arm_cmplx_mag_f32.c StatisticsFunctions/arm_max_f32.c StatisticsFunctions/arm_rms_f32.c + StatisticsFunctions/arm_std_f32.c StatisticsFunctions/arm_mean_f32.c ) list(TRANSFORM CMSIS_DSP_SRC PREPEND "${CMSIS_DSP_DIR}/Source/") diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index bb35557f317..22a9809fbdc 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -37,8 +37,10 @@ #include "fc/config.h" STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; +STATIC_FASTRAM float32_t adaptiveFilterRaw[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0; +STATIC_FASTRAM pt1Filter_t stdFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM pt1Filter_t rmsFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM pt1Filter_t hpfFilter[XYZ_AXIS_COUNT]; @@ -62,6 +64,7 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) //Push new sample to the buffer so later we can compute RMS and other measures adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = filteredGyro; + adaptiveFilterRaw[index][adaptiveFilterSampleIndex] = value; adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE; } @@ -73,11 +76,14 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { //Initialize the filter for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pt1FilterInit(&rmsFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ); + pt1FilterInit(&stdFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ); } adaptiveFilterInitialized = 1; } float combinedRms = 0.0f; + float combinedStd = 0.0f; + float combinedStdRaw = 0.0f; //Compute RMS for each axis for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { @@ -91,18 +97,33 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { float32_t rms; arm_rms_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms); + float32_t std; + memcpy(tempBuffer, adaptiveFilterRaw[axis], sizeof(adaptiveFilterRaw[axis])); + arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std); + float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms); + float32_t filteredStd = pt1FilterApply(&stdFilter[axis], std); combinedRms += filteredRms; - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, axis, filteredRms * 1000.0f); + combinedStd += filteredStd; + combinedStdRaw += std; + + if (axis == 0) { + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, rms * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, filteredRms * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, std * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, filteredStd * 1000.0f); + } } combinedRms /= XYZ_AXIS_COUNT; + combinedStd /= XYZ_AXIS_COUNT; + combinedStdRaw /= XYZ_AXIS_COUNT; - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, combinedRms * 1000.0f); - - + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, combinedRms * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, combinedStdRaw * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 6, combinedStd * 1000.0f); } From f86365ba5f43d5dcf8fc8df20925608e6053a106 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Apr 2024 13:14:11 +0200 Subject: [PATCH 012/112] Settle only on Standard Deviation on high pass filter gyro data --- .vscode/c_cpp_properties.json | 33 +++++-------------------------- src/main/flight/adaptive_filter.c | 31 +++++------------------------ 2 files changed, 10 insertions(+), 54 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index a2c5f582e1c..3a8d8d1c8ca 100755 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -3,24 +3,19 @@ { "name": "Linux", "includePath": [ - "${workspaceRoot}/src/main/**", - "${workspaceRoot}/lib/main/**", - "/usr/include/**" + "${workspaceRoot}", + "${workspaceRoot}/src/main/**" ], "browse": { "limitSymbolsToIncludedHeaders": false, "path": [ - "${workspaceRoot}/src/main/**", - "${workspaceRoot}/lib/main/**" + "${workspaceRoot}/**" ] }, - "intelliSenseMode": "linux-gcc-arm", + "intelliSenseMode": "msvc-x64", "cStandard": "c11", "cppStandard": "c++17", "defines": [ - "MCU_FLASH_SIZE 512", - "USE_NAV", - "NAV_FIXED_WING_LANDING", "USE_OSD", "USE_GYRO_NOTCH_1", "USE_GYRO_NOTCH_2", @@ -34,31 +29,13 @@ "USE_RPM_FILTER", "USE_GLOBAL_FUNCTIONS", "USE_DYNAMIC_FILTERS", - "USE_IMU_BNO055", - "USE_SECONDARY_IMU", "USE_DSHOT", "FLASH_SIZE 480", "USE_I2C_IO_EXPANDER", "USE_PCF8574", "USE_ESC_SENSOR", - "USE_PROGRAMMING_FRAMEWORK", - "USE_SERIALRX_GHST", - "USE_TELEMETRY_GHST", - "USE_CMS", - "USE_DJI_HD_OSD", - "USE_GYRO_KALMAN", - "USE_RANGEFINDER", - "USE_RATE_DYNAMICS", - "USE_SMITH_PREDICTOR", - "USE_ALPHA_BETA_GAMMA_FILTER", - "USE_MAG_VCM5883", - "USE_TELEMETRY_JETIEXBUS", - "USE_NAV", - "USE_SDCARD_SDIO", - "USE_SDCARD", - "USE_Q_TUNE", - "USE_GYRO_FFT_FILTER", "USE_ADAPTIVE_FILTER", + "MCU_FLASH_SIZE 1024", ], "configurationProvider": "ms-vscode.cmake-tools" } diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index 22a9809fbdc..b6a584f92db 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -37,7 +37,6 @@ #include "fc/config.h" STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; -STATIC_FASTRAM float32_t adaptiveFilterRaw[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0; STATIC_FASTRAM pt1Filter_t stdFilter[XYZ_AXIS_COUNT]; @@ -60,11 +59,11 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) hpfFilterInitialized = 1; } + //Apply high pass filter, we are not interested in slowly changing values, only noise const float filteredGyro = value - pt1FilterApply(&hpfFilter[index], value); //Push new sample to the buffer so later we can compute RMS and other measures adaptiveFilterSamples[index][adaptiveFilterSampleIndex] = filteredGyro; - adaptiveFilterRaw[index][adaptiveFilterSampleIndex] = value; adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE; } @@ -75,54 +74,34 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { if (!adaptiveFilterInitialized) { //Initialize the filter for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - pt1FilterInit(&rmsFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ); pt1FilterInit(&stdFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ); } adaptiveFilterInitialized = 1; } - float combinedRms = 0.0f; float combinedStd = 0.0f; - float combinedStdRaw = 0.0f; //Compute RMS for each axis for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { //Copy axis samples to a temporary buffer float32_t tempBuffer[ADAPTIVE_FILTER_BUFFER_SIZE]; - //Use memcpy to copy the samples to the temporary buffer - memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis])); - - //Compute RMS from normalizedBuffer using arm_rms_f32 - float32_t rms; - arm_rms_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &rms); - + + //Copute STD from buffer using arm_std_f32 float32_t std; - memcpy(tempBuffer, adaptiveFilterRaw[axis], sizeof(adaptiveFilterRaw[axis])); + memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis])); arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std); - float32_t filteredRms = pt1FilterApply(&rmsFilter[axis], rms); float32_t filteredStd = pt1FilterApply(&stdFilter[axis], std); - combinedRms += filteredRms; - - combinedStd += filteredStd; - combinedStdRaw += std; + combinedStd += std; if (axis == 0) { - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, rms * 1000.0f); - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, filteredRms * 1000.0f); - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, std * 1000.0f); - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, filteredStd * 1000.0f); } } - combinedRms /= XYZ_AXIS_COUNT; combinedStd /= XYZ_AXIS_COUNT; - combinedStdRaw /= XYZ_AXIS_COUNT; - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, combinedRms * 1000.0f); - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, combinedStdRaw * 1000.0f); DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 6, combinedStd * 1000.0f); } From 369613643de343e35f8903966577b69dd3e004a9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Apr 2024 16:04:08 +0200 Subject: [PATCH 013/112] Compute the integator of adaptive filter --- src/main/flight/adaptive_filter.c | 43 +++++++++++++++++++++++++++---- src/main/flight/adaptive_filter.h | 1 + src/main/flight/pid.c | 3 +++ 3 files changed, 42 insertions(+), 5 deletions(-) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index b6a584f92db..448e37fd782 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -40,12 +40,21 @@ STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_B STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0; STATIC_FASTRAM pt1Filter_t stdFilter[XYZ_AXIS_COUNT]; -STATIC_FASTRAM pt1Filter_t rmsFilter[XYZ_AXIS_COUNT]; STATIC_FASTRAM pt1Filter_t hpfFilter[XYZ_AXIS_COUNT]; +/* + We want to run adaptive filter only when UAV is commanded to stay stationary + Any rotation request on axis will add noise that we are not interested in as it will + automatically cause LPF frequency to be lowered +*/ +STATIC_FASTRAM float axisAttenuationFactor[XYZ_AXIS_COUNT]; + STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0; STATIC_FASTRAM uint8_t hpfFilterInitialized = 0; +STATIC_FASTRAM float adaptiveFilterIntegrator; +STATIC_FASTRAM float adaptiveIntegratorTarget; + /** * This function is called at pid rate, so has to be initialized at PID loop frequency */ @@ -67,11 +76,19 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) adaptiveFilterSampleIndex = (adaptiveFilterSampleIndex + 1) % ADAPTIVE_FILTER_BUFFER_SIZE; } +void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate) { + const float maxRate = configRate * 10.0f; + axisAttenuationFactor[index] = scaleRangef(fabsf(rate), 0.0f, maxRate, 1.0f, 0.0f); + axisAttenuationFactor[index] = constrainf(axisAttenuationFactor[index], 0.0f, 1.0f); +} + void adaptiveFilterTask(timeUs_t currentTimeUs) { - static timeUs_t previousUpdateTimeUs; - const float dT = US2S(currentTimeUs - previousUpdateTimeUs); + static timeUs_t previousUpdateTimeUs = 0; if (!adaptiveFilterInitialized) { + adaptiveIntegratorTarget = 3.5f; + previousUpdateTimeUs = currentTimeUs; + //Initialize the filter for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pt1FilterInit(&stdFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ); @@ -79,6 +96,8 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { adaptiveFilterInitialized = 1; } + const float dT = US2S(currentTimeUs - previousUpdateTimeUs); + float combinedStd = 0.0f; //Compute RMS for each axis @@ -92,17 +111,31 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { memcpy(tempBuffer, adaptiveFilterSamples[axis], sizeof(adaptiveFilterSamples[axis])); arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std); - float32_t filteredStd = pt1FilterApply(&stdFilter[axis], std); + const float filteredStd = pt1FilterApply(&stdFilter[axis], std); + + const float error = filteredStd - adaptiveIntegratorTarget; + + const float adjustedError = error * axisAttenuationFactor[axis]; + + const float timeAdjustedError = adjustedError * dT; + + //Put into integrator + adaptiveFilterIntegrator += timeAdjustedError; combinedStd += std; if (axis == 0) { + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f); } } combinedStd /= XYZ_AXIS_COUNT; - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 6, combinedStd * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator); } diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h index 9a9d4dc88fb..2885023bbd6 100644 --- a/src/main/flight/adaptive_filter.h +++ b/src/main/flight/adaptive_filter.h @@ -31,4 +31,5 @@ #define ADAPTIVE_FILTER_HPF_HZ 15 void adaptiveFilterPush(const flight_dynamics_index_t index, const float value); +void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate); void adaptiveFilterTask(timeUs_t currentTimeUs); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3da150e39dc..d1dbb75ce67 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -47,6 +47,7 @@ #include "flight/rpm_filter.h" #include "flight/kalman.h" #include "flight/smith_predictor.h" +#include "flight/adaptive_filter.h" #include "io/gps.h" @@ -1168,6 +1169,8 @@ void FAST_CODE pidController(float dT) // Limit desired rate to something gyro can measure reliably pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); + adaptiveFilterPushRate(axis, pidState[axis].rateTarget, currentControlRateProfile->stabilized.rates[axis]); + #ifdef USE_GYRO_KALMAN gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget); #endif From 561f7b8be254bc049524ada5d6582ea2022e3028 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Apr 2024 16:39:25 +0200 Subject: [PATCH 014/112] Compute integrator --- src/main/flight/adaptive_filter.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index 448e37fd782..3a453679af3 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -85,6 +85,7 @@ void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rat void adaptiveFilterTask(timeUs_t currentTimeUs) { static timeUs_t previousUpdateTimeUs = 0; + //Initialization procedure, filter setup etc. if (!adaptiveFilterInitialized) { adaptiveIntegratorTarget = 3.5f; previousUpdateTimeUs = currentTimeUs; @@ -96,7 +97,9 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { adaptiveFilterInitialized = 1; } + //Prepare time delta to normalize time factor of the integrator const float dT = US2S(currentTimeUs - previousUpdateTimeUs); + previousUpdateTimeUs = currentTimeUs; float combinedStd = 0.0f; From ac242b566a07e96d61705940248b9514f26e826c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Apr 2024 21:01:39 +0200 Subject: [PATCH 015/112] Basic approach to adaptive filter. --- src/main/flight/adaptive_filter.c | 55 +++++++++++++++++++++++++++---- src/main/flight/adaptive_filter.h | 3 ++ src/main/sensors/gyro.c | 4 +++ 3 files changed, 56 insertions(+), 6 deletions(-) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index 3a453679af3..7501d0b5d3d 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -35,6 +35,7 @@ #include "common/filter.h" #include "build/debug.h" #include "fc/config.h" +#include "sensors/gyro.h" STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; STATIC_FASTRAM uint8_t adaptiveFilterSampleIndex = 0; @@ -52,6 +53,12 @@ STATIC_FASTRAM float axisAttenuationFactor[XYZ_AXIS_COUNT]; STATIC_FASTRAM uint8_t adaptiveFilterInitialized = 0; STATIC_FASTRAM uint8_t hpfFilterInitialized = 0; +//Defines if current, min and max values for the filter were set and filter is ready to work +STATIC_FASTRAM uint8_t targetsSet = 0; +STATIC_FASTRAM float currentLpf; +STATIC_FASTRAM float minLpf; +STATIC_FASTRAM float maxLpf; + STATIC_FASTRAM float adaptiveFilterIntegrator; STATIC_FASTRAM float adaptiveIntegratorTarget; @@ -82,7 +89,24 @@ void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rat axisAttenuationFactor[index] = constrainf(axisAttenuationFactor[index], 0.0f, 1.0f); } +void adaptiveFilterResetIntegrator(void) { + adaptiveFilterIntegrator = 0.0f; +} + +void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max) { + currentLpf = lpf; + minLpf = min; + maxLpf = max; + targetsSet = 1; +} + void adaptiveFilterTask(timeUs_t currentTimeUs) { + + //If we don't have current, min and max values for the filter, we can't run it yet + if (!targetsSet) { + return; + } + static timeUs_t previousUpdateTimeUs = 0; //Initialization procedure, filter setup etc. @@ -127,18 +151,37 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { combinedStd += std; - if (axis == 0) { - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f); - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f); - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f); - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f); - } + // if (axis == 0) { + // DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f); + // DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f); + // DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f); + // DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f); + // } } + //TODO filter gets updated only when ARMED + + if (adaptiveFilterIntegrator > ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) { + //In this case there is too much noise, we need to lower the LPF frequency + currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf); + gyroUpdateDynamicLpf(currentLpf); + adaptiveFilterResetIntegrator(); + } else if (adaptiveFilterIntegrator < -ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) { + //In this case there is too little noise, we can to increase the LPF frequency + currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf); + gyroUpdateDynamicLpf(currentLpf); + adaptiveFilterResetIntegrator(); + } + + // if (ARMING_FLAG(ARMED)) { + // // + // } + combinedStd /= XYZ_AXIS_COUNT; DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f); DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, currentLpf); } diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h index 2885023bbd6..725f992a665 100644 --- a/src/main/flight/adaptive_filter.h +++ b/src/main/flight/adaptive_filter.h @@ -29,7 +29,10 @@ #define ADAPTIVE_FILTER_RATE_HZ 100 #define ADAPTIVE_FILTER_LPF_HZ 1 #define ADAPTIVE_FILTER_HPF_HZ 15 +#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD 20.0f void adaptiveFilterPush(const flight_dynamics_index_t index, const float value); void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate); +void adaptiveFilterResetIntegrator(void); +void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max); void adaptiveFilterTask(timeUs_t currentTimeUs); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 5873c9d7950..0a9c8be96fd 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -264,6 +264,10 @@ static void gyroInitFilters(void) //Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime()); +#ifdef USE_ADAPTIVE_FILTER + adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, 50, 150); +#endif + #ifdef USE_GYRO_KALMAN if (gyroConfig()->kalmanEnabled) { gyroKalmanInitialize(gyroConfig()->kalman_q); From 63993ba7ff868ff5f2078c5e296ec7851790b9dd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Apr 2024 21:04:38 +0200 Subject: [PATCH 016/112] Compilation fix --- src/main/flight/pid.c | 4 +++- src/main/sensors/gyro.c | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d1dbb75ce67..61a5083dfe0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1168,8 +1168,10 @@ void FAST_CODE pidController(float dT) // Limit desired rate to something gyro can measure reliably pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT); - + +#ifdef USE_ADAPTIVE_FILTER adaptiveFilterPushRate(axis, pidState[axis].rateTarget, currentControlRateProfile->stabilized.rates[axis]); +#endif #ifdef USE_GYRO_KALMAN gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 0a9c8be96fd..c439f8659ee 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -458,7 +458,9 @@ void FAST_CODE NOINLINE gyroFilter(void) gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf); +#ifdef USE_ADAPTIVE_FILTER adaptiveFilterPush(axis, gyroADCf); +#endif #ifdef USE_DYNAMIC_FILTERS if (dynamicGyroNotchState.enabled) { From 731f0608c46e2f218585d4226b8516456f40959c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 7 Apr 2024 14:22:30 +0200 Subject: [PATCH 017/112] Run damping of FW with stick movements --- src/main/flight/pid.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3da150e39dc..d6c8fe81f39 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -758,16 +758,19 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh { const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); + const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; + const float dampingFactor = bellCurve(MIN(rateTarget, maxRate), maxRate); + const float rateError = rateTarget - pidState->gyroRate; - const float newPTerm = pTermProcess(pidState, rateError, dT); - const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv); + const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor; + const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactor; const float newFFTerm = rateTarget * pidState->kFF; /* * Integral should be updated only if axis Iterm is not frozen */ if (!pidState->itermFreezeActive) { - pidState->errorGyroIf += rateError * pidState->kI * dT; + pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactor; } applyItermLimiting(pidState); From 1f723837e96e37ea05e0aba99b983d91c1d87152 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 7 Apr 2024 20:28:45 +0200 Subject: [PATCH 018/112] Improve attenuation --- src/main/common/maths.c | 18 +++++++++++++++++- src/main/common/maths.h | 2 ++ src/main/flight/pid.c | 4 +++- 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 1bc61ae4c36..eed7b863bae 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -516,9 +516,25 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu return sensorCalibrationValidateResult(result); } +float gaussian(const float x, const float mu, const float sigma) { + return exp(-pow(x - mu, 2) / (2 * pow(sigma, 2))); +} + float bellCurve(const float x, const float curveWidth) { - return powf(M_Ef, -sq(x) / (2.0f * sq(curveWidth))); + return gaussian(x, 0.0f, curveWidth); +} + +/** + * @brief Calculate the attenuation of a value using a Gaussian function. + * Retuns 1 for input 0 and ~0 for input width. + * @param input The input value. + * @param width The width of the Gaussian function. + * @return The attenuation of the input value. +*/ +float attenuation(const float input, const float width) { + const float sigma = width / 2.35482f; // Approximately width / sqrt(2 * ln(2)) + return gaussian(input, 0.0f, sigma); } float fast_fsqrtf(const float value) { diff --git a/src/main/common/maths.h b/src/main/common/maths.h index f3f2fd62746..ce96b7064fd 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -195,6 +195,8 @@ float acos_approx(float x); void arraySubInt32(int32_t *dest, int32_t *array1, int32_t *array2, int count); float bellCurve(const float x, const float curveWidth); +float attenuation(const float input, const float width); +float gaussian(const float x, const float mu, const float sigma); float fast_fsqrtf(const float value); float calc_length_pythagorean_2D(const float firstElement, const float secondElement); float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d6c8fe81f39..9e50522ae3f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -759,7 +759,9 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; - const float dampingFactor = bellCurve(MIN(rateTarget, maxRate), maxRate); + const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f); + + DEBUG_SET(DEBUG_ALWAYS, axis, dampingFactor * 1000); const float rateError = rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor; From 37bb8580db0f7cadf109bbeba079bd2999aa1556 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 7 Apr 2024 21:46:36 +0200 Subject: [PATCH 019/112] Fix compilation for SITL --- src/main/common/maths.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index eed7b863bae..b8ec59c33a2 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -517,7 +517,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu } float gaussian(const float x, const float mu, const float sigma) { - return exp(-pow(x - mu, 2) / (2 * pow(sigma, 2))); + return exp(-pow((double)(x - mu), 2) / (2 * pow((double)sigma, 2))); } float bellCurve(const float x, const float curveWidth) From 5f4dbb79138eab5c5c1f598c6ca338fb89dffe5c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 7 Apr 2024 21:54:04 +0200 Subject: [PATCH 020/112] Drop no longer previous generation dampening solution --- src/main/fc/settings.yaml | 6 ------ src/main/flight/pid.c | 27 ++++----------------------- src/main/flight/pid.h | 1 - 3 files changed, 4 insertions(+), 30 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index cb586eacf06..a3b117d7a3b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1934,12 +1934,6 @@ groups: field: fixedWingCoordinatedPitchGain min: 0 max: 2 - - name: fw_iterm_limit_stick_position - description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side" - default_value: 0.5 - field: fixedWingItermLimitOnStickPosition - min: 0 - max: 1 - name: fw_yaw_iterm_freeze_bank_angle description: "Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled." default_value: 0 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9e50522ae3f..b92aec705f2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -158,7 +158,6 @@ static EXTENDED_FASTRAM uint8_t usedPidControllerType; typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv); static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; -static EXTENDED_FASTRAM bool levelingEnabled = false; static EXTENDED_FASTRAM bool restartAngleHoldMode = true; static EXTENDED_FASTRAM bool angleHoldIsLevel = false; @@ -170,7 +169,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 6); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 7); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -271,7 +270,6 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT, .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT, .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT, - .fixedWingItermLimitOnStickPosition = SETTING_FW_ITERM_LIMIT_STICK_POSITION_DEFAULT, .fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT, .navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT, @@ -672,19 +670,6 @@ static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_i } } -bool isFixedWingItermLimitActive(float stickPosition) -{ - /* - * Iterm anti windup whould be active only when pilot controls the rotation - * velocity directly, not when ANGLE or HORIZON are used - */ - if (levelingEnabled) { - return false; - } - - return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition; -} - static float pTermProcess(pidState_t *pidState, float rateError, float dT) { float newPTerm = rateError * pidState->kP; @@ -1050,11 +1035,9 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng void checkItermLimitingActive(pidState_t *pidState) { - bool shouldActivate; - if (usedPidControllerType == PID_TYPE_PIFF) { - shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); - } else - { + bool shouldActivate = false; + + if (usedPidControllerType == PID_TYPE_PID) { shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent } @@ -1184,7 +1167,6 @@ void FAST_CODE pidController(float dT) // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ANGLEHOLD_MODE const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f; - levelingEnabled = false; angleHoldIsLevel = false; for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) { @@ -1204,7 +1186,6 @@ void FAST_CODE pidController(float dT) // Apply the Level PID controller pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with ANGLE/HORIZON - levelingEnabled = true; } else { restartAngleHoldMode = true; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e0a8b9d310b..2e28848d3cc 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -123,7 +123,6 @@ typedef struct pidProfile_s { float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. - float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint16_t fixedWingYawItermBankFreeze; // Freeze yaw Iterm when bank angle is more than this many degrees float navVelXyDTermLpfHz; From 292db1f14c9bac72fbe7ac17963f6c2a4b21e36e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 8 Apr 2024 15:39:33 +0200 Subject: [PATCH 021/112] Allo only gradual relaxation of the Axis factor --- src/main/flight/pid.c | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b92aec705f2..19708b50f00 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -107,6 +107,8 @@ typedef struct { pt3Filter_t rateTargetFilter; smithPredictor_t smithPredictor; + + float dampingFactor; } pidState_t; STATIC_FASTRAM bool pidFiltersConfigured = false; @@ -746,7 +748,16 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f); - DEBUG_SET(DEBUG_ALWAYS, axis, dampingFactor * 1000); + if (fabsf(dampingFactor) < fabsf(pidState->dampingFactor)) { + pidState->dampingFactor = dampingFactor; + } else { + pidState->dampingFactor = pidState->dampingFactor + (dampingFactor - pidState->dampingFactor) * dT * 10; + } + + float newDF = pidState->dampingFactor; + + DEBUG_SET(DEBUG_ALWAYS, axis * 2, dampingFactor * 1000); + DEBUG_SET(DEBUG_ALWAYS, (axis * 2) + 1, newDF * 1000); const float rateError = rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor; @@ -1262,6 +1273,8 @@ void pidInit(void) for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { + pidState[axis].dampingFactor = 1.0f; + #ifdef USE_D_BOOST // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration. // We assume, max acceleration is when pilot deflects the stick fully in 100ms From cf10c071e072589527d072b5fa381b7e416bb148 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 8 Apr 2024 15:40:21 +0200 Subject: [PATCH 022/112] Docs update --- docs/Settings.md | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index b6fd431d950..c33efe6ef9d 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1292,16 +1292,6 @@ Fixed-wing rate stabilisation I-gain for YAW --- -### fw_iterm_limit_stick_position - -Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side - -| Default | Min | Max | -| --- | --- | --- | -| 0.5 | 0 | 1 | - ---- - ### fw_level_pitch_gain I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations From c41abacd2a37fee7e0ef2af0aabcb4d818a96f5b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 8 Apr 2024 19:53:11 +0200 Subject: [PATCH 023/112] Reset adaptive filter on disarm --- src/main/flight/adaptive_filter.c | 29 +++++++++++++++-------------- src/main/flight/adaptive_filter.h | 3 ++- 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index 7501d0b5d3d..138203a6b0e 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -35,6 +35,7 @@ #include "common/filter.h" #include "build/debug.h" #include "fc/config.h" +#include "fc/runtime_config.h" #include "sensors/gyro.h" STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; @@ -56,6 +57,7 @@ STATIC_FASTRAM uint8_t hpfFilterInitialized = 0; //Defines if current, min and max values for the filter were set and filter is ready to work STATIC_FASTRAM uint8_t targetsSet = 0; STATIC_FASTRAM float currentLpf; +STATIC_FASTRAM float initialLpf; STATIC_FASTRAM float minLpf; STATIC_FASTRAM float maxLpf; @@ -97,6 +99,8 @@ void adaptiveFilterSetDefaultFrequency(int lpf, int min, int max) { currentLpf = lpf; minLpf = min; maxLpf = max; + initialLpf = currentLpf; + targetsSet = 1; } @@ -121,6 +125,14 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { adaptiveFilterInitialized = 1; } + //If not armed, leave this routine but reset integrator and set default LPF + if (!ARMING_FLAG(ARMED)) { + currentLpf = initialLpf; + adaptiveFilterResetIntegrator(); + gyroUpdateDynamicLpf(currentLpf); + return; + } + //Prepare time delta to normalize time factor of the integrator const float dT = US2S(currentTimeUs - previousUpdateTimeUs); previousUpdateTimeUs = currentTimeUs; @@ -150,37 +162,26 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { adaptiveFilterIntegrator += timeAdjustedError; combinedStd += std; - - // if (axis == 0) { - // DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, filteredStd * 1000.0f); - // DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 3, error * 1000.0f); - // DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 4, adjustedError * 1000.0f); - // DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 5, timeAdjustedError * 1000.0f); - // } } //TODO filter gets updated only when ARMED - if (adaptiveFilterIntegrator > ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) { + if (adaptiveFilterIntegrator > ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH) { //In this case there is too much noise, we need to lower the LPF frequency currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf); gyroUpdateDynamicLpf(currentLpf); adaptiveFilterResetIntegrator(); - } else if (adaptiveFilterIntegrator < -ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD) { + } else if (adaptiveFilterIntegrator < ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW) { //In this case there is too little noise, we can to increase the LPF frequency currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf); gyroUpdateDynamicLpf(currentLpf); adaptiveFilterResetIntegrator(); } - // if (ARMING_FLAG(ARMED)) { - // // - // } - combinedStd /= XYZ_AXIS_COUNT; DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 0, combinedStd * 1000.0f); - DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator); + DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 1, adaptiveFilterIntegrator * 10.0f); DEBUG_SET(DEBUG_ADAPTIVE_FILTER, 2, currentLpf); } diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h index 725f992a665..0a8696d90dd 100644 --- a/src/main/flight/adaptive_filter.h +++ b/src/main/flight/adaptive_filter.h @@ -29,7 +29,8 @@ #define ADAPTIVE_FILTER_RATE_HZ 100 #define ADAPTIVE_FILTER_LPF_HZ 1 #define ADAPTIVE_FILTER_HPF_HZ 15 -#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD 20.0f +#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH 5.0f +#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW -2.0f void adaptiveFilterPush(const flight_dynamics_index_t index, const float value); void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate); From 6fd60772510f2351ca667d138b506e470bdb0119 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 8 Apr 2024 20:03:57 +0200 Subject: [PATCH 024/112] Run the filter only if throttle is raised --- src/main/flight/adaptive_filter.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index 138203a6b0e..e94df2d19fe 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -36,6 +36,7 @@ #include "build/debug.h" #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/rc_controls.h" #include "sensors/gyro.h" STATIC_FASTRAM float32_t adaptiveFilterSamples[XYZ_AXIS_COUNT][ADAPTIVE_FILTER_BUFFER_SIZE]; @@ -133,6 +134,11 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { return; } + //Do not run adaptive filter when throttle is low + if (rcCommand[THROTTLE] < 1200) { + return; + } + //Prepare time delta to normalize time factor of the integrator const float dT = US2S(currentTimeUs - previousUpdateTimeUs); previousUpdateTimeUs = currentTimeUs; From 3005c69f8f99616f3822056baebfa9f6cae980a8 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 9 Apr 2024 10:30:36 +0200 Subject: [PATCH 025/112] Some progress --- src/main/flight/pid.c | 50 ++++++++++++++++++++++++++++++++----------- 1 file changed, 38 insertions(+), 12 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 19708b50f00..50c67c86074 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -108,7 +108,10 @@ typedef struct { smithPredictor_t smithPredictor; - float dampingFactor; + float dampingFactorPrevious; + float dampingFactorLockValue; + float dampingFactotLockUntilMs; + pt1Filter_t dampingFactorFilter; } pidState_t; STATIC_FASTRAM bool pidFiltersConfigured = false; @@ -322,6 +325,7 @@ bool pidInitFilters(void) for (int i = 0; i < XYZ_AXIS_COUNT; i++) { pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate)); + pt1FilterInit(&pidState[i].dampingFactorFilter, 2.0f, US2S(refreshRate)); } #ifdef USE_ANTIGRAVITY @@ -748,27 +752,49 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f); - if (fabsf(dampingFactor) < fabsf(pidState->dampingFactor)) { - pidState->dampingFactor = dampingFactor; + float dampingFactorP; + float dampingFactorD; + float dampingFactorI; + + if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) { + dampingFactorP = dampingFactor; + dampingFactorD = dampingFactor; + dampingFactorI = dampingFactor; + + pidState->dampingFactotLockUntilMs = millis() + scaleRangef(fabsf(dampingFactor), 1.0f, 0.0f, 0, 300); + pidState->dampingFactorLockValue = dampingFactor; + + // pt1FilterReset(&pidState->dampingFactorFilter, pidState->dampingFactorPrevious); + pidState->dampingFactorPrevious = dampingFactor; } else { - pidState->dampingFactor = pidState->dampingFactor + (dampingFactor - pidState->dampingFactor) * dT * 10; + dampingFactorP = dampingFactor; + dampingFactorD = dampingFactor; + + if (millis() > pidState->dampingFactorLockValue) { + dampingFactorI = dampingFactor; + pidState->dampingFactorPrevious = dampingFactor; + } else { + dampingFactorI = pidState->dampingFactorLockValue; + } } - float newDF = pidState->dampingFactor; - - DEBUG_SET(DEBUG_ALWAYS, axis * 2, dampingFactor * 1000); - DEBUG_SET(DEBUG_ALWAYS, (axis * 2) + 1, newDF * 1000); + if (axis == FD_ROLL) { + DEBUG_SET(DEBUG_ALWAYS, 0, pidState->dampingFactorPrevious * 1000); + DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorP * 1000); + DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorI * 1000); + DEBUG_SET(DEBUG_ALWAYS, 3, dampingFactorD * 1000); + } const float rateError = rateTarget - pidState->gyroRate; - const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactor; - const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactor; + const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactorP; + const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactorD; const float newFFTerm = rateTarget * pidState->kFF; /* * Integral should be updated only if axis Iterm is not frozen */ if (!pidState->itermFreezeActive) { - pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactor; + pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactorI; } applyItermLimiting(pidState); @@ -1273,7 +1299,7 @@ void pidInit(void) for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { - pidState[axis].dampingFactor = 1.0f; + pidState[axis].dampingFactorPrevious = 1.0f; #ifdef USE_D_BOOST // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration. From bff23ee0e5b23110a8225535478177ed9824f88e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 9 Apr 2024 13:53:25 +0200 Subject: [PATCH 026/112] Some updates --- src/main/flight/pid.c | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 50c67c86074..90a31a4aa16 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -756,9 +756,16 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh float dampingFactorD; float dampingFactorI; + /* + TODO conditions have to be reworked + when drowing, damp + when releasing, lock + locking procedure has to be separated from action + + + */ + if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) { - dampingFactorP = dampingFactor; - dampingFactorD = dampingFactor; dampingFactorI = dampingFactor; pidState->dampingFactotLockUntilMs = millis() + scaleRangef(fabsf(dampingFactor), 1.0f, 0.0f, 0, 300); @@ -767,9 +774,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh // pt1FilterReset(&pidState->dampingFactorFilter, pidState->dampingFactorPrevious); pidState->dampingFactorPrevious = dampingFactor; } else { - dampingFactorP = dampingFactor; - dampingFactorD = dampingFactor; - + if (millis() > pidState->dampingFactorLockValue) { dampingFactorI = dampingFactor; pidState->dampingFactorPrevious = dampingFactor; @@ -778,6 +783,10 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh } } + //P & D damping factors are always the same and based on current damping factor + dampingFactorP = dampingFactor; + dampingFactorD = dampingFactor; + if (axis == FD_ROLL) { DEBUG_SET(DEBUG_ALWAYS, 0, pidState->dampingFactorPrevious * 1000); DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorP * 1000); From aef3fbe3b966696cdd3fe5555d35dfce99749a71 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 9 Apr 2024 18:42:20 +0200 Subject: [PATCH 027/112] New conditions for Iterm dampener --- src/main/flight/pid.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 90a31a4aa16..436e4b17471 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -757,12 +757,12 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh float dampingFactorI; /* - TODO conditions have to be reworked - when drowing, damp - when releasing, lock - locking procedure has to be separated from action - + New idea.... + Iterm damping is applied (down to 0) when: + abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark) + itermAttenuation = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1) + */ if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) { From 3eece0a3773e5dc27dfe454f6199cc5ec541d41d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 10 Apr 2024 12:39:32 +0200 Subject: [PATCH 028/112] Commit switch to longer buffer --- src/main/flight/adaptive_filter.c | 3 --- src/main/flight/adaptive_filter.h | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index e94df2d19fe..a8e3dec0293 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -157,11 +157,8 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { arm_std_f32(tempBuffer, ADAPTIVE_FILTER_BUFFER_SIZE, &std); const float filteredStd = pt1FilterApply(&stdFilter[axis], std); - const float error = filteredStd - adaptiveIntegratorTarget; - const float adjustedError = error * axisAttenuationFactor[axis]; - const float timeAdjustedError = adjustedError * dT; //Put into integrator diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h index 0a8696d90dd..76117850d3e 100644 --- a/src/main/flight/adaptive_filter.h +++ b/src/main/flight/adaptive_filter.h @@ -25,7 +25,7 @@ #include "common/axis.h" #include "common/time.h" -#define ADAPTIVE_FILTER_BUFFER_SIZE 32 +#define ADAPTIVE_FILTER_BUFFER_SIZE 64 #define ADAPTIVE_FILTER_RATE_HZ 100 #define ADAPTIVE_FILTER_LPF_HZ 1 #define ADAPTIVE_FILTER_HPF_HZ 15 From 9ac3b540a1eabdb83ef6a4af2103667f9b898614 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 11 Apr 2024 11:59:06 +0200 Subject: [PATCH 029/112] Make the filter configurable --- src/main/fc/config.c | 10 ++++++ src/main/fc/settings.yaml | 55 +++++++++++++++++++++++++++++++ src/main/flight/adaptive_filter.c | 10 +++--- src/main/flight/adaptive_filter.h | 4 --- src/main/sensors/gyro.c | 16 +++++++-- src/main/sensors/gyro.h | 10 ++++++ 6 files changed, 93 insertions(+), 12 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c9a50c60a25..c403b0cfefd 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -190,6 +190,16 @@ uint32_t getGyroLooptime(void) void validateAndFixConfig(void) { + + // gyroConfig()->adaptiveFilterMinHz has to be at least 5 units lower than gyroConfig()->gyro_main_lpf_hz + if (gyroConfig()->adaptiveFilterMinHz + 5 > gyroConfig()->gyro_main_lpf_hz) { + gyroConfigMutable()->adaptiveFilterMinHz = gyroConfig()->gyro_main_lpf_hz - 5; + } + //gyroConfig()->adaptiveFilterMaxHz has to be at least 5 units higher than gyroConfig()->gyro_main_lpf_hz + if (gyroConfig()->adaptiveFilterMaxHz - 5 < gyroConfig()->gyro_main_lpf_hz) { + gyroConfigMutable()->adaptiveFilterMaxHz = gyroConfig()->gyro_main_lpf_hz + 5; + } + if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) { accelerometerConfigMutable()->acc_notch_hz = 0; } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9fd6c10e860..4196a7cefc3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -330,6 +330,61 @@ groups: field: gravity_cmss_cal min: 0 max: 2000 + - name: gyro_adaptive_filter + description: "Enable adaptive filter for gyro data" + default_value: OFF + field: adaptiveFilterEnabled + type: bool + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_target + description: "Target value for adaptive filter" + default_value: 3.5 + field: adaptiveFilterTarget + min: 2 + max: 6 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_min_hz + description: "Minimum frequency for adaptive filter" + default_value: 50 + field: adaptiveFilterMinHz + min: 30 + max: 250 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_max_hz + description: "Maximum frequency for adaptive filter" + default_value: 150 + field: adaptiveFilterMaxHz + min: 100 + max: 500 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_std_lpf_hz + description: "Standard deviation low pass filter cutoff frequency" + default_value: 1 + field: adaptiveFilterStdLpfHz + min: 0 + max: 5 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_hpf_hz + description: "High pass filter cutoff frequency" + default_value: 15 + field: adaptiveFilterHpfHz + min: 1 + max: 50 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_integrator_threshold_high + description: "High threshold for adaptive filter integrator" + default_value: 5 + field: adaptiveFilterIntegratorThresholdHigh + min: 1 + max: 10 + condition: USE_ADAPTIVE_FILTER + - name: gyro_adaptive_filter_integrator_threshold_low + description: "Low threshold for adaptive filter integrator" + default_value: -2 + field: adaptiveFilterIntegratorThresholdLow + min: -10 + max: 0 + condition: USE_ADAPTIVE_FILTER - name: PG_ADC_CHANNEL_CONFIG type: adcChannelConfig_t diff --git a/src/main/flight/adaptive_filter.c b/src/main/flight/adaptive_filter.c index a8e3dec0293..4770e9763ab 100644 --- a/src/main/flight/adaptive_filter.c +++ b/src/main/flight/adaptive_filter.c @@ -73,7 +73,7 @@ void adaptiveFilterPush(const flight_dynamics_index_t index, const float value) if (!hpfFilterInitialized) { //Initialize the filter for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - pt1FilterInit(&hpfFilter[axis], ADAPTIVE_FILTER_HPF_HZ, US2S(getLooptime())); + pt1FilterInit(&hpfFilter[axis], gyroConfig()->adaptiveFilterHpfHz, US2S(getLooptime())); } hpfFilterInitialized = 1; } @@ -121,7 +121,7 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { //Initialize the filter for (flight_dynamics_index_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - pt1FilterInit(&stdFilter[axis], ADAPTIVE_FILTER_LPF_HZ, 1.0f / ADAPTIVE_FILTER_RATE_HZ); + pt1FilterInit(&stdFilter[axis], gyroConfig()->adaptiveFilterStdLpfHz, 1.0f / ADAPTIVE_FILTER_RATE_HZ); } adaptiveFilterInitialized = 1; } @@ -167,14 +167,12 @@ void adaptiveFilterTask(timeUs_t currentTimeUs) { combinedStd += std; } - //TODO filter gets updated only when ARMED - - if (adaptiveFilterIntegrator > ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH) { + if (adaptiveFilterIntegrator > gyroConfig()->adaptiveFilterIntegratorThresholdHigh) { //In this case there is too much noise, we need to lower the LPF frequency currentLpf = constrainf(currentLpf - 1.0f, minLpf, maxLpf); gyroUpdateDynamicLpf(currentLpf); adaptiveFilterResetIntegrator(); - } else if (adaptiveFilterIntegrator < ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW) { + } else if (adaptiveFilterIntegrator < gyroConfig()->adaptiveFilterIntegratorThresholdLow) { //In this case there is too little noise, we can to increase the LPF frequency currentLpf = constrainf(currentLpf + 1.0f, minLpf, maxLpf); gyroUpdateDynamicLpf(currentLpf); diff --git a/src/main/flight/adaptive_filter.h b/src/main/flight/adaptive_filter.h index 76117850d3e..c8696a91f8a 100644 --- a/src/main/flight/adaptive_filter.h +++ b/src/main/flight/adaptive_filter.h @@ -27,10 +27,6 @@ #define ADAPTIVE_FILTER_BUFFER_SIZE 64 #define ADAPTIVE_FILTER_RATE_HZ 100 -#define ADAPTIVE_FILTER_LPF_HZ 1 -#define ADAPTIVE_FILTER_HPF_HZ 15 -#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH 5.0f -#define ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW -2.0f void adaptiveFilterPush(const flight_dynamics_index_t index, const float value); void adaptiveFilterPushRate(const flight_dynamics_index_t index, const float rate, const uint8_t configRate); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 8585d3a0522..fd2036ef9a2 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -96,7 +96,7 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT, @@ -123,6 +123,16 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .init_gyro_cal_enabled = SETTING_INIT_GYRO_CAL_DEFAULT, .gyro_zero_cal = {SETTING_GYRO_ZERO_X_DEFAULT, SETTING_GYRO_ZERO_Y_DEFAULT, SETTING_GYRO_ZERO_Z_DEFAULT}, .gravity_cmss_cal = SETTING_INS_GRAVITY_CMSS_DEFAULT, +#ifdef USE_ADAPTIVE_FILTER + .adaptiveFilterEnabled = SETTING_GYRO_ADAPTIVE_FILTER_DEFAULT, + .adaptiveFilterTarget = SETTING_GYRO_ADAPTIVE_FILTER_TARGET_DEFAULT, + .adaptiveFilterMinHz = SETTING_GYRO_ADAPTIVE_FILTER_MIN_HZ_DEFAULT, + .adaptiveFilterMaxHz = SETTING_GYRO_ADAPTIVE_FILTER_MAX_HZ_DEFAULT, + .adaptiveFilterStdLpfHz = SETTING_GYRO_ADAPTIVE_FILTER_STD_LPF_HZ_DEFAULT, + .adaptiveFilterHpfHz = SETTING_GYRO_ADAPTIVE_FILTER_HPF_HZ_DEFAULT, + .adaptiveFilterIntegratorThresholdHigh = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH_DEFAULT, + .adaptiveFilterIntegratorThresholdLow = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW_DEFAULT, +#endif ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) @@ -251,7 +261,9 @@ static void gyroInitFilters(void) initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime()); #ifdef USE_ADAPTIVE_FILTER - adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, 50, 150); + if (gyroConfig()->adaptiveFilterEnabled) { + adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, gyroConfig()->adaptiveFilterMinHz, gyroConfig()->adaptiveFilterMaxHz); + } #endif #ifdef USE_GYRO_KALMAN diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 2ea70c589da..75ff7983372 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -87,6 +87,16 @@ typedef struct gyroConfig_s { bool init_gyro_cal_enabled; int16_t gyro_zero_cal[XYZ_AXIS_COUNT]; float gravity_cmss_cal; +#ifdef USE_ADAPTIVE_FILTER + uint8_t adaptiveFilterEnabled; + float adaptiveFilterTarget; + uint16_t adaptiveFilterMinHz; + uint16_t adaptiveFilterMaxHz; + float adaptiveFilterStdLpfHz; + float adaptiveFilterHpfHz; + float adaptiveFilterIntegratorThresholdHigh; + float adaptiveFilterIntegratorThresholdLow; +#endif } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); From 8c8840f432f730f9985812ef528bd4ccd4c3d7c3 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 11 Apr 2024 15:05:09 +0200 Subject: [PATCH 030/112] Update defaults --- src/main/fc/settings.yaml | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4196a7cefc3..a495e691cbc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -340,7 +340,7 @@ groups: description: "Target value for adaptive filter" default_value: 3.5 field: adaptiveFilterTarget - min: 2 + min: 1 max: 6 condition: USE_ADAPTIVE_FILTER - name: gyro_adaptive_filter_min_hz @@ -359,21 +359,21 @@ groups: condition: USE_ADAPTIVE_FILTER - name: gyro_adaptive_filter_std_lpf_hz description: "Standard deviation low pass filter cutoff frequency" - default_value: 1 + default_value: 2 field: adaptiveFilterStdLpfHz min: 0 - max: 5 + max: 10 condition: USE_ADAPTIVE_FILTER - name: gyro_adaptive_filter_hpf_hz description: "High pass filter cutoff frequency" - default_value: 15 + default_value: 10 field: adaptiveFilterHpfHz min: 1 max: 50 condition: USE_ADAPTIVE_FILTER - name: gyro_adaptive_filter_integrator_threshold_high description: "High threshold for adaptive filter integrator" - default_value: 5 + default_value: 4 field: adaptiveFilterIntegratorThresholdHigh min: 1 max: 10 From 02d4a52d5da9329b005e62f1445716c80e14e755 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 15 Apr 2024 11:44:25 +0200 Subject: [PATCH 031/112] Enable adaptive filter for all targets --- src/main/target/common.h | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/target/common.h b/src/main/target/common.h index 09f1b71e7e2..a6da8c5a1d3 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -211,9 +211,4 @@ #endif #define USE_EZ_TUNE - -#ifdef STM32H7 - #define USE_ADAPTIVE_FILTER - -#endif From d27f3fdaa0e5b9f676a7f4e94a1c90e659e43703 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 17 Apr 2024 15:18:09 +0200 Subject: [PATCH 032/112] Updated Iterm locking mechanism --- src/main/flight/pid.c | 61 ++++++++++++++++--------------------------- 1 file changed, 23 insertions(+), 38 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ae51385cf11..941e08a1a2e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -107,10 +107,8 @@ typedef struct { smithPredictor_t smithPredictor; - float dampingFactorPrevious; - float dampingFactorLockValue; - float dampingFactotLockUntilMs; - pt1Filter_t dampingFactorFilter; + timeMs_t targetOverThresholdTimeMs; + } pidState_t; STATIC_FASTRAM bool pidFiltersConfigured = false; @@ -322,7 +320,6 @@ bool pidInitFilters(void) for (int i = 0; i < XYZ_AXIS_COUNT; i++) { pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate)); - pt1FilterInit(&pidState[i].dampingFactorFilter, 2.0f, US2S(refreshRate)); } #ifdef USE_ANTIGRAVITY @@ -745,53 +742,43 @@ static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axi static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) { const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); + const float rateError = rateTarget - pidState->gyroRate; const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f); - float dampingFactorP; - float dampingFactorD; - float dampingFactorI; - /* - New idea.... - Iterm damping is applied (down to 0) when: - abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark) + * Iterm damping is applied (down to 0) when: + * abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark) - itermAttenuation = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1) - - */ + * itermAttenuation = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1) + */ - if (fabsf(dampingFactor) <= fabsf(pidState->dampingFactorPrevious)) { - dampingFactorI = dampingFactor; + //If error is greater than 10% or max rate + const bool errorThresholdReached = fabsf(rateError) > maxRate * 0.1f; - pidState->dampingFactotLockUntilMs = millis() + scaleRangef(fabsf(dampingFactor), 1.0f, 0.0f, 0, 300); - pidState->dampingFactorLockValue = dampingFactor; + //If stick (setpoint) was moved above threshold in the last 500ms + if (fabsf(rateTarget) > maxRate * 0.2f) { + pidState->targetOverThresholdTimeMs = millis(); + } - // pt1FilterReset(&pidState->dampingFactorFilter, pidState->dampingFactorPrevious); - pidState->dampingFactorPrevious = dampingFactor; - } else { - - if (millis() > pidState->dampingFactorLockValue) { - dampingFactorI = dampingFactor; - pidState->dampingFactorPrevious = dampingFactor; - } else { - dampingFactorI = pidState->dampingFactorLockValue; - } + //If error is below threshold, we no longer track time for lock mechanism + if (!errorThresholdReached) { + pidState->targetOverThresholdTimeMs = 0; } + const float dampingFactorI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f); + //P & D damping factors are always the same and based on current damping factor - dampingFactorP = dampingFactor; - dampingFactorD = dampingFactor; + const float dampingFactorP = dampingFactor; + const float dampingFactorD = dampingFactor; if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_ALWAYS, 0, pidState->dampingFactorPrevious * 1000); - DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorP * 1000); - DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorI * 1000); - DEBUG_SET(DEBUG_ALWAYS, 3, dampingFactorD * 1000); + DEBUG_SET(DEBUG_ALWAYS, 0, dampingFactorP * 1000); + DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorI * 1000); + DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorD * 1000); } - const float rateError = rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactorP; const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactorD; const float newFFTerm = rateTarget * pidState->kFF; @@ -1309,8 +1296,6 @@ void pidInit(void) for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) { - pidState[axis].dampingFactorPrevious = 1.0f; - #ifdef USE_D_BOOST // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration. // We assume, max acceleration is when pilot deflects the stick fully in 100ms From b810f99b310e6bae1840d1068eb954cab7fac101 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 17 Apr 2024 16:45:40 +0200 Subject: [PATCH 033/112] Refactor to contain logic in a single function --- src/main/flight/pid.c | 51 ++++++++++++++++++++++++++----------------- 1 file changed, 31 insertions(+), 20 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 941e08a1a2e..805450ca3e6 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -65,6 +65,14 @@ #include "programming/logic_condition.h" +typedef struct { + float aP; + float aI; + float aD; + float aFF; + timeMs_t targetOverThresholdTimeMs; +} fwPidAttenuation_t; + typedef struct { uint8_t axis; float kP; // Proportional gain @@ -107,8 +115,7 @@ typedef struct { smithPredictor_t smithPredictor; - timeMs_t targetOverThresholdTimeMs; - + fwPidAttenuation_t attenuation; } pidState_t; STATIC_FASTRAM bool pidFiltersConfigured = false; @@ -739,12 +746,8 @@ static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axi UNUSED(dT_inv); } -static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) -{ - const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); - const float rateError = rateTarget - pidState->gyroRate; - - const float maxRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; +static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, const float rateError) { + const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f; const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f); /* @@ -759,35 +762,43 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh //If stick (setpoint) was moved above threshold in the last 500ms if (fabsf(rateTarget) > maxRate * 0.2f) { - pidState->targetOverThresholdTimeMs = millis(); + pidState->attenuation.targetOverThresholdTimeMs = millis(); } //If error is below threshold, we no longer track time for lock mechanism if (!errorThresholdReached) { - pidState->targetOverThresholdTimeMs = 0; + pidState->attenuation.targetOverThresholdTimeMs = 0; } - const float dampingFactorI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f); + pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f); //P & D damping factors are always the same and based on current damping factor - const float dampingFactorP = dampingFactor; - const float dampingFactorD = dampingFactor; + pidState->attenuation.aP = dampingFactor; + pidState->attenuation.aD = dampingFactor; - if (axis == FD_ROLL) { - DEBUG_SET(DEBUG_ALWAYS, 0, dampingFactorP * 1000); - DEBUG_SET(DEBUG_ALWAYS, 1, dampingFactorI * 1000); - DEBUG_SET(DEBUG_ALWAYS, 2, dampingFactorD * 1000); + if (pidState->axis == FD_ROLL) { + DEBUG_SET(DEBUG_ALWAYS, 0, pidState->attenuation.aP * 1000); + DEBUG_SET(DEBUG_ALWAYS, 1, pidState->attenuation.aI * 1000); + DEBUG_SET(DEBUG_ALWAYS, 2, pidState->attenuation.aD * 1000); } +} + +static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) +{ + const float rateTarget = getFlightAxisRateOverride(axis, pidState->rateTarget); + const float rateError = rateTarget - pidState->gyroRate; + + fwRateAttenuation(pidState, rateTarget, rateError); - const float newPTerm = pTermProcess(pidState, rateError, dT) * dampingFactorP; - const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * dampingFactorD; + const float newPTerm = pTermProcess(pidState, rateError, dT) * pidState->attenuation.aP; + const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * pidState->attenuation.aD; const float newFFTerm = rateTarget * pidState->kFF; /* * Integral should be updated only if axis Iterm is not frozen */ if (!pidState->itermFreezeActive) { - pidState->errorGyroIf += rateError * pidState->kI * dT * dampingFactorI; + pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI; } applyItermLimiting(pidState); From 14755bdd566014bcff0786b0e8f75cea9f0241ed Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 29 Apr 2024 19:48:50 +0200 Subject: [PATCH 034/112] Unify the gyro filter settings --- docs/Settings.md | 88 +++++++++++++++++++++++++++++++---- src/main/fc/settings.yaml | 19 ++++---- src/main/flight/dynamic_lpf.c | 2 +- src/main/flight/ez_tune.c | 2 +- src/main/sensors/gyro.c | 7 ++- src/main/sensors/gyro.h | 9 +++- 6 files changed, 99 insertions(+), 28 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 7ed9437ced1..3c6037fc31c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1562,6 +1562,76 @@ For developer ground test use. Disables motors, sets heading status = Trusted on --- +### gyro_adaptive_filter_hpf_hz + +High pass filter cutoff frequency + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 1 | 50 | + +--- + +### gyro_adaptive_filter_integrator_threshold_high + +High threshold for adaptive filter integrator + +| Default | Min | Max | +| --- | --- | --- | +| 4 | 1 | 10 | + +--- + +### gyro_adaptive_filter_integrator_threshold_low + +Low threshold for adaptive filter integrator + +| Default | Min | Max | +| --- | --- | --- | +| -2 | -10 | 0 | + +--- + +### gyro_adaptive_filter_max_hz + +Maximum frequency for adaptive filter + +| Default | Min | Max | +| --- | --- | --- | +| 150 | 100 | 500 | + +--- + +### gyro_adaptive_filter_min_hz + +Minimum frequency for adaptive filter + +| Default | Min | Max | +| --- | --- | --- | +| 50 | 30 | 250 | + +--- + +### gyro_adaptive_filter_std_lpf_hz + +Standard deviation low pass filter cutoff frequency + +| Default | Min | Max | +| --- | --- | --- | +| 2 | 0 | 10 | + +--- + +### gyro_adaptive_filter_target + +Target value for adaptive filter + +| Default | Min | Max | +| --- | --- | --- | +| 3.5 | 1 | 6 | + +--- + ### gyro_anti_aliasing_lpf_hz Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz @@ -1602,33 +1672,33 @@ Minimum frequency of the gyro Dynamic LPF --- -### gyro_main_lpf_hz +### gyro_filter_mode -Software based gyro main lowpass filter. Value is cutoff frequency (Hz) +Specifies the type of the software LPF of the gyro signals. | Default | Min | Max | | --- | --- | --- | -| 60 | 0 | 500 | +| STATIC | | | --- -### gyro_to_use +### gyro_main_lpf_hz -On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro +Software based gyro main lowpass filter. Value is cutoff frequency (Hz) | Default | Min | Max | | --- | --- | --- | -| 0 | 0 | 2 | +| 60 | 0 | 500 | --- -### gyro_use_dyn_lpf +### gyro_to_use -Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. +On multi-gyro targets, allows to choose which gyro to use. 0 = first gyro, 1 = second gyro | Default | Min | Max | | --- | --- | --- | -| OFF | OFF | ON | +| 0 | 0 | 2 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 40ca93c829d..f5e8df233e3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -191,6 +191,9 @@ tables: - name: led_pin_pwm_mode values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"] enum: led_pin_pwm_mode_e + - name: gyro_filter_mode + values: ["STATIC", "DYNAMIC", "ADAPTIVE"] + enum: gyroFilterType_e constants: RPYL_PID_MIN: 0 @@ -226,11 +229,11 @@ groups: field: gyro_main_lpf_hz min: 0 max: 500 - - name: gyro_use_dyn_lpf - description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position." - default_value: OFF - field: useDynamicLpf - type: bool + - name: gyro_filter_mode + description: "Specifies the type of the software LPF of the gyro signals." + default_value: "STATIC" + field: gyroFilterMode + table: gyro_filter_mode - name: gyro_dyn_lpf_min_hz description: "Minimum frequency of the gyro Dynamic LPF" default_value: 200 @@ -330,12 +333,6 @@ groups: field: gravity_cmss_cal min: 0 max: 2000 - - name: gyro_adaptive_filter - description: "Enable adaptive filter for gyro data" - default_value: OFF - field: adaptiveFilterEnabled - type: bool - condition: USE_ADAPTIVE_FILTER - name: gyro_adaptive_filter_target description: "Target value for adaptive filter" default_value: 3.5 diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c index 0e5b4f6ef28..31e594312ab 100644 --- a/src/main/flight/dynamic_lpf.c +++ b/src/main/flight/dynamic_lpf.c @@ -37,7 +37,7 @@ static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLp void dynamicLpfGyroTask(void) { - if (!gyroConfig()->useDynamicLpf) { + if (!gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_DYNAMIC) { return; } diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 4bde5b645e8..e67e3fe9b47 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -109,7 +109,7 @@ void ezTuneUpdate(void) { #endif //Disable dynamic LPF - gyroConfigMutable()->useDynamicLpf = 0; + gyroConfigMutable()->gyroFilterMode = GYRO_FILTER_MODE_STATIC; //Setup PID controller diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index fd2036ef9a2..7f7d1d171d9 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -96,7 +96,7 @@ EXTENDED_FASTRAM secondaryDynamicGyroNotchState_t secondaryDynamicGyroNotchState #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT, @@ -105,7 +105,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT, #endif .gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT, - .useDynamicLpf = SETTING_GYRO_USE_DYN_LPF_DEFAULT, .gyroDynamicLpfMinHz = SETTING_GYRO_DYN_LPF_MIN_HZ_DEFAULT, .gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT, .gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT, @@ -124,7 +123,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_zero_cal = {SETTING_GYRO_ZERO_X_DEFAULT, SETTING_GYRO_ZERO_Y_DEFAULT, SETTING_GYRO_ZERO_Z_DEFAULT}, .gravity_cmss_cal = SETTING_INS_GRAVITY_CMSS_DEFAULT, #ifdef USE_ADAPTIVE_FILTER - .adaptiveFilterEnabled = SETTING_GYRO_ADAPTIVE_FILTER_DEFAULT, .adaptiveFilterTarget = SETTING_GYRO_ADAPTIVE_FILTER_TARGET_DEFAULT, .adaptiveFilterMinHz = SETTING_GYRO_ADAPTIVE_FILTER_MIN_HZ_DEFAULT, .adaptiveFilterMaxHz = SETTING_GYRO_ADAPTIVE_FILTER_MAX_HZ_DEFAULT, @@ -132,6 +130,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .adaptiveFilterHpfHz = SETTING_GYRO_ADAPTIVE_FILTER_HPF_HZ_DEFAULT, .adaptiveFilterIntegratorThresholdHigh = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH_DEFAULT, .adaptiveFilterIntegratorThresholdLow = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW_DEFAULT, + .gyroFilterMode = SETTING_GYRO_FILTER_MODE_DEFAULT, #endif ); @@ -261,7 +260,7 @@ static void gyroInitFilters(void) initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_hz, getLooptime()); #ifdef USE_ADAPTIVE_FILTER - if (gyroConfig()->adaptiveFilterEnabled) { + if (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE) { adaptiveFilterSetDefaultFrequency(gyroConfig()->gyro_main_lpf_hz, gyroConfig()->adaptiveFilterMinHz, gyroConfig()->adaptiveFilterMaxHz); } #endif diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 75ff7983372..7f767e370d1 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -52,6 +52,12 @@ typedef enum { DYNAMIC_NOTCH_MODE_3D } dynamicGyroNotchMode_e; +typedef enum { + GYRO_FILTER_MODE_STATIC = 0, + GYRO_FILTER_MODE_DYNAMIC = 1, + GYRO_FILTER_MODE_ADAPTIVE = 2 +} gyroFilterMode_e; + typedef struct gyro_s { bool initialized; uint32_t targetLooptime; @@ -69,7 +75,6 @@ typedef struct gyroConfig_s { uint8_t gyro_to_use; #endif uint16_t gyro_main_lpf_hz; - uint8_t useDynamicLpf; uint16_t gyroDynamicLpfMinHz; uint16_t gyroDynamicLpfMaxHz; uint8_t gyroDynamicLpfCurveExpo; @@ -88,7 +93,6 @@ typedef struct gyroConfig_s { int16_t gyro_zero_cal[XYZ_AXIS_COUNT]; float gravity_cmss_cal; #ifdef USE_ADAPTIVE_FILTER - uint8_t adaptiveFilterEnabled; float adaptiveFilterTarget; uint16_t adaptiveFilterMinHz; uint16_t adaptiveFilterMaxHz; @@ -96,6 +100,7 @@ typedef struct gyroConfig_s { float adaptiveFilterHpfHz; float adaptiveFilterIntegratorThresholdHigh; float adaptiveFilterIntegratorThresholdLow; + uint8_t gyroFilterMode; #endif } gyroConfig_t; From fe0f08959db5e5a43e29737f9f8fcf3b5d2b052f Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 30 Apr 2024 15:22:02 +0200 Subject: [PATCH 035/112] Compiloation fix --- src/main/flight/dynamic_lpf.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/dynamic_lpf.c b/src/main/flight/dynamic_lpf.c index 31e594312ab..2c10613ede7 100644 --- a/src/main/flight/dynamic_lpf.c +++ b/src/main/flight/dynamic_lpf.c @@ -37,7 +37,7 @@ static float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLp void dynamicLpfGyroTask(void) { - if (!gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_DYNAMIC) { + if (gyroConfig()->gyroFilterMode != GYRO_FILTER_MODE_DYNAMIC) { return; } From ab07785fa9c14d05f37ccdc490b61260027df60e Mon Sep 17 00:00:00 2001 From: Manuel Ilg <29724064+manuelilg@users.noreply.github.com> Date: Sun, 5 May 2024 13:35:12 +0200 Subject: [PATCH 036/112] Fix channel selection for DMA2 Stream6. To use Channel6 which is only for TIM1_CH3 instead of the Channel0 which is for TIM1_CH1, TIM1_CH2 and TIM1_CH3. --- src/main/target/DALRCF405/target.c | 2 +- src/main/target/FLYWOOF411/target.c | 4 ++-- src/main/target/HAKRCF405V2/target.c | 2 +- src/main/target/HAKRCF722V2/target.c | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/target/DALRCF405/target.c b/src/main/target/DALRCF405/target.c index 816ad3cdeca..e428504c1bc 100644 --- a/src/main/target/DALRCF405/target.c +++ b/src/main/target/DALRCF405/target.c @@ -26,7 +26,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 (1,7) DEF_TIM(TIM8, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 (2,2) - DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 (2,6) + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 (2,6) DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S4 (2,1) (2.3 2.6) DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S5 (2,4) (2.2) DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 (1,2) diff --git a/src/main/target/FLYWOOF411/target.c b/src/main/target/FLYWOOF411/target.c index 3946ad0b404..764d29e3144 100644 --- a/src/main/target/FLYWOOF411/target.c +++ b/src/main/target/FLYWOOF411/target.c @@ -39,8 +39,8 @@ timerHardware_t timerHardware[] = { #else DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S1_OUT 2,1 DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S2_OUT 2,2 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S3_OUT 2,6 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR , 0, 0), // S4_OUT 1,7 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT 2,6 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR , 0, 0), // S4_OUT 1,7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_ANY, 0, 0), // RSSI 1,2 DEF_TIM(TIM5, CH4, PA3, TIM_USE_ANY, 0, 1), // RX2 1,0 diff --git a/src/main/target/HAKRCF405V2/target.c b/src/main/target/HAKRCF405V2/target.c index a44aabc0142..a0e79ff5b88 100644 --- a/src/main/target/HAKRCF405V2/target.c +++ b/src/main/target/HAKRCF405V2/target.c @@ -26,7 +26,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT D2_ST7 DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3_OUT D2_ST1 DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S4_OUT D2_ST2 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT D2_ST6 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S5_OUT D2_ST6 DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT D1_ST4 DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S7_OUT D1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT D1_ST2 diff --git a/src/main/target/HAKRCF722V2/target.c b/src/main/target/HAKRCF722V2/target.c index 02098ce755d..9738db56d0c 100644 --- a/src/main/target/HAKRCF722V2/target.c +++ b/src/main/target/HAKRCF722V2/target.c @@ -39,7 +39,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH2, PA9, TIM_USE_OUTPUT_AUTO, 0, 1), // S4 DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 - DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 + DEF_TIM(TIM1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0, 1), // S7 DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0), // 2812LED From d251b2eca3db7cdee04a9e739290503b7efd9e52 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 11 May 2024 20:54:44 +0200 Subject: [PATCH 037/112] First round of changes Need to make sure it shows up in configurator --- src/main/drivers/pwm_mapping.c | 7 +++++++ src/main/drivers/timer.h | 1 + src/main/fc/cli.c | 3 +++ src/main/flight/mixer.h | 3 ++- 4 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 0486eb5ac19..3f9b713c6cd 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -47,6 +47,7 @@ enum { MAP_TO_NONE, MAP_TO_MOTOR_OUTPUT, MAP_TO_SERVO_OUTPUT, + MAP_TO_LED_OUTPUT }; typedef struct { @@ -335,6 +336,8 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU type = MAP_TO_SERVO_OUTPUT; } else if (TIM_IS_MOTOR(timHw->usageFlags) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { type = MAP_TO_MOTOR_OUTPUT; + } else if (TIM_IS_LED(timHw->usageFlags) && !pwmHasMotorOnTimer(timOutputs, timHw->tim) && !pwmHasServoOnTimer(timOutputs, timHw->tim)) { + type = MAP_TO_LED_OUTPUT; } switch(type) { @@ -348,6 +351,10 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU timOutputs->timServos[timOutputs->maxTimServoCount++] = timHw; pwmClaimTimer(timHw->tim, timHw->usageFlags); break; + case MAP_TO_LED_OUTPUT: + timHw->usageFlags &= TIM_USE_LED; + pwmClaimTimer(timHw->tim, timHw->usageFlags); + break; default: break; } diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index a207aff0c56..dec602f3c93 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -123,6 +123,7 @@ typedef enum { #define TIM_IS_MOTOR(flags) ((flags) & TIM_USE_MOTOR) #define TIM_IS_SERVO(flags) ((flags) & TIM_USE_SERVO) +#define TIM_IS_LED(flags) ((flags) & TIM_USE_LED) #define TIM_IS_MOTOR_ONLY(flags) (TIM_IS_MOTOR(flags) && !TIM_IS_SERVO(flags)) #define TIM_IS_SERVO_ONLY(flags) (!TIM_IS_MOTOR(flags) && TIM_IS_SERVO(flags)) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 97de1bd6ed1..3bedf7d324d 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -163,6 +163,7 @@ static const char * outputModeNames[] = { "AUTO", "MOTORS", "SERVOS", + "LED", NULL }; @@ -2821,6 +2822,8 @@ static void cliTimerOutputMode(char *cmdline) mode = OUTPUT_MODE_MOTORS; } else if(!sl_strcasecmp("SERVOS", tok)) { mode = OUTPUT_MODE_SERVOS; + } else if(!sl_strcasecmp("LED", tok)) { + mode = OUTPUT_MODE_LED; } else { cliShowParseError(); return; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 0d40fdc11ec..8af82173069 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -48,7 +48,8 @@ typedef enum { typedef enum { OUTPUT_MODE_AUTO = 0, OUTPUT_MODE_MOTORS, - OUTPUT_MODE_SERVOS + OUTPUT_MODE_SERVOS, + OUTPUT_MODE_LED } outputMode_e; typedef struct motorAxisCorrectionLimits_s { From f82b3fc26ebfa5ba8b09cb2666e6b78e3351ea66 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 11 May 2024 23:48:51 +0200 Subject: [PATCH 038/112] Allow led pin to be re-used as servo or motor more easily. --- src/main/drivers/pwm_mapping.c | 16 ++++++++-------- src/main/drivers/timer.h | 6 +++--- src/main/fc/fc_msp.c | 11 +++++++++++ src/main/msp/msp_protocol_v2_inav.h | 1 + 4 files changed, 23 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 3f9b713c6cd..b9ce0624592 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -214,16 +214,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw) static void timerHardwareOverride(timerHardware_t * timer) { switch (timerOverrides(timer2id(timer->tim))->outputMode) { case OUTPUT_MODE_MOTORS: - if (TIM_IS_SERVO(timer->usageFlags)) { - timer->usageFlags &= ~TIM_USE_SERVO; - timer->usageFlags |= TIM_USE_MOTOR; - } + timer->usageFlags &= ~(TIM_USE_SERVO|TIM_USE_LED); + timer->usageFlags |= TIM_USE_MOTOR; break; case OUTPUT_MODE_SERVOS: - if (TIM_IS_MOTOR(timer->usageFlags)) { - timer->usageFlags &= ~TIM_USE_MOTOR; - timer->usageFlags |= TIM_USE_SERVO; - } + timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_LED); + timer->usageFlags |= TIM_USE_SERVO; + break; + case OUTPUT_MODE_LED: + timer->usageFlags &= ~(TIM_USE_MOTOR|TIM_USE_SERVO); + timer->usageFlags |= TIM_USE_LED; break; } } diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index dec602f3c93..d87e0400d52 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -113,9 +113,9 @@ typedef enum { TIM_USE_MOTOR = (1 << 2), // Motor output TIM_USE_SERVO = (1 << 3), // Servo output TIM_USE_MC_CHNFW = (1 << 4), // Deprecated and not used after removal of CHANNEL_FORWARDING feature - //TIM_USE_FW_MOTOR = (1 << 5), // We no longer differentiate mc from fw on pwm allocation - //TIM_USE_FW_SERVO = (1 << 6), - TIM_USE_LED = (1 << 24), + //TIM_USE_FW_MOTOR = (1 << 5), // We no longer differentiate mc from fw on pwm allocation + //TIM_USE_FW_SERVO = (1 << 6), + TIM_USE_LED = (1 << 24), // Remapping needs to be in the lower 8 bits. TIM_USE_BEEPER = (1 << 25), } timerUsageFlag_e; diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index d44a6e1fcad..2c0210617e8 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1586,6 +1586,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timerHardware[i].usageFlags); } break; + case MSP2_INAV_OUTPUT_MAPPING_EXT2: + for (uint8_t i = 0; i < timerHardwareCount; ++i) + if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { + #if defined(SITL_BUILD) + sbufWriteU8(dst, i); + #else + sbufWriteU8(dst, timer2id(timerHardware[i].tim)); + #endif + sbufWriteU32(dst, timerHardware[i].usageFlags); + } + break; case MSP2_INAV_MC_BRAKING: #ifdef USE_MR_BRAKING_MODE diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 3312e5bc25d..1b64b3f604d 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -34,6 +34,7 @@ #define MSP2_INAV_OUTPUT_MAPPING_EXT 0x200D #define MSP2_INAV_TIMER_OUTPUT_MODE 0x200E #define MSP2_INAV_SET_TIMER_OUTPUT_MODE 0x200F +#define MSP2_INAV_OUTPUT_MAPPING_EXT2 0x210D #define MSP2_INAV_MIXER 0x2010 #define MSP2_INAV_SET_MIXER 0x2011 From 0c44cf5b0d14862b05b3ce6feed4454385239d71 Mon Sep 17 00:00:00 2001 From: rmaia <9812730+rmaia3d@users.noreply.github.com> Date: Sun, 12 May 2024 00:45:43 -0300 Subject: [PATCH 039/112] BFCOMPAT: Add text label (AT) for auto-throttle and BF font symbols for pitch and roll --- src/main/io/displayport_msp_bf_compat.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index 4219d2b2db8..1f6ca45bd81 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -328,28 +328,28 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_ZERO_HALF_LEADING_DOT: return BF_SYM_ZERO_HALF_LEADING_DOT; +*/ case SYM_AUTO_THR0: - return BF_SYM_AUTO_THR0; + return 'A'; case SYM_AUTO_THR1: - return BF_SYM_AUTO_THR1; + return 'T'; case SYM_ROLL_LEFT: - return BF_SYM_ROLL_LEFT; + return BF_SYM_ROLL; case SYM_ROLL_LEVEL: - return BF_SYM_ROLL_LEVEL; + return BF_SYM_ROLL; case SYM_ROLL_RIGHT: - return BF_SYM_ROLL_RIGHT; + return BF_SYM_ROLL; case SYM_PITCH_UP: - return BF_SYM_PITCH_UP; + return BF_SYM_PITCH; case SYM_PITCH_DOWN: - return BF_SYM_PITCH_DOWN; - */ + return BF_SYM_PITCH; case SYM_GFORCE: return 'G'; From ce9ea2d8fcb17c946f26cab49a0b906fc205c849 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 12 May 2024 11:08:53 +0200 Subject: [PATCH 040/112] Add obsolete warning to MSP2_INAV_OUTPUT_MAPPING_EXT --- src/main/fc/fc_msp.c | 2 ++ src/main/msp/msp_protocol_v2_inav.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index e099d11f648..75e18ef3681 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1575,6 +1575,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; + // Obsolete, replaced by MSP2_INAV_OUTPUT_MAPPING_EXT2 case MSP2_INAV_OUTPUT_MAPPING_EXT: for (uint8_t i = 0; i < timerHardwareCount; ++i) if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { @@ -1583,6 +1584,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #else sbufWriteU8(dst, timer2id(timerHardware[i].tim)); #endif + // usageFlags is u32, cuts out the higher 24bits sbufWriteU8(dst, timerHardware[i].usageFlags); } break; diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 1b64b3f604d..7c10a4df990 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -31,7 +31,7 @@ #define MSP2_INAV_OUTPUT_MAPPING 0x200A #define MSP2_INAV_MC_BRAKING 0x200B #define MSP2_INAV_SET_MC_BRAKING 0x200C -#define MSP2_INAV_OUTPUT_MAPPING_EXT 0x200D +#define MSP2_INAV_OUTPUT_MAPPING_EXT 0x200D // Obsolete, replaced by MSP2_INAV_OUTPUT_MAPPING_EXT2 #define MSP2_INAV_TIMER_OUTPUT_MODE 0x200E #define MSP2_INAV_SET_TIMER_OUTPUT_MODE 0x200F #define MSP2_INAV_OUTPUT_MAPPING_EXT2 0x210D From ac3277656142d63d3c43c46729c53647da6a41a5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 12 May 2024 13:21:11 +0200 Subject: [PATCH 041/112] vimrx synthax on by default --- .vimrc | 1 + 1 file changed, 1 insertion(+) diff --git a/.vimrc b/.vimrc index 547d37812cb..4808f6bb38d 100644 --- a/.vimrc +++ b/.vimrc @@ -5,5 +5,6 @@ set expandtab set bs=2 set sw=4 set ts=4 +syn on From 4f6220b0fe7ef3cf264f3660caaaabe7421b09d1 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sun, 12 May 2024 13:21:29 +0200 Subject: [PATCH 042/112] Change conflict checking to rely on timerUsageFlag Previously it used IO_TAG to check for the LED pin directly --- src/main/drivers/pwm_mapping.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index b9ce0624592..e953c86b96e 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -168,10 +168,16 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw) #if defined(USE_LED_STRIP) if (feature(FEATURE_LED_STRIP)) { - const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); - if (ledTimHw != NULL && timHw->tim == ledTimHw->tim) { - return true; + for (int i = 0; i < timerHardwareCount; i++) { + if (timHw->tim == timerHardware[i].tim && timerHardware[i].usageFlags & TIM_USE_LED) { + return true; + } } + + //const timerHardware_t * ledTimHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); + //if (ledTimHw != NULL && timHw->tim == ledTimHw->tim) { + // return true; + //} } #endif From b838606528e4830a13cb2416b8bc934d53172fb4 Mon Sep 17 00:00:00 2001 From: rmaia <9812730+rmaia3d@users.noreply.github.com> Date: Sun, 12 May 2024 11:28:10 -0300 Subject: [PATCH 043/112] Keep throttle symbol, just prepending with the letter A when auto-throttle is engaged --- src/main/io/displayport_msp_bf_compat.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index 1f6ca45bd81..e7f75ec2fa5 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -334,7 +334,7 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) return 'A'; case SYM_AUTO_THR1: - return 'T'; + return BF_SYM_THR; case SYM_ROLL_LEFT: return BF_SYM_ROLL; From 1dd0d873da670c2a9074e923ba4d8eef6cdef571 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Mon, 13 May 2024 01:11:48 +0200 Subject: [PATCH 044/112] Prefer default LED pin This changes led hardware setup to prefer original led pin, but fall back to reassigned LED pin if that was changed to something else. --- src/main/drivers/light_ws2811strip.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index cb56a7fe79a..fe5f405d032 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -124,6 +124,10 @@ void ws2811LedStripInit(void) { const timerHardware_t * timHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); + if (!(timHw->usageFlags & TIM_USE_LED)) { // Check if it has not been reassigned + timHw = timerGetByUsageFlag(TIM_USE_LED); // Get first pin marked as LED + } + if (timHw == NULL) { return; } @@ -133,14 +137,14 @@ void ws2811LedStripInit(void) return; } - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); + ws2811IO = IOGetByTag(timHw->tag); //IOGetByTag(IO_TAG(WS2811_PIN)); IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP_FAST, timHw->alternateFunction); - if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW ) { + if (ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW) { ledConfigurePWM(); *timerCCR(ws2811TCH) = 0; - } else if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH ) { + } else if (ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH) { ledConfigurePWM(); *timerCCR(ws2811TCH) = 100; } else { From 6aeb5c3e7c0dcf2a7afe3166849c094ac3149477 Mon Sep 17 00:00:00 2001 From: Julio Cesar Date: Mon, 13 May 2024 21:58:30 -0300 Subject: [PATCH 045/112] Make the 'declination.py' file generate Declination, Inclination and Intensity. --- .../navigation/navigation_declination_gen.c | 120 +++--- src/main/target/common.h | 2 - src/utils/declination.py | 345 ++++++++++++------ 3 files changed, 299 insertions(+), 168 deletions(-) diff --git a/src/main/navigation/navigation_declination_gen.c b/src/main/navigation/navigation_declination_gen.c index c7d632cd87c..65b44cfcbcb 100644 --- a/src/main/navigation/navigation_declination_gen.c +++ b/src/main/navigation/navigation_declination_gen.c @@ -1,63 +1,77 @@ /* this file is automatically generated by src/utils/declination.py - DO NOT EDIT! */ -/* Updated on 2024-04-19 12:54:15.842704 */ +/* Updated on 2024-05-13 21:53:14.731603 */ -#include +const float SAMPLING_RES = 10; +const float SAMPLING_MIN_LAT = -90; +const float SAMPLING_MAX_LAT = 90; +const float SAMPLING_MIN_LON = -180; +const float SAMPLING_MAX_LON = 180; - - -#if defined(NAV_AUTO_MAG_DECLINATION_PRECISE) -#define SAMPLING_RES 10.00000f -#define SAMPLING_MIN_LON -180.00000f -#define SAMPLING_MAX_LON 180.00000f -#define SAMPLING_MIN_LAT -90.00000f -#define SAMPLING_MAX_LAT 90.00000f - -static const float declination_table[19][37] = { - {148.63040f,138.63040f,128.63040f,118.63040f,108.63040f,98.63040f,88.63040f,78.63040f,68.63040f,58.63040f,48.63040f,38.63040f,28.63040f,18.63040f,8.63040f,-1.36960f,-11.36960f,-21.36960f,-31.36960f,-41.36960f,-51.36960f,-61.36960f,-71.36960f,-81.36960f,-91.36960f,-101.36960f,-111.36960f,-121.36960f,-131.36960f,-141.36960f,-151.36960f,-161.36960f,-171.36960f,178.63040f,168.63040f,158.63040f,148.63040f}, - {128.87816f,116.70769f,105.62443f,95.48127f,86.09683f,77.29367f,68.91718f,60.84302f,52.97822f,45.25811f,37.64003f,30.09478f,22.59702f,15.11676f,7.61418f,0.03947f,-7.66207f,-15.54157f,-23.63819f,-31.97410f,-40.55489f,-49.37538f,-58.42917f,-67.71936f,-77.26833f,-87.12519f,-97.37007f,-108.11458f,-119.49579f,-131.65830f,-144.71588f,-158.68662f,-173.41452f,171.47213f,156.49935f,142.18469f,128.87816f}, - {85.86755f,77.88182f,71.42849f,65.89217f,60.86962f,56.04972f,51.17842f,46.06489f,40.60132f,34.77619f,28.67056f,22.43282f,16.23087f,10.18755f,4.31917f,-1.49067f,-7.45797f,-13.81467f,-20.71641f,-28.18762f,-36.12471f,-44.34676f,-52.66473f,-60.93937f,-69.11381f,-77.22546f,-85.41334f,-93.94372f,-103.29064f,-114.35005f,-128.97727f,-151.06023f,174.83520f,137.67818f,112.23020f,96.52063f,85.86756f}, - {48.43188f,46.98353f,45.36982f,43.80186f,42.32760f,40.82382f,38.99678f,36.46379f,32.90329f,28.18877f,22.45599f,16.10387f,9.71432f,3.86202f,-1.14650f,-5.45997f,-9.64078f,-14.38658f,-20.18766f,-27.11174f,-34.80967f,-42.71859f,-50.31107f,-57.23008f,-63.28782f,-68.39719f,-72.47622f,-75.31221f,-76.30788f,-73.74072f,-61.55512f,-18.96160f,29.20071f,44.46666f,48.67974f,49.28540f,48.43188f}, - {31.58850f,31.74835f,31.39924f,30.87678f,30.43574f,30.19800f,29.98360f,29.24010f,27.23113f,23.37602f,17.54538f,10.24099f,2.57680f,-4.11819f,-8.98330f,-12.08874f,-14.32383f,-16.91831f,-20.97106f,-26.92020f,-34.17714f,-41.56631f,-48.09571f,-53.19469f,-56.53278f,-57.82558f,-56.66031f,-52.27310f,-43.55594f,-29.95085f,-13.24765f,2.54212f,14.60798f,22.72224f,27.72553f,30.44935f,31.58850f}, - {22.79850f,23.32533f,23.34746f,23.05694f,22.66226f,22.42885f,22.46289f,22.41289f,21.37705f,18.23370f,12.30982f,4.02530f,-4.93197f,-12.42012f,-17.23389f,-19.62803f,-20.56810f,-20.93312f,-21.98947f,-25.41090f,-31.19026f,-37.44166f,-42.49725f,-45.51856f,-46.02918f,-43.74555f,-38.55838f,-30.56411f,-20.73773f,-11.07131f,-2.79308f,4.21937f,10.21441f,15.15242f,18.92948f,21.44642f,22.79850f}, - {17.16987f,17.68582f,17.85385f,17.73377f,17.34906f,16.86130f,16.52747f,16.34574f,15.56790f,12.74839f,6.74610f,-2.00418f,-11.22826f,-18.37193f,-22.46233f,-24.19496f,-24.47888f,-23.22908f,-20.72783f,-19.60150f,-21.98233f,-26.43395f,-30.43203f,-32.35125f,-31.47499f,-27.95264f,-22.40690f,-15.51422f,-8.63620f,-3.35146f,0.42144f,3.87188f,7.48128f,10.95260f,13.91406f,16.01563f,17.16987f}, - {13.46149f,13.75829f,13.85281f,13.81813f,13.52893f,12.94281f,12.32698f,11.88976f,11.00215f,8.11359f,1.99478f,-6.62228f,-15.08744f,-21.05798f,-23.92224f,-24.34577f,-23.04831f,-19.84546f,-14.88685f,-10.38631f,-9.06427f,-11.31395f,-15.06340f,-17.65505f,-17.71488f,-15.54256f,-11.89991f,-7.27581f,-2.87289f,-0.16517f,1.24684f,2.97092f,5.51371f,8.29954f,10.80542f,12.58786f,13.46149f}, - {11.19370f,11.20122f,11.07257f,11.02175f,10.82482f,10.26986f,9.62418f,9.09716f,7.97068f,4.77978f,-1.35469f,-9.31946f,-16.55524f,-21.15274f,-22.50861f,-21.01857f,-17.63827f,-13.15823f,-8.29683f,-4.01764f,-1.51484f,-1.87967f,-4.62279f,-7.47003f,-8.63091f,-8.05888f,-6.24442f,-3.42551f,-0.62936f,0.67783f,0.89029f,1.80510f,3.93778f,6.48671f,8.83523f,10.50781f,11.19370f}, - {9.96310f,9.79689f,9.46606f,9.40362f,9.31492f,8.84240f,8.22130f,7.55735f,6.02850f,2.43377f,-3.55150f,-10.60044f,-16.54723f,-19.75861f,-19.59018f,-16.58982f,-12.16100f,-7.67553f,-3.93189f,-0.91522f,1.34973f,1.87637f,0.22653f,-2.15228f,-3.61566f,-3.90720f,-3.29932f,-1.83286f,-0.23997f,0.20367f,-0.19418f,0.32122f,2.29252f,4.85536f,7.33778f,9.21090f,9.96310f}, - {9.18888f,9.24336f,8.98253f,9.05281f,9.17191f,8.83598f,8.10836f,7.00345f,4.80464f,0.69064f,-5.15618f,-11.30079f,-15.93916f,-17.74937f,-16.42349f,-12.86230f,-8.44857f,-4.38855f,-1.42704f,0.70613f,2.51072f,3.31152f,2.34785f,0.48172f,-0.89199f,-1.49634f,-1.59901f,-1.18437f,-0.66243f,-0.94842f,-1.78088f,-1.62463f,0.11657f,2.73547f,5.55820f,7.95812f,9.18888f}, - {8.09034f,8.95675f,9.30400f,9.80188f,10.27100f,10.13647f,9.19791f,7.38846f,4.25004f,-0.60600f,-6.53915f,-11.93135f,-15.26054f,-15.79066f,-13.79060f,-10.27561f,-6.25103f,-2.56551f,0.07346f,1.80766f,3.24155f,4.05015f,3.51428f,2.10305f,0.93136f,0.25053f,-0.27213f,-0.70545f,-1.24346f,-2.37500f,-3.75122f,-4.07408f,-2.71911f,-0.15132f,2.99380f,6.02278f,8.09034f}, - {6.27792f,8.37723f,9.88296f,11.16334f,12.08487f,12.15767f,11.03142f,8.51947f,4.34248f,-1.47518f,-7.78911f,-12.71269f,-15.01118f,-14.61457f,-12.25578f,-8.87216f,-5.13675f,-1.63793f,0.99774f,2.71417f,3.98293f,4.78698f,4.67468f,3.82712f,2.93970f,2.18792f,1.25958f,-0.01231f,-1.71794f,-3.92282f,-6.03566f,-6.90283f,-5.95491f,-3.51614f,-0.20102f,3.30598f,6.27792f}, - {4.15672f,7.47883f,10.31556f,12.59798f,14.06987f,14.34572f,13.08042f,9.97537f,4.77447f,-2.16465f,-9.14687f,-14.00316f,-15.77819f,-14.88455f,-12.28640f,-8.84845f,-5.11869f,-1.55612f,1.34605f,3.41756f,4.91977f,6.01696f,6.56254f,6.51350f,6.02774f,5.09138f,3.48456f,1.07344f,-2.08240f,-5.60383f,-8.56486f,-9.89888f,-9.20525f,-6.83312f,-3.41374f,0.42499f,4.15672f}, - {2.38746f,6.60961f,10.51138f,13.76537f,15.94256f,16.59043f,15.27093f,11.52705f,5.06990f,-3.36003f,-11.36932f,-16.51244f,-18.15167f,-17.03678f,-14.21651f,-10.51524f,-6.47146f,-2.49805f,1.03216f,3.92789f,6.27640f,8.24229f,9.81368f,10.78355f,10.85070f,9.68870f,7.04509f,2.91449f,-2.25894f,-7.44786f,-11.30963f,-12.92043f,-12.20858f,-9.70263f,-6.08438f,-1.92562f,2.38746f}, - {1.14371f,5.95768f,10.55039f,14.56595f,17.51248f,18.77049f,17.56825f,12.99646f,4.54568f,-6.33792f,-15.91121f,-21.43028f,-22.88293f,-21.44177f,-18.22110f,-13.99911f,-9.28961f,-4.46489f,0.18470f,4.47851f,8.37200f,11.87759f,14.90388f,17.13814f,18.04159f,16.93503f,13.18748f,6.66358f,-1.56678f,-9.20492f,-14.20053f,-15.96059f,-14.99797f,-12.15657f,-8.19328f,-3.64820f,1.14371f}, - {-0.35955f,4.92766f,9.99739f,14.51863f,17.99503f,19.64821f,18.23668f,11.99980f,-0.09657f,-14.74873f,-25.55246f,-30.33910f,-30.58104f,-28.00627f,-23.75441f,-18.51083f,-12.70165f,-6.61593f,-0.46628f,5.58665f,11.41580f,16.88997f,21.81133f,25.83916f,28.40134f,28.57592f,24.99531f,16.28607f,3.28542f,-9.10495f,-16.59733f,-19.05958f,-18.04316f,-14.91590f,-10.58486f,-5.61478f,-0.35955f}, - {-5.35521f,-0.10619f,4.69597f,8.54683f,10.65268f,9.65875f,3.42678f,-9.71040f,-26.04578f,-37.63969f,-42.28407f,-42.02380f,-38.82980f,-33.88684f,-27.86664f,-21.16414f,-14.02720f,-6.62426f,0.91924f,8.49972f,16.02040f,23.37681f,30.43782f,37.01500f,42.80483f,47.26542f,49.32663f,46.69337f,34.75696f,10.85970f,-10.89085f,-20.36338f,-21.94791f,-19.71753f,-15.63466f,-10.66363f,-5.35521f}, - {-166.99999f,-156.99999f,-146.99999f,-136.99999f,-126.99999f,-116.99999f,-106.99999f,-96.99999f,-86.99999f,-76.99999f,-66.99999f,-56.99999f,-46.99999f,-36.99999f,-26.99999f,-16.99999f,-6.99999f,3.00001f,13.00001f,23.00001f,33.00000f,43.00000f,53.00000f,63.00000f,73.00000f,83.00000f,93.00000f,103.00000f,113.00000f,123.00000f,133.00000f,143.00000f,153.00000f,163.00000f,173.00000f,-177.00000f,-167.00000f} +const float declination_table[19][37] = { + {148.83402f,138.83401f,128.83401f,118.83402f,108.83402f,98.83402f,88.83402f,78.83402f,68.83402f,58.83402f,48.83402f,38.83402f,28.83402f,18.83402f,8.83402f,-1.16598f,-11.16598f,-21.16598f,-31.16598f,-41.16598f,-51.16598f,-61.16598f,-71.16598f,-81.16598f,-91.16598f,-101.16598f,-111.16598f,-121.16598f,-131.16598f,-141.16598f,-151.16598f,-161.16598f,-171.16598f,178.83402f,168.83402f,158.83402f,148.83402f}, + {129.09306f,116.89412f,105.78898f,95.63017f,86.23504f,77.42476f,69.04348f,60.96591f,53.09841f,45.37592f,37.75568f,30.20867f,22.70998f,15.23027f,7.73037f,0.16098f,-7.53238f,-15.40109f,-23.48496f,-31.80703f,-40.37375f,-49.18062f,-58.22152f,-67.49946f,-77.03640f,-86.88079f,-97.11212f,-107.84156f,-119.20626f,-131.35191f,-144.39481f,-158.35719f,-173.08769f,171.78274f,156.78187f,142.43310f,129.09306f}, + {85.81367f,77.83602f,71.40003f,65.88433f,60.88263f,56.08204f,51.22755f,46.12774f,40.67419f,34.85457f,28.74909f,22.50583f,16.29363f,10.23812f,4.36029f,-1.45095f,-7.40778f,-13.74178f,-20.61213f,-28.04922f,-35.95530f,-44.15322f,-52.45486f,-60.71951f,-68.88732f,-76.99183f,-85.16746f,-93.67473f,-102.97950f,-113.96429f,-128.46293f,-150.36073f,175.55488f,138.00554f,112.28051f,96.48319f,85.81367f}, + {48.22805f,46.81921f,45.24300f,43.71189f,42.27245f,40.80022f,39.00177f,36.49549f,32.95972f,28.26545f,22.54379f,16.18846f,9.77818f,3.88971f,-1.16066f,-5.50500f,-9.68832f,-14.40052f,-20.13990f,-26.99385f,-34.63319f,-42.50584f,-50.08478f,-57.00760f,-63.07978f,-68.20773f,-72.30336f,-75.14845f,-76.14220f,-73.56875f,-61.46573f,-19.66028f,28.39616f,43.98783f,48.35027f,49.03265f,48.22805f}, + {31.42931f,31.60530f,31.28145f,30.79047f,30.38024f,30.16777f,29.97487f,29.25597f,27.27948f,23.46202f,17.66378f,10.37176f,2.68673f,-4.06559f,-9.01317f,-12.19989f,-14.47861f,-17.04830f,-21.00672f,-26.83158f,-33.98769f,-41.32654f,-47.85214f,-52.97945f,-56.36644f,-57.72068f,-56.62201f,-52.29959f,-43.64045f,-30.08348f,-13.41611f,2.35315f,14.41455f,22.53506f,27.54777f,30.28028f,31.42931f}, + {22.67985f,23.21169f,23.25494f,22.99742f,22.63545f,22.42369f,22.46877f,22.43187f,21.42571f,18.33186f,12.46440f,4.21692f,-4.75068f,-12.30560f,-17.22721f,-19.74039f,-20.77499f,-21.16946f,-22.14607f,-25.39336f,-31.01102f,-37.19196f,-42.26227f,-45.34969f,-45.95366f,-43.77299f,-38.67388f,-30.72261f,-20.87969f,-11.16390f,-2.84711f,4.17466f,10.15622f,15.07307f,18.83144f,21.33469f,22.67985f}, + {17.07334f,17.59062f,17.77934f,17.69577f,17.34929f,16.88674f,16.56217f,16.38656f,15.63204f,12.86517f,6.93562f,-1.75751f,-10.98436f,-18.20308f,-22.41588f,-24.27993f,-24.67590f,-23.50719f,-21.00605f,-19.72369f,-21.87960f,-26.21214f,-30.23195f,-32.25069f,-31.49417f,-28.07385f,-22.58093f,-15.67611f,-8.73630f,-3.38117f,0.43868f,3.90117f,7.49078f,10.92679f,13.85484f,15.93237f,17.07334f}, + {13.37426f,13.67217f,13.78744f,13.78998f,13.54222f,12.98780f,12.38852f,11.96065f,11.09372f,8.25499f,2.21162f,-6.34723f,-14.82596f,-20.89032f,-23.89434f,-24.45752f,-23.26867f,-20.14461f,-15.21492f,-10.62227f,-9.09952f,-11.18829f,-14.91638f,-17.58833f,-17.75073f,-15.65321f,-12.03643f,-7.38772f,-2.92537f,-0.15156f,1.30857f,3.04660f,5.56513f,8.30463f,10.76538f,12.51642f,13.37426f}, + {11.10969f,11.11944f,11.01236f,10.99728f,10.84097f,10.32286f,9.70494f,9.19687f,8.09157f,4.94277f,-1.12864f,-9.05484f,-16.32980f,-21.04376f,-22.55109f,-21.19496f,-17.89373f,-13.44411f,-8.58084f,-4.24867f,-1.62827f,-1.86509f,-4.55045f,-7.42469f,-8.64775f,-8.12536f,-6.32716f,-3.48850f,-0.64489f,0.72027f,0.97976f,1.90952f,4.01623f,6.51266f,8.80749f,10.44185f,11.10969f}, + {9.88349f,9.72040f,9.41289f,9.38439f,9.33291f,8.89921f,8.31537f,7.68011f,6.17272f,2.60695f,-3.33977f,-10.37899f,-16.38823f,-19.72660f,-19.70115f,-16.80477f,-12.41185f,-7.91607f,-4.15177f,-1.10381f,1.22204f,1.82879f,0.23517f,-2.13784f,-3.62813f,-3.94630f,-3.34494f,-1.85961f,-0.22644f,0.26762f,-0.08803f,0.44077f,2.38763f,4.89878f,7.32401f,9.15291f,9.88349f}, + {9.12464f,9.18177f,8.94457f,9.04553f,9.19618f,8.89744f,8.21223f,7.14260f,4.96349f,0.86055f,-4.97931f,-11.14357f,-15.85640f,-17.78532f,-16.57645f,-13.08478f,-8.67898f,-4.59243f,-1.60427f,0.55144f,2.39028f,3.23916f,2.31654f,0.46434f,-0.91732f,-1.53003f,-1.62589f,-1.18564f,-0.62403f,-0.86624f,-1.66562f,-1.50145f,0.21812f,2.79294f,5.56329f,7.91823f,9.12464f}, + {8.06290f,8.93172f,9.29767f,9.81684f,10.30693f,10.20311f,9.30721f,7.53634f,4.41412f,-0.45099f,-6.41113f,-11.85029f,-15.25565f,-15.88209f,-13.96742f,-10.49677f,-6.46933f,-2.75504f,-0.08743f,1.66921f,3.12874f,3.96903f,3.45981f,2.05876f,0.88578f,0.20951f,-0.29117f,-0.68669f,-1.18101f,-2.27487f,-3.63054f,-3.95562f,-2.62221f,-0.08793f,3.01927f,6.01454f,8.06290f}, + {6.31041f,8.41617f,9.93058f,11.21483f,12.13996f,12.23034f,11.14011f,8.66524f,4.49928f,-1.34733f,-7.71989f,-12.71272f,-15.08305f,-14.75641f,-12.45343f,-9.09705f,-5.35725f,-1.83470f,0.82823f,2.57001f,3.86519f,4.69588f,4.60243f,3.76104f,2.87601f,2.13967f,1.24833f,0.02788f,-1.62859f,-3.80231f,-5.90990f,-6.79346f,-5.87079f,-3.45535f,-0.15720f,3.34006f,6.31041f}, + {4.26294f,7.59975f,10.43666f,12.70491f,14.16020f,14.43398f,13.18770f,10.10768f,4.90586f,-2.08386f,-9.15208f,-14.09196f,-15.93051f,-15.08268f,-12.51517f,-9.09089f,-5.35759f,-1.77919f,1.14514f,3.24292f,4.77489f,5.90082f,6.46683f,6.42854f,5.95440f,5.04613f,3.49019f,1.14215f,-1.96091f,-5.46021f,-8.43365f,-9.79880f,-9.13330f,-6.77311f,-3.34752f,0.50970f,4.26294f}, + {2.57464f,6.81988f,10.72127f,13.95370f,16.10049f,16.72460f,15.39548f,11.64478f,5.15172f,-3.36781f,-11.49173f,-16.72206f,-18.40831f,-17.31430f,-14.50140f,-10.79977f,-6.74992f,-2.76545f,0.78143f,3.70100f,6.08013f,8.07892f,9.67963f,10.67456f,10.77058f,9.65347f,7.07600f,3.02043f,-2.09809f,-7.27683f,-11.16806f,-12.81861f,-12.13043f,-9.62141f,-5.97603f,-1.77704f,2.57464f}, + {1.43503f,6.27921f,10.88019f,14.88261f,17.80063f,19.02158f,17.77297f,13.12636f,4.53718f,-6.54055f,-16.27050f,-21.85552f,-23.31365f,-21.85455f,-18.61101f,-14.36797f,-9.64057f,-4.79964f,-0.13239f,4.18385f,8.10605f,11.64519f,14.70734f,16.97990f,17.92977f,16.88820f,13.23164f,6.81146f,-1.34665f,-8.97876f,-14.01257f,-15.81005f,-14.85926f,-12.00033f,-7.99733f,-3.40265f,1.43502f}, + {0.13094f,5.45479f,10.54962f,15.08235f,18.55436f,20.17754f,18.67574f,12.20150f,-0.36226f,-15.49225f,-26.46004f,-31.19380f,-31.33251f,-28.66356f,-24.33699f,-19.03627f,-13.18344f,-7.06335f,-0.88475f,5.19490f,11.05071f,16.55252f,21.50321f,25.56424f,28.16988f,28.41286f,24.95220f,16.43585f,3.63861f,-8.67079f,-16.18909f,-18.69045f,-17.68753f,-14.54612f,-10.18137f,-5.16795f,0.13094f}, + {-4.14830f,1.12345f,5.96040f,9.84415f,11.94596f,10.80871f,4.02869f,-10.36405f,-27.94912f,-39.79617f,-44.16477f,-43.57950f,-40.12657f,-34.99189f,-28.83083f,-22.02403f,-14.80922f,-7.34799f,0.23881f,7.85050f,15.39197f,22.75942f,29.82156f,36.38924f,42.15868f,46.59144f,48.63821f,46.09967f,34.72041f,12.13936f,-8.98865f,-18.65098f,-20.47841f,-18.40133f,-14.39834f,-9.45818f,-4.14830f}, + {-169.79948f,-159.79948f,-149.79948f,-139.79948f,-129.79948f,-119.79948f,-109.79948f,-99.79948f,-89.79948f,-79.79948f,-69.79948f,-59.79948f,-49.79948f,-39.79948f,-29.79948f,-19.79948f,-9.79948f,0.20052f,10.20052f,20.20052f,30.20052f,40.20052f,50.20052f,60.20052f,70.20052f,80.20052f,90.20052f,100.20052f,110.20052f,120.20052f,130.20052f,140.20052f,150.20052f,160.20052f,170.20052f,-179.79948f,-169.79948f} }; -#else /* !NAV_AUTO_MAG_DECLINATION_PRECISE */ -#define SAMPLING_RES 10.00000f -#define SAMPLING_MIN_LON -180.00000f -#define SAMPLING_MAX_LON 180.00000f -#define SAMPLING_MIN_LAT -60.00000f -#define SAMPLING_MAX_LAT 60.00000f - -static const int8_t declination_table[13][37] = { - {48,47,45,44,42,41,39,36,33,28,22,16,10,4,-1,-5,-10,-14,-20,-27,-35,-43,-50,-57,-63,-68,-72,-75,-76,-74,-62,-19,29,44,49,49,48}, - {32,32,31,31,30,30,30,29,27,23,18,10,3,-4,-9,-12,-14,-17,-21,-27,-34,-42,-48,-53,-57,-58,-57,-52,-44,-30,-13,3,15,23,28,30,32}, - {23,23,23,23,23,22,22,22,21,18,12,4,-5,-12,-17,-20,-21,-21,-22,-25,-31,-37,-42,-46,-46,-44,-39,-31,-21,-11,-3,4,10,15,19,21,23}, - {17,18,18,18,17,17,17,16,16,13,7,-2,-11,-18,-22,-24,-24,-23,-21,-20,-22,-26,-30,-32,-31,-28,-22,-16,-9,-3,0,4,7,11,14,16,17}, - {13,14,14,14,14,13,12,12,11,8,2,-7,-15,-21,-24,-24,-23,-20,-15,-10,-9,-11,-15,-18,-18,-16,-12,-7,-3,0,1,3,6,8,11,13,13}, - {11,11,11,11,11,10,10,9,8,5,-1,-9,-17,-21,-23,-21,-18,-13,-8,-4,-2,-2,-5,-7,-9,-8,-6,-3,-1,1,1,2,4,6,9,11,11}, - {10,10,9,9,9,9,8,8,6,2,-4,-11,-17,-20,-20,-17,-12,-8,-4,-1,1,2,0,-2,-4,-4,-3,-2,0,0,0,0,2,5,7,9,10}, - {9,9,9,9,9,9,8,7,5,1,-5,-11,-16,-18,-16,-13,-8,-4,-1,1,3,3,2,0,-1,-1,-2,-1,-1,-1,-2,-2,0,3,6,8,9}, - {8,9,9,10,10,10,9,7,4,-1,-7,-12,-15,-16,-14,-10,-6,-3,0,2,3,4,4,2,1,0,0,-1,-1,-2,-4,-4,-3,0,3,6,8}, - {6,8,10,11,12,12,11,9,4,-1,-8,-13,-15,-15,-12,-9,-5,-2,1,3,4,5,5,4,3,2,1,0,-2,-4,-6,-7,-6,-4,0,3,6}, - {4,7,10,13,14,14,13,10,5,-2,-9,-14,-16,-15,-12,-9,-5,-2,1,3,5,6,7,7,6,5,3,1,-2,-6,-9,-10,-9,-7,-3,0,4}, - {2,7,11,14,16,17,15,12,5,-3,-11,-17,-18,-17,-14,-11,-6,-2,1,4,6,8,10,11,11,10,7,3,-2,-7,-11,-13,-12,-10,-6,-2,2}, - {1,6,11,15,18,19,18,13,5,-6,-16,-21,-23,-21,-18,-14,-9,-4,0,4,8,12,15,17,18,17,13,7,-2,-9,-14,-16,-15,-12,-8,-4,1} +const float inclination_table[19][37] = { + {-72.02070f,-72.02071f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02070f,-72.02071f,-72.02070f,-72.02071f}, + {-78.23208f,-77.46612f,-76.54604f,-75.51344f,-74.40408f,-73.25043f,-72.08420f,-70.93779f,-69.84384f,-68.83332f,-67.93241f,-67.15960f,-66.52407f,-66.02648f,-65.66205f,-65.42519f,-65.31392f,-65.33255f,-65.49161f,-65.80511f,-66.28626f,-66.94289f,-67.77395f,-68.76776f,-69.90201f,-71.14489f,-72.45664f,-73.79090f,-75.09549f,-76.31306f,-77.38240f,-78.24183f,-78.83644f,-79.12876f,-79.10879f,-78.79618f,-78.23208f}, + {-80.80007f,-78.97830f,-77.14756f,-75.29021f,-73.38193f,-71.40769f,-69.37743f,-67.33857f,-65.37896f,-63.61481f,-62.16302f,-61.10460f,-60.45316f,-60.14557f,-60.06470f,-60.08698f,-60.13217f,-60.19219f,-60.32958f,-60.65151f,-61.27183f,-62.27481f,-63.69257f,-65.50302f,-67.64474f,-70.03800f,-72.60073f,-75.25455f,-77.92173f,-80.51496f,-82.91263f,-84.88736f,-85.95658f,-85.63251f,-84.30522f,-82.60297f,-80.80007f}, + {-77.45516f,-75.43606f,-73.49604f,-71.57699f,-69.59653f,-67.45749f,-65.08505f,-62.48902f,-59.81600f,-57.35291f,-55.46368f,-54.46054f,-54.44855f,-55.23685f,-56.40404f,-57.49090f,-58.18917f,-58.42457f,-58.34278f,-58.24952f,-58.51231f,-59.42525f,-61.10217f,-63.46936f,-66.34503f,-69.53469f,-72.88656f,-76.29320f,-79.66218f,-82.88130f,-85.73520f,-87.38955f,-86.36194f,-84.15583f,-81.83294f,-79.58616f,-77.45517f}, + {-71.58214f,-69.61565f,-67.71488f,-65.86479f,-64.01505f,-62.04510f,-59.77482f,-57.07400f,-54.02405f,-51.03212f,-48.80491f,-48.09823f,-49.26412f,-51.92460f,-55.16562f,-58.07793f,-60.10267f,-61.01513f,-60.84749f,-59.97822f,-59.15799f,-59.18575f,-60.45940f,-62.86069f,-65.97595f,-69.36736f,-72.71868f,-75.80268f,-78.35851f,-80.06262f,-80.67983f,-80.25871f,-79.08901f,-77.46953f,-75.59576f,-73.59787f,-71.58215f}, + {-64.39807f,-62.40221f,-60.40960f,-58.42549f,-56.48151f,-54.55795f,-52.49863f,-50.03954f,-47.03044f,-43.80319f,-41.39118f,-41.16169f,-43.78688f,-48.51954f,-53.83971f,-58.60413f,-62.35170f,-64.81766f,-65.60605f,-64.62579f,-62.64650f,-61.08659f,-61.05364f,-62.68316f,-65.34905f,-68.24163f,-70.78236f,-72.62117f,-73.51797f,-73.52965f,-73.02048f,-72.24528f,-71.21594f,-69.88980f,-68.25886f,-66.38361f,-64.39807f}, + {-55.02826f,-52.87120f,-50.69073f,-48.44731f,-46.19513f,-44.04849f,-42.00465f,-39.74123f,-36.79801f,-33.33110f,-30.77621f,-31.26765f,-35.68330f,-42.58861f,-49.78570f,-56.05256f,-61.22645f,-65.24992f,-67.52021f,-67.43115f,-65.26549f,-62.35447f,-60.45735f,-60.49007f,-61.96843f,-63.81210f,-65.26064f,-65.90429f,-65.53604f,-64.51130f,-63.52424f,-62.77292f,-61.94296f,-60.76846f,-59.16172f,-57.17787f,-55.02826f}, + {-42.23934f,-39.72248f,-37.30606f,-34.84704f,-32.29950f,-29.84715f,-27.62882f,-25.25073f,-22.02346f,-18.13398f,-15.56621f,-17.09131f,-23.52381f,-32.78684f,-42.11183f,-49.91489f,-55.93881f,-60.34177f,-62.85919f,-63.04596f,-60.93631f,-57.41244f,-54.21948f,-52.82417f,-53.10885f,-54.02972f,-54.81946f,-54.92826f,-53.94911f,-52.38029f,-51.27875f,-50.79557f,-50.20937f,-49.05107f,-47.23568f,-44.84941f,-42.23934f}, + {-25.31616f,-22.26353f,-19.64432f,-17.13765f,-14.48794f,-11.90452f,-9.56551f,-6.93532f,-3.35246f,0.62534f,2.69744f,0.20162f,-7.49683f,-18.49333f,-29.80260f,-38.99555f,-45.22087f,-48.81124f,-50.28975f,-49.88122f,-47.49942f,-43.57896f,-39.80201f,-37.81589f,-37.58213f,-38.12733f,-38.79073f,-38.91636f,-37.84560f,-36.15384f,-35.28930f,-35.35164f,-35.11620f,-33.90633f,-31.71407f,-28.67497f,-25.31616f}, + {-5.22365f,-1.68002f,0.98130f,3.28459f,5.73141f,8.13291f,10.33797f,12.92226f,16.28019f,19.52644f,20.64919f,17.75076f,10.26968f,-0.68527f,-12.39654f,-21.81573f,-27.55757f,-29.98547f,-30.24089f,-29.18412f,-26.62224f,-22.51726f,-18.50023f,-16.36665f,-16.02599f,-16.47452f,-17.18522f,-17.56335f,-16.82114f,-15.49276f,-15.21405f,-16.04947f,-16.38744f,-15.33409f,-12.92178f,-9.31548f,-5.22365f}, + {14.65727f,18.24457f,20.73560f,22.68750f,24.74090f,26.83478f,28.82717f,31.06453f,33.64927f,35.73650f,35.91607f,33.06931f,26.82267f,17.82545f,8.14938f,0.35249f,-4.22224f,-5.65846f,-5.08244f,-3.67265f,-1.30171f,2.36894f,6.00997f,7.96495f,8.31670f,8.01335f,7.45026f,6.96852f,7.19717f,7.76654f,7.31456f,5.77666f,4.68883f,5.07867f,7.05547f,10.50432f,14.65727f}, + {31.00203f,34.03206f,36.22486f,37.93041f,39.71929f,41.67672f,43.64451f,45.63057f,47.51149f,48.64179f,48.13476f,45.46014f,40.64785f,34.34474f,27.90343f,22.77471f,19.78522f,19.08118f,19.96093f,21.39191f,23.28877f,25.96005f,28.61507f,30.09860f,30.42073f,30.29938f,30.04062f,29.74069f,29.65970f,29.53545f,28.53162f,26.62276f,24.91817f,24.37385f,25.31100f,27.72542f,31.00203f}, + {43.33608f,45.45851f,47.28808f,48.92364f,50.69187f,52.68920f,54.75852f,56.70372f,58.24993f,58.87748f,58.03709f,55.61656f,52.01961f,47.93440f,44.13769f,41.24459f,39.61639f,39.39065f,40.22833f,41.47069f,42.89022f,44.59331f,46.23881f,47.24171f,47.57154f,47.63935f,47.66249f,47.62919f,47.50096f,47.00810f,45.71114f,43.68014f,41.65440f,40.38784f,40.27252f,41.37670f,43.33608f}, + {53.10131f,54.36161f,55.82606f,57.45873f,59.32146f,61.39014f,63.50628f,65.43734f,66.85905f,67.31408f,66.44780f,64.38450f,61.68268f,58.96721f,56.68844f,55.08044f,54.25566f,54.24982f,54.89128f,55.82780f,56.83438f,57.87845f,58.85920f,59.59271f,60.06042f,60.40887f,60.71534f,60.90716f,60.81057f,60.15911f,58.74748f,56.73559f,54.64618f,53.02448f,52.20550f,52.28132f,53.10131f}, + {61.90363f,62.59082f,63.73000f,65.25974f,67.10373f,69.13404f,71.16923f,72.97878f,74.24745f,74.58969f,73.79370f,72.08739f,69.99806f,68.01780f,66.44167f,65.38391f,64.86230f,64.84287f,65.21557f,65.80372f,66.46017f,67.12970f,67.80720f,68.49179f,69.18909f,69.89166f,70.52514f,70.92481f,70.86509f,70.14456f,68.73300f,66.86155f,64.92449f,63.29260f,62.20331f,61.74779f,61.90363f}, + {70.57319f,70.97549f,71.81891f,73.05700f,74.60513f,76.33503f,78.07584f,79.60158f,80.60606f,80.76720f,79.99878f,78.58008f,76.92858f,75.37566f,74.11340f,73.22087f,72.70518f,72.52979f,72.62677f,72.91213f,73.31657f,73.81370f,74.42101f,75.17239f,76.07666f,77.07532f,78.01176f,78.63384f,78.66938f,77.98598f,76.70086f,75.10445f,73.50638f,72.14451f,71.16426f,70.63412f,70.57319f}, + {78.82351f,79.06440f,79.60559f,80.41612f,81.44382f,82.60991f,83.79747f,84.82323f,85.41014f,85.29919f,84.52961f,83.39228f,82.15604f,80.99185f,79.99787f,79.22370f,78.68527f,78.37555f,78.27394f,78.35635f,78.60546f,79.01788f,79.60326f,80.37283f,81.31875f,82.38760f,83.44917f,84.26774f,84.54142f,84.12292f,83.18704f,82.03917f,80.91872f,79.97342f,79.28584f,78.89722f,78.82351f}, + {85.92404f,85.99807f,86.21370f,86.55320f,86.98729f,87.46743f,87.90712f,88.15645f,88.05609f,87.61718f,86.98684f,86.28678f,85.58976f,84.94098f,84.37090f,83.90045f,83.54357f,83.30879f,83.20077f,83.22179f,83.37274f,83.65354f,84.06234f,84.59369f,85.23581f,85.96691f,86.74942f,87.51716f,88.14109f,88.39226f,88.15089f,87.63861f,87.08412f,86.59723f,86.22810f,86.00055f,85.92404f}, + {88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f,88.21420f} }; -#endif +const float intensity_table[19][37] = { + {0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f,0.54507f}, + {0.60562f,0.59923f,0.59133f,0.58213f,0.57184f,0.56070f,0.54892f,0.53679f,0.52458f,0.51262f,0.50121f,0.49067f,0.48128f,0.47330f,0.46697f,0.46249f,0.46006f,0.45986f,0.46203f,0.46665f,0.47372f,0.48312f,0.49460f,0.50778f,0.52217f,0.53717f,0.55217f,0.56652f,0.57964f,0.59102f,0.60027f,0.60715f,0.61152f,0.61342f,0.61294f,0.61026f,0.60562f}, + {0.62993f,0.61663f,0.60163f,0.58513f,0.56721f,0.54792f,0.52741f,0.50597f,0.48417f,0.46274f,0.44252f,0.42424f,0.40840f,0.39524f,0.38483f,0.37722f,0.37262f,0.37149f,0.37444f,0.38211f,0.39496f,0.41303f,0.43591f,0.46271f,0.49220f,0.52288f,0.55321f,0.58160f,0.60660f,0.62705f,0.64217f,0.65166f,0.65569f,0.65477f,0.64966f,0.64113f,0.62993f}, + {0.61859f,0.59954f,0.57951f,0.55859f,0.53652f,0.51281f,0.48703f,0.45921f,0.43010f,0.40125f,0.37460f,0.35184f,0.33383f,0.32034f,0.31043f,0.30313f,0.29816f,0.29624f,0.29887f,0.30789f,0.32478f,0.35003f,0.38290f,0.42162f,0.46385f,0.50716f,0.54918f,0.58755f,0.62009f,0.64502f,0.66138f,0.66918f,0.66921f,0.66276f,0.65127f,0.63614f,0.61859f}, + {0.58434f,0.56134f,0.53820f,0.51511f,0.49174f,0.46715f,0.44010f,0.40984f,0.37699f,0.34382f,0.31364f,0.28967f,0.27359f,0.26453f,0.25966f,0.25610f,0.25262f,0.24997f,0.25053f,0.25779f,0.27518f,0.30435f,0.34417f,0.39141f,0.44206f,0.49253f,0.54000f,0.58194f,0.61593f,0.64015f,0.65395f,0.65794f,0.65353f,0.64245f,0.62626f,0.60641f,0.58434f}, + {0.53921f,0.51448f,0.48995f,0.46595f,0.44250f,0.41888f,0.39359f,0.36521f,0.33366f,0.30097f,0.27115f,0.24895f,0.23726f,0.23477f,0.23677f,0.23885f,0.23920f,0.23803f,0.23691f,0.23986f,0.25296f,0.28060f,0.32245f,0.37391f,0.42855f,0.48085f,0.52753f,0.56643f,0.59562f,0.61429f,0.62316f,0.62341f,0.61626f,0.60311f,0.58508f,0.56327f,0.53921f}, + {0.48785f,0.46379f,0.43988f,0.41636f,0.39367f,0.37182f,0.34998f,0.32674f,0.30099f,0.27346f,0.24780f,0.22959f,0.22251f,0.22507f,0.23198f,0.23916f,0.24558f,0.25057f,0.25278f,0.25389f,0.26060f,0.28058f,0.31692f,0.36595f,0.41928f,0.46891f,0.51065f,0.54231f,0.56238f,0.57214f,0.57493f,0.57236f,0.56426f,0.55095f,0.53309f,0.51149f,0.48785f}, + {0.43209f,0.41089f,0.38995f,0.36941f,0.34979f,0.33155f,0.31470f,0.29826f,0.28044f,0.26067f,0.24164f,0.22828f,0.22418f,0.22862f,0.23770f,0.24867f,0.26122f,0.27397f,0.28296f,0.28650f,0.28895f,0.29863f,0.32271f,0.36133f,0.40653f,0.44908f,0.48374f,0.50731f,0.51774f,0.51826f,0.51530f,0.51075f,0.50252f,0.48985f,0.47321f,0.45336f,0.43209f}, + {0.37890f,0.36283f,0.34739f,0.33271f,0.31922f,0.30724f,0.29696f,0.28780f,0.27809f,0.26658f,0.25432f,0.24439f,0.23994f,0.24247f,0.25117f,0.26398f,0.27930f,0.29517f,0.30771f,0.31403f,0.31546f,0.31823f,0.33068f,0.35590f,0.38846f,0.42051f,0.44683f,0.46322f,0.46680f,0.46142f,0.45465f,0.44832f,0.43950f,0.42721f,0.41228f,0.39568f,0.37890f}, + {0.34114f,0.33192f,0.32348f,0.31616f,0.31061f,0.30672f,0.30410f,0.30225f,0.29980f,0.29492f,0.28700f,0.27755f,0.26956f,0.26653f,0.27083f,0.28123f,0.29446f,0.30791f,0.31935f,0.32678f,0.32987f,0.33216f,0.33985f,0.35562f,0.37648f,0.39779f,0.41572f,0.42632f,0.42691f,0.42012f,0.41134f,0.40209f,0.39087f,0.37775f,0.36429f,0.35181f,0.34114f}, + {0.32818f,0.32516f,0.32319f,0.32283f,0.32517f,0.32980f,0.33535f,0.34062f,0.34397f,0.34296f,0.33615f,0.32475f,0.31211f,0.30265f,0.30010f,0.30443f,0.31265f,0.32241f,0.33228f,0.34067f,0.34700f,0.35327f,0.36224f,0.37416f,0.38773f,0.40158f,0.41356f,0.42060f,0.42076f,0.41455f,0.40362f,0.38928f,0.37303f,0.35700f,0.34334f,0.33369f,0.32818f}, + {0.33981f,0.34000f,0.34266f,0.34809f,0.35721f,0.36937f,0.38240f,0.39400f,0.40180f,0.40293f,0.39568f,0.38157f,0.36502f,0.35110f,0.34343f,0.34233f,0.34605f,0.35337f,0.36304f,0.37282f,0.38187f,0.39163f,0.40273f,0.41394f,0.42487f,0.43601f,0.44625f,0.45294f,0.45399f,0.44803f,0.43420f,0.41393f,0.39124f,0.37028f,0.35389f,0.34377f,0.33981f}, + {0.37236f,0.37289f,0.37826f,0.38816f,0.40237f,0.41956f,0.43724f,0.45274f,0.46333f,0.46592f,0.45864f,0.44316f,0.42447f,0.40808f,0.39741f,0.39283f,0.39339f,0.39860f,0.40735f,0.41725f,0.42701f,0.43745f,0.44902f,0.46095f,0.47308f,0.48570f,0.49764f,0.50631f,0.50895f,0.50305f,0.48719f,0.46316f,0.43591f,0.41060f,0.39068f,0.37789f,0.37236f}, + {0.42245f,0.42215f,0.42846f,0.44064f,0.45724f,0.47601f,0.49435f,0.50993f,0.52038f,0.52304f,0.51631f,0.50148f,0.48288f,0.46550f,0.45264f,0.44515f,0.44269f,0.44488f,0.45083f,0.45875f,0.46742f,0.47714f,0.48867f,0.50225f,0.51759f,0.53373f,0.54863f,0.55948f,0.56339f,0.55795f,0.54232f,0.51859f,0.49131f,0.46537f,0.44426f,0.42977f,0.42245f}, + {0.48319f,0.48226f,0.48758f,0.49842f,0.51308f,0.52913f,0.54416f,0.55622f,0.56358f,0.56459f,0.55836f,0.54575f,0.52954f,0.51323f,0.49956f,0.48985f,0.48445f,0.48324f,0.48569f,0.49085f,0.49796f,0.50710f,0.51894f,0.53390f,0.55144f,0.56981f,0.58634f,0.59810f,0.60259f,0.59827f,0.58527f,0.56571f,0.54318f,0.52144f,0.50331f,0.49032f,0.48319f}, + {0.53929f,0.53799f,0.54070f,0.54691f,0.55552f,0.56497f,0.57363f,0.58010f,0.58328f,0.58236f,0.57702f,0.56775f,0.55586f,0.54317f,0.53137f,0.52173f,0.51505f,0.51167f,0.51159f,0.51458f,0.52039f,0.52900f,0.54059f,0.55506f,0.57159f,0.58845f,0.60334f,0.61392f,0.61846f,0.61626f,0.60790f,0.59512f,0.58032f,0.56589f,0.55363f,0.54459f,0.53929f}, + {0.57278f,0.57090f,0.57068f,0.57187f,0.57403f,0.57656f,0.57878f,0.58007f,0.57991f,0.57795f,0.57409f,0.56849f,0.56162f,0.55414f,0.54681f,0.54040f,0.53554f,0.53271f,0.53223f,0.53423f,0.53873f,0.54564f,0.55475f,0.56559f,0.57735f,0.58891f,0.59900f,0.60643f,0.61040f,0.61066f,0.60758f,0.60203f,0.59514f,0.58801f,0.58156f,0.57637f,0.57278f}, + {0.57900f,0.57728f,0.57584f,0.57463f,0.57360f,0.57263f,0.57160f,0.57039f,0.56890f,0.56707f,0.56489f,0.56241f,0.55976f,0.55710f,0.55465f,0.55263f,0.55127f,0.55074f,0.55121f,0.55273f,0.55530f,0.55883f,0.56314f,0.56798f,0.57300f,0.57785f,0.58216f,0.58562f,0.58802f,0.58928f,0.58943f,0.58865f,0.58716f,0.58523f,0.58309f,0.58097f,0.57900f}, + {0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f,0.56830f} +}; \ No newline at end of file diff --git a/src/main/target/common.h b/src/main/target/common.h index b550df42d0f..0b878067c45 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -117,8 +117,6 @@ #define NAV_NON_VOLATILE_WAYPOINT_CLI -#define NAV_AUTO_MAG_DECLINATION_PRECISE - #define USE_D_BOOST #define USE_ANTIGRAVITY diff --git a/src/utils/declination.py b/src/utils/declination.py index 44f5c9f21db..da0f805b392 100755 --- a/src/utils/declination.py +++ b/src/utils/declination.py @@ -1,133 +1,252 @@ -#!/usr/bin/env python - -# Ported from https://github.com/ArduPilot/ardupilot/tree/master/libraries/AP_Declination/generate -# Run this script with python3! -# To install the igrf module, use python3 -m pip install --user igrf12 -# Note that it requires a fortran compiler - -''' -generate field tables from IGRF12 +#!/usr/bin/env python3 ''' +generate field tables from IGRF13. Note that this requires python3 -import collections -import datetime -import pathlib -import sys - -import igrf -import numpy as np +To run the generator you need the igrf module. Install like this: -SAMPLING_RES = 10 -SAMPLING_MIN_LAT = -90 -SAMPLING_MAX_LAT = 90 -SAMPLING_MIN_LON = -180 -SAMPLING_MAX_LON = 180 + python3 -m pip install --user igrf -# This is used for flash constrained environments. We limit -# the latitude range to [-60, 60], so the values fit in an int8_t -SAMPLING_COMPACT_MIN_LAT = -60 -SAMPLING_COMPACT_MAX_LAT = 60 +And run: -PREPROCESSOR_SYMBOL = 'NAV_AUTO_MAG_DECLINATION_PRECISE' + python3 src/utils/declination.py -Query = collections.namedtuple('Query', ['date', 'res', 'min_lat', 'max_lat', 'min_lon', 'max_lon']) -Result = collections.namedtuple('Result', ['query', 'lats', 'lons', 'declination', 'inclination', 'intensity']) +It will updates the navigation_declination_gen.c code +''' -def write_table(f, name, table, compact): +import igrf +import numpy as np +import datetime +import pathlib +from math import sin, cos, sqrt +import math + +class Vector3(object): + '''a vector''' + def __init__(self, x=None, y=None, z=None): + if x is not None and y is not None and z is not None: + self.x = float(x) + self.y = float(y) + self.z = float(z) + elif x is not None and len(x) == 3: + self.x = float(x[0]) + self.y = float(x[1]) + self.z = float(x[2]) + elif x is not None: + raise ValueError('bad initializer') + else: + self.x = float(0) + self.y = float(0) + self.z = float(0) + + def length(self): + return sqrt(self.x**2 + self.y**2 + self.z**2) + + def __sub__(self, other): + return Vector3(self.x - other.x, self.y - other.y, self.z - other.z) + + def __mul__(self, other): + if isinstance(other, (int, float)): + return Vector3(self.x * other, self.y * other, self.z * other) + elif isinstance(other, Vector3): + return self.x * other.x + self.y * other.y + self.z * other.z + else: + raise ValueError('Multiplication with unsupported type') + +class Matrix3(object): + '''a 3x3 matrix, intended as a rotation matrix''' + def __init__(self, a=None, b=None, c=None): + if a is not None and b is not None and c is not None: + self.a = a.copy() + self.b = b.copy() + self.c = c.copy() + else: + self.identity() + + def identity(self): + self.a = Vector3(1,0,0) + self.b = Vector3(0,1,0) + self.c = Vector3(0,0,1) + + def from_euler(self, roll, pitch, yaw): + '''fill the matrix from Euler angles in radians''' + cp = cos(pitch) + sp = sin(pitch) + sr = sin(roll) + cr = cos(roll) + sy = sin(yaw) + cy = cos(yaw) + + self.a.x = cp * cy + self.a.y = (sr * sp * cy) - (cr * sy) + self.a.z = (cr * sp * cy) + (sr * sy) + self.b.x = cp * sy + self.b.y = (sr * sp * sy) + (cr * cy) + self.b.z = (cr * sp * sy) - (sr * cy) + self.c.x = -sp + self.c.y = sr * cp + self.c.z = cr * cp + + def __mul__(self, vector): + if isinstance(vector, Vector3): + x = self.a.x * vector.x + self.a.y * vector.y + self.a.z * vector.z + y = self.b.x * vector.x + self.b.y * vector.y + self.b.z * vector.z + z = self.c.x * vector.x + self.c.y * vector.y + self.c.z * vector.z + return Vector3(x, y, z) + else: + raise ValueError('Multiplication with unsupported type') + +def write_table(f, name, table): '''write one table''' - - if compact: - format_entry = lambda x: '%d' % round(x) - table_type = 'int8_t' - else: - table_type = 'float' - format_entry = lambda x: '%.5ff' % x - - num_lat = len(table) - num_lon = len(table[0]) - - f.write("static const %s %s[%u][%u] = {\n" % - (table_type, name, num_lat, num_lon)) - for i in range(num_lat): + f.write("const float %s[%u][%u] = {\n" % + (name, NUM_LAT, NUM_LON)) + for i in range(NUM_LAT): f.write(" {") - for j in range(num_lon): - f.write(format_entry(table[i][j])) - if j != num_lon - 1: + for j in range(NUM_LON): + f.write("%.5ff" % table[i][j]) + if j != NUM_LON-1: f.write(",") f.write("}") - if i != num_lat - 1: + if i != NUM_LAT-1: f.write(",") f.write("\n") f.write("};\n\n") -def declination_tables(query): - lats = np.arange(query.min_lat, query.max_lat + query.res, query.res) - lons = np.arange(query.min_lon, query.max_lon + query.res, query.res) - - num_lat = lats.size - num_lon = lons.size - - intensity = np.empty((num_lat, num_lon)) - inclination = np.empty((num_lat, num_lon)) - declination = np.empty((num_lat, num_lon)) - - for i, lat in enumerate(lats): - for j, lon in enumerate(lons): - mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1) - intensity[i][j] = mag.total / 1e5 - inclination[i][j] = mag.incl - declination[i][j] = mag.decl - - return Result(query=query, lats=lats, lons=lons, - declination=declination, inclination=inclination, intensity=intensity) - -def generate_constants(f, query): - f.write('#define SAMPLING_RES\t\t%.5ff\n' % query.res) - f.write('#define SAMPLING_MIN_LON\t%.5ff\n' % query.min_lon) - f.write('#define SAMPLING_MAX_LON\t%.5ff\n' % query.max_lon) - f.write('#define SAMPLING_MIN_LAT\t%.5ff\n' % query.min_lat) - f.write('#define SAMPLING_MAX_LAT\t%.5ff\n' % query.max_lat) - f.write('\n') - -def generate_tables(f, query, compact): - result = declination_tables(query) - write_table(f, 'declination_table', result.declination, compact) - - # We're not using these tables for now - #if not compact: - # write_table(f, 'inclination_table', result.inclination, False) - # write_table(f, 'intensity_table', result.intensity, False) - -def generate_code(f, date): - - compact_query = Query(date=date, res=SAMPLING_RES, - min_lat=SAMPLING_COMPACT_MIN_LAT, max_lat=SAMPLING_COMPACT_MAX_LAT, - min_lon=SAMPLING_MIN_LON, max_lon=SAMPLING_MAX_LON) +date = datetime.datetime.now() - precise_query = Query(date=date, res=SAMPLING_RES, - min_lat=SAMPLING_MIN_LAT, max_lat=SAMPLING_MAX_LAT, - min_lon=SAMPLING_MIN_LON, max_lon=SAMPLING_MAX_LON) +SAMPLING_RES = 10 +SAMPLING_MIN_LAT = -90 +SAMPLING_MAX_LAT = 90 +SAMPLING_MIN_LON = -180 +SAMPLING_MAX_LON = 180 +lats = np.arange(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+SAMPLING_RES, SAMPLING_RES) +lons = np.arange(SAMPLING_MIN_LON, SAMPLING_MAX_LON+SAMPLING_RES, SAMPLING_RES) + +NUM_LAT = lats.size +NUM_LON = lons.size + +intensity_table = np.empty((NUM_LAT, NUM_LON)) +inclination_table = np.empty((NUM_LAT, NUM_LON)) +declination_table = np.empty((NUM_LAT, NUM_LON)) + +max_error = 0 +max_error_pos = None +max_error_field = None + +def get_igrf(lat, lon): + '''return field as [declination_deg, inclination_deg, intensity_gauss]''' + mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1) + intensity = float(mag.total/1e5) + inclination = float(mag.incl) + declination = float(mag.decl) + return [declination, inclination, intensity] + +def interpolate_table(table, latitude_deg, longitude_deg): + '''interpolate inside a table for a given lat/lon in degrees''' + # round down to nearest sampling resolution + min_lat = int(math.floor(latitude_deg / SAMPLING_RES) * SAMPLING_RES) + min_lon = int(math.floor(longitude_deg / SAMPLING_RES) * SAMPLING_RES) + + # find index of nearest low sampling point + min_lat_index = int(math.floor((min_lat - SAMPLING_MIN_LAT) / SAMPLING_RES)) + min_lon_index = int(math.floor((min_lon - SAMPLING_MIN_LON) / SAMPLING_RES)) + + # calculate intensity + data_sw = table[min_lat_index][min_lon_index] + data_se = table[min_lat_index][min_lon_index + 1] + data_ne = table[min_lat_index + 1][min_lon_index + 1] + data_nw = table[min_lat_index + 1][min_lon_index] + + # perform bilinear interpolation on the four grid corners + data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw + data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw + + value = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min + return value + +def interpolate_field(latitude_deg, longitude_deg): + '''calculate magnetic field intensity and orientation, interpolating in tables + + returns array [declination_deg, inclination_deg, intensity] or None''' + # limit to table bounds + if latitude_deg < SAMPLING_MIN_LAT or latitude_deg >= SAMPLING_MAX_LAT: + return None + if longitude_deg < SAMPLING_MIN_LON or longitude_deg >= SAMPLING_MAX_LON: + return None + + intensity_gauss = interpolate_table(intensity_table, latitude_deg, longitude_deg) + declination_deg = interpolate_table(declination_table, latitude_deg, longitude_deg) + inclination_deg = interpolate_table(inclination_table, latitude_deg, longitude_deg) + + return [declination_deg, inclination_deg, intensity_gauss] + +def field_to_Vector3(mag): + '''return mGauss field from dec, inc and intensity''' + R = Matrix3() + mag_ef = Vector3(mag[2]*1000.0, 0.0, 0.0) + R.from_euler(0.0, -math.radians(mag[1]), math.radians(mag[0])) + return R * mag_ef + +def test_error(lat, lon): + '''check for error from lat, lon''' + global max_error, max_error_pos, max_error_field + mag1 = get_igrf(lat, lon) + mag2 = interpolate_field(lat, lon) + if mag2 is None: + return + ef1 = field_to_Vector3(mag1) + ef2 = field_to_Vector3(mag2) + err = (ef1 - ef2).length() + if err > max_error or err > 100: + print(lat, lon, err, ef1, ef2) + max_error = err + max_error_pos = (lat, lon) + max_error_field = ef1 - ef2 + +def test_max_error(lat, lon): + '''check for maximum error from lat, lon over SAMPLING_RES range''' + steps = 3 + delta = SAMPLING_RES/steps + for i in range(steps): + for j in range(steps): + lat2 = lat + i * delta + lon2 = lon + j * delta + if lat2 > SAMPLING_MAX_LAT or lon2 > SAMPLING_MAX_LON: + continue + if lat2 < SAMPLING_MIN_LAT or lon2 < SAMPLING_MIN_LON: + continue + test_error(lat2, lon2) + +for i, lat in enumerate(lats): + for j, lon in enumerate(lons): + mag = get_igrf(lat, lon) + declination_table[i][j] = mag[0] + inclination_table[i][j] = mag[1] + intensity_table[i][j] = mag[2] + +with open(pathlib.Path(__file__).parent / '..' / 'main' / 'navigation' / 'navigation_declination_gen.c', 'w') as f: f.write('/* this file is automatically generated by src/utils/declination.py - DO NOT EDIT! */\n\n\n') f.write('/* Updated on %s */\n\n\n' % date) - f.write('#include \n\n') - - - f.write('\n\n#if defined(%s)\n' % PREPROCESSOR_SYMBOL) - generate_constants(f, precise_query) - generate_tables(f, precise_query, False) - # We're not using these tables for now - # write_table(f, 'inclination_table', inclination_table) - # write_table(f, 'intensity_table', intensity_table) - f.write('#else /* !%s */\n' % PREPROCESSOR_SYMBOL) - generate_constants(f, compact_query) - generate_tables(f, compact_query, True) - f.write('#endif\n') - -if __name__ == '__main__': - - output = pathlib.PurePath(__file__).parent / '..' / 'main' / 'navigation' / 'navigation_declination_gen.c' - date = datetime.datetime.now() - with open(output, 'w') as f: - generate_code(f, date) + f.write('''const float SAMPLING_RES = %u; +const float SAMPLING_MIN_LAT = %u; +const float SAMPLING_MAX_LAT = %u; +const float SAMPLING_MIN_LON = %u; +const float SAMPLING_MAX_LON = %u; + +''' % (SAMPLING_RES, + SAMPLING_MIN_LAT, + SAMPLING_MAX_LAT, + SAMPLING_MIN_LON, + SAMPLING_MAX_LON)) + + write_table(f, 'declination_table', declination_table) + write_table(f, 'inclination_table', inclination_table) + write_table(f, 'intensity_table', intensity_table) + +print("Checking for maximum error") +for lat in range(-60, 60, 1): + for lon in range(-180, 180, 1): + test_max_error(lat, lon) +print("Generated with max error %.2f %s at (%.2f,%.2f)" % ( + max_error, max_error_field, max_error_pos[0], max_error_pos[1])) From c9779c2dee475b3627d8cfa469963e2e27a5ea1c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 14 May 2024 11:42:13 +0100 Subject: [PATCH 046/112] rth trackback tweaks --- src/main/navigation/navigation.c | 8 ++------ src/main/navigation/rth_trackback.c | 15 ++++++++------- src/main/navigation/rth_trackback.h | 2 +- 3 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c776ce13f54..cc4e269ef4b 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -301,10 +301,6 @@ bool validateRTHSanityChecker(void); void updateHomePosition(void); bool abortLaunchAllowed(void); -// static bool rthAltControlStickOverrideCheck(unsigned axis); -// static void updateRthTrackback(bool forceSaveTrackPoint); -// static fpVector3_t * rthGetTrackbackPos(void); - #ifdef USE_FW_AUTOLAND static float getLandAltitude(void); static int32_t calcWindDiff(int32_t heading, int32_t windHeading); @@ -1449,7 +1445,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati } else { // Switch to RTH trackback - if (rthTrackBackIsActive() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) { + if (rthTrackBackCanBeActivated() && rth_trackback.activePointIndex >= 0 && !isWaypointMissionRTHActive()) { rthTrackBackUpdate(true); // save final trackpoint for altitude and max trackback distance reference posControl.flags.rthTrackbackActive = true; calculateAndSetActiveWaypointToLocalPosition(getRthTrackBackPosition()); @@ -1568,7 +1564,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_TRACKBACK(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - if (rthTrackBackSetNewPosition()) { + if (!rthTrackBackSetNewPosition()) { return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE; } diff --git a/src/main/navigation/rth_trackback.c b/src/main/navigation/rth_trackback.c index 37467d9b385..dfdcf801f7c 100644 --- a/src/main/navigation/rth_trackback.c +++ b/src/main/navigation/rth_trackback.c @@ -37,9 +37,10 @@ rth_trackback_t rth_trackback; -bool rthTrackBackIsActive(void) +bool rthTrackBackCanBeActivated(void) { - return navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated); + return posControl.flags.estPosStatus >= EST_USABLE && + (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_ON || (navConfig()->general.flags.rth_trackback_mode == RTH_TRACKBACK_FS && posControl.flags.forcedRTHActivated)); } void rthTrackBackUpdate(bool forceSaveTrackPoint) @@ -127,7 +128,7 @@ void rthTrackBackUpdate(bool forceSaveTrackPoint) bool rthTrackBackSetNewPosition(void) { if (posControl.flags.estPosStatus == EST_NONE) { - return false; + return false; // will fall back to RTH initialize allowing full RTH to handle position loss correctly } const int32_t distFromStartTrackback = CENTIMETERS_TO_METERS(calculateDistanceToDestination(&rth_trackback.pointsList[rth_trackback.lastSavedIndex])); @@ -142,7 +143,7 @@ bool rthTrackBackSetNewPosition(void) if (rth_trackback.activePointIndex < 0 || cancelTrackback) { rth_trackback.WrapAroundCounter = rth_trackback.activePointIndex = -1; posControl.flags.rthTrackbackActive = false; - return true; // Procede to home after final trackback point + return false; // No more trackback points to set, procede to home } if (isWaypointReached(&posControl.activeWaypoint.pos, &posControl.activeWaypoint.bearing)) { @@ -161,7 +162,7 @@ bool rthTrackBackSetNewPosition(void) setDesiredPosition(getRthTrackBackPosition(), 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING); } - return false; + return true; } fpVector3_t *getRthTrackBackPosition(void) @@ -174,9 +175,9 @@ fpVector3_t *getRthTrackBackPosition(void) return &rth_trackback.pointsList[rth_trackback.activePointIndex]; } -void resetRthTrackBack(void) +void resetRthTrackBack(void) { rth_trackback.activePointIndex = -1; posControl.flags.rthTrackbackActive = false; - rth_trackback.WrapAroundCounter = -1; + rth_trackback.WrapAroundCounter = -1; } \ No newline at end of file diff --git a/src/main/navigation/rth_trackback.h b/src/main/navigation/rth_trackback.h index e3c71d0d45e..f3337bb4b79 100644 --- a/src/main/navigation/rth_trackback.h +++ b/src/main/navigation/rth_trackback.h @@ -35,7 +35,7 @@ typedef struct extern rth_trackback_t rth_trackback; -bool rthTrackBackIsActive(void); +bool rthTrackBackCanBeActivated(void); bool rthTrackBackSetNewPosition(void); void rthTrackBackUpdate(bool forceSaveTrackPoint); fpVector3_t *getRthTrackBackPosition(void); From 9c64e9b65d1321af35acace2696001a2aff58f40 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 15 May 2024 11:44:47 +0100 Subject: [PATCH 047/112] baro altitude estimation fix --- docs/Settings.md | 10 ------ src/main/fc/fc_msp_box.c | 2 +- src/main/fc/settings.yaml | 5 --- src/main/navigation/navigation.h | 2 -- .../navigation/navigation_pos_estimator.c | 31 +++++++++++-------- 5 files changed, 19 insertions(+), 31 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 30533330adf..14eb3b3dfce 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1812,16 +1812,6 @@ Allows to chose when the home position is reset. Can help prevent resetting home --- -### inav_use_gps_no_baro - -Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed. - -| Default | Min | Max | -| --- | --- | --- | -| ON | OFF | ON | - ---- - ### inav_w_acc_bias Weight for accelerometer drift estimation diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 2745f7e041b..7ffbbefd95e 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -215,7 +215,7 @@ void initActiveBoxIds(void) bool navReadyAltControl = getHwBarometerStatus() != HW_SENSOR_NONE; #ifdef USE_GPS - navReadyAltControl = navReadyAltControl || (feature(FEATURE_GPS) && (STATE(AIRPLANE) || positionEstimationConfig()->use_gps_no_baro)); + navReadyAltControl = navReadyAltControl || feature(FEATURE_GPS); const bool navFlowDeadReckoning = sensors(SENSOR_OPFLOW) && sensors(SENSOR_ACC) && positionEstimationConfig()->allow_dead_reckoning; bool navReadyPosControl = sensors(SENSOR_ACC) && feature(FEATURE_GPS); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index dfa7bb97d35..898c1b7be7a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2271,11 +2271,6 @@ groups: field: gravity_calibration_tolerance min: 0 max: 255 - - name: inav_use_gps_no_baro - description: "Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed." - field: use_gps_no_baro - type: bool - default_value: ON - name: inav_allow_dead_reckoning description: "Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation" default_value: OFF diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 0613a19c24e..276fb61cc1e 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -260,8 +260,6 @@ typedef struct positionEstimationConfig_s { float max_eph_epv; // Max estimated position error acceptable for estimation (cm) float baro_epv; // Baro position error - uint8_t use_gps_no_baro; - #ifdef USE_GPS_FIX_ESTIMATION uint8_t allow_gps_fix_estimation; #endif diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 19018983cc5..30fa4012e46 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -56,7 +56,7 @@ navigationPosEstimator_t posEstimator; static float initialBaroAltitudeOffset = 0.0f; -PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 6); +PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 7); PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, // Inertial position estimator parameters @@ -64,7 +64,6 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .reset_altitude_type = SETTING_INAV_RESET_ALTITUDE_DEFAULT, .reset_home_type = SETTING_INAV_RESET_HOME_DEFAULT, .gravity_calibration_tolerance = SETTING_INAV_GRAVITY_CAL_TOLERANCE_DEFAULT, // 5 cm/s/s calibration error accepted (0.5% of gravity) - .use_gps_no_baro = SETTING_INAV_USE_GPS_NO_BARO_DEFAULT, // Use GPS altitude if no baro is available on all aircrafts .allow_dead_reckoning = SETTING_INAV_ALLOW_DEAD_RECKONING_DEFAULT, .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT, @@ -180,12 +179,12 @@ void onNewGPSData(void) newLLH.lon = gpsSol.llh.lon; newLLH.alt = gpsSol.llh.alt; - if (sensors(SENSOR_GPS) + if (sensors(SENSOR_GPS) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) #endif ) { - if (!(STATE(GPS_FIX) + if (!(STATE(GPS_FIX) #ifdef USE_GPS_FIX_ESTIMATION || STATE(GPS_ESTIMATED_FIX) #endif @@ -553,15 +552,20 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count bool correctOK = false; - - //ignore baro if difference is too big, baro is probably wrong - const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f; - //fade out the baro to prevent sudden jump - const float start_epv = positionEstimationConfig()->max_eph_epv; - const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f; - const float wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f); - //use both baro and gps - if ((ctx->newFlags & EST_BARO_VALID) && (!positionEstimationConfig()->use_gps_no_baro) && (wBaro > 0.01f)) { + + float wBaro = 0.0f; + if (ctx->newFlags & EST_BARO_VALID) { + // Ignore baro if difference is too big, baro is probably wrong + const float gpsBaroResidual = ctx->newFlags & EST_GPS_Z_VALID ? fabsf(posEstimator.gps.pos.z - posEstimator.baro.alt) : 0.0f; + + // Fade out the baro to prevent sudden jump + const float start_epv = positionEstimationConfig()->max_eph_epv; + const float end_epv = positionEstimationConfig()->max_eph_epv * 2.0f; + wBaro = scaleRangef(constrainf(gpsBaroResidual, start_epv, end_epv), start_epv, end_epv, 1.0f, 0.0f); + } + + // Always use Baro if no GPS otherwise only use if accuracy OK compared to GPS + if (wBaro > 0.01f) { timeUs_t currentTimeUs = micros(); if (!ARMING_FLAG(ARMED)) { @@ -597,6 +601,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) correctOK = true; } + if (ctx->newFlags & EST_GPS_Z_VALID) { // Reset current estimate to GPS altitude if estimate not valid if (!(ctx->newFlags & EST_Z_VALID)) { From 283baa7c0a28685ec184dd1f57a822009c46480b Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Wed, 15 May 2024 20:01:08 +0100 Subject: [PATCH 048/112] Initial changes --- src/main/CMakeLists.txt | 4 +- src/main/drivers/osd.h | 4 +- src/main/drivers/osd_symbols.h | 6 +- src/main/io/bf_osd_symbols.h | 163 ---- src/main/io/displayport_msp_bf_compat.c | 748 ------------------ src/main/io/displayport_msp_dji_compat.c | 713 +++++++++++++++++ ..._compat.h => displayport_msp_dji_compat.h} | 12 +- src/main/io/displayport_msp_osd.c | 18 +- src/main/io/dji_osd_symbols.h | 164 ++++ src/main/io/osd.c | 127 +-- src/main/io/osd_grid.c | 8 +- src/main/io/osd_hud.c | 4 +- src/main/io/osd_utils.c | 4 +- 13 files changed, 948 insertions(+), 1027 deletions(-) delete mode 100644 src/main/io/bf_osd_symbols.h delete mode 100644 src/main/io/displayport_msp_bf_compat.c create mode 100644 src/main/io/displayport_msp_dji_compat.c rename src/main/io/{displayport_msp_bf_compat.h => displayport_msp_dji_compat.h} (69%) create mode 100644 src/main/io/dji_osd_symbols.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 28c068966f8..c87abc1782d 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -500,8 +500,8 @@ main_sources(COMMON_SRC io/displayport_max7456.h io/displayport_msp.c io/displayport_msp.h - io/displayport_msp_bf_compat.c - io/displayport_msp_bf_compat.h + io/displayport_msp_dji_compat.c + io/displayport_msp_dji_compat.h io/displayport_oled.c io/displayport_oled.h io/displayport_msp_osd.c diff --git a/src/main/drivers/osd.h b/src/main/drivers/osd.h index e9ae4d5ef3f..a8b0bbc8e42 100644 --- a/src/main/drivers/osd.h +++ b/src/main/drivers/osd.h @@ -49,8 +49,8 @@ typedef enum { VIDEO_SYSTEM_HDZERO, VIDEO_SYSTEM_DJIWTF, VIDEO_SYSTEM_AVATAR, - VIDEO_SYSTEM_BFCOMPAT, - VIDEO_SYSTEM_BFCOMPAT_HD + VIDEO_SYSTEM_DJICOMPAT, + VIDEO_SYSTEM_DJICOMPAT_HD } videoSystem_e; typedef enum { diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 91cdcbb5f6b..90c0bc97131 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -44,9 +44,9 @@ #define SYM_DBM 0x13 // 019 dBm #define SYM_SNR 0x14 // 020 SNR -#define SYM_AH_DIRECTION_UP 0x15 // 021 Arrow up AHI -#define SYM_AH_DIRECTION_DOWN 0x16 // 022 Arrow down AHI -#define SYM_DIRECTION 0x17 // 023 to 030, directional little arrows +#define SYM_AH_DECORATION_UP 0x15 // 021 Arrow up AHI +#define SYM_AH_DECORATION_DOWN 0x16 // 022 Arrow down AHI +#define SYM_DECORATION 0x17 // 023 to 030, directional little arrows #define SYM_VOLT 0x1F // 031 VOLTS #define SYM_MAH 0x99 // 153 mAh diff --git a/src/main/io/bf_osd_symbols.h b/src/main/io/bf_osd_symbols.h deleted file mode 100644 index a63b9470fb3..00000000000 --- a/src/main/io/bf_osd_symbols.h +++ /dev/null @@ -1,163 +0,0 @@ -/* @file max7456_symbols.h - * @brief max7456 symbols for the mwosd font set - * - * @author Nathan Tsoi nathan@vertile.com - * - * Copyright (C) 2016 Nathan Tsoi - * - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see - */ - -#pragma once - -//Misc -#define BF_SYM_NONE 0x00 -#define BF_SYM_END_OF_FONT 0xFF -#define BF_SYM_BLANK 0x20 -#define BF_SYM_HYPHEN 0x2D -#define BF_SYM_BBLOG 0x10 -#define BF_SYM_HOMEFLAG 0x11 -#define BF_SYM_RPM 0x12 -#define BF_SYM_ROLL 0x14 -#define BF_SYM_PITCH 0x15 -#define BF_SYM_TEMPERATURE 0x7A - -// GPS and navigation -#define BF_SYM_LAT 0x89 -#define BF_SYM_LON 0x98 -#define BF_SYM_ALTITUDE 0x7F -#define BF_SYM_TOTAL_DISTANCE 0x71 -#define BF_SYM_OVER_HOME 0x05 - -// RSSI -#define BF_SYM_RSSI 0x01 -#define BF_SYM_LINK_QUALITY 0x7B - -// Throttle Position (%) -#define BF_SYM_THR 0x04 - -// Unit Icons (Metric) -#define BF_SYM_M 0x0C -#define BF_SYM_KM 0x7D -#define BF_SYM_C 0x0E - -// Unit Icons (Imperial) -#define BF_SYM_FT 0x0F -#define BF_SYM_MILES 0x7E -#define BF_SYM_F 0x0D - -// Heading Graphics -#define BF_SYM_HEADING_N 0x18 -#define BF_SYM_HEADING_S 0x19 -#define BF_SYM_HEADING_E 0x1A -#define BF_SYM_HEADING_W 0x1B -#define BF_SYM_HEADING_DIVIDED_LINE 0x1C -#define BF_SYM_HEADING_LINE 0x1D - -// AH Center screen Graphics -#define BF_SYM_AH_CENTER_LINE 0x72 -#define BF_SYM_AH_CENTER 0x73 -#define BF_SYM_AH_CENTER_LINE_RIGHT 0x74 -#define BF_SYM_AH_RIGHT 0x02 -#define BF_SYM_AH_LEFT 0x03 -#define BF_SYM_AH_DECORATION 0x13 - -// Satellite Graphics -#define BF_SYM_SAT_L 0x1E -#define BF_SYM_SAT_R 0x1F - -// Direction arrows -#define BF_SYM_ARROW_SOUTH 0x60 -#define BF_SYM_ARROW_2 0x61 -#define BF_SYM_ARROW_3 0x62 -#define BF_SYM_ARROW_4 0x63 -#define BF_SYM_ARROW_EAST 0x64 -#define BF_SYM_ARROW_6 0x65 -#define BF_SYM_ARROW_7 0x66 -#define BF_SYM_ARROW_8 0x67 -#define BF_SYM_ARROW_NORTH 0x68 -#define BF_SYM_ARROW_10 0x69 -#define BF_SYM_ARROW_11 0x6A -#define BF_SYM_ARROW_12 0x6B -#define BF_SYM_ARROW_WEST 0x6C -#define BF_SYM_ARROW_14 0x6D -#define BF_SYM_ARROW_15 0x6E -#define BF_SYM_ARROW_16 0x6F - -#define BF_SYM_ARROW_SMALL_UP 0x75 -#define BF_SYM_ARROW_SMALL_DOWN 0x76 - -// AH Bars -#define BF_SYM_AH_BAR9_0 0x80 -#define BF_SYM_AH_BAR9_1 0x81 -#define BF_SYM_AH_BAR9_2 0x82 -#define BF_SYM_AH_BAR9_3 0x83 -#define BF_SYM_AH_BAR9_4 0x84 -#define BF_SYM_AH_BAR9_5 0x85 -#define BF_SYM_AH_BAR9_6 0x86 -#define BF_SYM_AH_BAR9_7 0x87 -#define BF_SYM_AH_BAR9_8 0x88 - -// Progress bar -#define BF_SYM_PB_START 0x8A -#define BF_SYM_PB_FULL 0x8B -#define BF_SYM_PB_HALF 0x8C -#define BF_SYM_PB_EMPTY 0x8D -#define BF_SYM_PB_END 0x8E -#define BF_SYM_PB_CLOSE 0x8F - -// Batt evolution -#define BF_SYM_BATT_FULL 0x90 -#define BF_SYM_BATT_5 0x91 -#define BF_SYM_BATT_4 0x92 -#define BF_SYM_BATT_3 0x93 -#define BF_SYM_BATT_2 0x94 -#define BF_SYM_BATT_1 0x95 -#define BF_SYM_BATT_EMPTY 0x96 - -// Batt Icons -#define BF_SYM_MAIN_BATT 0x97 - -// Voltage and amperage -#define BF_SYM_VOLT 0x06 -#define BF_SYM_AMP 0x9A -#define BF_SYM_MAH 0x07 -#define BF_SYM_WATT 0x57 // 0x57 is 'W' - -// Time -#define BF_SYM_ON_M 0x9B -#define BF_SYM_FLY_M 0x9C - -// Speed -#define BF_SYM_SPEED 0x70 -#define BF_SYM_KPH 0x9E -#define BF_SYM_MPH 0x9D -#define BF_SYM_MPS 0x9F -#define BF_SYM_FTPS 0x99 - -// Menu cursor -#define BF_SYM_CURSOR BF_SYM_AH_LEFT - -// Stick overlays -#define BF_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08 -#define BF_SYM_STICK_OVERLAY_SPRITE_MID 0x09 -#define BF_SYM_STICK_OVERLAY_SPRITE_LOW 0x0A -#define BF_SYM_STICK_OVERLAY_CENTER 0x0B -#define BF_SYM_STICK_OVERLAY_VERTICAL 0x16 -#define BF_SYM_STICK_OVERLAY_HORIZONTAL 0x17 - -// GPS degree/minute/second symbols -#define BF_SYM_GPS_DEGREE BF_SYM_STICK_OVERLAY_SPRITE_HIGH // kind of looks like the degree symbol -#define BF_SYM_GPS_MINUTE 0x27 // ' -#define BF_SYM_GPS_SECOND 0x22 // " diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c deleted file mode 100644 index e7f75ec2fa5..00000000000 --- a/src/main/io/displayport_msp_bf_compat.c +++ /dev/null @@ -1,748 +0,0 @@ -/* - * This file is part of INAV Project. - * - * INAV is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include "platform.h" - -#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT) - -#ifndef DISABLE_MSP_BF_COMPAT - -#include "io/displayport_msp_bf_compat.h" -#include "io/bf_osd_symbols.h" -#include "drivers/osd_symbols.h" - -uint8_t getBfCharacter(uint8_t ch, uint8_t page) -{ - uint16_t ech = ch | ((page & 0x3)<< 8) ; - - if (ech >= 0x20 && ech <= 0x5F) { // ASCII range - return ch; - } - - if (ech >= SYM_AH_DECORATION_MIN && ech <= SYM_AH_DECORATION_MAX) { - return BF_SYM_AH_DECORATION; - } - - switch (ech) { - case SYM_RSSI: - return BF_SYM_RSSI; - - case SYM_LQ: - return BF_SYM_LINK_QUALITY; - - case SYM_LAT: - return BF_SYM_LAT; - - case SYM_LON: - return BF_SYM_LON; - - case SYM_SAT_L: - return BF_SYM_SAT_L; - - case SYM_SAT_R: - return BF_SYM_SAT_R; - - case SYM_HOME_NEAR: - return BF_SYM_HOMEFLAG; - - case SYM_DEGREES: - return BF_SYM_GPS_DEGREE; - -/* - case SYM_HEADING: - return BF_SYM_HEADING; - - case SYM_SCALE: - return BF_SYM_SCALE; - - case SYM_HDP_L: - return BF_SYM_HDP_L; - - case SYM_HDP_R: - return BF_SYM_HDP_R; -*/ - case SYM_HOME: - return BF_SYM_HOMEFLAG; - - case SYM_2RSS: - return BF_SYM_RSSI; - -/* - case SYM_DB: - return BF_SYM_DB - - case SYM_DBM: - return BF_SYM_DBM; - - case SYM_SNR: - return BF_SYM_SNR; - - case SYM_AH_DECORATION_UP: - return BF_SYM_AH_DECORATION_UP; - - case SYM_AH_DECORATION_DOWN: - return BF_SYM_AH_DECORATION_DOWN; -*/ - case SYM_DIRECTION: - return BF_SYM_ARROW_NORTH; - - case SYM_DIRECTION + 1: // NE pointing arrow - return BF_SYM_ARROW_7; - - case SYM_DIRECTION + 2: // E pointing arrow - return BF_SYM_ARROW_EAST; - - case SYM_DIRECTION + 3: // SE pointing arrow - return BF_SYM_ARROW_3; - - case SYM_DIRECTION + 4: // S pointing arrow - return BF_SYM_ARROW_SOUTH; - - case SYM_DIRECTION + 5: // SW pointing arrow - return BF_SYM_ARROW_15; - - case SYM_DIRECTION + 6: // W pointing arrow - return BF_SYM_ARROW_WEST; - - case SYM_DIRECTION + 7: // NW pointing arrow - return BF_SYM_ARROW_11; - - case SYM_VOLT: - return BF_SYM_VOLT; - - case SYM_MAH: - return BF_SYM_MAH; - - case SYM_AH_KM: - return BF_SYM_KM; - - case SYM_AH_MI: - return BF_SYM_MILES; -/* - case SYM_VTX_POWER: - return BF_SYM_VTX_POWER; - - case SYM_AH_NM: - return BF_SYM_AH_NM; - - case SYM_MAH_NM_0: - return BF_SYM_MAH_NM_0; - - case SYM_MAH_NM_1: - return BF_SYM_MAH_NM_1; - - case SYM_MAH_KM_0: - return BF_SYM_MAH_KM_0; - - case SYM_MAH_KM_1: - return BF_SYM_MAH_KM_1; - - case SYM_MILLIOHM: - return BF_SYM_MILLIOHM; -*/ - case SYM_BATT_FULL: - return BF_SYM_BATT_FULL; - - case SYM_BATT_5: - return BF_SYM_BATT_5; - - case SYM_BATT_4: - return BF_SYM_BATT_4; - - case SYM_BATT_3: - return BF_SYM_BATT_3; - - case SYM_BATT_2: - return BF_SYM_BATT_2; - - case SYM_BATT_1: - return BF_SYM_BATT_1; - - case SYM_BATT_EMPTY: - return BF_SYM_BATT_EMPTY; - - case SYM_AMP: - return BF_SYM_AMP; -/* - case SYM_WH: - return BF_SYM_WH; - - case SYM_WH_KM: - return BF_SYM_WH_KM; - - case SYM_WH_MI: - return BF_SYM_WH_MI; - - case SYM_WH_NM: - return BF_SYM_WH_NM; -*/ - case SYM_WATT: - return BF_SYM_WATT; -/* - case SYM_MW: - return BF_SYM_MW; - - case SYM_KILOWATT: - return BF_SYM_KILOWATT; -*/ - case SYM_FT: - return BF_SYM_FT; - - case SYM_ALT_FT: - return BF_SYM_FT; - - case SYM_ALT_M: - return BF_SYM_M; - - case SYM_TOTAL: - return BF_SYM_TOTAL_DISTANCE; -/* - case SYM_ALT_KM: - return BF_SYM_ALT_KM; - - case SYM_ALT_KFT: - return BF_SYM_ALT_KFT; - - case SYM_DIST_M: - return BF_SYM_DIST_M; - - case SYM_DIST_KM: - return BF_SYM_DIST_KM; - - case SYM_DIST_FT: - return BF_SYM_DIST_FT; - - case SYM_DIST_MI: - return BF_SYM_DIST_MI; - - case SYM_DIST_NM: - return BF_SYM_DIST_NM; -*/ - case SYM_M: - return BF_SYM_M; - - case SYM_KM: - return BF_SYM_KM; - - case SYM_MI: - return BF_SYM_MILES; -/* - case SYM_NM: - return BF_SYM_NM; -*/ - case SYM_WIND_HORIZONTAL: - return 'W'; // W for wind - -/* - case SYM_WIND_VERTICAL: - return BF_SYM_WIND_VERTICAL; - - case SYM_3D_KT: - return BF_SYM_3D_KT; -*/ - case SYM_AIR: - return 'A'; // A for airspeed - - case SYM_3D_KMH: - return BF_SYM_KPH; - - case SYM_3D_MPH: - return BF_SYM_MPH; - - case SYM_RPM: - return BF_SYM_RPM; - - case SYM_FTS: - return BF_SYM_FTPS; -/* - case SYM_100FTM: - return BF_SYM_100FTM; -*/ - case SYM_MS: - return BF_SYM_MPS; - - case SYM_KMH: - return BF_SYM_KPH; - - case SYM_MPH: - return BF_SYM_MPH; -/* - case SYM_KT: - return BF_SYM_KT - - case SYM_MAH_MI_0: - return BF_SYM_MAH_MI_0; - - case SYM_MAH_MI_1: - return BF_SYM_MAH_MI_1; -*/ - case SYM_THR: - return BF_SYM_THR; - - case SYM_TEMP_F: - return BF_SYM_F; - - case SYM_TEMP_C: - return BF_SYM_C; - - case SYM_BLANK: - return BF_SYM_BLANK; -/* - case SYM_ON_H: - return BF_SYM_ON_H; - - case SYM_FLY_H: - return BF_SYM_FLY_H; -*/ - case SYM_ON_M: - return BF_SYM_ON_M; - - case SYM_FLY_M: - return BF_SYM_FLY_M; -/* - case SYM_GLIDESLOPE: - return BF_SYM_GLIDESLOPE; - - case SYM_WAYPOINT: - return BF_SYM_WAYPOINT; - - case SYM_CLOCK: - return BF_SYM_CLOCK; - - case SYM_ZERO_HALF_TRAILING_DOT: - return BF_SYM_ZERO_HALF_TRAILING_DOT; - - case SYM_ZERO_HALF_LEADING_DOT: - return BF_SYM_ZERO_HALF_LEADING_DOT; -*/ - - case SYM_AUTO_THR0: - return 'A'; - - case SYM_AUTO_THR1: - return BF_SYM_THR; - - case SYM_ROLL_LEFT: - return BF_SYM_ROLL; - - case SYM_ROLL_LEVEL: - return BF_SYM_ROLL; - - case SYM_ROLL_RIGHT: - return BF_SYM_ROLL; - - case SYM_PITCH_UP: - return BF_SYM_PITCH; - - case SYM_PITCH_DOWN: - return BF_SYM_PITCH; - - case SYM_GFORCE: - return 'G'; - -/* - case SYM_GFORCE_X: - return BF_SYM_GFORCE_X; - - case SYM_GFORCE_Y: - return BF_SYM_GFORCE_Y; - - case SYM_GFORCE_Z: - return BF_SYM_GFORCE_Z; - - case SYM_BARO_TEMP: - return BF_SYM_BARO_TEMP; - - case SYM_IMU_TEMP: - return BF_SYM_IMU_TEMP; - - case SYM_TEMP: - return BF_SYM_TEMP; - - case SYM_TEMP_SENSOR_FIRST: - return BF_SYM_TEMP_SENSOR_FIRST; - - case SYM_ESC_TEMP: - return BF_SYM_ESC_TEMP; - - case SYM_TEMP_SENSOR_LAST: - return BF_SYM_TEMP_SENSOR_LAST; - - case TEMP_SENSOR_SYM_COUNT: - return BF_TEMP_SENSOR_SYM_COUNT; -*/ - case SYM_HEADING_N: - return BF_SYM_HEADING_N; - - case SYM_HEADING_S: - return BF_SYM_HEADING_S; - - case SYM_HEADING_E: - return BF_SYM_HEADING_E; - - case SYM_HEADING_W: - return BF_SYM_HEADING_W; - - case SYM_HEADING_DIVIDED_LINE: - return BF_SYM_HEADING_DIVIDED_LINE; - - case SYM_HEADING_LINE: - return BF_SYM_HEADING_LINE; -/* - case SYM_MAX: - return BF_SYM_MAX; - - case SYM_PROFILE: - return BF_SYM_PROFILE; - - case SYM_SWITCH_INDICATOR_LOW: - return BF_SYM_SWITCH_INDICATOR_LOW; - - case SYM_SWITCH_INDICATOR_MID: - return BF_SYM_SWITCH_INDICATOR_MID; - - case SYM_SWITCH_INDICATOR_HIGH: - return BF_SYM_SWITCH_INDICATOR_HIGH; - - case SYM_AH: - return BF_SYM_AH; - - case SYM_GLIDE_DIST: - return BF_SYM_GLIDE_DIST; - - case SYM_GLIDE_MINS: - return BF_SYM_GLIDE_MINS; - - case SYM_AH_V_FT_0: - return BF_SYM_AH_V_FT_0; - - case SYM_AH_V_FT_1: - return BF_SYM_AH_V_FT_1; - - case SYM_AH_V_M_0: - return BF_SYM_AH_V_M_0; - - case SYM_AH_V_M_1: - return BF_SYM_AH_V_M_1; - - case SYM_FLIGHT_MINS_REMAINING: - return BF_SYM_FLIGHT_MINS_REMAINING; - - case SYM_FLIGHT_HOURS_REMAINING: - return BF_SYM_FLIGHT_HOURS_REMAINING; - - case SYM_GROUND_COURSE: - return BF_SYM_GROUND_COURSE; - - case SYM_CROSS_TRACK_ERROR: - return BF_SYM_CROSS_TRACK_ERROR; - - case SYM_LOGO_START: - return BF_SYM_LOGO_START; - - case SYM_LOGO_WIDTH: - return BF_SYM_LOGO_WIDTH; - - case SYM_LOGO_HEIGHT: - return BF_SYM_LOGO_HEIGHT; -*/ - case SYM_AH_LEFT: - return BF_SYM_AH_LEFT; - - case SYM_AH_RIGHT: - return BF_SYM_AH_RIGHT; - -/* - case SYM_AH_DECORATION_COUNT: - return BF_SYM_AH_DECORATION_COUNT; -*/ - case SYM_AH_CH_LEFT: - case SYM_AH_CH_TYPE3: - case SYM_AH_CH_TYPE4: - case SYM_AH_CH_TYPE5: - case SYM_AH_CH_TYPE6: - case SYM_AH_CH_TYPE7: - case SYM_AH_CH_TYPE8: - case SYM_AH_CH_AIRCRAFT1: - return BF_SYM_AH_CENTER_LINE; - - case SYM_AH_CH_RIGHT: - case (SYM_AH_CH_TYPE3+2): - case (SYM_AH_CH_TYPE4+2): - case (SYM_AH_CH_TYPE5+2): - case (SYM_AH_CH_TYPE6+2): - case (SYM_AH_CH_TYPE7+2): - case (SYM_AH_CH_TYPE8+2): - case SYM_AH_CH_AIRCRAFT3: - return BF_SYM_AH_CENTER_LINE_RIGHT; - - case SYM_AH_CH_AIRCRAFT0: - case SYM_AH_CH_AIRCRAFT4: - return ' '; - - case SYM_ARROW_UP: - return BF_SYM_ARROW_NORTH; - - case SYM_ARROW_2: - return BF_SYM_ARROW_8; - - case SYM_ARROW_3: - return BF_SYM_ARROW_7; - - case SYM_ARROW_4: - return BF_SYM_ARROW_6; - - case SYM_ARROW_RIGHT: - return BF_SYM_ARROW_EAST; - - case SYM_ARROW_6: - return BF_SYM_ARROW_4; - - case SYM_ARROW_7: - return BF_SYM_ARROW_3; - - case SYM_ARROW_8: - return BF_SYM_ARROW_2; - - case SYM_ARROW_DOWN: - return BF_SYM_ARROW_SOUTH; - - case SYM_ARROW_10: - return BF_SYM_ARROW_16; - - case SYM_ARROW_11: - return BF_SYM_ARROW_15; - - case SYM_ARROW_12: - return BF_SYM_ARROW_14; - - case SYM_ARROW_LEFT: - return BF_SYM_ARROW_WEST; - - case SYM_ARROW_14: - return BF_SYM_ARROW_12; - - case SYM_ARROW_15: - return BF_SYM_ARROW_11; - - case SYM_ARROW_16: - return BF_SYM_ARROW_10; - - case SYM_AH_H_START: - return BF_SYM_AH_BAR9_0; - - case (SYM_AH_H_START+1): - return BF_SYM_AH_BAR9_1; - - case (SYM_AH_H_START+2): - return BF_SYM_AH_BAR9_2; - - case (SYM_AH_H_START+3): - return BF_SYM_AH_BAR9_3; - - case (SYM_AH_H_START+4): - return BF_SYM_AH_BAR9_4; - - case (SYM_AH_H_START+5): - return BF_SYM_AH_BAR9_5; - - case (SYM_AH_H_START+6): - return BF_SYM_AH_BAR9_6; - - case (SYM_AH_H_START+7): - return BF_SYM_AH_BAR9_7; - - case (SYM_AH_H_START+8): - return BF_SYM_AH_BAR9_8; - - // BF does not have vertical artificial horizon. replace with middle horizontal one - case SYM_AH_V_START: - case (SYM_AH_V_START+1): - case (SYM_AH_V_START+2): - case (SYM_AH_V_START+3): - case (SYM_AH_V_START+4): - case (SYM_AH_V_START+5): - return BF_SYM_AH_BAR9_4; - - // BF for ESP_RADAR Symbols - case SYM_HUD_CARDINAL: - return BF_SYM_ARROW_SOUTH; - case SYM_HUD_CARDINAL + 1: - return BF_SYM_ARROW_16; - case SYM_HUD_CARDINAL + 2: - return BF_SYM_ARROW_15; - case SYM_HUD_CARDINAL + 3: - return BF_SYM_ARROW_WEST; - case SYM_HUD_CARDINAL + 4: - return BF_SYM_ARROW_12; - case SYM_HUD_CARDINAL + 5: - return BF_SYM_ARROW_11; - case SYM_HUD_CARDINAL + 6: - return BF_SYM_ARROW_NORTH; - case SYM_HUD_CARDINAL + 7: - return BF_SYM_ARROW_7; - case SYM_HUD_CARDINAL + 8: - return BF_SYM_ARROW_6; - case SYM_HUD_CARDINAL + 9: - return BF_SYM_ARROW_EAST; - case SYM_HUD_CARDINAL + 10: - return BF_SYM_ARROW_3; - case SYM_HUD_CARDINAL + 11: - return BF_SYM_ARROW_2; - - case SYM_HUD_ARROWS_R3: - return BF_SYM_AH_RIGHT; - case SYM_HUD_ARROWS_L3: - return BF_SYM_AH_LEFT; - - case SYM_HUD_SIGNAL_0: - return BF_SYM_AH_BAR9_1; - case SYM_HUD_SIGNAL_1: - return BF_SYM_AH_BAR9_3; - case SYM_HUD_SIGNAL_2: - return BF_SYM_AH_BAR9_4; - case SYM_HUD_SIGNAL_3: - return BF_SYM_AH_BAR9_5; - case SYM_HUD_SIGNAL_4: - return BF_SYM_AH_BAR9_7; -/* - case SYM_VARIO_UP_2A: - return BF_SYM_VARIO_UP_2A; - - case SYM_VARIO_UP_1A: - return BF_SYM_VARIO_UP_1A; - - case SYM_VARIO_DOWN_1A: - return BF_SYM_VARIO_DOWN_1A; - - case SYM_VARIO_DOWN_2A: - return BF_SYM_VARIO_DOWN_2A; -*/ - case SYM_ALT: - return BF_SYM_ALTITUDE; -/* - case SYM_HUD_SIGNAL_0: - return BF_SYM_HUD_SIGNAL_0; - - case SYM_HUD_SIGNAL_1: - return BF_SYM_HUD_SIGNAL_1; - - case SYM_HUD_SIGNAL_2: - return BF_SYM_HUD_SIGNAL_2; - - case SYM_HUD_SIGNAL_3: - return BF_SYM_HUD_SIGNAL_3; - - case SYM_HUD_SIGNAL_4: - return BF_SYM_HUD_SIGNAL_4; - - case SYM_HOME_DIST: - return BF_SYM_HOME_DIST; -*/ - - case SYM_AH_CH_CENTER: - case (SYM_AH_CH_TYPE3+1): - case (SYM_AH_CH_TYPE4+1): - case (SYM_AH_CH_TYPE5+1): - case (SYM_AH_CH_TYPE6+1): - case (SYM_AH_CH_TYPE7+1): - case (SYM_AH_CH_TYPE8+1): - case SYM_AH_CH_AIRCRAFT2: - return BF_SYM_AH_CENTER; -/* - case SYM_FLIGHT_DIST_REMAINING: - return BF_SYM_FLIGHT_DIST_REMAINING; - - case SYM_AH_CH_TYPE3: - return BF_SYM_AH_CH_TYPE3; - - case SYM_AH_CH_TYPE4: - return BF_SYM_AH_CH_TYPE4; - - case SYM_AH_CH_TYPE5: - return BF_SYM_AH_CH_TYPE5; - - case SYM_AH_CH_TYPE6: - return BF_SYM_AH_CH_TYPE6; - - case SYM_AH_CH_TYPE7: - return BF_SYM_AH_CH_TYPE7; - - case SYM_AH_CH_TYPE8: - return BF_SYM_AH_CH_TYPE8; - - case SYM_AH_CH_AIRCRAFT0: - return BF_SYM_AH_CH_AIRCRAFT0; - - case SYM_AH_CH_AIRCRAFT1: - return BF_SYM_AH_CH_AIRCRAFT1; - - case SYM_AH_CH_AIRCRAFT2: - return BF_SYM_AH_CH_AIRCRAFT2; - - case SYM_AH_CH_AIRCRAFT3: - return BF_SYM_AH_CH_AIRCRAFT3; - - case SYM_AH_CH_AIRCRAFT4: - return BF_SYM_AH_CH_AIRCRAFT4; - - case SYM_HUD_ARROWS_L1: - return BF_SYM_HUD_ARROWS_L1; - - case SYM_HUD_ARROWS_L2: - return BF_SYM_HUD_ARROWS_L2; - - case SYM_HUD_ARROWS_L3: - return BF_SYM_HUD_ARROWS_L3; - - case SYM_HUD_ARROWS_R1: - return BF_SYM_HUD_ARROWS_R1; - - case SYM_HUD_ARROWS_R2: - return BF_SYM_HUD_ARROWS_R2; - - case SYM_HUD_ARROWS_R3: - return BF_SYM_HUD_ARROWS_R3; - - case SYM_HUD_ARROWS_U1: - return BF_SYM_HUD_ARROWS_U1; - - case SYM_HUD_ARROWS_U2: - return BF_SYM_HUD_ARROWS_U2; - - case SYM_HUD_ARROWS_U3: - return BF_SYM_HUD_ARROWS_U3; - - case SYM_HUD_ARROWS_D1: - return BF_SYM_HUD_ARROWS_D1; - - case SYM_HUD_ARROWS_D2: - return BF_SYM_HUD_ARROWS_D2; - - case SYM_HUD_ARROWS_D3: - return BF_SYM_HUD_ARROWS_D3; -*/ - default: - break; - } - - return '?'; // Missing/not mapped character -} - -#endif - -#endif diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c new file mode 100644 index 00000000000..de4fa8656fa --- /dev/null +++ b/src/main/io/displayport_msp_dji_compat.c @@ -0,0 +1,713 @@ +/* + * This file is part of INAV Project. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include "platform.h" + +#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT) + +#ifndef DISABLE_MSP_DJI_COMPAT + +#include "io/displayport_msp_dji_compat.h" +#include "io/dji_osd_symbols.h" +#include "drivers/osd_symbols.h" + +uint8_t getDJICharacter(uint8_t ch, uint8_t page) +{ + uint16_t ech = ch | ((page & 0x3)<< 8) ; + + if (ech >= 0x20 && ech <= 0x5F) { // ASCII range + return ch; + } + + if (ech >= SYM_AH_DECORATION_MIN && ech <= SYM_AH_DECORATION_MAX) { + return DJI_SYM_AH_DECORATION; + } + + switch (ech) { + case SYM_RSSI: + return DJI_SYM_RSSI; + +/* + case SYM_LQ: + return DJI_SYM_LINK_QUALITY; +*/ + + case SYM_LAT: + return DJI_SYM_LAT; + + case SYM_LON: + return DJI_SYM_LON; + + case SYM_SAT_L: + return DJI_SYM_SAT_L; + + case SYM_SAT_R: + return DJI_SYM_SAT_R; + + case SYM_HOME_NEAR: + return DJI_SYM_HOMEFLAG; + + case SYM_DEGREES: + return DJI_SYM_GPS_DEGREE; + +/* + case SYM_HEADING: + return DJI_SYM_HEADING; + + case SYM_SCALE: + return DJI_SYM_SCALE; + + case SYM_HDP_L: + return DJI_SYM_HDP_L; + + case SYM_HDP_R: + return DJI_SYM_HDP_R; +*/ + case SYM_HOME: + return DJI_SYM_HOMEFLAG; + + case SYM_2RSS: + return DJI_SYM_RSSI; + +/* + case SYM_DB: + return DJI_SYM_DB + + case SYM_DBM: + return DJI_SYM_DBM; + + case SYM_SNR: + return DJI_SYM_SNR; +*/ + + case SYM_AH_DECORATION_UP: + return DJI_SYM_ARROW_SMALL_UP; + + case SYM_AH_DECORATION_DOWN: + return DJI_SYM_ARROW_SMALL_DOWN; + + case SYM_DECORATION: + return DJI_SYM_ARROW_SMALL_UP; + + case SYM_DECORATION + 1: // NE pointing arrow + return DJI_SYM_ARROW_7; + + case SYM_DECORATION + 2: // E pointing arrow + return DJI_SYM_ARROW_EAST; + + case SYM_DECORATION + 3: // SE pointing arrow + return DJI_SYM_ARROW_3; + + case SYM_DECORATION + 4: // S pointing arrow + return DJI_SYM_ARROW_SOUTH; + + case SYM_DECORATION + 5: // SW pointing arrow + return DJI_SYM_ARROW_15; + + case SYM_DECORATION + 6: // W pointing arrow + return DJI_SYM_ARROW_WEST; + + case SYM_DECORATION + 7: // NW pointing arrow + return DJI_SYM_ARROW_11; + + case SYM_VOLT: + return DJI_SYM_VOLT; + + case SYM_MAH: + return DJI_SYM_MAH; + + case SYM_AH_KM: + return 'K'; + + case SYM_AH_MI: + return 'M'; +/* + case SYM_VTX_POWER: + return DJI_SYM_VTX_POWER; + + case SYM_AH_NM: + return DJI_SYM_AH_NM; + + case SYM_MAH_NM_0: + return DJI_SYM_MAH_NM_0; + + case SYM_MAH_NM_1: + return DJI_SYM_MAH_NM_1; + + case SYM_MAH_KM_0: + return DJI_SYM_MAH_KM_0; + + case SYM_MAH_KM_1: + return DJI_SYM_MAH_KM_1; + + case SYM_MILLIOHM: + return DJI_SYM_MILLIOHM; +*/ + case SYM_BATT_FULL: + return DJI_SYM_BATT_FULL; + + case SYM_BATT_5: + return DJI_SYM_BATT_5; + + case SYM_BATT_4: + return DJI_SYM_BATT_4; + + case SYM_BATT_3: + return DJI_SYM_BATT_3; + + case SYM_BATT_2: + return DJI_SYM_BATT_2; + + case SYM_BATT_1: + return DJI_SYM_BATT_1; + + case SYM_BATT_EMPTY: + return DJI_SYM_BATT_EMPTY; + + case SYM_AMP: + return DJI_SYM_AMP; +/* + case SYM_WH: + return DJI_SYM_WH; + + case SYM_WH_KM: + return DJI_SYM_WH_KM; + + case SYM_WH_MI: + return DJI_SYM_WH_MI; + + case SYM_WH_NM: + return DJI_SYM_WH_NM; +*/ + case SYM_WATT: + return DJI_SYM_WATT; +/* + case SYM_MW: + return DJI_SYM_MW; + + case SYM_KILOWATT: + return DJI_SYM_KILOWATT; +*/ + case SYM_FT: + return DJI_SYM_FT; + + case SYM_ALT_FT: + return DJI_SYM_FT; + + case SYM_ALT_M: + return DJI_SYM_M; + + case SYM_TOTAL: + return DJI_SYM_FLY_H; +/* + + case SYM_ALT_KM: + return DJI_SYM_ALT_KM; + + case SYM_ALT_KFT: + return DJI_SYM_ALT_KFT; + + case SYM_DIST_M: + return DJI_SYM_DIST_M; + + case SYM_DIST_KM: + return DJI_SYM_DIST_KM; + + case SYM_DIST_FT: + return DJI_SYM_DIST_FT; + + case SYM_DIST_MI: + return DJI_SYM_DIST_MI; + + case SYM_DIST_NM: + return DJI_SYM_DIST_NM; +*/ + case SYM_M: + return DJI_SYM_M; + + case SYM_KM: + return 'K'; + + case SYM_MI: + return 'M'; +/* + case SYM_NM: + return DJI_SYM_NM; +*/ + case SYM_WIND_HORIZONTAL: + return 'W'; // W for wind + +/* + case SYM_WIND_VERTICAL: + return DJI_SYM_WIND_VERTICAL; + + case SYM_3D_KT: + return DJI_SYM_3D_KT; +*/ + case SYM_AIR: + return 'A'; // A for airspeed + + case SYM_3D_KMH: + return DJI_SYM_KPH; + + case SYM_3D_MPH: + return DJI_SYM_MPH; + + case SYM_RPM: + return DJI_SYM_RPM; + + case SYM_FTS: + return DJI_SYM_FTPS; +/* + case SYM_100FTM: + return DJI_SYM_100FTM; +*/ + case SYM_MS: + return DJI_SYM_MPS; + + case SYM_KMH: + return DJI_SYM_KPH; + + case SYM_MPH: + return DJI_SYM_MPH; +/* + case SYM_KT: + return DJI_SYM_KT + + case SYM_MAH_MI_0: + return DJI_SYM_MAH_MI_0; + + case SYM_MAH_MI_1: + return DJI_SYM_MAH_MI_1; +*/ + case SYM_THR: + return DJI_SYM_THR; + + case SYM_TEMP_F: + return DJI_SYM_F; + + case SYM_TEMP_C: + return DJI_SYM_C; + + case SYM_BLANK: + return DJI_SYM_BLANK; + + case SYM_ON_H: + return DJI_SYM_ON_H; + + case SYM_FLY_H: + return DJI_SYM_FLY_H; + + case SYM_ON_M: + return DJI_SYM_ON_M; + + case SYM_FLY_M: + return DJI_SYM_FLY_M; +/* + case SYM_GLIDESLOPE: + return DJI_SYM_GLIDESLOPE; + + case SYM_WAYPOINT: + return DJI_SYM_WAYPOINT; + + case SYM_CLOCK: + return DJI_SYM_CLOCK; + + case SYM_ZERO_HALF_TRAILING_DOT: + return DJI_SYM_ZERO_HALF_TRAILING_DOT; + + case SYM_ZERO_HALF_LEADING_DOT: + return DJI_SYM_ZERO_HALF_LEADING_DOT; +*/ + + case SYM_AUTO_THR0: + return 'A'; + + case SYM_AUTO_THR1: + return DJI_SYM_THR; + + case SYM_ROLL_LEFT: + return DJI_SYM_ROLL; + + case SYM_ROLL_LEVEL: + return DJI_SYM_ROLL; + + case SYM_ROLL_RIGHT: + return DJI_SYM_ROLL; + + case SYM_PITCH_UP: + return DJI_SYM_PITCH; + + case SYM_PITCH_DOWN: + return DJI_SYM_PITCH; + + case SYM_GFORCE: + return 'G'; + +/* + case SYM_GFORCE_X: + return DJI_SYM_GFORCE_X; + + case SYM_GFORCE_Y: + return DJI_SYM_GFORCE_Y; + + case SYM_GFORCE_Z: + return DJI_SYM_GFORCE_Z; +*/ + case SYM_BARO_TEMP: + return DJI_SYM_TEMPERATURE; + + case SYM_IMU_TEMP: + return DJI_SYM_TEMPERATURE; + + case SYM_TEMP: + return DJI_SYM_TEMPERATURE; + + case SYM_ESC_TEMP: + return DJI_SYM_TEMPERATURE; +/* + case SYM_TEMP_SENSOR_FIRST: + return DJI_SYM_TEMP_SENSOR_FIRST; + + case SYM_TEMP_SENSOR_LAST: + return DJI_SYM_TEMP_SENSOR_LAST; + + case TEMP_SENSOR_SYM_COUNT: + return DJI_TEMP_SENSOR_SYM_COUNT; +*/ + case SYM_HEADING_N: + return DJI_SYM_HEADING_N; + + case SYM_HEADING_S: + return DJI_SYM_HEADING_S; + + case SYM_HEADING_E: + return DJI_SYM_HEADING_E; + + case SYM_HEADING_W: + return DJI_SYM_HEADING_W; + + case SYM_HEADING_DIVIDED_LINE: + return DJI_SYM_HEADING_DIVIDED_LINE; + + case SYM_HEADING_LINE: + return DJI_SYM_HEADING_LINE; + + case SYM_MAX: + return DJI_SYM_MAX; +/* + case SYM_PROFILE: + return DJI_SYM_PROFILE; +*/ + case SYM_SWITCH_INDICATOR_LOW: + return DJI_SYM_STICK_OVERLAY_SPRITE_LOW; + + case SYM_SWITCH_INDICATOR_MID: + return DJI_SYM_STICK_OVERLAY_SPRITE_MID; + + case SYM_SWITCH_INDICATOR_HIGH: + return DJI_SYM_STICK_OVERLAY_SPRITE_HIGH; +/* + case SYM_AH: + return DJI_SYM_AH; + + case SYM_GLIDE_DIST: + return DJI_SYM_GLIDE_DIST; + + case SYM_GLIDE_MINS: + return DJI_SYM_GLIDE_MINS; + + case SYM_AH_V_FT_0: + return DJI_SYM_AH_V_FT_0; + + case SYM_AH_V_FT_1: + return DJI_SYM_AH_V_FT_1; + + case SYM_AH_V_M_0: + return DJI_SYM_AH_V_M_0; + + case SYM_AH_V_M_1: + return DJI_SYM_AH_V_M_1; + + case SYM_FLIGHT_MINS_REMAINING: + return DJI_SYM_FLIGHT_MINS_REMAINING; + + case SYM_FLIGHT_HOURS_REMAINING: + return DJI_SYM_FLIGHT_HOURS_REMAINING; + + case SYM_GROUND_COURSE: + return DJI_SYM_GROUND_COURSE; + + case SYM_CROSS_TRACK_ERROR: + return DJI_SYM_CROSS_TRACK_ERROR; + + case SYM_LOGO_START: + return DJI_SYM_LOGO_START; + + case SYM_LOGO_WIDTH: + return DJI_SYM_LOGO_WIDTH; + + case SYM_LOGO_HEIGHT: + return DJI_SYM_LOGO_HEIGHT; +*/ + case SYM_AH_LEFT: + return DJI_SYM_AH_LEFT; + + case SYM_AH_RIGHT: + return DJI_SYM_AH_RIGHT; + +/* + case SYM_AH_DECORATION_COUNT: + return DJI_SYM_AH_DECORATION_COUNT; +*/ + case SYM_AH_CH_LEFT: + case SYM_AH_CH_TYPE3: + case SYM_AH_CH_TYPE4: + case SYM_AH_CH_TYPE5: + case SYM_AH_CH_TYPE6: + case SYM_AH_CH_TYPE7: + case SYM_AH_CH_TYPE8: + case SYM_AH_CH_AIRCRAFT1: + return DJI_SYM_CROSSHAIR_LEFT; + + case SYM_AH_CH_CENTER: + case (SYM_AH_CH_TYPE3+1): + case (SYM_AH_CH_TYPE4+1): + case (SYM_AH_CH_TYPE5+1): + case (SYM_AH_CH_TYPE6+1): + case (SYM_AH_CH_TYPE7+1): + case (SYM_AH_CH_TYPE8+1): + case SYM_AH_CH_AIRCRAFT2: + return DJI_SYM_CROSSHAIR_CENTRE; + + case SYM_AH_CH_RIGHT: + case (SYM_AH_CH_TYPE3+2): + case (SYM_AH_CH_TYPE4+2): + case (SYM_AH_CH_TYPE5+2): + case (SYM_AH_CH_TYPE6+2): + case (SYM_AH_CH_TYPE7+2): + case (SYM_AH_CH_TYPE8+2): + case SYM_AH_CH_AIRCRAFT3: + return DJI_SYM_CROSSHAIR_RIGHT; + + case SYM_AH_CH_AIRCRAFT0: + case SYM_AH_CH_AIRCRAFT4: + return DJI_SYM_BLANK; + + case SYM_ARROW_UP: + return DJI_SYM_ARROW_NORTH; + + case SYM_ARROW_2: + return DJI_SYM_ARROW_8; + + case SYM_ARROW_3: + return DJI_SYM_ARROW_7; + + case SYM_ARROW_4: + return DJI_SYM_ARROW_6; + + case SYM_ARROW_RIGHT: + return DJI_SYM_ARROW_EAST; + + case SYM_ARROW_6: + return DJI_SYM_ARROW_4; + + case SYM_ARROW_7: + return DJI_SYM_ARROW_3; + + case SYM_ARROW_8: + return DJI_SYM_ARROW_2; + + case SYM_ARROW_DOWN: + return DJI_SYM_ARROW_SOUTH; + + case SYM_ARROW_10: + return DJI_SYM_ARROW_16; + + case SYM_ARROW_11: + return DJI_SYM_ARROW_15; + + case SYM_ARROW_12: + return DJI_SYM_ARROW_14; + + case SYM_ARROW_LEFT: + return DJI_SYM_ARROW_WEST; + + case SYM_ARROW_14: + return DJI_SYM_ARROW_12; + + case SYM_ARROW_15: + return DJI_SYM_ARROW_11; + + case SYM_ARROW_16: + return DJI_SYM_ARROW_10; + + case SYM_AH_H_START: + return DJI_SYM_AH_BAR9_0; + + case (SYM_AH_H_START+1): + return DJI_SYM_AH_BAR9_1; + + case (SYM_AH_H_START+2): + return DJI_SYM_AH_BAR9_2; + + case (SYM_AH_H_START+3): + return DJI_SYM_AH_BAR9_3; + + case (SYM_AH_H_START+4): + return DJI_SYM_AH_BAR9_4; + + case (SYM_AH_H_START+5): + return DJI_SYM_AH_BAR9_5; + + case (SYM_AH_H_START+6): + return DJI_SYM_AH_BAR9_6; + + case (SYM_AH_H_START+7): + return DJI_SYM_AH_BAR9_7; + + case (SYM_AH_H_START+8): + return DJI_SYM_AH_BAR9_8; + + // DJI does not have vertical artificial horizon. replace with middle horizontal one + case SYM_AH_V_START: + case (SYM_AH_V_START+1): + case (SYM_AH_V_START+2): + case (SYM_AH_V_START+3): + case (SYM_AH_V_START+4): + case (SYM_AH_V_START+5): + return DJI_SYM_AH_BAR9_4; + + // DJI for ESP_RADAR Symbols + case SYM_HUD_CARDINAL: + return DJI_SYM_ARROW_SOUTH; + case SYM_HUD_CARDINAL + 1: + return DJI_SYM_ARROW_16; + case SYM_HUD_CARDINAL + 2: + return DJI_SYM_ARROW_15; + case SYM_HUD_CARDINAL + 3: + return DJI_SYM_ARROW_WEST; + case SYM_HUD_CARDINAL + 4: + return DJI_SYM_ARROW_12; + case SYM_HUD_CARDINAL + 5: + return DJI_SYM_ARROW_11; + case SYM_HUD_CARDINAL + 6: + return DJI_SYM_ARROW_NORTH; + case SYM_HUD_CARDINAL + 7: + return DJI_SYM_ARROW_7; + case SYM_HUD_CARDINAL + 8: + return DJI_SYM_ARROW_6; + case SYM_HUD_CARDINAL + 9: + return DJI_SYM_ARROW_EAST; + case SYM_HUD_CARDINAL + 10: + return DJI_SYM_ARROW_3; + case SYM_HUD_CARDINAL + 11: + return DJI_SYM_ARROW_2; + + case SYM_HUD_SIGNAL_0: + return DJI_SYM_AH_BAR9_1; + case SYM_HUD_SIGNAL_1: + return DJI_SYM_AH_BAR9_3; + case SYM_HUD_SIGNAL_2: + return DJI_SYM_AH_BAR9_4; + case SYM_HUD_SIGNAL_3: + return DJI_SYM_AH_BAR9_5; + case SYM_HUD_SIGNAL_4: + return DJI_SYM_AH_BAR9_7; + + case SYM_VARIO_UP_2A: + return DJI_SYM_ARROW_SMALL_UP; + + case SYM_VARIO_UP_1A: + return DJI_SYM_ARROW_SMALL_UP; + + case SYM_VARIO_DOWN_1A: + return DJI_SYM_ARROW_SMALL_DOWN; + + case SYM_VARIO_DOWN_2A: + return DJI_SYM_ARROW_SMALL_DOWN; + + case SYM_ALT: + return DJI_SYM_ALTITUDE; +/* + case SYM_HUD_SIGNAL_0: + return DJI_SYM_HUD_SIGNAL_0; + + case SYM_HUD_SIGNAL_1: + return DJI_SYM_HUD_SIGNAL_1; + + case SYM_HUD_SIGNAL_2: + return DJI_SYM_HUD_SIGNAL_2; + + case SYM_HUD_SIGNAL_3: + return DJI_SYM_HUD_SIGNAL_3; + + case SYM_HUD_SIGNAL_4: + return DJI_SYM_HUD_SIGNAL_4; + + case SYM_HOME_DIST: + return DJI_SYM_HOME_DIST; + + case SYM_FLIGHT_DIST_REMAINING: + return DJI_SYM_FLIGHT_DIST_REMAINING; +*/ + case SYM_HUD_ARROWS_L1: + return DJI_SYM_ARROW_SMALL_LEFT; + + case SYM_HUD_ARROWS_L2: + return DJI_SYM_ARROW_SMALL_LEFT; + + case SYM_HUD_ARROWS_L3: + return DJI_SYM_ARROW_SMALL_LEFT; + + case SYM_HUD_ARROWS_R1: + return DJI_SYM_ARROW_SMALL_RIGHT; + + case SYM_HUD_ARROWS_R2: + return DJI_SYM_ARROW_SMALL_RIGHT; + + case SYM_HUD_ARROWS_R3: + return DJI_SYM_ARROW_SMALL_RIGHT; + + case SYM_HUD_ARROWS_U1: + return DJI_SYM_ARROW_SMALL_UP; + + case SYM_HUD_ARROWS_U2: + return DJI_SYM_ARROW_SMALL_UP; + + case SYM_HUD_ARROWS_U3: + return DJI_SYM_ARROW_SMALL_UP; + + case SYM_HUD_ARROWS_D1: + return DJI_SYM_ARROW_SMALL_DOWN; + + case SYM_HUD_ARROWS_D2: + return DJI_SYM_ARROW_SMALL_DOWN; + + case SYM_HUD_ARROWS_D3: + return DJI_SYM_ARROW_SMALL_DOWN; + + default: + break; + } + + return '?'; // Missing/not mapped character +} + +#endif + +#endif diff --git a/src/main/io/displayport_msp_bf_compat.h b/src/main/io/displayport_msp_dji_compat.h similarity index 69% rename from src/main/io/displayport_msp_bf_compat.h rename to src/main/io/displayport_msp_dji_compat.h index 95dab7b518d..6380770aa00 100644 --- a/src/main/io/displayport_msp_bf_compat.h +++ b/src/main/io/displayport_msp_dji_compat.h @@ -22,15 +22,15 @@ #include "platform.h" -#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT) && !defined(DISABLE_MSP_BF_COMPAT) +#if defined(USE_OSD) && defined(USE_MSP_DISPLAYPORT) && !defined(DISABLE_MSP_DJI_COMPAT) #include "osd.h" -uint8_t getBfCharacter(uint8_t ch, uint8_t page); -#define isBfCompatibleVideoSystem(osdConfigPtr) (osdConfigPtr->video_system == VIDEO_SYSTEM_BFCOMPAT || osdConfigPtr->video_system == VIDEO_SYSTEM_BFCOMPAT_HD) +uint8_t getDJICharacter(uint8_t ch, uint8_t page); +#define isDJICompatibleVideoSystem(osdConfigPtr) (osdConfigPtr->video_system == VIDEO_SYSTEM_DJICOMPAT || osdConfigPtr->video_system == VIDEO_SYSTEM_DJICOMPAT_HD) #else -#define getBfCharacter(x, page) (x) +#define getDJICharacter(x, page) (x) #ifdef OSD_UNIT_TEST -#define isBfCompatibleVideoSystem(osdConfigPtr) (true) +#define isDJICompatibleVideoSystem(osdConfigPtr) (true) #else -#define isBfCompatibleVideoSystem(osdConfigPtr) (false) +#define isDJICompatibleVideoSystem(osdConfigPtr) (false) #endif #endif \ No newline at end of file diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index d587104e9b9..2e425e6aecb 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -51,7 +51,7 @@ #include "msp/msp_serial.h" #include "displayport_msp_osd.h" -#include "displayport_msp_bf_compat.h" +#include "displayport_msp_dji_compat.h" #define FONT_VERSION 3 @@ -307,7 +307,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz uint8_t len = 4; do { bitArrayClr(dirty, pos); - subcmd[len] = isBfCompatibleVideoSystem(osdConfig()) ? getBfCharacter(screen[pos++], page): screen[pos++]; + subcmd[len] = isDJICompatibleVideoSystem(osdConfig()) ? getDJICharacter(screen[pos++], page): screen[pos++]; len++; if (bitArrayGet(dirty, pos)) { @@ -315,7 +315,7 @@ static int drawScreen(displayPort_t *displayPort) // 250Hz } } while (next == pos && next < endOfLine && getAttrPage(attrs[next]) == page && getAttrBlink(attrs[next]) == blink); - if (!isBfCompatibleVideoSystem(osdConfig())) { + if (!isDJICompatibleVideoSystem(osdConfig())) { attributes |= (page << DISPLAYPORT_MSP_ATTR_FONTPAGE); } @@ -465,7 +465,7 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem) if (mspOsdSerialInit()) { switch(videoSystem) { case VIDEO_SYSTEM_AUTO: - case VIDEO_SYSTEM_BFCOMPAT: + case VIDEO_SYSTEM_DJICOMPAT: case VIDEO_SYSTEM_PAL: currentOsdMode = SD_3016; screenRows = PAL_ROWS; @@ -486,7 +486,7 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem) screenRows = DJI_ROWS; screenCols = DJI_COLS; break; - case VIDEO_SYSTEM_BFCOMPAT_HD: + case VIDEO_SYSTEM_DJICOMPAT_HD: case VIDEO_SYSTEM_AVATAR: currentOsdMode = HD_5320; screenRows = AVATAR_ROWS; @@ -500,10 +500,10 @@ displayPort_t* mspOsdDisplayPortInit(const videoSystem_e videoSystem) init(); displayInit(&mspOsdDisplayPort, &mspOsdVTable); - if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT) { - mspOsdDisplayPort.displayPortType = "MSP DisplayPort: BetaFlight Compatability mode"; - } else if (osdVideoSystem == VIDEO_SYSTEM_BFCOMPAT_HD) { - mspOsdDisplayPort.displayPortType = "MSP DisplayPort: BetaFlight Compatability mode (HD)"; + if (osdVideoSystem == VIDEO_SYSTEM_DJICOMPAT) { + mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode"; + } else if (osdVideoSystem == VIDEO_SYSTEM_DJICOMPAT_HD) { + mspOsdDisplayPort.displayPortType = "MSP DisplayPort: DJI Compatability mode (HD)"; } else { mspOsdDisplayPort.displayPortType = "MSP DisplayPort"; } diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h new file mode 100644 index 00000000000..66972a810e7 --- /dev/null +++ b/src/main/io/dji_osd_symbols.h @@ -0,0 +1,164 @@ +/* @file max7456_symbols.h + * @brief max7456 symbols for the mwosd font set + * + * @author Nathan Tsoi nathan@vertile.com + * + * Copyright (C) 2016 Nathan Tsoi + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see + */ + +#pragma once + +//Misc +#define DJI_SYM_NONE 0x00 +#define DJI_SYM_END_OF_FONT 0xFF +#define DJI_SYM_BLANK 0x20 +#define DJI_SYM_HYPHEN 0x2D +#define DJI_SYM_BBLOG 0x10 +#define DJI_SYM_HOMEFLAG 0x11 +#define DJI_SYM_RPM 0x12 +#define DJI_SYM_ROLL 0x14 +#define DJI_SYM_PITCH 0x15 +#define DJI_SYM_TEMPERATURE 0x7A +#define DJI_SYM_MAX 0x24 + +// GPS and navigation +#define DJI_SYM_LAT 0x89 +#define DJI_SYM_LON 0x98 +#define DJI_SYM_ALTITUDE 0x7F +#define DJI_SYM_OVER_HOME 0x05 + +// RSSI +#define DJI_SYM_RSSI 0x01 + +// Throttle Position (%) +#define DJI_SYM_THR 0x04 + +// Unit Icons (Metric) +#define DJI_SYM_M 0x0C +#define DJI_SYM_C 0x0E + +// Unit Icons (Imperial) +#define DJI_SYM_F 0x0D +#define DJI_SYM_FT 0x0F + +// Heading Graphics +#define DJI_SYM_HEADING_N 0x18 +#define DJI_SYM_HEADING_S 0x19 +#define DJI_SYM_HEADING_E 0x1A +#define DJI_SYM_HEADING_W 0x1B +#define DJI_SYM_HEADING_DIVIDED_LINE 0x1C +#define DJI_SYM_HEADING_LINE 0x1D + +// AH Center screen Graphics +#define DJI_SYM_CROSSHAIR_LEFT 0x72 +#define DJI_SYM_CROSSHAIR_CENTRE 0x73 +#define DJI_SYM_CROSSHAIR_RIGHT 0x74 +#define DJI_SYM_AH_RIGHT 0x02 +#define DJI_SYM_AH_LEFT 0x03 +#define DJI_SYM_AH_DECORATION 0x13 +#define DJI_SYM_SMALL_CROSSHAIR 0x7E + +// Satellite Graphics +#define DJI_SYM_SAT_L 0x1E +#define DJI_SYM_SAT_R 0x1F + +// Direction arrows +#define DJI_SYM_ARROW_SOUTH 0x60 +#define DJI_SYM_ARROW_2 0x61 +#define DJI_SYM_ARROW_3 0x62 +#define DJI_SYM_ARROW_4 0x63 +#define DJI_SYM_ARROW_EAST 0x64 +#define DJI_SYM_ARROW_6 0x65 +#define DJI_SYM_ARROW_7 0x66 +#define DJI_SYM_ARROW_8 0x67 +#define DJI_SYM_ARROW_NORTH 0x68 +#define DJI_SYM_ARROW_10 0x69 +#define DJI_SYM_ARROW_11 0x6A +#define DJI_SYM_ARROW_12 0x6B +#define DJI_SYM_ARROW_WEST 0x6C +#define DJI_SYM_ARROW_14 0x6D +#define DJI_SYM_ARROW_15 0x6E +#define DJI_SYM_ARROW_16 0x6F + +#define DJI_SYM_ARROW_SMALL_UP 0x75 +#define DJI_SYM_ARROW_SMALL_DOWN 0x76 +#define DJI_SYM_ARROW_SMALL_RIGHT 0x77 +#define DJI_SYM_ARROW_SMALL_LEFT 0x78 + +// AH Bars +#define DJI_SYM_AH_BAR9_0 0x80 +#define DJI_SYM_AH_BAR9_1 0x81 +#define DJI_SYM_AH_BAR9_2 0x82 +#define DJI_SYM_AH_BAR9_3 0x83 +#define DJI_SYM_AH_BAR9_4 0x84 +#define DJI_SYM_AH_BAR9_5 0x85 +#define DJI_SYM_AH_BAR9_6 0x86 +#define DJI_SYM_AH_BAR9_7 0x87 +#define DJI_SYM_AH_BAR9_8 0x88 + +// Progress bar +#define DJI_SYM_PB_START 0x8A +#define DJI_SYM_PB_FULL 0x8B +#define DJI_SYM_PB_HALF 0x8C +#define DJI_SYM_PB_EMPTY 0x8D +#define DJI_SYM_PB_END 0x8E +#define DJI_SYM_PB_CLOSE 0x8F + +// Batt evolution +#define DJI_SYM_BATT_FULL 0x90 +#define DJI_SYM_BATT_5 0x91 +#define DJI_SYM_BATT_4 0x92 +#define DJI_SYM_BATT_3 0x93 +#define DJI_SYM_BATT_2 0x94 +#define DJI_SYM_BATT_1 0x95 +#define DJI_SYM_BATT_EMPTY 0x96 + +// Batt Icons +#define DJI_SYM_MAIN_BATT 0x97 + +// Voltage and amperage +#define DJI_SYM_VOLT 0x06 +#define DJI_SYM_AMP 0x9A +#define DJI_SYM_MAH 0x07 +#define DJI_SYM_WATT 0x57 // 0x57 is 'W' + +// Time +#define DJI_SYM_ON_H 0x70 +#define DJI_SYM_FLY_H 0x71 +#define DJI_SYM_ON_M 0x9B +#define DJI_SYM_FLY_M 0x9C + +// Speed +#define DJI_SYM_KPH 0x9E +#define DJI_SYM_MPH 0x9D +#define DJI_SYM_MPS 0x9F +#define DJI_SYM_FTPS 0x99 + +// Menu cursor +#define DJI_SYM_CURSOR DJI_SYM_AH_LEFT + +// Stick overlays +#define DJI_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08 +#define DJI_SYM_STICK_OVERLAY_SPRITE_MID 0x09 +#define DJI_SYM_STICK_OVERLAY_SPRITE_LOW 0x0A +#define DJI_SYM_STICK_OVERLAY_CENTER 0x0B +#define DJI_SYM_STICK_OVERLAY_VERTICAL 0x16 +#define DJI_SYM_STICK_OVERLAY_HORIZONTAL 0x17 + +// GPS degree/minute/second symbols +#define DJI_SYM_GPS_DEGREE DJI_SYM_STICK_OVERLAY_SPRITE_HIGH // kind of looks like the degree symbol +#define DJI_SYM_GPS_MINUTE 0x27 // ' +#define DJI_SYM_GPS_SECOND 0x22 // " diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 56245ea04af..f3f00ae7bdb 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -70,7 +70,7 @@ #include "io/osd_common.h" #include "io/osd_hud.h" #include "io/osd_utils.h" -#include "io/displayport_msp_bf_compat.h" +#include "io/displayport_msp_dji_compat.h" #include "io/vtx.h" #include "io/vtx_string.h" @@ -253,51 +253,6 @@ bool osdIsNotMetric(void) { return !(osdConfig()->units == OSD_UNIT_METRIC || osdConfig()->units == OSD_UNIT_METRIC_MPH); } -/* - * Aligns text to the left side. Adds spaces at the end to keep string length unchanged. - */ -/* -- Currently unused -- -static void osdLeftAlignString(char *buff) -{ - uint8_t sp = 0, ch = 0; - uint8_t len = strlen(buff); - while (buff[sp] == ' ') sp++; - for (ch = 0; ch < (len - sp); ch++) buff[ch] = buff[ch + sp]; - for (sp = ch; sp < len; sp++) buff[sp] = ' '; -}*/ - -/* - * This is a simplified distance conversion code that does not use any scaling - * but is fully compatible with the DJI G2 MSP Displayport OSD implementation. - * (Based on osdSimpleAltitudeSymbol() implementation) - */ -/* void osdSimpleDistanceSymbol(char *buff, int32_t dist) { - - int32_t convertedDistance; - char suffix; - - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_GA: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - convertedDistance = CENTIMETERS_TO_FEET(dist); - suffix = SYM_ALT_FT; - break; - case OSD_UNIT_METRIC_MPH: - FALLTHROUGH; - case OSD_UNIT_METRIC: - convertedDistance = CENTIMETERS_TO_METERS(dist); - suffix = SYM_ALT_M; // Intentionally use the altitude symbol, as the distance symbol is not defined in BFCOMPAT mode - break; - } - - tfp_sprintf(buff, "%5d", (int) convertedDistance); // 5 digits, allowing up to 99999 meters/feet, which should be plenty for 99.9% of use cases - buff[5] = suffix; - buff[6] = '\0'; -} */ - /** * Converts distance into a string based on the current unit system * prefixed by a a symbol to indicate the unit used. @@ -313,12 +268,12 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) uint8_t symbol_mi = SYM_DIST_MI; uint8_t symbol_nm = SYM_DIST_NM; -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values + if (isDJICompatibleVideoSystem(osdConfig())) { // Add one digit so up no switch to scaled decimal occurs above 99 digits = 4U; sym_index = 4U; - // Use altitude symbols on purpose, as it seems distance symbols are not defined in BFCOMPAT mode + // Use altitude symbols on purpose, as it seems distance symbols are not defined in DJICOMPAT mode symbol_m = SYM_ALT_M; symbol_km = SYM_ALT_KM; symbol_ft = SYM_ALT_FT; @@ -570,8 +525,8 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) buff[0] = ' '; } -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values + if (isDJICompatibleVideoSystem(osdConfig())) { totalDigits++; digits++; symbolIndex++; @@ -792,21 +747,21 @@ static void osdFormatCoordinate(char *buff, char sym, int32_t val) int32_t decimalPart = abs(val % (int)GPS_DEGREES_DIVIDER); STATIC_ASSERT(GPS_DEGREES_DIVIDER == 1e7, adjust_max_decimal_digits); int decimalDigits; - bool bfcompat = false; // Assume BFCOMPAT mode is no enabled + bool djiCompat = false; // Assume DJICOMPAT mode is no enabled -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { - bfcompat = true; +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it + if(isDJICompatibleVideoSystem(osdConfig())) { + djiCompat = true; } #endif - if (!bfcompat) { + if (!djiCompat) { decimalDigits = tfp_sprintf(buff + 1 + integerDigits, "%07d", (int)decimalPart); // Embbed the decimal separator buff[1 + integerDigits - 1] += SYM_ZERO_HALF_TRAILING_DOT - '0'; buff[1 + integerDigits] += SYM_ZERO_HALF_LEADING_DOT - '0'; } else { - // BFCOMPAT mode enabled + // DJICOMPAT mode enabled decimalDigits = tfp_sprintf(buff + 1 + integerDigits, ".%06d", (int)decimalPart); } // Fill up to coordinateLength with zeros @@ -1752,8 +1707,8 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_MAIN_BATT_VOLTAGE: { uint8_t base_digits = 2U; -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it + if(isDJICompatibleVideoSystem(osdConfig())) { base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space } #endif @@ -1763,8 +1718,8 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE: { uint8_t base_digits = 2U; -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it + if(isDJICompatibleVideoSystem(osdConfig())) { base_digits = 3U; // Add extra digit to account for decimal point taking an extra character space } #endif @@ -1787,9 +1742,9 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_MAH_DRAWN: { uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if (isBfCompatibleVideoSystem(osdConfig())) { - //BFcompat is unable to work with scaled values and it only has mAh symbol to work with +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it + if (isDJICompatibleVideoSystem(osdConfig())) { + //DJIcompat is unable to work with scaled values and it only has mAh symbol to work with tfp_sprintf(buff, "%5d", (int)getMAhDrawn()); // Use 5 digits to allow packs below 100Ah buff[5] = SYM_MAH; buff[6] = '\0'; @@ -1828,9 +1783,9 @@ static bool osdDrawSingleElement(uint8_t item) else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) { uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if (isBfCompatibleVideoSystem(osdConfig())) { - //BFcompat is unable to work with scaled values and it only has mAh symbol to work with +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it + if (isDJICompatibleVideoSystem(osdConfig())) { + //DJIcompat is unable to work with scaled values and it only has mAh symbol to work with tfp_sprintf(buff, "%5d", (int)getBatteryRemainingCapacity()); // Use 5 digits to allow packs below 100Ah buff[5] = SYM_MAH; buff[6] = '\0'; @@ -2139,8 +2094,8 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = SYM_HDP_R; int32_t centiHDOP = 100 * gpsSol.hdop / HDOP_SCALE; uint8_t digits = 2U; -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values + if (isDJICompatibleVideoSystem(osdConfig())) { digits = 3U; } #endif @@ -2799,8 +2754,8 @@ static bool osdDrawSingleElement(uint8_t item) // time will be longer than 99 minutes. If it is, it will show 99:^^ if (glideTime > (99 * 60) + 59) { tfp_sprintf(buff + 1, "%02d:", (int)(glideTime / 60)); - buff[4] = SYM_DIRECTION; - buff[5] = SYM_DIRECTION; + buff[4] = SYM_DECORATION; + buff[5] = SYM_DECORATION; } else { tfp_sprintf(buff + 1, "%02d:%02d", (int)(glideTime / 60), (int)(glideTime % 60)); } @@ -3147,8 +3102,8 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_MAIN_BATT_CELL_VOLTAGE: { uint8_t base_digits = 3U; -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it + if(isDJICompatibleVideoSystem(osdConfig())) { base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space } #endif @@ -3159,8 +3114,8 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE: { uint8_t base_digits = 3U; -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it - if(isBfCompatibleVideoSystem(osdConfig())) { +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it + if(isDJICompatibleVideoSystem(osdConfig())) { base_digits = 4U; // Add extra digit to account for decimal point taking an extra character space } #endif @@ -3215,8 +3170,8 @@ static bool osdDrawSingleElement(uint8_t item) timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); uint8_t digits = 3U; -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values + if (isDJICompatibleVideoSystem(osdConfig())) { // Increase number of digits so values above 99 don't get scaled by osdFormatCentiNumber digits = 4U; } @@ -3248,7 +3203,7 @@ static bool osdDrawSingleElement(uint8_t item) } if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = buff[3] = '-'; - buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in BFCOMPAT mode + buff[digits] = SYM_MAH_MI_0; // This will overwrite the "-" at buff[3] if not in DJICOMPAT mode buff[digits + 1] = SYM_MAH_MI_1; buff[digits + 2] = '\0'; } @@ -3418,7 +3373,7 @@ static bool osdDrawSingleElement(uint8_t item) horizontalWindSpeed = getEstimatedHorizontalWindSpeed(&angle); int16_t windDirection = osdGetHeadingAngle( CENTIDEGREES_TO_DEGREES((int)angle) - DECIDEGREES_TO_DEGREES(attitude.values.yaw) + 22); buff[0] = SYM_WIND_HORIZONTAL; - buff[1] = SYM_DIRECTION + (windDirection*2 / 90); + buff[1] = SYM_DECORATION + (windDirection*2 / 90); osdFormatWindSpeedStr(buff + 2, horizontalWindSpeed, valid); break; } @@ -3435,10 +3390,10 @@ static bool osdDrawSingleElement(uint8_t item) float verticalWindSpeed; verticalWindSpeed = -getEstimatedWindSpeed(Z); //from NED to NEU if (verticalWindSpeed < 0) { - buff[1] = SYM_AH_DIRECTION_DOWN; + buff[1] = SYM_AH_DECORATION_DOWN; verticalWindSpeed = -verticalWindSpeed; } else { - buff[1] = SYM_AH_DIRECTION_UP; + buff[1] = SYM_AH_DECORATION_UP; } osdFormatWindSpeedStr(buff + 2, verticalWindSpeed, valid); break; @@ -3552,7 +3507,7 @@ static bool osdDrawSingleElement(uint8_t item) } else { referenceSymbol = '-'; } - displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DIRECTION); + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_DECORATION); displayWriteChar(osdDisplayPort, elemPosX, elemPosY + 1, referenceSymbol); return true; } @@ -4186,8 +4141,8 @@ uint8_t drawLogos(bool singular, uint8_t row) { uint8_t logoColOffset = 0; bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD()); -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is in use, the pilot logo cannot be used, due to font issues. - if (isBfCompatibleVideoSystem(osdConfig())) +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is in use, the pilot logo cannot be used, due to font issues. + if (isDJICompatibleVideoSystem(osdConfig())) usePilotLogo = false; #endif @@ -4727,8 +4682,8 @@ uint8_t drawStat_AverageEfficiency(uint8_t col, uint8_t row, uint8_t statValX, b tfp_sprintf(outBuff, ": "); uint8_t digits = 3U; // Total number of digits (including decimal point) -#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it and change the values - if (isBfCompatibleVideoSystem(osdConfig())) { +#ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it and change the values + if (isDJICompatibleVideoSystem(osdConfig())) { // Add one digit so no switch to scaled decimal occurs above 99 digits = 4U; } diff --git a/src/main/io/osd_grid.c b/src/main/io/osd_grid.c index 5d7e8736d91..9e79194498d 100644 --- a/src/main/io/osd_grid.c +++ b/src/main/io/osd_grid.c @@ -315,16 +315,16 @@ void osdGridDrawSidebars(displayPort_t *display) // Arrows if (osdConfig()->sidebar_scroll_arrows) { displayWriteChar(display, elemPosX - hudwidth, elemPosY - hudheight - 1, - left.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DIRECTION_UP : SYM_BLANK); + left.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK); displayWriteChar(display, elemPosX + hudwidth, elemPosY - hudheight - 1, - right.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DIRECTION_UP : SYM_BLANK); + right.arrow == OSD_SIDEBAR_ARROW_UP ? SYM_AH_DECORATION_UP : SYM_BLANK); displayWriteChar(display, elemPosX - hudwidth, elemPosY + hudheight + 1, - left.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DIRECTION_DOWN : SYM_BLANK); + left.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK); displayWriteChar(display, elemPosX + hudwidth, elemPosY + hudheight + 1, - right.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DIRECTION_DOWN : SYM_BLANK); + right.arrow == OSD_SIDEBAR_ARROW_DOWN ? SYM_AH_DECORATION_DOWN : SYM_BLANK); } // Draw AH sides diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c index 07bc181d1c1..8a6a68f467a 100644 --- a/src/main/io/osd_hud.c +++ b/src/main/io/osd_hud.c @@ -207,7 +207,7 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu if (poiType == 1) { // POI from the ESP radar error_x = hudWrap360(poiP1 - DECIDEGREES_TO_DEGREES(osdGetHeading())); - osdHudWrite(poi_x - 1, poi_y, SYM_DIRECTION + ((error_x + 22) / 45) % 8, 1); + osdHudWrite(poi_x - 1, poi_y, SYM_DECORATION + ((error_x + 22) / 45) % 8, 1); osdHudWrite(poi_x + 1, poi_y, SYM_HUD_SIGNAL_0 + poiP2, 1); } else if (poiType == 2) { // Waypoint, @@ -248,7 +248,7 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu tfp_sprintf(buff, "%3d", altc); } - buff[0] = (poiAltitude >= 0) ? SYM_AH_DIRECTION_UP : SYM_AH_DIRECTION_DOWN; + buff[0] = (poiAltitude >= 0) ? SYM_AH_DECORATION_UP : SYM_AH_DECORATION_DOWN; } else { // Display the distance by default switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: diff --git a/src/main/io/osd_utils.c b/src/main/io/osd_utils.c index 6675be8783b..9c9fb0608a0 100644 --- a/src/main/io/osd_utils.c +++ b/src/main/io/osd_utils.c @@ -20,7 +20,7 @@ #include "common/maths.h" #include "common/typeconversion.h" #include "drivers/osd_symbols.h" -#include "io/displayport_msp_bf_compat.h" +#include "io/displayport_msp_dji_compat.h" #if defined(USE_OSD) || defined(OSD_UNIT_TEST) @@ -45,7 +45,7 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma int decimals = maxDecimals; bool negative = false; bool scaled = false; - bool explicitDecimal = isBfCompatibleVideoSystem(osdConfig()); + bool explicitDecimal = isDJICompatibleVideoSystem(osdConfig()); buff[length] = '\0'; From 10d7dd58c42bfab845172e98104c0254663d6552 Mon Sep 17 00:00:00 2001 From: Scavanger Date: Wed, 15 May 2024 22:20:16 -0300 Subject: [PATCH 049/112] snapp --- docs/Broken USB recovery.md | 118 ++++++++++++++++++++++++++++++++++++ 1 file changed, 118 insertions(+) create mode 100644 docs/Broken USB recovery.md diff --git a/docs/Broken USB recovery.md b/docs/Broken USB recovery.md new file mode 100644 index 00000000000..46db627c83e --- /dev/null +++ b/docs/Broken USB recovery.md @@ -0,0 +1,118 @@ +# Broken USB recovery + +It is possible to flash INAV without USB over UART 1 or 3. + +## Prerequisites: +- USB/UART adapter (FT232, CP2102, etc.) +- STM32 Cube Programmer (https://www.st.com/en/development-tools/stm32cubeprog.html) + +To gain access to the FC via Configurator, MSP must be activated on a UART as standard. Some FCs already have this enabled by default, if not a custom firmware must be created. + +The following targets have MSP activated on a UART by default: + +| Target | Standard MSP Port | +|-----------| ----------- | +| AOCODARCF4V3 | UART5 | +| ATOMRCF405NAVI_DELUXE | UART1 | +| FF_F35_LIGHTNING | UART1 | +| FLYCOLORF7V2 | UART4 | +| GEPRCF405_BT_HD | UART5* | +| GEPRCF722_BT_HD | UART4* | +| IFLIGHT_BLITZ_F7_AIO | UART1 | +| JHEMCUF405WING | UART6 | +| JHEMCUH743HD | UART4 | +| KAKUTEH7 | UART1 and UART2* | +| KAKUTEH7WING | UART6 | +| MAMBAF405_2022A | UART4 | +| MAMBAF405US | UART4 | +| MAMBAF722 | UART4 | +| MAMBAF722 APP | UART4*| +| MAMBAF722WING | UART4 | +| MAMBAF722_X8 | UART4 | +| MAMBAH743 | UART4* | +| MATEKF405SE | UART1 | +| NEUTRONRCH743BT | UART3* | +| SDMODELH7V1 | UART1 and UART2 | +| SKYSTARSH743HD | UART4 | +| SPEEDYBEEF4 | UART5* | +| SPEEDYBEEF405MINI | UART4* | +| SPEEDYBEEF405V3 | UART4* | +| SPEEDYBEEF405V4 | UART4* | +| SPEEDYBEEF405WING | UART6 | +| SPEEDYBEEF7 | UART6 | +| SPRACINGF4EVO | UART1 | +| TMOTORF7V2 | UART5 | + +(*) No Pads/Pins, Port is used interally (Bluetooth) + +## Custom firmware: + +If the FC does not have MSP activated on a UART by default or does not have a connector for it, a custom firmware must be built. +The following procedure describes the process under Windows 10/11: + +Please read [Building in Windows 2010 or 11 with Linux Subsystem](https://github.com/iNavFlight/inav/blob/master/docs/development/Building%20in%20Windows%2010%20or%2011%20with%20Linux%20Subsystem.md) +and follow the instructions up to "Building with Make". + +To activate MSP by default, go to the directory `src/main/target/[your target]`. +If no config.c exists, create a new text file with this name and the following content: + +``` +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].msp_baudrateIndex = BAUD_115200; +} + +``` + +If the file already exists, add the following lines in the function `void targetConfiguration(void)` (before the last `}`) + +``` +serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].functionMask = FUNCTION_MSP; +serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].msp_baudrateIndex = BAUD_115200; +``` + +Replace the X in SERIAL_PORT_USARTX (in both lines) with the number of UART/serial port on which MSP is to be activated. + +Example: +For UART 2: `SERIAL_PORT_USART2` +For UART 3: `SERIAL_PORT_USART3` +etc. + +Save the file and build the firmware as described in the document above. + +## Flashing via Uart: + +1. Disconnect ALL peripherals and the USB Cable from the FC. To power the FC use a battery or use the 5V provided from the USB/Serial Converter. +2. Connect UART 1 or 3 (other UARTS will not work) and GND to the USB/Serial converter (RX -> TX, TX -> RX) +3. Keep the boot/dfu button pressed +4. Switch on the FC / supply with power +5. Start STM32 CubeProgrammer and go to "Erasing & Programming", second option in the menu. +6. Select UART (blue dropdown field) and select the COM port of the USB/Serial adapter and press "Connect". The corresponding processor should now be displayed below. +7. Click on "Full flash erase". This is also necessary if you are flashing the same firmware version that was previously on the FC, otherwise MSP may not be activated on the UART. +8. Under "Download" load the previously created firmware (`INAV_X.X.X_[Your Target].hex`) or the standard firmware if UART is already activated there. The option "Verify programming" is optional but recommended. Make sure that "Skip flash erase while programming" is NOT activated. +9. Click "Start Programming" + +After the process is completed, switch the FC off and on again and then the Configurator can connect to the FC via USB/serial adapter and the previously configured UART. From 3d9d1f3b91aa255c80919d90d89dbfa755894a64 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 16 May 2024 07:43:56 +0100 Subject: [PATCH 050/112] Update docs --- docs/Betaflight 4.3 compatible OSD.md | 50 --------------------------- docs/DJI compatible OSD.md | 50 +++++++++++++++++++++++++++ 2 files changed, 50 insertions(+), 50 deletions(-) delete mode 100644 docs/Betaflight 4.3 compatible OSD.md create mode 100644 docs/DJI compatible OSD.md diff --git a/docs/Betaflight 4.3 compatible OSD.md b/docs/Betaflight 4.3 compatible OSD.md deleted file mode 100644 index 0e9644dae24..00000000000 --- a/docs/Betaflight 4.3 compatible OSD.md +++ /dev/null @@ -1,50 +0,0 @@ -# Betaflight 4.3 compatible MSP DisplayPort OSD (DJI O3 "Canvas Mode") - -INAV 6.0 includes a special mode for MSP DisplayPort that supports incomplete implementations of MSP DisplayPort that only support BetaFlight, like the DJI O3 Air Unit. INAV 6.1 expands this to include HD canvas sizes from BetaFlight 4.4. - -Different flight controllers have different OSD symbols and elements and require different fonts. BetaFlight's font is a single page and supports a maximum of 256 glyphs, INAV's font is currently 2 pages and supports up to 512 different glyphs. - -While there is some overlap between the glyphs in BetaFlight and INAV, it is not possible to perform a 1 to 1 mapping for all the them. In cases where there is no suitable glyph in the BetaFlight font, a question mark `?` will be displayed. - -This mode can be enabled by selecting BF43COMPAT or BFHDCOMPAT as video format in the OSD tab of the configurator or by typing the following command on the CLI: - -`set osd_video_system = BF43COMPAT` - -or - -`set osd_video_system = BFHDCOMPAT` - -## Limitations - -* Canvas size needs to be manually changed to HD on the Display menu in DJI's goggles (you may need a firmware update) and set as BFHDCOMPAT in the OSD tab of the configurator. -* Unsupported Glyphs show up as `?` - -## FAQ - -### I see a lot of `?` on my OSD. - -That is expected, when your INAV OSD widgets use glyphs that don't have a suitable mapping in BetaFlight's font. - -### Does it work with the G2 and Original Air Unit/Vista? - -Yes. - -### Is this a replacement for WTFOS? - -Not exactly. WTFOS is a full implementation of MSP-Displayport for rooted Air Unit/Vista/Googles V2 and actually works much better than BetaFlight compatibility mode, being able to display all INAV's glyphs. - -### Can INAV fix DJI's product? - -No. OSD renderinng happens on the googles/air unit side of things. Please ask DJI to fix their incomplete MSP DisplayPort implemenation. You can probably request it in [DJI's forum](https://forum.dji.com/forum.php?mod=forumdisplay&fid=129&filter=typeid&typeid=767). - -### BetaFlight X.Y now has more symbols, can you update INAV? - -Maybe. If a future version of BetaFlight includes more Glyphs that can be mapped into INAV it is fairly simple to add the mapping, but the problem with DJI's implemenation persists. Even if we update the mapping, if DJI does not update the fonts on their side the problem will persist. - -### Can you replace glyph `X` with text `x description`? - -While it might technically be possible to replace some glyphs with text in multiple cells, it will introduce a lot of complexity in the OSD rendering and configuration for something we hope is a temporary workaround. - -### Does DJI support Canvas Mode? - -Actually, no. What DJI calls Canvas Mode is actually MSP DisplayPort and is a character based OSD. diff --git a/docs/DJI compatible OSD.md b/docs/DJI compatible OSD.md new file mode 100644 index 00000000000..6af25670306 --- /dev/null +++ b/docs/DJI compatible OSD.md @@ -0,0 +1,50 @@ +# DJI compatible MSP DisplayPort OSD (DJI O3 "Canvas Mode") + +INAV 6.0 includes a special mode for MSP DisplayPort that supports DJI's incomplete implementations of MSP DisplayPort. This can be found on products like the DJI O3 Air Unit. INAV 6.1 expands this to include HD canvas sizes from BetaFlight 4.4. + +Different flight controller firmware have different OSD symbols and elements and require different fonts. BetaFlight's font is a single page and supports a maximum of 256 glyphs, INAV's font is currently 2 pages and supports up to 512 different glyphs. DJI's font is single page and based, but not the same as, BetaFlight's font. + +While there is some overlap between the glyphs in DJI and INAV, it is not possible to perform a 1 to 1 mapping for all the them. In cases where there is no suitable glyph in the DJI font, a question mark `?` will be displayed. + +This mode can be enabled by selecting DJI43COMPAT or DJIHDCOMPAT as video format in the OSD tab of the configurator or by typing the following command on the CLI: + +`set osd_video_system = DJI43COMPAT` + +or + +`set osd_video_system = DJIHDCOMPAT` + +## Limitations + +* Canvas size needs to be manually changed to HD on the Display menu in DJI's goggles (you may need a firmware update) and set as DJIHDCOMPAT in the OSD tab of the configurator. +* Unsupported Glyphs show up as `?` + +## FAQ + +### I see a lot of `?` on my OSD. + +That is expected. When your INAV OSD widgets use glyphs that don't have a suitable mapping in DJI's font. + +### Does it work with the G2 and Original Air Unit/Vista? + +Yes. + +### Is this a replacement for WTFOS? + +Not exactly. WTFOS is a full implementation of MSP-Displayport for rooted Air Unit/Vista/Googles V2 and actually works much better than DJI compatibility mode. It can use all of INAV's OSD elements as intended. If you have the option of WTFOS or DJI compatability mode. WTFOS is the best option. + +### Can INAV fix DJI's product? + +No. OSD renderinng happens on the googles/air unit side of things. Please ask DJI to fix their incomplete MSP DisplayPort implemenation. You can probably request it in [DJI's forum](https://forum.dji.com/forum.php?mod=forumdisplay&fid=129&filter=typeid&typeid=767). To see what you're missing out on with O3. Check out what WTFOS did with the original system. Not only could the pilots upload the fonts of their choosing (who doesn't want a cool SneakyFPV font on their OSD). But there were no problems supporting and firmware. Plus, there was even an option to save the OSD to a file and overlay that over your DVR video. If you're reading this far. Please recommend to DJI that they fix their product, to at least what was possible with WTFOS. + +### DJI's font now has more symbols, can you update INAV? + +Maybe. If a future version of DJI's font includes more Glyphs that can be mapped into INAV. It is fairly simple to add the mapping. However, the best solution would be full support of MSP DisplayPort by DJI. Then there will never be an issue with missing icons. As the latest INAV font would be able to be uploaded on to the goggles. + +### Can you replace glyph `X` with text `x description`? + +While it might technically be possible to replace some glyphs with text in multiple cells, it will introduce a lot of complexity in the OSD rendering and configuration for something we hope is a temporary workaround. + +### Does DJI support Canvas Mode? + +Actually, no. What DJI calls Canvas Mode is actually MSP DisplayPort and is a character based OSD. Currently, the only true implementaion of Canvas Mode is with FrSKY PixelOSD. This was found on some F722 flight controllers from Matek. From a818190ed12761ea7977f29f4d963c2a51310064 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 18 May 2024 13:49:04 +0200 Subject: [PATCH 051/112] Use letter `Q` for DJI LinkQuality OSD element --- src/main/io/displayport_msp_dji_compat.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c index de4fa8656fa..46e14c00470 100644 --- a/src/main/io/displayport_msp_dji_compat.c +++ b/src/main/io/displayport_msp_dji_compat.c @@ -41,10 +41,8 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_RSSI: return DJI_SYM_RSSI; -/* case SYM_LQ: - return DJI_SYM_LINK_QUALITY; -*/ + return 'Q'; case SYM_LAT: return DJI_SYM_LAT; From ec7ebd1c4b3c66155896c0615e3c0f11590b62d7 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 18 May 2024 14:15:59 +0200 Subject: [PATCH 052/112] Do not render DJI logo on OSD when using DJI Compat modes --- src/main/io/osd.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f3f00ae7bdb..62186908ff9 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4140,10 +4140,13 @@ uint8_t drawLogos(bool singular, uint8_t row) { uint8_t logoRow = row; uint8_t logoColOffset = 0; bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD()); + bool useINAVLogo = (singular && !usePilotLogo) || !singular; #ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is in use, the pilot logo cannot be used, due to font issues. - if (isDJICompatibleVideoSystem(osdConfig())) + if (isDJICompatibleVideoSystem(osdConfig())) { usePilotLogo = false; + useINAVLogo = false; + } #endif uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing; @@ -4160,7 +4163,7 @@ uint8_t drawLogos(bool singular, uint8_t row) { } // Draw INAV logo - if ((singular && !usePilotLogo) || !singular) { + if (useINAVLogo) { unsigned logo_c = SYM_LOGO_START; uint8_t logo_x = logoColOffset; for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { @@ -4191,7 +4194,7 @@ uint8_t drawLogos(bool singular, uint8_t row) { } return logoRow; - } +} #ifdef USE_STATS uint8_t drawStat_Stats(uint8_t statNameX, uint8_t row, uint8_t statValueX, bool isBootStats) From 3efb86c848123475d904f97ab6ee1ecb3716a0f3 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 18 May 2024 14:16:29 +0200 Subject: [PATCH 053/112] Do not render DJI logo on OSD when using DJI compat mode --- src/main/io/osd.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 62186908ff9..b2407b2325b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4181,9 +4181,9 @@ uint8_t drawLogos(bool singular, uint8_t row) { logoRow = row; if (singular) { logo_x = logoColOffset; - } else { - logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing; - } + } else { + logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing; + } for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) { for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) { @@ -4193,6 +4193,10 @@ uint8_t drawLogos(bool singular, uint8_t row) { } } + if (!usePilotLogo && !useINAVLogo) { + logoRow += SYM_LOGO_HEIGHT; + } + return logoRow; } From 3615e6fb0981d7e03125fe454e8acaff7b1f6a34 Mon Sep 17 00:00:00 2001 From: dzaro-dev <23706061+dzaro-dev@users.noreply.github.com> Date: Sat, 18 May 2024 16:58:22 +0100 Subject: [PATCH 054/112] Adding Crosshair Styles --- src/main/io/dji_osd_symbols.h | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h index 66972a810e7..3ef5a6c5e9b 100644 --- a/src/main/io/dji_osd_symbols.h +++ b/src/main/io/dji_osd_symbols.h @@ -71,6 +71,26 @@ #define DJI_SYM_AH_DECORATION 0x13 #define DJI_SYM_SMALL_CROSSHAIR 0x7E +// Crosshair Styles +#define DJI_SYM_AH_CH_TYPE3 0x00 +#define DJI_SYM_AH_CH_TYPE3_1 0x7E +#define DJI_SYM_AH_CH_TYPE3_2 0x00 +#define DJI_SYM_AH_CH_TYPE4 0x2D +#define DJI_SYM_AH_CH_TYPE4_1 0x7E +#define DJI_SYM_AH_CH_TYPE4_2 0x2D +#define DJI_SYM_AH_CH_TYPE5 0x17 +#define DJI_SYM_AH_CH_TYPE5_1 0x7E +#define DJI_SYM_AH_CH_TYPE5_2 0x17 +#define DJI_SYM_AH_CH_TYPE6 0x00 +#define DJI_SYM_AH_CH_TYPE6_1 0x09 +#define DJI_SYM_AH_CH_TYPE6_2 0x00 +#define DJI_SYM_AH_CH_TYPE7 0x78 +#define DJI_SYM_AH_CH_TYPE7_1 0x7E +#define DJI_SYM_AH_CH_TYPE7_2 0x77 +#define DJI_SYM_AH_CH_TYPE8 0x02 +#define DJI_SYM_AH_CH_TYPE8_1 0x7E +#define DJI_SYM_AH_CH_TYPE8_2 0x03 + // Satellite Graphics #define DJI_SYM_SAT_L 0x1E #define DJI_SYM_SAT_R 0x1F From cc4b3db899a995eae8514c63309d985338efa2d3 Mon Sep 17 00:00:00 2001 From: dzaro-dev <23706061+dzaro-dev@users.noreply.github.com> Date: Sat, 18 May 2024 17:02:59 +0100 Subject: [PATCH 055/112] Adding Crosshair Styles --- src/main/io/displayport_msp_dji_compat.c | 49 ++++++++++++++++++++---- 1 file changed, 42 insertions(+), 7 deletions(-) diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c index de4fa8656fa..93ec60b9ce8 100644 --- a/src/main/io/displayport_msp_dji_compat.c +++ b/src/main/io/displayport_msp_dji_compat.c @@ -469,40 +469,75 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_AH_RIGHT: return DJI_SYM_AH_RIGHT; - /* case SYM_AH_DECORATION_COUNT: return DJI_SYM_AH_DECORATION_COUNT; */ case SYM_AH_CH_LEFT: + case SYM_AH_CH_AIRCRAFT1: + return DJI_SYM_CROSSHAIR_LEFT; + case SYM_AH_CH_TYPE3: + return DJI_SYM_AH_CH_TYPE3; + case SYM_AH_CH_TYPE4: + return DJI_SYM_AH_CH_TYPE4; + case SYM_AH_CH_TYPE5: + return DJI_SYM_AH_CH_TYPE5; + case SYM_AH_CH_TYPE6: + return DJI_SYM_AH_CH_TYPE6; + case SYM_AH_CH_TYPE7: + return DJI_SYM_AH_CH_TYPE7; + case SYM_AH_CH_TYPE8: - case SYM_AH_CH_AIRCRAFT1: - return DJI_SYM_CROSSHAIR_LEFT; + return DJI_SYM_AH_CH_TYPE8; case SYM_AH_CH_CENTER: + case SYM_AH_CH_AIRCRAFT2: + return DJI_SYM_CROSSHAIR_CENTRE; + case (SYM_AH_CH_TYPE3+1): + return DJI_SYM_AH_CH_TYPE3_1; + case (SYM_AH_CH_TYPE4+1): + return DJI_SYM_AH_CH_TYPE4_1; + case (SYM_AH_CH_TYPE5+1): + return DJI_SYM_AH_CH_TYPE5_1; + case (SYM_AH_CH_TYPE6+1): + return DJI_SYM_AH_CH_TYPE6_1; + case (SYM_AH_CH_TYPE7+1): + return DJI_SYM_AH_CH_TYPE7_1; + case (SYM_AH_CH_TYPE8+1): - case SYM_AH_CH_AIRCRAFT2: - return DJI_SYM_CROSSHAIR_CENTRE; + return DJI_SYM_AH_CH_TYPE8_1; case SYM_AH_CH_RIGHT: + case SYM_AH_CH_AIRCRAFT3: + return DJI_SYM_CROSSHAIR_RIGHT; + case (SYM_AH_CH_TYPE3+2): + return DJI_SYM_AH_CH_TYPE3_2; + case (SYM_AH_CH_TYPE4+2): + return DJI_SYM_AH_CH_TYPE4_2; + case (SYM_AH_CH_TYPE5+2): + return DJI_SYM_AH_CH_TYPE5_2; + case (SYM_AH_CH_TYPE6+2): + return DJI_SYM_AH_CH_TYPE6_2; + case (SYM_AH_CH_TYPE7+2): + return DJI_SYM_AH_CH_TYPE7_2; + case (SYM_AH_CH_TYPE8+2): - case SYM_AH_CH_AIRCRAFT3: - return DJI_SYM_CROSSHAIR_RIGHT; + return DJI_SYM_AH_CH_TYPE8_2; case SYM_AH_CH_AIRCRAFT0: case SYM_AH_CH_AIRCRAFT4: From 5e85fe07b1d8ca589390c060f915c9c64ecb26ba Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 18 May 2024 20:31:04 +0100 Subject: [PATCH 056/112] Allow pilots the option to hide question marks from the OSD. --- docs/Settings.md | 10 ++++++ src/main/fc/settings.yaml | 41 +++--------------------- src/main/io/displayport_msp_dji_compat.c | 2 +- src/main/io/osd.c | 5 ++- src/main/io/osd.h | 5 ++- 5 files changed, 24 insertions(+), 39 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 30533330adf..29a745544e4 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -4412,6 +4412,16 @@ Value under which the OSD axis g force indicators will blink (g) --- +### osd_highlight_djis_missing_font_symbols + +Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes. + +| Default | Min | Max | +| --- | --- | --- | +| ON | OFF | ON | + +--- + ### osd_home_position_arm_screen Should home position coordinates be displayed on the arming screen. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index dfa7bb97d35..d276aca274d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3453,7 +3453,6 @@ groups: min: 8 max: 11 default_value: 9 - - name: osd_adsb_distance_warning description: "Distance in meters of ADSB aircraft that is displayed" default_value: 20000 @@ -3478,7 +3477,6 @@ groups: min: 0 max: 64000 type: uint16_t - - name: osd_estimations_wind_compensation description: "Use wind estimation for remaining flight time/distance estimation" default_value: ON @@ -3491,12 +3489,10 @@ groups: condition: USE_WIND_ESTIMATOR field: estimations_wind_mps type: bool - - name: osd_failsafe_switch_layout description: "If enabled the OSD automatically switches to the first layout during failsafe" default_value: OFF type: bool - - name: osd_plus_code_digits description: "Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm." field: plus_code_digits @@ -3508,213 +3504,186 @@ groups: field: plus_code_short default_value: "0" table: osd_plus_code_short - - name: osd_ahi_style description: "Sets OSD Artificial Horizon style \"DEFAULT\" or \"LINE\" for the FrSky Graphical OSD." field: ahi_style default_value: "DEFAULT" table: osd_ahi_style type: uint8_t - - name: osd_force_grid field: force_grid type: bool default_value: OFF description: Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) - - name: osd_ahi_bordered field: ahi_bordered type: bool description: Shows a border/corners around the AHI region (pixel OSD only) default_value: OFF - - name: osd_ahi_width field: ahi_width max: 255 description: AHI width in pixels (pixel OSD only) default_value: 132 - - name: osd_ahi_height field: ahi_height max: 255 description: AHI height in pixels (pixel OSD only) default_value: 162 - - name: osd_ahi_vertical_offset field: ahi_vertical_offset min: -128 max: 127 description: AHI vertical offset from center (pixel OSD only) default_value: -18 - - name: osd_sidebar_horizontal_offset field: sidebar_horizontal_offset min: -128 max: 127 default_value: 0 description: Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. - - name: osd_left_sidebar_scroll_step field: left_sidebar_scroll_step max: 255 default_value: 0 description: How many units each sidebar step represents. 0 means the default value for the scroll type. - - name: osd_right_sidebar_scroll_step field: right_sidebar_scroll_step max: 255 default_value: 0 description: Same as left_sidebar_scroll_step, but for the right sidebar - - name: osd_sidebar_height field: sidebar_height min: 0 max: 5 default_value: 3 description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) - - name: osd_ahi_pitch_interval field: ahi_pitch_interval min: 0 max: 30 default_value: 0 description: Draws AHI at increments of the set pitch interval over the full pitch range. AHI line is drawn with ends offset when pitch first exceeds interval with offset increasing with increasing pitch. Offset direction changes between climb and dive. Set to 0 to disable (Not for pixel OSD) - - name: osd_home_position_arm_screen type: bool default_value: ON description: Should home position coordinates be displayed on the arming screen. - - name: osd_pan_servo_index description: Index of the pan servo, used to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. field: pan_servo_index min: 0 max: 16 default_value: 0 - - name: osd_pan_servo_pwm2centideg description: Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. field: pan_servo_pwm2centideg default_value: 0 min: -36 max: 36 - - name: osd_pan_servo_offcentre_warning description: Degrees either side of the pan servo centre; where it is assumed camera is wanted to be facing forwards, but isn't at 0. If in this range and not 0 for longer than 10 seconds, the pan servo offset OSD element will blink. 0 means the warning is disabled. field: pan_servo_offcentre_warning min: 0 max: 45 default_value: 10 - - name: osd_pan_servo_indicator_show_degrees description: Show the degress of offset from centre on the pan servo OSD display element. field: pan_servo_indicator_show_degrees type: bool default_value: OFF - - name: osd_esc_rpm_precision description: Number of characters used to display the RPM value. field: esc_rpm_precision min: 3 max: 6 default_value: 3 - - name: osd_mah_precision description: Number of digits used for mAh precision. Currently used by mAh Used and Battery Remaining Capacity field: mAh_precision min: 4 max: 6 default_value: 4 - - name: osd_use_pilot_logo description: Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511 field: use_pilot_logo type: bool default_value: OFF - - name: osd_inav_to_pilot_logo_spacing description: The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen. field: inav_to_pilot_logo_spacing min: 0 max: 20 default_value: 8 - - name: osd_arm_screen_display_time description: Amount of time to display the arm screen [ms] field: arm_screen_display_time min: 1000 max: 5000 default_value: 1500 - - name: osd_switch_indicator_zero_name description: "Character to use for OSD switch incicator 0." field: osd_switch_indicator0_name type: string max: 5 default_value: "FLAP" - - name: osd_switch_indicator_one_name description: "Character to use for OSD switch incicator 1." field: osd_switch_indicator1_name type: string max: 5 default_value: "GEAR" - - name: osd_switch_indicator_two_name description: "Character to use for OSD switch incicator 2." field: osd_switch_indicator2_name type: string max: 5 default_value: "CAM" - - name: osd_switch_indicator_three_name description: "Character to use for OSD switch incicator 3." field: osd_switch_indicator3_name type: string max: 5 default_value: "LIGT" - - name: osd_switch_indicator_zero_channel description: "RC Channel to use for OSD switch indicator 0." field: osd_switch_indicator0_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 - - name: osd_switch_indicator_one_channel description: "RC Channel to use for OSD switch indicator 1." field: osd_switch_indicator1_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 - - name: osd_switch_indicator_two_channel description: "RC Channel to use for OSD switch indicator 2." field: osd_switch_indicator2_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 - - name: osd_switch_indicator_three_channel description: "RC Channel to use for OSD switch indicator 3." field: osd_switch_indicator3_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 - - name: osd_switch_indicators_align_left description: "Align text to left of switch indicators" field: osd_switch_indicators_align_left type: bool default_value: ON - - name: osd_system_msg_display_time description: System message display cycle time for multiple messages (milliseconds). field: system_msg_display_time default_value: 1000 min: 500 max: 5000 - + - name: osd_highlight_djis_missing_font_symbols + description: Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes. + field: highlight_djis_fuckup + default_value: ON + type: bool - name: PG_OSD_COMMON_CONFIG type: osdCommonConfig_t headers: ["io/osd_common.h"] diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c index de4fa8656fa..e6c2d0b3c03 100644 --- a/src/main/io/displayport_msp_dji_compat.c +++ b/src/main/io/displayport_msp_dji_compat.c @@ -705,7 +705,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) break; } - return '?'; // Missing/not mapped character + return (osdConfig()->highlight_djis_fuckup) ? '?' : SYM_BLANK; // Missing/not mapped character } #endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f3f00ae7bdb..881a8b4e767 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -223,7 +223,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 10); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 11); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1); void osdStartedSaveProcess(void) { @@ -3874,6 +3874,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .airspeed_alarm_min = SETTING_OSD_AIRSPEED_ALARM_MIN_DEFAULT, .airspeed_alarm_max = SETTING_OSD_AIRSPEED_ALARM_MAX_DEFAULT, #endif +#ifndef DISABLE_MSP_DJI_COMPAT + .highlight_djis_fuckup = SETTING_OSD_HIGHLIGHT_DJIS_MISSING_FONT_SYMBOLS_DEFAULT, +#endif .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT, .row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index ca5dca66137..ed60e26e4b8 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -457,7 +457,10 @@ typedef struct osdConfig_s { bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo. uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. uint16_t arm_screen_display_time; // Length of time the arm screen is displayed - #ifdef USE_ADSB +#ifndef DISABLE_MSP_DJI_COMPAT + bool highlight_djis_fuckup; // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol +#endif +#ifdef USE_ADSB uint16_t adsb_distance_warning; // in metres uint16_t adsb_distance_alert; // in metres uint16_t adsb_ignore_plane_above_me_limit; // in metres From 135406decf4e63ef89fc4afc47885a54b7b5949d Mon Sep 17 00:00:00 2001 From: dzaro-dev <23706061+dzaro-dev@users.noreply.github.com> Date: Sun, 19 May 2024 00:44:12 +0100 Subject: [PATCH 057/112] Update dji_osd_symbols.h as per request --- src/main/io/dji_osd_symbols.h | 40 +++++++++++++++++------------------ 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h index 3ef5a6c5e9b..2489bb176d9 100644 --- a/src/main/io/dji_osd_symbols.h +++ b/src/main/io/dji_osd_symbols.h @@ -71,26 +71,6 @@ #define DJI_SYM_AH_DECORATION 0x13 #define DJI_SYM_SMALL_CROSSHAIR 0x7E -// Crosshair Styles -#define DJI_SYM_AH_CH_TYPE3 0x00 -#define DJI_SYM_AH_CH_TYPE3_1 0x7E -#define DJI_SYM_AH_CH_TYPE3_2 0x00 -#define DJI_SYM_AH_CH_TYPE4 0x2D -#define DJI_SYM_AH_CH_TYPE4_1 0x7E -#define DJI_SYM_AH_CH_TYPE4_2 0x2D -#define DJI_SYM_AH_CH_TYPE5 0x17 -#define DJI_SYM_AH_CH_TYPE5_1 0x7E -#define DJI_SYM_AH_CH_TYPE5_2 0x17 -#define DJI_SYM_AH_CH_TYPE6 0x00 -#define DJI_SYM_AH_CH_TYPE6_1 0x09 -#define DJI_SYM_AH_CH_TYPE6_2 0x00 -#define DJI_SYM_AH_CH_TYPE7 0x78 -#define DJI_SYM_AH_CH_TYPE7_1 0x7E -#define DJI_SYM_AH_CH_TYPE7_2 0x77 -#define DJI_SYM_AH_CH_TYPE8 0x02 -#define DJI_SYM_AH_CH_TYPE8_1 0x7E -#define DJI_SYM_AH_CH_TYPE8_2 0x03 - // Satellite Graphics #define DJI_SYM_SAT_L 0x1E #define DJI_SYM_SAT_R 0x1F @@ -182,3 +162,23 @@ #define DJI_SYM_GPS_DEGREE DJI_SYM_STICK_OVERLAY_SPRITE_HIGH // kind of looks like the degree symbol #define DJI_SYM_GPS_MINUTE 0x27 // ' #define DJI_SYM_GPS_SECOND 0x22 // " + +// Crosshair Styles +#define DJI_SYM_AH_CH_TYPE3 DJI_SYM_NONE +#define DJI_SYM_AH_CH_TYPE3_1 DJI_SYM_SMALL_CROSSHAIR +#define DJI_SYM_AH_CH_TYPE3_2 DJI_SYM_NONE +#define DJI_SYM_AH_CH_TYPE4 DJI_SYM_HYPHEN +#define DJI_SYM_AH_CH_TYPE4_1 DJI_SYM_SMALL_CROSSHAIR +#define DJI_SYM_AH_CH_TYPE4_2 DJI_SYM_HYPHEN +#define DJI_SYM_AH_CH_TYPE5 DJI_SYM_STICK_OVERLAY_HORIZONTAL +#define DJI_SYM_AH_CH_TYPE5_1 DJI_SYM_SMALL_CROSSHAIR +#define DJI_SYM_AH_CH_TYPE5_2 DJI_SYM_STICK_OVERLAY_HORIZONTAL +#define DJI_SYM_AH_CH_TYPE6 DJI_SYM_NONE +#define DJI_SYM_AH_CH_TYPE6_1 DJI_SYM_STICK_OVERLAY_SPRITE_MID +#define DJI_SYM_AH_CH_TYPE6_2 DJI_SYM_NONE +#define DJI_SYM_AH_CH_TYPE7 DJI_SYM_ARROW_SMALL_LEFT +#define DJI_SYM_AH_CH_TYPE7_1 DJI_SYM_SMALL_CROSSHAIR +#define DJI_SYM_AH_CH_TYPE7_2 DJI_SYM_ARROW_SMALL_RIGHT +#define DJI_SYM_AH_CH_TYPE8 DJI_SYM_AH_LEFT +#define DJI_SYM_AH_CH_TYPE8_1 DJI_SYM_SMALL_CROSSHAIR +#define DJI_SYM_AH_CH_TYPE8_2 DJI_SYM_AH_RIGHT From 655b325fcef1f550046a1cb4f643f62fbe48aeb3 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 19 May 2024 08:33:14 +0100 Subject: [PATCH 058/112] Tidy up DJI crosshairs Re-ordered to keep the same crosshairs together. --- src/main/io/displayport_msp_dji_compat.c | 82 ++++++++++-------------- src/main/io/dji_osd_symbols.h | 20 ------ 2 files changed, 34 insertions(+), 68 deletions(-) diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c index 93ec60b9ce8..ba6f7399c66 100644 --- a/src/main/io/displayport_msp_dji_compat.c +++ b/src/main/io/displayport_msp_dji_compat.c @@ -476,72 +476,58 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) case SYM_AH_CH_LEFT: case SYM_AH_CH_AIRCRAFT1: return DJI_SYM_CROSSHAIR_LEFT; - - case SYM_AH_CH_TYPE3: - return DJI_SYM_AH_CH_TYPE3; - - case SYM_AH_CH_TYPE4: - return DJI_SYM_AH_CH_TYPE4; - - case SYM_AH_CH_TYPE5: - return DJI_SYM_AH_CH_TYPE5; - - case SYM_AH_CH_TYPE6: - return DJI_SYM_AH_CH_TYPE6; - - case SYM_AH_CH_TYPE7: - return DJI_SYM_AH_CH_TYPE7; - - case SYM_AH_CH_TYPE8: - return DJI_SYM_AH_CH_TYPE8; - case SYM_AH_CH_CENTER: case SYM_AH_CH_AIRCRAFT2: return DJI_SYM_CROSSHAIR_CENTRE; - - case (SYM_AH_CH_TYPE3+1): - return DJI_SYM_AH_CH_TYPE3_1; - - case (SYM_AH_CH_TYPE4+1): - return DJI_SYM_AH_CH_TYPE4_1; - - case (SYM_AH_CH_TYPE5+1): - return DJI_SYM_AH_CH_TYPE5_1; - - case (SYM_AH_CH_TYPE6+1): - return DJI_SYM_AH_CH_TYPE6_1; - - case (SYM_AH_CH_TYPE7+1): - return DJI_SYM_AH_CH_TYPE7_1; - - case (SYM_AH_CH_TYPE8+1): - return DJI_SYM_AH_CH_TYPE8_1; - case SYM_AH_CH_RIGHT: case SYM_AH_CH_AIRCRAFT3: return DJI_SYM_CROSSHAIR_RIGHT; + + case SYM_AH_CH_AIRCRAFT0: + case SYM_AH_CH_AIRCRAFT4: + return DJI_SYM_BLANK; + case SYM_AH_CH_TYPE3: + return DJI_SYM_NONE; + case (SYM_AH_CH_TYPE3+1): + return DJI_SYM_SMALL_CROSSHAIR; case (SYM_AH_CH_TYPE3+2): - return DJI_SYM_AH_CH_TYPE3_2; + return DJI_SYM_NONE; + case SYM_AH_CH_TYPE4: + return DJI_SYM_HYPHEN; + case (SYM_AH_CH_TYPE4+1): + return DJI_SYM_SMALL_CROSSHAIR; case (SYM_AH_CH_TYPE4+2): - return DJI_SYM_AH_CH_TYPE4_2; + return DJI_SYM_HYPHEN; + case SYM_AH_CH_TYPE5: + return DJI_SYM_STICK_OVERLAY_HORIZONTAL; + case (SYM_AH_CH_TYPE5+1): + return DJI_SYM_SMALL_CROSSHAIR; case (SYM_AH_CH_TYPE5+2): - return DJI_SYM_AH_CH_TYPE5_2; + return DJI_SYM_STICK_OVERLAY_HORIZONTAL; + case SYM_AH_CH_TYPE6: + return DJI_SYM_NONE; + case (SYM_AH_CH_TYPE6+1): + return DJI_SYM_STICK_OVERLAY_SPRITE_MID; case (SYM_AH_CH_TYPE6+2): - return DJI_SYM_AH_CH_TYPE6_2; + return DJI_SYM_NONE; + case SYM_AH_CH_TYPE7: + return DJI_SYM_ARROW_SMALL_LEFT; + case (SYM_AH_CH_TYPE7+1): + return DJI_SYM_SMALL_CROSSHAIR; case (SYM_AH_CH_TYPE7+2): - return DJI_SYM_AH_CH_TYPE7_2; + return DJI_SYM_ARROW_SMALL_RIGHT; + case SYM_AH_CH_TYPE8: + return DJI_SYM_AH_LEFT; + case (SYM_AH_CH_TYPE8+1): + return DJI_SYM_SMALL_CROSSHAIR; case (SYM_AH_CH_TYPE8+2): - return DJI_SYM_AH_CH_TYPE8_2; - - case SYM_AH_CH_AIRCRAFT0: - case SYM_AH_CH_AIRCRAFT4: - return DJI_SYM_BLANK; + return DJI_SYM_AH_RIGHT; case SYM_ARROW_UP: return DJI_SYM_ARROW_NORTH; diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h index 2489bb176d9..66972a810e7 100644 --- a/src/main/io/dji_osd_symbols.h +++ b/src/main/io/dji_osd_symbols.h @@ -162,23 +162,3 @@ #define DJI_SYM_GPS_DEGREE DJI_SYM_STICK_OVERLAY_SPRITE_HIGH // kind of looks like the degree symbol #define DJI_SYM_GPS_MINUTE 0x27 // ' #define DJI_SYM_GPS_SECOND 0x22 // " - -// Crosshair Styles -#define DJI_SYM_AH_CH_TYPE3 DJI_SYM_NONE -#define DJI_SYM_AH_CH_TYPE3_1 DJI_SYM_SMALL_CROSSHAIR -#define DJI_SYM_AH_CH_TYPE3_2 DJI_SYM_NONE -#define DJI_SYM_AH_CH_TYPE4 DJI_SYM_HYPHEN -#define DJI_SYM_AH_CH_TYPE4_1 DJI_SYM_SMALL_CROSSHAIR -#define DJI_SYM_AH_CH_TYPE4_2 DJI_SYM_HYPHEN -#define DJI_SYM_AH_CH_TYPE5 DJI_SYM_STICK_OVERLAY_HORIZONTAL -#define DJI_SYM_AH_CH_TYPE5_1 DJI_SYM_SMALL_CROSSHAIR -#define DJI_SYM_AH_CH_TYPE5_2 DJI_SYM_STICK_OVERLAY_HORIZONTAL -#define DJI_SYM_AH_CH_TYPE6 DJI_SYM_NONE -#define DJI_SYM_AH_CH_TYPE6_1 DJI_SYM_STICK_OVERLAY_SPRITE_MID -#define DJI_SYM_AH_CH_TYPE6_2 DJI_SYM_NONE -#define DJI_SYM_AH_CH_TYPE7 DJI_SYM_ARROW_SMALL_LEFT -#define DJI_SYM_AH_CH_TYPE7_1 DJI_SYM_SMALL_CROSSHAIR -#define DJI_SYM_AH_CH_TYPE7_2 DJI_SYM_ARROW_SMALL_RIGHT -#define DJI_SYM_AH_CH_TYPE8 DJI_SYM_AH_LEFT -#define DJI_SYM_AH_CH_TYPE8_1 DJI_SYM_SMALL_CROSSHAIR -#define DJI_SYM_AH_CH_TYPE8_2 DJI_SYM_AH_RIGHT From 508b3ddc904b9c1ddf9194fd4d51e4eaf3b063e9 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 19 May 2024 09:07:16 +0100 Subject: [PATCH 059/112] Remove unused define from DJI OSD symbols I searched the code and DJI_SYM_CURSOR isn't used anywhere. The only OSD use of cursor at all is in the CMS, which just uses the '>' character. So this define can go. After that the dji_osd_symbols.h file is a pure character to variable map. --- src/main/io/dji_osd_symbols.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/io/dji_osd_symbols.h b/src/main/io/dji_osd_symbols.h index 2489bb176d9..35a034c7f54 100644 --- a/src/main/io/dji_osd_symbols.h +++ b/src/main/io/dji_osd_symbols.h @@ -147,9 +147,6 @@ #define DJI_SYM_MPS 0x9F #define DJI_SYM_FTPS 0x99 -// Menu cursor -#define DJI_SYM_CURSOR DJI_SYM_AH_LEFT - // Stick overlays #define DJI_SYM_STICK_OVERLAY_SPRITE_HIGH 0x08 #define DJI_SYM_STICK_OVERLAY_SPRITE_MID 0x09 From 536c5c3958f8623d02cf3c46ae9956b005a381b6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 19 May 2024 20:24:18 +0200 Subject: [PATCH 060/112] Make Iterm Lock configurable --- src/main/fc/settings.yaml | 12 ++++++++++++ src/main/flight/pid.c | 9 ++++++--- src/main/flight/pid.h | 5 +++++ 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 307f6d9cb53..3abe94e12c9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2227,6 +2227,18 @@ groups: field: fixedWingLevelTrimGain min: 0 max: 20 + - name: fw_iterm_lock_time_max_ms + description: Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release + default_value: 500 + field: fwItermLockTimeMaxMs + min: 100 + max: 1000 + - name: fw_iterm_lock_rate_threshold + description: Defines rate percentage when full P I and D attenuation should happen. 100 disables Iterm Lock for P and D term + field: fwItermLockRateLimit + default_value: 40 + min: 10 + max: 100 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 479d8e8d346..711b3a0af07 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -178,7 +178,7 @@ static EXTENDED_FASTRAM bool angleHoldIsLevel = false; static EXTENDED_FASTRAM float fixedWingLevelTrim; static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 8); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 9); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -312,6 +312,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT, .smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT, #endif + .fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT, + .fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT, ); bool pidInitFilters(void) @@ -748,7 +750,8 @@ static void nullRateController(pidState_t *pidState, float dT, float dT_inv) { static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, const float rateError) { const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f; - const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f); + + const float dampingFactor = attenuation(rateTarget, maxRate * pidProfile()->fwItermLockRateLimit / 100.0f); /* * Iterm damping is applied (down to 0) when: @@ -770,7 +773,7 @@ static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, cons pidState->attenuation.targetOverThresholdTimeMs = 0; } - pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f); + pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < pidProfile()->fwItermLockTimeMaxMs) ? 0.0f : 1.0f); //P & D damping factors are always the same and based on current damping factor pidState->attenuation.aP = dampingFactor; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c8df7231ed6..93e713b61a9 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -155,6 +155,11 @@ typedef struct pidProfile_s { float smithPredictorDelay; uint16_t smithPredictorFilterHz; #endif + + + uint16_t fwItermLockTimeMaxMs; + uint8_t fwItermLockRateLimit; + } pidProfile_t; typedef struct pidAutotuneConfig_s { From 2118cebf255d2cce359863636d68f19ad6538c0e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 19 May 2024 20:24:41 +0200 Subject: [PATCH 061/112] Iterm Lock configurability --- src/main/fc/settings.yaml | 6 ++++++ src/main/flight/pid.c | 3 ++- src/main/flight/pid.h | 1 + 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3abe94e12c9..a2d2195bde0 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2239,6 +2239,12 @@ groups: default_value: 40 min: 10 max: 100 + - name: fw_iterm_lock_engage_threshold + description: Defines error rate (in percents of max rate) when Iterm Lock is engaged when sticks are release. Iterm Lock will stay active until error drops below this number + default_value: 10 + min: 5 + max: 25 + field: fwItermLockEngageThreshold - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 711b3a0af07..a9d1291877f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -314,6 +314,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, #endif .fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT, .fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT, + .fwItermLockEngageThreshold = SETTING_FW_ITERM_LOCK_ENGAGE_THRESHOLD_DEFAULT, ); bool pidInitFilters(void) @@ -761,7 +762,7 @@ static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, cons */ //If error is greater than 10% or max rate - const bool errorThresholdReached = fabsf(rateError) > maxRate * 0.1f; + const bool errorThresholdReached = fabsf(rateError) > maxRate * pidProfile()->fwItermLockEngageThreshold / 100.0f; //If stick (setpoint) was moved above threshold in the last 500ms if (fabsf(rateTarget) > maxRate * 0.2f) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 93e713b61a9..26aeb86990d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -159,6 +159,7 @@ typedef struct pidProfile_s { uint16_t fwItermLockTimeMaxMs; uint8_t fwItermLockRateLimit; + uint8_t fwItermLockEngageThreshold; } pidProfile_t; From fc9457f4708b616594de728681ae072bc03bcda6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 19 May 2024 20:25:52 +0200 Subject: [PATCH 062/112] Docs update --- docs/Settings.md | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index 9f3baaed662..96141fa65d2 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1302,6 +1302,36 @@ Fixed-wing rate stabilisation I-gain for YAW --- +### fw_iterm_lock_engage_threshold + +Defines error rate (in percents of max rate) when Iterm Lock is engaged when sticks are release. Iterm Lock will stay active until error drops below this number + +| Default | Min | Max | +| --- | --- | --- | +| 10 | 5 | 25 | + +--- + +### fw_iterm_lock_rate_threshold + +Defines rate percentage when full P I and D attenuation should happen. 100 disables Iterm Lock for P and D term + +| Default | Min | Max | +| --- | --- | --- | +| 40 | 10 | 100 | + +--- + +### fw_iterm_lock_time_max_ms + +Defines max time in milliseconds for how long ITerm Lock will shut down Iterm after sticks are release + +| Default | Min | Max | +| --- | --- | --- | +| 500 | 100 | 1000 | + +--- + ### fw_level_pitch_gain I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations From 5a2b1ee9a7a21090c2586fe9d8b8088ee98760a4 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 21 May 2024 11:39:23 +0100 Subject: [PATCH 063/112] move activewp and rxupdaterate fields --- src/main/blackbox/blackbox.c | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 727488cc1b1..4433e7b4aba 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -402,13 +402,15 @@ static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = { // Rarely-updated fields static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { + {"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"flightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"stateFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"failsafePhase", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}, {"rxSignalReceived", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}, {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}, - + {"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)}, + {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, @@ -436,8 +438,6 @@ static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { {"escRPM", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"escTemperature", -1, SIGNED, PREDICT(PREVIOUS), ENCODING(SIGNED_VB)}, #endif - {"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)}, - {"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, }; typedef enum BlackboxState { @@ -1259,7 +1259,8 @@ static void writeSlowFrame(void) int32_t values[3]; blackboxWrite('S'); - + + blackboxWriteUnsignedVB(slowHistory.activeWpNumber); blackboxWriteUnsignedVB(slowHistory.flightModeFlags); blackboxWriteUnsignedVB(slowHistory.stateFlags); @@ -1268,9 +1269,11 @@ static void writeSlowFrame(void) */ values[0] = slowHistory.failsafePhase; values[1] = slowHistory.rxSignalReceived ? 1 : 0; - values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0; + values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0; blackboxWriteTag2_3S32(values); + blackboxWriteUnsignedVB(slowHistory.rxUpdateRate); + blackboxWriteUnsignedVB(slowHistory.hwHealthStatus); blackboxWriteUnsignedVB(slowHistory.powerSupplyImpedance); @@ -1296,8 +1299,6 @@ static void writeSlowFrame(void) blackboxWriteUnsignedVB(slowHistory.escRPM); blackboxWriteSignedVB(slowHistory.escTemperature); #endif - blackboxWriteUnsignedVB(slowHistory.rxUpdateRate); - blackboxWriteUnsignedVB(slowHistory.activeWpNumber); blackboxSlowFrameIterationTimer = 0; } @@ -1307,6 +1308,7 @@ static void writeSlowFrame(void) */ static void loadSlowState(blackboxSlowState_t *slow) { + slow->activeWpNumber = getActiveWpNumber(); memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags; // Also log Nav auto enabled flight modes rather than just those selected by boxmode if (FLIGHT_MODE(ANGLE_MODE)) { @@ -1328,6 +1330,7 @@ static void loadSlowState(blackboxSlowState_t *slow) slow->failsafePhase = failsafePhase(); slow->rxSignalReceived = rxIsReceivingSignal(); slow->rxFlightChannelsValid = rxAreFlightChannelsValid(); + slow->rxUpdateRate = getRcUpdateFrequency(); slow->hwHealthStatus = (getHwGyroStatus() << 2 * 0) | // Pack hardware health status into a bit field. (getHwAccelerometerStatus() << 2 * 1) | // Use raw hardwareSensorStatus_e values and pack them using 2 bits per value (getHwCompassStatus() << 2 * 2) | // Report GYRO in 2 lowest bits, then ACC, COMPASS, BARO, GPS, RANGEFINDER and PITOT @@ -1379,9 +1382,6 @@ static void loadSlowState(blackboxSlowState_t *slow) slow->escRPM = escSensor->rpm; slow->escTemperature = escSensor->temperature; #endif - - slow->rxUpdateRate = getRcUpdateFrequency(); - slow->activeWpNumber = getActiveWpNumber(); } /** From 946241d995ca8b7c8c762330a31b9d2ed9f1d1be Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 21 May 2024 21:41:30 +0100 Subject: [PATCH 064/112] Made it more boring --- src/main/fc/settings.yaml | 2 +- src/main/io/displayport_msp_dji_compat.c | 2 +- src/main/io/osd.c | 2 +- src/main/io/osd.h | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d276aca274d..fa21316463e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3681,7 +3681,7 @@ groups: max: 5000 - name: osd_highlight_djis_missing_font_symbols description: Show question marks where there is no symbol in the DJI font to represent the INAV OSD element's symbol. When off, blank spaces will be used. Only relevent for DJICOMPAT modes. - field: highlight_djis_fuckup + field: highlight_djis_missing_characters default_value: ON type: bool - name: PG_OSD_COMMON_CONFIG diff --git a/src/main/io/displayport_msp_dji_compat.c b/src/main/io/displayport_msp_dji_compat.c index e6c2d0b3c03..cf57b49be67 100644 --- a/src/main/io/displayport_msp_dji_compat.c +++ b/src/main/io/displayport_msp_dji_compat.c @@ -705,7 +705,7 @@ uint8_t getDJICharacter(uint8_t ch, uint8_t page) break; } - return (osdConfig()->highlight_djis_fuckup) ? '?' : SYM_BLANK; // Missing/not mapped character + return (osdConfig()->highlight_djis_missing_characters) ? '?' : SYM_BLANK; // Missing/not mapped character } #endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 881a8b4e767..34135ce2fd7 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3875,7 +3875,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .airspeed_alarm_max = SETTING_OSD_AIRSPEED_ALARM_MAX_DEFAULT, #endif #ifndef DISABLE_MSP_DJI_COMPAT - .highlight_djis_fuckup = SETTING_OSD_HIGHLIGHT_DJIS_MISSING_FONT_SYMBOLS_DEFAULT, + .highlight_djis_missing_characters = SETTING_OSD_HIGHLIGHT_DJIS_MISSING_FONT_SYMBOLS_DEFAULT, #endif .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT, diff --git a/src/main/io/osd.h b/src/main/io/osd.h index ed60e26e4b8..7ac14054994 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -458,7 +458,7 @@ typedef struct osdConfig_s { uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width. uint16_t arm_screen_display_time; // Length of time the arm screen is displayed #ifndef DISABLE_MSP_DJI_COMPAT - bool highlight_djis_fuckup; // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol + bool highlight_djis_missing_characters; // If enabled, show question marks where there is no character in DJI's font to represent an OSD element symbol #endif #ifdef USE_ADSB uint16_t adsb_distance_warning; // in metres From b4dfc9302a84b31f03e79ed179c8754aab99fb76 Mon Sep 17 00:00:00 2001 From: HishamGhosheh Date: Thu, 23 May 2024 01:57:25 +0200 Subject: [PATCH 065/112] Add JHEF405PRO (aka GHF405AIO-HD) support --- src/main/target/JHEF405PRO/CMakeLists.txt | 1 + src/main/target/JHEF405PRO/target.c | 34 +++++ src/main/target/JHEF405PRO/target.h | 154 ++++++++++++++++++++++ 3 files changed, 189 insertions(+) create mode 100644 src/main/target/JHEF405PRO/CMakeLists.txt create mode 100644 src/main/target/JHEF405PRO/target.c create mode 100644 src/main/target/JHEF405PRO/target.h diff --git a/src/main/target/JHEF405PRO/CMakeLists.txt b/src/main/target/JHEF405PRO/CMakeLists.txt new file mode 100644 index 00000000000..eb7f0108832 --- /dev/null +++ b/src/main/target/JHEF405PRO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(JHEF405PRO) diff --git a/src/main/target/JHEF405PRO/target.c b/src/main/target/JHEF405PRO/target.c new file mode 100644 index 00000000000..23c1604c0ca --- /dev/null +++ b/src/main/target/JHEF405PRO/target.c @@ -0,0 +1,34 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S2 + DEF_TIM(TIM2, CH4, PA3, TIM_USE_MOTOR, 0, 1), // S3 + DEF_TIM(TIM2, CH3, PA2, TIM_USE_MOTOR, 0, 0), // S4 + + DEF_TIM(TIM4, CH3, PB8, TIM_USE_PPM, 0, 0), // PPM + DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 1 ), //LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/JHEF405PRO/target.h b/src/main/target/JHEF405PRO/target.h new file mode 100644 index 00000000000..43547cd2efa --- /dev/null +++ b/src/main/target/JHEF405PRO/target.h @@ -0,0 +1,154 @@ +/* + * This is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "JHEF405PRO" +#define USBD_PRODUCT_STRING "JHEF405PRO" + +#define LED0 PC14 +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** SPI1 Gyro & ACC ********************** +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW90_DEG +#define ICM42605_CS_PIN PB12 +#define ICM42605_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PB13 +#define USE_MPU_DATA_READY_SIGNAL + +// *************** SPI3 OSD & Flash *************************** +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +// OSD +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PB14 + +// Flash +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PB3 +#define M25P16_SPI_BUS BUS_SPI3 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 + +// Baro +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_DPS310 + +// Mag +#define USE_MAG +#define USE_MAG_ALL +#define MAG_I2C_BUS DEFAULT_I2C_BUS + +// Temperature +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS + +// Range finder +#define USE_RANGEFINDER +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS + +// PITOT +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +// *************** UART ***************************** +#define USB_IO +#define USE_VCP +#define VBUS_SENSING_PIN PA8 // TODO validate +#define VBUS_SENSING_ENABLED + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PB6 + +#define USE_UART2 +#define UART2_RX_PIN PD6 +#define UART2_TX_PIN PD5 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 6 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC3 +#define ADC_CHANNEL_2_PIN PC2 +#define ADC_CHANNEL_3_PIN PC0 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define CURRENT_METER_SCALE 250 + +// *************** OTHERS ************************* +#define DEFAULT_FEATURES (FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_TELEMETRY) + +// Rx defaults +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF +#define SERIALRX_UART SERIAL_PORT_USART2 + + +// *************** LED2812 ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA9 + + + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +//TIMER +#define MAX_PWM_OUTPUT_PORTS 4 + +#define USE_DSHOT From b7a24063af29560cc5a95cfdbbdded5ea3d638f4 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Thu, 23 May 2024 09:43:41 +0200 Subject: [PATCH 066/112] Add PSA about ICM426xx changes in INAV 7.1 Some users have reported issues with INAV 7.1+ when upgrading from previews versions in aircraft that had this gyro. Recalibrating the gyro, or redoing the setup in INAV 7.1 seems to have fixed most of them. --- readme.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/readme.md b/readme.md index a70c42d4c86..a83ba6c6cfd 100644 --- a/readme.md +++ b/readme.md @@ -6,6 +6,10 @@ > INAV 7 is the last INAV official release available for F411 based flight controllers. The next milestone, INAV 8 will not be available for F411 boards. +# ICM426xx IMUs PSA + +> The filtering settings for the ICM426xx has changed to matche what is used by Ardupilot and Betaflight in INAV 7.1. When upgrading from older versions you may need to recalibrate the Accelerometer and if you are not using INAV's default tune you may also want to check if the tune is still good. + ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png) # PosHold, Navigation and RTH without compass PSA From e37ed9f270ba1729463e4d01f29923584d70c9fd Mon Sep 17 00:00:00 2001 From: HishamGhosheh Date: Thu, 23 May 2024 10:27:55 +0200 Subject: [PATCH 067/112] Add SKIP_RELEASES to JHEF405PRO --- src/main/target/JHEF405PRO/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/JHEF405PRO/CMakeLists.txt b/src/main/target/JHEF405PRO/CMakeLists.txt index eb7f0108832..939221cf744 100644 --- a/src/main/target/JHEF405PRO/CMakeLists.txt +++ b/src/main/target/JHEF405PRO/CMakeLists.txt @@ -1 +1 @@ -target_stm32f405xg(JHEF405PRO) +target_stm32f405xg(JHEF405PRO SKIP_RELEASES) From 39bd781552867e2e22a176cac222498270bfed2d Mon Sep 17 00:00:00 2001 From: Scavanger Date: Thu, 23 May 2024 10:51:41 -0300 Subject: [PATCH 068/112] USB-Rescue --- cmake/at32.cmake | 5 ++++ cmake/sitl.cmake | 4 +++ cmake/stm32.cmake | 5 ++++ docs/Broken USB recovery.md | 51 +++++-------------------------------- src/main/fc/config.c | 8 ++++++ 5 files changed, 28 insertions(+), 45 deletions(-) diff --git a/cmake/at32.cmake b/cmake/at32.cmake index 54e178deb7b..6ac70bc528e 100644 --- a/cmake/at32.cmake +++ b/cmake/at32.cmake @@ -326,6 +326,11 @@ function(target_at32) math(EXPR hse_value "${hse_mhz} * 1000000") list(APPEND target_definitions "HSE_VALUE=${hse_value}") + + if (MSP_UART) + list(APPEND target_definitions "MSP_UART=${MSP_UART}") + endif() + if(args_COMPILE_DEFINITIONS) list(APPEND target_definitions ${args_COMPILE_DEFINITIONS}) endif() diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index ee43aa9a93a..39e6456830a 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -99,6 +99,10 @@ function (target_sitl name) math(EXPR hse_value "${hse_mhz} * 1000000") list(APPEND target_definitions "HSE_VALUE=${hse_value}") + if (MSP_UART) + list(APPEND target_definitions "MSP_UART=${MSP_UART}") + endif() + string(TOLOWER ${PROJECT_NAME} lowercase_project_name) set(binary_name ${lowercase_project_name}_${FIRMWARE_VERSION}_${name}) if(DEFINED BUILD_SUFFIX AND NOT "" STREQUAL "${BUILD_SUFFIX}") diff --git a/cmake/stm32.cmake b/cmake/stm32.cmake index f02185e9aff..c4534a30500 100644 --- a/cmake/stm32.cmake +++ b/cmake/stm32.cmake @@ -333,6 +333,11 @@ function(target_stm32) math(EXPR hse_value "${hse_mhz} * 1000000") list(APPEND target_definitions "HSE_VALUE=${hse_value}") + + if (MSP_UART) + list(APPEND target_definitions "MSP_UART=${MSP_UART}") + endif() + if(args_COMPILE_DEFINITIONS) list(APPEND target_definitions ${args_COMPILE_DEFINITIONS}) endif() diff --git a/docs/Broken USB recovery.md b/docs/Broken USB recovery.md index 46db627c83e..3406653b981 100644 --- a/docs/Broken USB recovery.md +++ b/docs/Broken USB recovery.md @@ -53,55 +53,16 @@ The following procedure describes the process under Windows 10/11: Please read [Building in Windows 2010 or 11 with Linux Subsystem](https://github.com/iNavFlight/inav/blob/master/docs/development/Building%20in%20Windows%2010%20or%2011%20with%20Linux%20Subsystem.md) and follow the instructions up to "Building with Make". -To activate MSP by default, go to the directory `src/main/target/[your target]`. -If no config.c exists, create a new text file with this name and the following content: - -``` -/* - * This file is part of INAV. - * - * INAV is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * INAV is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with INAV. If not, see . - */ - -#include - -#include "platform.h" -#include "io/serial.h" - -void targetConfiguration(void) -{ - serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].functionMask = FUNCTION_MSP; - serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].msp_baudrateIndex = BAUD_115200; -} - -``` - -If the file already exists, add the following lines in the function `void targetConfiguration(void)` (before the last `}`) - -``` -serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].functionMask = FUNCTION_MSP; -serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USARTX)].msp_baudrateIndex = BAUD_115200; -``` - -Replace the X in SERIAL_PORT_USARTX (in both lines) with the number of UART/serial port on which MSP is to be activated. +In the step 'prepare build environment' add the option `-DMSP_UART=SERIAL_PORT_USARTX` to `cmake` + +Replace the X in SERIAL_PORT_USARTX with the number of UART/serial port on which MSP is to be activated. Example: -For UART 2: `SERIAL_PORT_USART2` -For UART 3: `SERIAL_PORT_USART3` +For UART 2: `cmake -DMSP_UART=SERIAL_PORT_USART2 ..` +For UART 3: `cmake -DMSP_UART=SERIAL_PORT_USART3 ..` etc. -Save the file and build the firmware as described in the document above. +Build the firmware as described in the document above (`make [YOUR_TARGET]`). ## Flashing via Uart: diff --git a/src/main/fc/config.c b/src/main/fc/config.c index c9a50c60a25..e5f164988df 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -284,6 +284,14 @@ void createDefaultConfig(void) featureSet(FEATURE_AIRMODE); targetConfiguration(); + +#ifdef MSP_UART + int port = findSerialPortIndexByIdentifier(MSP_UART); + if (port) { + serialConfigMutable()->portConfigs[port].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[port].msp_baudrateIndex = BAUD_115200; + } +#endif } void resetConfigs(void) From a974ccc85b6f3f79a67f58149c6b960b603e15eb Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 24 May 2024 10:01:08 -0500 Subject: [PATCH 069/112] revert buggy inav_use_gps_no_baro to default off --- src/main/fc/settings.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d2d8c5c433e..de413d456af 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2288,10 +2288,10 @@ groups: field: use_gps_velned type: bool - name: inav_use_gps_no_baro - description: "Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed." + description: "Defines if INAV should use only use GPS data for altitude estimation and not barometer. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed." field: use_gps_no_baro type: bool - default_value: ON + default_value: OFF - name: inav_allow_dead_reckoning description: "Defines if INAV will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation" default_value: OFF From 4e1e59eb1f16ccfea2d4d4728bdf094eabcced66 Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 24 May 2024 13:07:07 -0500 Subject: [PATCH 070/112] inav_use_gps_no_baro Settings.md --- docs/Settings.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 9c31498ef19..9817422595e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1844,11 +1844,11 @@ Allows to chose when the home position is reset. Can help prevent resetting home ### inav_use_gps_no_baro -Defines if INAV should use only use GPS data for altitude estimation when barometer is not available. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed. +Defines if INAV should use only use GPS data for altitude estimation and not barometer. If set to ON, INAV will allow GPS assisted modes and RTH even when there is no barometer installed. | Default | Min | Max | | --- | --- | --- | -| ON | OFF | ON | +| OFF | OFF | ON | --- From 9be624037c2c812b3e08a352e3d952febdf77dcb Mon Sep 17 00:00:00 2001 From: WizzardDr <93602460+WizzardDr@users.noreply.github.com> Date: Fri, 24 May 2024 22:13:13 +0100 Subject: [PATCH 071/112] Skip `pollVersion()` and `pollGnssCapabilities()` if `gps_auto_config = OFF` Modified gps_ublox.c to skip `pollVersion()` and `pollGnssCapabilities()` if `gps_auto_config = OFF`. This is to support an RX only UART connection of the GPS module. --- src/main/io/gps_ublox.c | 37 +++++++++++++++++++------------------ 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 8375774e044..8c071613289 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1048,25 +1048,26 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; gpsState.autoConfigStep = 0; - do { - pollVersion(); - gpsState.autoConfigStep++; - ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); - } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); - - gpsState.autoConfigStep = 0; - ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; - // M7 and earlier will never get pass this step, so skip it (#9440). - // UBLOX documents that this is M8N and later - if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX7) { - do { - pollGnssCapabilities(); - gpsState.autoConfigStep++; - ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); - } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); - } // Configure GPS module if enabled if (gpsState.gpsConfig->autoConfig) { + do { + pollVersion(); + gpsState.autoConfigStep++; + ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); + } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); + + gpsState.autoConfigStep = 0; + ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; + // M7 and earlier will never get pass this step, so skip it (#9440). + // UBLOX documents that this is M8N and later + if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX7) { + do { + pollGnssCapabilities(); + gpsState.autoConfigStep++; + ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); + } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); + } + // Configure GPS ptSpawn(gpsConfigure); } @@ -1079,7 +1080,7 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) ptSemaphoreWait(semNewDataReady); gpsProcessNewSolutionData(false); - if ((gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) { + if ((gpsState.gpsConfig->autoConfig) && (gpsState.gpsConfig->provider == GPS_UBLOX || gpsState.gpsConfig->provider == GPS_UBLOX7PLUS)) { if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) { gpsState.lastCapaPoolMs = millis(); From 06eb0ef3c04135df62ae42ac87e9f1d9c9acd387 Mon Sep 17 00:00:00 2001 From: Fernando Schuindt Date: Wed, 29 May 2024 01:23:33 -0300 Subject: [PATCH 072/112] Fix OSD mentioned twice on README.md feature list --- readme.md | 1 - 1 file changed, 1 deletion(-) diff --git a/readme.md b/readme.md index a70c42d4c86..b8e9f21105d 100644 --- a/readme.md +++ b/readme.md @@ -51,7 +51,6 @@ Fly safe, fly smart with INAV 7.1 and a compass by your side! * SmartAudio and IRC Tramp VTX support * Telemetry: SmartPort, FPort, MAVlink, LTM, CRSF * Multi-color RGB LED Strip support -* On Screen Display (OSD) - both character and pixel style * And many more! For a list of features, changes and some discussion please review consult the releases [page](https://github.com/iNavFlight/inav/releases) and the documentation. From 2667c03850189242486e5554ae62bea741a797a3 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 30 May 2024 13:03:25 +0100 Subject: [PATCH 073/112] update flight modes --- src/main/blackbox/blackbox.c | 57 +++++++++++++++++++----------------- src/main/fc/fc_core.c | 2 +- src/main/flight/pid.c | 4 +-- 3 files changed, 33 insertions(+), 30 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4433e7b4aba..419b7b4add4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -402,15 +402,22 @@ static const blackboxSimpleFieldDefinition_t blackboxGpsHFields[] = { // Rarely-updated fields static const blackboxSimpleFieldDefinition_t blackboxSlowFields[] = { - {"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + /* "flightModeFlags" renamed internally to more correct ref of rcModeFlags, since it logs rc boxmode selections, + * but name kept for external compatibility reasons. + * "activeFlightModeFlags" logs actual active flight modes rather than rc boxmodes. + * 'active' should at least distinguish it from the existing "flightModeFlags" */ + + {"activeWpNumber", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"flightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"flightModeFlags2", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, + {"activeFlightModeFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"stateFlags", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"failsafePhase", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}, {"rxSignalReceived", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}, {"rxFlightChannelsValid", -1, UNSIGNED, PREDICT(0), ENCODING(TAG2_3S32)}, {"rxUpdateRate", -1, UNSIGNED, PREDICT(PREVIOUS), ENCODING(UNSIGNED_VB)}, - + {"hwHealthStatus", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"powerSupplyImpedance", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, {"sagCompensatedVBat", -1, UNSIGNED, PREDICT(0), ENCODING(UNSIGNED_VB)}, @@ -533,7 +540,9 @@ typedef struct blackboxGpsState_s { // This data is updated really infrequently: typedef struct blackboxSlowState_s { - uint32_t flightModeFlags; // extend this data size (from uint16_t) + uint32_t rcModeFlags; + uint32_t rcModeFlags2; + uint32_t activeFlightModeFlags; uint32_t stateFlags; uint8_t failsafePhase; bool rxSignalReceived; @@ -566,7 +575,7 @@ extern boxBitmask_t rcModeActivationMask; static BlackboxState blackboxState = BLACKBOX_STATE_DISABLED; static uint32_t blackboxLastArmingBeep = 0; -static uint32_t blackboxLastFlightModeFlags = 0; +static uint32_t blackboxLastRcModeFlags = 0; static struct { uint32_t headerIndex; @@ -1259,9 +1268,11 @@ static void writeSlowFrame(void) int32_t values[3]; blackboxWrite('S'); - + blackboxWriteUnsignedVB(slowHistory.activeWpNumber); - blackboxWriteUnsignedVB(slowHistory.flightModeFlags); + blackboxWriteUnsignedVB(slowHistory.rcModeFlags); + blackboxWriteUnsignedVB(slowHistory.rcModeFlags2); + blackboxWriteUnsignedVB(slowHistory.activeFlightModeFlags); blackboxWriteUnsignedVB(slowHistory.stateFlags); /* @@ -1269,10 +1280,10 @@ static void writeSlowFrame(void) */ values[0] = slowHistory.failsafePhase; values[1] = slowHistory.rxSignalReceived ? 1 : 0; - values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0; + values[2] = slowHistory.rxFlightChannelsValid ? 1 : 0; blackboxWriteTag2_3S32(values); - blackboxWriteUnsignedVB(slowHistory.rxUpdateRate); + blackboxWriteUnsignedVB(slowHistory.rxUpdateRate); blackboxWriteUnsignedVB(slowHistory.hwHealthStatus); @@ -1309,28 +1320,20 @@ static void writeSlowFrame(void) static void loadSlowState(blackboxSlowState_t *slow) { slow->activeWpNumber = getActiveWpNumber(); - memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags; + + slow->rcModeFlags = rcModeActivationMask.bits[0]; // first 32 bits of boxId_e + slow->rcModeFlags2 = rcModeActivationMask.bits[1]; // remaining bits of boxId_e + // Also log Nav auto enabled flight modes rather than just those selected by boxmode - if (FLIGHT_MODE(ANGLE_MODE)) { - slow->flightModeFlags |= (1 << BOXANGLE); - } - if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { - slow->flightModeFlags |= (1 << BOXNAVALTHOLD); - } - if (FLIGHT_MODE(NAV_RTH_MODE)) { - slow->flightModeFlags |= (1 << BOXNAVRTH); - } if (navigationGetHeadingControlState() == NAV_HEADING_CONTROL_AUTO) { - slow->flightModeFlags |= (1 << BOXHEADINGHOLD); - } - if (navigationRequiresTurnAssistance()) { - slow->flightModeFlags |= (1 << BOXTURNASSIST); + slow->rcModeFlags |= (1 << BOXHEADINGHOLD); } + slow->activeFlightModeFlags = flightModeFlags; slow->stateFlags = stateFlags; slow->failsafePhase = failsafePhase(); slow->rxSignalReceived = rxIsReceivingSignal(); slow->rxFlightChannelsValid = rxAreFlightChannelsValid(); - slow->rxUpdateRate = getRcUpdateFrequency(); + slow->rxUpdateRate = getRcUpdateFrequency(); slow->hwHealthStatus = (getHwGyroStatus() << 2 * 0) | // Pack hardware health status into a bit field. (getHwAccelerometerStatus() << 2 * 1) | // Use raw hardwareSensorStatus_e values and pack them using 2 bits per value (getHwCompassStatus() << 2 * 2) | // Report GYRO in 2 lowest bits, then ACC, COMPASS, BARO, GPS, RANGEFINDER and PITOT @@ -1498,7 +1501,7 @@ void blackboxStart(void) * it finally plays the beep for this arming event. */ blackboxLastArmingBeep = getArmingBeepTimeMicros(); - memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); // record startup status + memcpy(&blackboxLastRcModeFlags, &rcModeActivationMask, sizeof(blackboxLastRcModeFlags)); // record startup status blackboxSetState(BLACKBOX_STATE_PREPARE_LOG_FILE); } @@ -2023,10 +2026,10 @@ static void blackboxCheckAndLogArmingBeep(void) static void blackboxCheckAndLogFlightMode(void) { // Use != so that we can still detect a change if the counter wraps - if (memcmp(&rcModeActivationMask, &blackboxLastFlightModeFlags, sizeof(blackboxLastFlightModeFlags))) { + if (memcmp(&rcModeActivationMask, &blackboxLastRcModeFlags, sizeof(blackboxLastRcModeFlags))) { flightLogEvent_flightMode_t eventData; // Add new data for current flight mode flags - eventData.lastFlags = blackboxLastFlightModeFlags; - memcpy(&blackboxLastFlightModeFlags, &rcModeActivationMask, sizeof(blackboxLastFlightModeFlags)); + eventData.lastFlags = blackboxLastRcModeFlags; + memcpy(&blackboxLastRcModeFlags, &rcModeActivationMask, sizeof(blackboxLastRcModeFlags)); memcpy(&eventData.flags, &rcModeActivationMask, sizeof(eventData.flags)); blackboxLogEvent(FLIGHT_LOG_EVENT_FLIGHTMODE, (flightLogEventData_t *)&eventData); } diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 4b231548b99..d96c198f83d 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -704,7 +704,7 @@ void processRx(timeUs_t currentTimeUs) } /* Turn assistant mode */ - if (IS_RC_MODE_ACTIVE(BOXTURNASSIST)) { + if (IS_RC_MODE_ACTIVE(BOXTURNASSIST) || navigationRequiresTurnAssistance()) { ENABLE_FLIGHT_MODE(TURN_ASSISTANT); } else { DISABLE_FLIGHT_MODE(TURN_ASSISTANT); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a9d1291877f..cb6502b0107 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1099,7 +1099,7 @@ void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); - if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ + if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT))) { pidState->itermFreezeActive = true; } else { @@ -1242,7 +1242,7 @@ void FAST_CODE pidController(float dT) } // Apply Turn Assistance - if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { + if (FLIGHT_MODE(TURN_ASSISTANT) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH])); pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget); From 5191ec369485f4d4941cf96ac8143595863d8dbf Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 31 May 2024 09:56:39 +0200 Subject: [PATCH 074/112] Update readme.md Fix small typo that sneaked in. --- readme.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/readme.md b/readme.md index c18b029563c..fac247ef9ab 100644 --- a/readme.md +++ b/readme.md @@ -8,7 +8,7 @@ # ICM426xx IMUs PSA -> The filtering settings for the ICM426xx has changed to matche what is used by Ardupilot and Betaflight in INAV 7.1. When upgrading from older versions you may need to recalibrate the Accelerometer and if you are not using INAV's default tune you may also want to check if the tune is still good. +> The filtering settings for the ICM426xx has changed to match what is used by Ardupilot and Betaflight in INAV 7.1. When upgrading from older versions you may need to recalibrate the Accelerometer and if you are not using INAV's default tune you may also want to check if the tune is still good. ![INAV](http://static.rcgroups.net/forums/attachments/6/1/0/3/7/6/a9088858-102-inav.png) From 5a3e21dfbed2d91cf0f09fcd6ebf6b64b5443715 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 31 May 2024 16:41:39 +0200 Subject: [PATCH 075/112] Add extra infor about special labels --- src/main/drivers/pwm_mapping.h | 5 +++++ src/main/fc/fc_msp.c | 23 +++++++++++++++-------- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index e9b3a0f9573..cfb96afadbf 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -53,6 +53,11 @@ typedef enum { SERVO_TYPE_SBUS_PWM } servoProtocolType_e; +typedef enum { + PIN_LABEL_NONE = 0, + PIN_LABEL_LED +} pinLabel_e; + typedef enum { PWM_INIT_ERROR_NONE = 0, PWM_INIT_ERROR_TOO_MANY_MOTORS, diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 75e18ef3681..2398b0d4157 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1589,15 +1589,22 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF } break; case MSP2_INAV_OUTPUT_MAPPING_EXT2: - for (uint8_t i = 0; i < timerHardwareCount; ++i) - if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { - #if defined(SITL_BUILD) - sbufWriteU8(dst, i); - #else - sbufWriteU8(dst, timer2id(timerHardware[i].tim)); - #endif - sbufWriteU32(dst, timerHardware[i].usageFlags); + { + ioTag_t led_tag = IO_TAG(WS2811_PIN); + for (uint8_t i = 0; i < timerHardwareCount; ++i) + + if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { + #if defined(SITL_BUILD) + sbufWriteU8(dst, i); + #else + sbufWriteU8(dst, timer2id(timerHardware[i].tim)); + #endif + sbufWriteU32(dst, timerHardware[i].usageFlags); + // Extra label to help identify repurposed PINs. + // Eventually, we can try to add more labels for PPM pins, etc. + sbufWriteU8(dst, timerHardware[i].tag == led_tag ? PIN_LABEL_LED : PIN_LABEL_NONE); } + } break; From 0b50f5519efe77a0aa51ca6bad35aba20330e80a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 31 May 2024 17:33:37 +0200 Subject: [PATCH 076/112] Fix sitl build --- src/main/fc/fc_msp.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 2398b0d4157..44fefaa378a 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1600,9 +1600,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timer2id(timerHardware[i].tim)); #endif sbufWriteU32(dst, timerHardware[i].usageFlags); + #if defined(SITIL_BUILD) + sbufWriteU8(dst, 0); + #else // Extra label to help identify repurposed PINs. // Eventually, we can try to add more labels for PPM pins, etc. sbufWriteU8(dst, timerHardware[i].tag == led_tag ? PIN_LABEL_LED : PIN_LABEL_NONE); + #endif } } break; From d78aa93c234183d34e908b3f1aafc2478b5951c4 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 31 May 2024 17:37:25 +0200 Subject: [PATCH 077/112] sitl can be a pain. One more fix :) --- src/main/fc/fc_msp.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 44fefaa378a..5deb8efafb2 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1590,7 +1590,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_OUTPUT_MAPPING_EXT2: { + #if defined(SITIL_BUILD) ioTag_t led_tag = IO_TAG(WS2811_PIN); + #endif for (uint8_t i = 0; i < timerHardwareCount; ++i) if (!(timerHardware[i].usageFlags & (TIM_USE_PPM | TIM_USE_PWM))) { From ad151352bd5c908029030237c6c51aa7ad0c900e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 31 May 2024 17:41:41 +0200 Subject: [PATCH 078/112] s/defined/!defined/g --- src/main/fc/fc_msp.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5deb8efafb2..17b51f037a3 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1590,7 +1590,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_OUTPUT_MAPPING_EXT2: { - #if defined(SITIL_BUILD) + #if !defined(SITIL_BUILD) ioTag_t led_tag = IO_TAG(WS2811_PIN); #endif for (uint8_t i = 0; i < timerHardwareCount; ++i) From d33488d91a343411220242ca74280c5101a5494b Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 31 May 2024 17:45:26 +0200 Subject: [PATCH 079/112] Now, tripple checked. ;) --- src/main/fc/fc_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 17b51f037a3..f70c00f221b 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1590,7 +1590,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_OUTPUT_MAPPING_EXT2: { - #if !defined(SITIL_BUILD) + #if !defined(SITL_BUILD) ioTag_t led_tag = IO_TAG(WS2811_PIN); #endif for (uint8_t i = 0; i < timerHardwareCount; ++i) @@ -1602,7 +1602,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timer2id(timerHardware[i].tim)); #endif sbufWriteU32(dst, timerHardware[i].usageFlags); - #if defined(SITIL_BUILD) + #if defined(SITL_BUILD) sbufWriteU8(dst, 0); #else // Extra label to help identify repurposed PINs. From e88b85f4ea4e4eaf4e56d119de3d73ba66d45544 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 31 May 2024 18:12:42 +0200 Subject: [PATCH 080/112] Make sure LED is at the end of the PWM list --- src/main/target/AIRBOTF7/target.c | 6 +++--- src/main/target/ANYFCM7/target.c | 15 ++++++++------- src/main/target/BLUEJAYF4/target.c | 4 ++-- src/main/target/F4BY/target.c | 18 ++++++++++-------- src/main/target/FOXEERF745AIO/target.c | 4 ++-- src/main/target/FRSKYPILOT/target.c | 4 ---- src/main/target/IFLIGHT_BLITZ_ATF435/target.c | 2 +- src/main/target/MATEKF405/target.c | 2 +- src/main/target/NEUTRONRCF435MINI/target.c | 6 +++--- src/main/target/NEUTRONRCF435WING/target.c | 3 +-- src/main/target/OMNIBUSF4/CMakeLists.txt | 1 - src/main/target/OMNIBUSF4/target.c | 7 ++----- src/main/target/SAGEATF4/target.c | 12 ++++++------ .../target/SPEEDYBEEF405MINI/CMakeLists.txt | 1 - src/main/target/SPEEDYBEEF405MINI/target.c | 11 +++-------- src/main/target/SPRACINGF7DUAL/target.c | 4 +++- src/main/target/TAKERF722SE/target.c | 2 +- src/main/target/TMOTORF7V2/target.c | 2 +- src/main/target/TMOTORVELOXF7V2/target.c | 2 +- 19 files changed, 48 insertions(+), 58 deletions(-) diff --git a/src/main/target/AIRBOTF7/target.c b/src/main/target/AIRBOTF7/target.c index 22d2564821a..6cad05c41fd 100644 --- a/src/main/target/AIRBOTF7/target.c +++ b/src/main/target/AIRBOTF7/target.c @@ -42,13 +42,13 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu1_mpu6500, DEVHW_MPU6500, GYRO_1_SPI_BUS, #endif timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED - DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // Cam control, SS, UNUSED - DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), //S1 DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), //S2 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), //S3 DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), //S4 + // + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), // LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // Cam control, SS, UNUSED }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ANYFCM7/target.c b/src/main/target/ANYFCM7/target.c index 7dd30d2250b..aae6922a374 100644 --- a/src/main/target/ANYFCM7/target.c +++ b/src/main/target/ANYFCM7/target.c @@ -26,12 +26,6 @@ #define TIM_EN_N TIMER_OUTPUT_ENABLED | TIMER_OUTPUT_N_CHANNEL timerHardware_t timerHardware[] = { - // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN - // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN - // DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 - // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 - // DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 - // DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7 DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S10_OUT 1 DMA1_ST7 DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT 2 DMA1_ST1 @@ -39,10 +33,17 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S1_OUT 4 DMA1_ST7 DMA1_ST6 DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT DMA1_ST4 DEF_TIM(TIM1, CH2N, PB0, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S3_OUT DMA2_ST6 DMA2_ST2 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S4_OUT DMA1_ST5 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_OUT DMA1_ST5 -- Used to have TIM_USE_LED DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S7_OUT DMA1_ST2 DEF_TIM(TIM1, CH3N, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S8_OUT DMA2_ST6 DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S9_OUT DMA1_ST4 + + // DEF_TIM(TIM12, CH1, PB14, TIM_USE_PWM | TIM_USE_PPM, 0, 0 ), // S1_IN + // DEF_TIM(TIM12, CH2, PB15, TIM_USE_PWM, 0, 0 ), // S2_IN + // DEF_TIM(TIM8, CH1, PC6, TIM_USE_PWM, 0, 0 ), // S3_IN DMA2_ST2 DMA2_ST2 + // DEF_TIM(TIM8, CH2, PC7, TIM_USE_PWM, 0, 0 ), // S4_IN DMA2_ST3 DMA2_ST2 + // DEF_TIM(TIM8, CH3, PC8, TIM_USE_PWM, 0, 0 ), // S5_IN DMA2_ST4 DMA2_ST2 + // DEF_TIM(TIM8, CH4, PC9, TIM_USE_PWM, 0, 0 ), // S6_IN DMA2_ST7 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/BLUEJAYF4/target.c b/src/main/target/BLUEJAYF4/target.c index ba48551f76c..37afb69b1ce 100644 --- a/src/main/target/BLUEJAYF4/target.c +++ b/src/main/target/BLUEJAYF4/target.c @@ -29,8 +29,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S2_OUT - DMA1_ST4 DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S3_OUT - DMA1_ST1 DEF_TIM(TIM9, CH2, PA3, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S4_OUT - no DMA - //DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4_OUT - DMA1_ST3 (Could be DMA1_ST1 with dmaopt=0) - DEF_TIM(TIM3, CH4, PB0, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0 ), // S5_OUT - DMA1_ST2 + //DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1 ), // S4_OUT - DMA1_ST3 (Could be DMA1_ST1 with dmaopt=0) + DEF_TIM(TIM3, CH4, PB0, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S5_OUT - DMA1_ST2 -- used to be TIM_USE_LED DEF_TIM(TIM3, CH3, PB1, TIM_USE_OUTPUT_AUTO, 0, 0 ), // S6_OUT - DMA1_ST7 }; diff --git a/src/main/target/F4BY/target.c b/src/main/target/F4BY/target.c index 8d1b4622f69..b1654e27ec0 100644 --- a/src/main/target/F4BY/target.c +++ b/src/main/target/F4BY/target.c @@ -6,14 +6,6 @@ #include "drivers/timer.h" timerHardware_t timerHardware[] = { - // DEF_TIM(TIM3, CH4, PC9, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // S1_IN - // DEF_TIM(TIM3, CH3, PC8, TIM_USE_PWM, 0, 0), // S2_IN - // DEF_TIM(TIM3, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN - // DEF_TIM(TIM3, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN - // DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM, 0, 0), // S5_IN - // DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM, 0, 0), // S6_IN - // DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM, 0, 0), // S7_IN - // DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM, 0, 0), // S8_IN DEF_TIM(TIM2, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1_OUT DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2_OUT @@ -25,6 +17,16 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM1, CH4, PE14, TIM_USE_OUTPUT_AUTO, 0, 0), // S8_OUT DEF_TIM(TIM9, CH2, PE6, TIM_USE_ANY, 0, 0), // sonar echo if needed + + + // DEF_TIM(TIM3, CH4, PC9, TIM_USE_PPM | TIM_USE_PWM, 0, 0), // S1_IN + // DEF_TIM(TIM3, CH3, PC8, TIM_USE_PWM, 0, 0), // S2_IN + // DEF_TIM(TIM3, CH1, PC6, TIM_USE_PWM, 0, 0), // S3_IN + // DEF_TIM(TIM3, CH2, PC7, TIM_USE_PWM, 0, 0), // S4_IN + // DEF_TIM(TIM4, CH4, PD15, TIM_USE_PWM, 0, 0), // S5_IN + // DEF_TIM(TIM4, CH3, PD14, TIM_USE_PWM, 0, 0), // S6_IN + // DEF_TIM(TIM4, CH2, PD13, TIM_USE_PWM, 0, 0), // S7_IN + // DEF_TIM(TIM4, CH1, PD12, TIM_USE_PWM, 0, 0), // S8_IN }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FOXEERF745AIO/target.c b/src/main/target/FOXEERF745AIO/target.c index f724f65913c..382306a8d50 100644 --- a/src/main/target/FOXEERF745AIO/target.c +++ b/src/main/target/FOXEERF745AIO/target.c @@ -25,12 +25,12 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP - D(2, 6, 0) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // M1 - D(1, 4, 5) DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // M2 - D(1, 5, 5) DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // M3 - D(1, 2, 5) DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // M4 - D(1, 7, 5) + // + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP - D(2, 6, 0) }; diff --git a/src/main/target/FRSKYPILOT/target.c b/src/main/target/FRSKYPILOT/target.c index 50b82d7687e..95989f5f42a 100644 --- a/src/main/target/FRSKYPILOT/target.c +++ b/src/main/target/FRSKYPILOT/target.c @@ -39,13 +39,9 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 DEF_TIM(TIM4, CH4, PB9, TIM_USE_OUTPUT_AUTO, 0, 0), // S9 -#ifdef FRSKYPILOT_LED - DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED | TIM_USE_LED, 0, 0), // S10 now LED, S11 & S12 UART 3 only -#else DEF_TIM(TIM2, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S10 DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 0), // S11 DEF_TIM(TIM2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0, 0), // S12 -#endif DEF_TIM(TIM5, CH1, PA0, TIM_USE_BEEPER, 0, 0), // beeper }; diff --git a/src/main/target/IFLIGHT_BLITZ_ATF435/target.c b/src/main/target/IFLIGHT_BLITZ_ATF435/target.c index 2de0fd50578..947d19c0d20 100644 --- a/src/main/target/IFLIGHT_BLITZ_ATF435/target.c +++ b/src/main/target/IFLIGHT_BLITZ_ATF435/target.c @@ -41,7 +41,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,11), // S5 DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,10), // S6 DEF_TIM(TMR2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0,5), //S7 - DEF_TIM(TMR2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0,6), //S8 + DEF_TIM(TMR2, CH4, PB11, TIM_USE_OUTPUT_AUTO, 0,6), //S8 DEF_TIM(TMR1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP }; diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 1706954b7f9..5b6f0ce559d 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -29,7 +29,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 UP(2,1) DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 UP(2,1) - DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO | TIM_USE_LED , 0, 0), // S5 UP(1,7) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 UP(1,7) -- LED DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 UP(2,5) DEF_TIM(TIM4, CH3, PB8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 D(1,7)!S5 UP(2,6) diff --git a/src/main/target/NEUTRONRCF435MINI/target.c b/src/main/target/NEUTRONRCF435MINI/target.c index 9400ae4be1b..0933322bb14 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.c +++ b/src/main/target/NEUTRONRCF435MINI/target.c @@ -26,13 +26,13 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,6), // PWM1 - LED MCO1 DMA1 CH2 - DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,0), // motor1 DMA2 CH7 DEF_TIM(TMR4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0,2), // motor2 DMA2 CH6 DEF_TIM(TMR2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0,1), // motor3 DMA2 CH5 DEF_TIM(TMR3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0,3), // motor4 DMA2 CH4 - + // + DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,6), // PWM1 - LED MCO1 DMA1 CH2 + // }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/NEUTRONRCF435WING/target.c b/src/main/target/NEUTRONRCF435WING/target.c index b1cd9031cc3..428654ebed3 100644 --- a/src/main/target/NEUTRONRCF435WING/target.c +++ b/src/main/target/NEUTRONRCF435WING/target.c @@ -26,8 +26,6 @@ timerHardware_t timerHardware[] = { - DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2 - DEF_TIM(TMR4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0,0), // motor1 DMA2 CH7 DEF_TIM(TMR1, CH3, PA10, TIM_USE_OUTPUT_AUTO, 0,2), // motor2 DMA2 CH6 DEF_TIM(TMR2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0,1), // motor3 DMA2 CH5 @@ -38,6 +36,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TMR3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0,8), // PWM3 - OUT7 DMA2 CH3 DEF_TIM(TMR2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0,11), // PWM4 - OUT8 DMA1 CH7 + DEF_TIM(TMR1, CH1, PA8, TIM_USE_ANY |TIM_USE_LED, 0,7), // PWM1 - LED MCO1 DMA1 CH2 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/OMNIBUSF4/CMakeLists.txt b/src/main/target/OMNIBUSF4/CMakeLists.txt index 72f3bbeca87..a6ccb483bf5 100644 --- a/src/main/target/OMNIBUSF4/CMakeLists.txt +++ b/src/main/target/OMNIBUSF4/CMakeLists.txt @@ -3,7 +3,6 @@ target_stm32f405xg(DYSF4PROV2) target_stm32f405xg(OMNIBUSF4) # the OMNIBUSF4SD has an SDCARD instead of flash, a BMP280 baro and therefore a slightly different ppm/pwm and SPI mapping target_stm32f405xg(OMNIBUSF4PRO) -target_stm32f405xg(OMNIBUSF4PRO_LEDSTRIPM5) target_stm32f405xg(OMNIBUSF4V3_S5_S6_2SS) target_stm32f405xg(OMNIBUSF4V3_S5S6_SS) target_stm32f405xg(OMNIBUSF4V3_S6_SS) diff --git a/src/main/target/OMNIBUSF4/target.c b/src/main/target/OMNIBUSF4/target.c index 86485aa49fc..be85e9fbce3 100644 --- a/src/main/target/OMNIBUSF4/target.c +++ b/src/main/target/OMNIBUSF4/target.c @@ -41,15 +41,12 @@ timerHardware_t timerHardware[] = { #elif defined(OMNIBUSF4V3_S6_SS) DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0), // S6_OUT SOFTSERIAL -#elif defined(OMNIBUSF4PRO_LEDSTRIPM5) - DEF_TIM(TIM5, CH2, PA1, TIM_USE_LED, 0, 0), // S5_OUT LED strip - DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT #else - DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO | TIM_USE_LED, 0, 0), // S5_OUT MOTOR, SERVO or LED + DEF_TIM(TIM5, CH2, PA1, TIM_USE_OUTPUT_AUTO, 0, 0), // S5_OUT MOTOR, SERVO or LED DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // S6_OUT #endif -#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) && !defined(OMNIBUSF4PRO_LEDSTRIPM5) +#if (defined(OMNIBUSF4PRO) || defined(OMNIBUSF4V3)) DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED strip for F4 V2 / F4-Pro-0X and later (RCD_CS for F4) #endif diff --git a/src/main/target/SAGEATF4/target.c b/src/main/target/SAGEATF4/target.c index 2d951598ae0..06f571c949b 100644 --- a/src/main/target/SAGEATF4/target.c +++ b/src/main/target/SAGEATF4/target.c @@ -26,19 +26,19 @@ timerHardware_t timerHardware[] = { - // DEF_TIM(TMR5, CH4, PA3, TIM_USE_PPM, 0, 4), // PPM UART2_RX_PIN DMA1 CH5 + // DEF_TIM(TMR5, CH4, PA3, TIM_USE_PPM, 0, 4), // PPM UART2_RX_PIN DMA1 CH5 - DEF_TIM(TMR4, CH1, PB6, TIM_USE_MOTOR, 0,0), // motor1 DMA1 CH1 - DEF_TIM(TMR4, CH2, PB7, TIM_USE_MOTOR, 0,1), // motor2 DMA1 CH2 - DEF_TIM(TMR4, CH3, PB8, TIM_USE_MOTOR, 0,2), // motor3 DMA1 CH3 - DEF_TIM(TMR4, CH4, PB9, TIM_USE_MOTOR, 0,3), // motor4 DMA1 CH4 + DEF_TIM(TMR4, CH1, PB6, TIM_USE_MOTOR, 0,0), // motor1 DMA1 CH1 + DEF_TIM(TMR4, CH2, PB7, TIM_USE_MOTOR, 0,1), // motor2 DMA1 CH2 + DEF_TIM(TMR4, CH3, PB8, TIM_USE_MOTOR, 0,2), // motor3 DMA1 CH3 + DEF_TIM(TMR4, CH4, PB9, TIM_USE_MOTOR, 0,3), // motor4 DMA1 CH4 DEF_TIM(TMR3, CH1, PC6, TIM_USE_OUTPUT_AUTO, 0,8), // PWM1 - OUT5 DMA2 CH2 DMA2_CHANNEL1->ADC DEF_TIM(TMR3, CH2, PC7, TIM_USE_OUTPUT_AUTO, 0,9), // PWM2 - OUT6 DMA2 CH3 DEF_TIM(TMR3, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0,10), // PWM3 - OUT7 DMA2 CH4 DEF_TIM(TMR3, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0,11), // PWM4 - OUT8 DMA2 CH5 - DEF_TIM(TMR2, CH3, PB10, TIM_USE_ANY | TIM_USE_LED, 0,5), // PWM1 - LED MCO1 DMA1 CH6 + DEF_TIM(TMR2, CH3, PB10, TIM_USE_ANY | TIM_USE_LED, 0,5), // PWM1 - LED MCO1 DMA1 CH6 DEF_TIM(TMR5, CH4, PB11, TIM_USE_ANY | TIM_USE_BEEPER, 0,6), // PWM2 - BB DMA1 CH7 }; diff --git a/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt index e18c0cd5ada..c4105609952 100644 --- a/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt +++ b/src/main/target/SPEEDYBEEF405MINI/CMakeLists.txt @@ -1,2 +1 @@ target_stm32f405xg(SPEEDYBEEF405MINI) -target_stm32f405xg(SPEEDYBEEF405MINI_6OUTPUTS) diff --git a/src/main/target/SPEEDYBEEF405MINI/target.c b/src/main/target/SPEEDYBEEF405MINI/target.c index 3cc7fbbebba..b1e6e10f6f4 100644 --- a/src/main/target/SPEEDYBEEF405MINI/target.c +++ b/src/main/target/SPEEDYBEEF405MINI/target.c @@ -32,15 +32,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 1, 0), // S3 DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 1, 0), // S4 -#ifdef SPEEDYBEEF405MINI_6OUTPUTS - DEF_TIM(TIM12, CH1, PB14, TIM_USE_OUTPUT_AUTO, 1, 0), // CAM_CTRL - DEF_TIM(TIM1, CH1, PA8, TIM_USE_OUTPUT_AUTO, 0, 0), // LED -#else - DEF_TIM(TIM12, CH1, PB14, TIM_USE_ANY, 1, 0), // CAM_CTRL - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED -#endif + DEF_TIM(TIM12, CH1, PB14, TIM_USE_ANY, 1, 0), // CAM_CTRL + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED - DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPRACINGF7DUAL/target.c b/src/main/target/SPRACINGF7DUAL/target.c index e94835b93c2..ead356956a4 100644 --- a/src/main/target/SPRACINGF7DUAL/target.c +++ b/src/main/target/SPRACINGF7DUAL/target.c @@ -56,7 +56,6 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // ESC 7 DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // ESC 8 - DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), // LED Strip // Additional 2 PWM channels available on UART3 RX/TX pins // However, when using led strip the timer cannot be used, but no code appears to prevent that right now DEF_TIM(TIM2, CH3, PB10, TIM_USE_MOTOR, 0, 0), // Shared with UART3 TX PIN and SPI3 TX (OSD) @@ -67,6 +66,9 @@ timerHardware_t timerHardware[] = { // However, when using transponder the timer cannot be used, but no code appears to prevent that right now DEF_TIM(TIM1, CH2, PA9, TIM_USE_SERVO | TIM_USE_PWM, 0, 1), // PWM 3 DEF_TIM(TIM1, CH3, PA10, TIM_USE_SERVO | TIM_USE_PWM, 0, 1), // PWM 4 + + + DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED, 0, 0), // LED Strip }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/TAKERF722SE/target.c b/src/main/target/TAKERF722SE/target.c index d67062a9fbc..c49e383a72f 100644 --- a/src/main/target/TAKERF722SE/target.c +++ b/src/main/target/TAKERF722SE/target.c @@ -40,7 +40,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/TMOTORF7V2/target.c b/src/main/target/TMOTORF7V2/target.c index c21b0f0229a..df6004b7a2f 100644 --- a/src/main/target/TMOTORF7V2/target.c +++ b/src/main/target/TMOTORF7V2/target.c @@ -35,8 +35,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED DEF_TIM(TIM2, CH2, PB3, TIM_USE_PWM | TIM_USE_SERVO, 0, 0), // "C.C" + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/TMOTORVELOXF7V2/target.c b/src/main/target/TMOTORVELOXF7V2/target.c index 7d9ae0d3292..b5f56ea6b79 100644 --- a/src/main/target/TMOTORVELOXF7V2/target.c +++ b/src/main/target/TMOTORVELOXF7V2/target.c @@ -37,7 +37,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 UP1-6 D(1, 0, 2) // used to be fw motor DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 UP1-6 D(1, 3, 2) // used to be fw motor - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 1), // LED + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 1), // LED }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); From 243c6775999d15105fa0bb7e57ef00f92b2fc74a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 31 May 2024 18:24:39 +0200 Subject: [PATCH 081/112] Some targets don't have led pin defined --- src/main/fc/fc_msp.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index f70c00f221b..4ae42184816 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1590,7 +1590,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_OUTPUT_MAPPING_EXT2: { - #if !defined(SITL_BUILD) + #if !defined(SITL_BUILD) && defined(WS2811_PIN) ioTag_t led_tag = IO_TAG(WS2811_PIN); #endif for (uint8_t i = 0; i < timerHardwareCount; ++i) @@ -1602,7 +1602,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, timer2id(timerHardware[i].tim)); #endif sbufWriteU32(dst, timerHardware[i].usageFlags); - #if defined(SITL_BUILD) + #if defined(SITL_BUILD) || !defined(WS2811_PIN) sbufWriteU8(dst, 0); #else // Extra label to help identify repurposed PINs. From 22697304b2173bd1305293b2bef4317c769033d3 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Fri, 31 May 2024 21:40:01 +0200 Subject: [PATCH 082/112] Update gps_ublox.c Fix formatting. --- src/main/io/gps_ublox.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 8c071613289..958de158d3e 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -1061,11 +1061,11 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // M7 and earlier will never get pass this step, so skip it (#9440). // UBLOX documents that this is M8N and later if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX7) { - do { - pollGnssCapabilities(); - gpsState.autoConfigStep++; - ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); - } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); + do { + pollGnssCapabilities(); + gpsState.autoConfigStep++; + ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); + } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); } // Configure GPS From 0e9ca2a58d8944f805d1089e20b23f8386206fcb Mon Sep 17 00:00:00 2001 From: Ray Morris Date: Fri, 31 May 2024 21:49:43 -0500 Subject: [PATCH 083/112] update CMakeLists.txt to version 7.1.2 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f74527e5f1b..3a2bd54e6e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,7 +51,7 @@ else() endif() endif() -project(INAV VERSION 7.1.1) +project(INAV VERSION 7.1.2) enable_language(ASM) From eb676ece935821beac71e16d4aaf551b52d52fa9 Mon Sep 17 00:00:00 2001 From: flywoo Date: Sat, 1 Jun 2024 11:39:48 +0800 Subject: [PATCH 084/112] Added new targets FLYWOOF722PRO, F405HD, updated F745, F405PRO config --- src/main/target/FLYWOOF405PRO/CMakeLists.txt | 1 + src/main/target/FLYWOOF405PRO/config.c | 34 ++++ src/main/target/FLYWOOF405PRO/target.c | 3 - src/main/target/FLYWOOF405PRO/target.h | 19 ++- src/main/target/FLYWOOF722PRO/CMakeLists.txt | 1 + src/main/target/FLYWOOF722PRO/config.c | 30 ++++ src/main/target/FLYWOOF722PRO/target.c | 48 ++++++ src/main/target/FLYWOOF722PRO/target.h | 158 +++++++++++++++++++ src/main/target/FLYWOOF745/target.c | 9 -- src/main/target/FLYWOOF745/target.h | 10 ++ 10 files changed, 296 insertions(+), 17 deletions(-) create mode 100644 src/main/target/FLYWOOF405PRO/config.c create mode 100644 src/main/target/FLYWOOF722PRO/CMakeLists.txt create mode 100644 src/main/target/FLYWOOF722PRO/config.c create mode 100644 src/main/target/FLYWOOF722PRO/target.c create mode 100644 src/main/target/FLYWOOF722PRO/target.h diff --git a/src/main/target/FLYWOOF405PRO/CMakeLists.txt b/src/main/target/FLYWOOF405PRO/CMakeLists.txt index 8447a117c00..2c5382d82ef 100644 --- a/src/main/target/FLYWOOF405PRO/CMakeLists.txt +++ b/src/main/target/FLYWOOF405PRO/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f405xg(FLYWOOF405PRO) +target_stm32f405xg(FLYWOOF405HD) \ No newline at end of file diff --git a/src/main/target/FLYWOOF405PRO/config.c b/src/main/target/FLYWOOF405PRO/config.c new file mode 100644 index 00000000000..dfe6c0ce100 --- /dev/null +++ b/src/main/target/FLYWOOF405PRO/config.c @@ -0,0 +1,34 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include "platform.h" + +#include "fc/fc_msp_box.h" + +#include "io/piniobox.h" + +#include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" + +void targetConfiguration(void) +{ + // Make sure S1-S4 default to Motors + + timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; + timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS; +} diff --git a/src/main/target/FLYWOOF405PRO/target.c b/src/main/target/FLYWOOF405PRO/target.c index c0f1c4eec2d..d4dd134dddc 100644 --- a/src/main/target/FLYWOOF405PRO/target.c +++ b/src/main/target/FLYWOOF405PRO/target.c @@ -24,15 +24,12 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 D(1,3,2) DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 D(1,0,2) - DEF_TIM(TIM2, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 1), // S3 D(1,7,5) DEF_TIM(TIM2, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 D(1,2,5) DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 D(2,4,7) DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 - - DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) // DEF_TIM(TIM10, CH1, PB8, TIM_USE_PPM, 0, 0), // PPM }; diff --git a/src/main/target/FLYWOOF405PRO/target.h b/src/main/target/FLYWOOF405PRO/target.h index 14b5e9e03cb..a5a02c5fe84 100644 --- a/src/main/target/FLYWOOF405PRO/target.h +++ b/src/main/target/FLYWOOF405PRO/target.h @@ -17,9 +17,13 @@ #pragma once +#ifdef FLYWOOF405PRO #define TARGET_BOARD_IDENTIFIER "F4PR" #define USBD_PRODUCT_STRING "FLYWOOF405PRO" - +#else +#define TARGET_BOARD_IDENTIFIER "F4HD" +#define USBD_PRODUCT_STRING "FLYWOOF405HD" +#endif #define LED0 PC14 //Green #define BEEPER PC13 @@ -84,11 +88,12 @@ #define SPI3_MISO_PIN PC11 #define SPI3_MOSI_PIN PC12 +#ifdef FLYWOOF405PRO #define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI3 -#define MAX7456_CS_PIN PB14// - +#define MAX7456_CS_PIN PB14 +#endif // *************** Onboard flash ******************** #define USE_FLASHFS @@ -109,6 +114,10 @@ #define UART1_TX_PIN PB6 #define UART1_RX_PIN PA10 +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + #define USE_UART3 #define UART3_TX_PIN PB10 #define UART3_RX_PIN PB11 @@ -125,7 +134,7 @@ #define UART6_TX_PIN PC6 #define UART6_RX_PIN PC7 -#define SERIAL_PORT_COUNT 6 +#define SERIAL_PORT_COUNT 7 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS @@ -159,6 +168,6 @@ #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) +#define TARGET_IO_PORTD 0xffff #define MAX_PWM_OUTPUT_PORTS 8 diff --git a/src/main/target/FLYWOOF722PRO/CMakeLists.txt b/src/main/target/FLYWOOF722PRO/CMakeLists.txt new file mode 100644 index 00000000000..1906be3d599 --- /dev/null +++ b/src/main/target/FLYWOOF722PRO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(FLYWOOF722PRO) diff --git a/src/main/target/FLYWOOF722PRO/config.c b/src/main/target/FLYWOOF722PRO/config.c new file mode 100644 index 00000000000..ae386d4bc0a --- /dev/null +++ b/src/main/target/FLYWOOF722PRO/config.c @@ -0,0 +1,30 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include "platform.h" +#include "drivers/pwm_mapping.h" +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + + timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; + timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS; +} diff --git a/src/main/target/FLYWOOF722PRO/target.c b/src/main/target/FLYWOOF722PRO/target.c new file mode 100644 index 00000000000..e8b78979f95 --- /dev/null +++ b/src/main/target/FLYWOOF722PRO/target.c @@ -0,0 +1,48 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_ICM42605, DEVHW_ICM42605, ICM42605_SPI_BUS, ICM42605_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_ICM42605_ALIGN); +BUSDEV_REGISTER_SPI_TAG(busdev_BMI270, DEVHW_BMI270, BMI270_SPI_BUS, BMI270_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_BMI270_ALIGN); + +timerHardware_t timerHardware[] = { + //DEF_TIM(TIM8, CH1, PC6, TIM_USE_PPM,0, 0), // PPM&SBUS + + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 - D(1,2) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // S2 - D(1,4) + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 0), // S3 - D(1,6) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_OUTPUT_AUTO, 0, 0), // S4 - D(1,5) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 0), // S5 - D(2,4) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // S6 - D(2,1) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_OUTPUT_AUTO, 0, 0), // S7 - D(1,0) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_OUTPUT_AUTO, 0, 0), // S8 - D(1,3) + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED STRIP(1,5) +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYWOOF722PRO/target.h b/src/main/target/FLYWOOF722PRO/target.h new file mode 100644 index 00000000000..34d6de266c4 --- /dev/null +++ b/src/main/target/FLYWOOF722PRO/target.h @@ -0,0 +1,158 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#define TARGET_BOARD_IDENTIFIER "FWF7" +#define USBD_PRODUCT_STRING "FLYWOOF722PRO" + +/*** Indicators ***/ +#define LED0 PC15 +#define USE_BEEPER +#define BEEPER PC14 +#define BEEPER_INVERTED + +/*** SPI/I2C bus ***/ +#define USE_SPI +#define USE_SPI_DEVICE_1 // Gyro 1/2 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_CS_PIN PA4 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PA4 + + +#define USE_SPI_DEVICE_2 // MAX7456 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_SPI_DEVICE_3 // FLASH +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define USE_I2C +#define USE_I2C_DEVICE_1 // I2C pads +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +/*** Onboard flash ***/ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_CS_PIN PC13 +#define M25P16_SPI_BUS BUS_SPI3 + +/*** OSD ***/ +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_CS_PIN PB12 +#define MAX7456_SPI_BUS BUS_SPI2 + +/*** Serial ports ***/ +#define USB_IO +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 7 + +/*** BARO & MAG ***/ +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +/*** ADC ***/ +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC2 + +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + + +/*** PINIO ***/ +#define USE_PINIO +#define USE_PINIOBOX +#define PINIO1_PIN PB0 + +/*** LED STRIP ***/ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +/*** Default settings ***/ +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define CURRENT_METER_SCALE_DEFAULT 170 +#define SERIALRX_UART SERIAL_PORT_USART1 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS + +/*** Timer/PWM output ***/ +#define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define MAX_PWM_OUTPUT_PORTS 6 +#define USE_DSHOT +#define USE_ESC_SENSOR + +/*** Used pins ***/ +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c index 04c083ac94f..a68d3d3199f 100644 --- a/src/main/target/FLYWOOF745/target.c +++ b/src/main/target/FLYWOOF745/target.c @@ -32,20 +32,11 @@ timerHardware_t timerHardware[] = { // DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, // DMA2_ST6 - -#ifdef FLYWOOF745 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST2 - DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA2_ST7 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M3 , DMA1_ST7 - DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 - DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M5 , DMA2_ST2 -#else /* FLYWOOF745NANO */ DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // M1 , DMA1_ST7 DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 0), // M2 , DMA1_ST2 DEF_TIM(TIM1, CH1, PE9, TIM_USE_OUTPUT_AUTO, 0, 2), // M3 , DMA2_ST2 DEF_TIM(TIM1, CH2, PE11, TIM_USE_OUTPUT_AUTO, 0, 1), // M4 , DMA2_ST4 DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 0), // M5 , DMA2_ST7 -#endif DEF_TIM(TIM5, CH4, PA3, TIM_USE_OUTPUT_AUTO, 0, 0), // M6 , DMA1_ST1 DEF_TIM(TIM3, CH1, PB4, TIM_USE_OUTPUT_AUTO, 0, 0), // M7 , DMA1_ST4 DEF_TIM(TIM3, CH2, PB5, TIM_USE_OUTPUT_AUTO, 0, 0), // M8 , DMA1_ST5 diff --git a/src/main/target/FLYWOOF745/target.h b/src/main/target/FLYWOOF745/target.h index c2b1937c611..d00c2455945 100644 --- a/src/main/target/FLYWOOF745/target.h +++ b/src/main/target/FLYWOOF745/target.h @@ -41,6 +41,16 @@ #define MPU6000_CS_PIN SPI4_NSS_PIN #define MPU6000_SPI_BUS BUS_SPI4 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW270_DEG +#define BMI270_CS_PIN SPI4_NSS_PIN +#define BMI270_SPI_BUS BUS_SPI4 + +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW270_DEG +#define ICM42605_CS_PIN SPI4_NSS_PIN +#define ICM42605_SPI_BUS BUS_SPI4 + #define USB_IO #define USE_VCP #define VBUS_SENSING_ENABLED From 985fa2b440403674ae7c9abf9388abec810a70aa Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 2 Jun 2024 16:01:52 +0100 Subject: [PATCH 085/112] Add battery profile to programming framework --- src/main/programming/logic_condition.c | 4 ++++ src/main/programming/logic_condition.h | 1 + 2 files changed, 5 insertions(+) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 343db2989f5..f927ac18d1b 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -815,6 +815,10 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int return getConfigProfile() + 1; break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE: //int + return getConfigBatteryProfile() + 1; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int return currentMixerProfileIndex + 1; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 3a0c0a87b1b..5defafaee67 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -142,6 +142,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1 // 39 LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg // 40 LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE, // 41 + LOGIC_CONDITION_OPERAND_FLIGHT_BATT_PROFILE, // int // 42 } logicFlightOperands_e; typedef enum { From b9fd9e3f56d67a2ae4c265df4477d78554a218fa Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 2 Jun 2024 19:02:38 +0100 Subject: [PATCH 086/112] Consolidate the old `PIDProfile` and `Profile` in to `Control Profile` - Consolidate the old `PIDProfile` and `Profile` in to `Control Profile` - Improve consistency of the order `Control Profile`, `Mixer Profile`, and `Battery Profile` - Added `mixer_profile` to the `diff`/`dump` commands - Rename and update the profile documentation - Update the programming framework documentaion --- docs/{Profiles.md => Control Profiles.md} | 49 ++--- docs/Programming Framework.md | 232 +++++++++++----------- src/main/fc/cli.c | 70 ++++--- 3 files changed, 177 insertions(+), 174 deletions(-) rename docs/{Profiles.md => Control Profiles.md} (64%) diff --git a/docs/Profiles.md b/docs/Control Profiles.md similarity index 64% rename from docs/Profiles.md rename to docs/Control Profiles.md index 925de755f85..af41fed53b9 100644 --- a/docs/Profiles.md +++ b/docs/Control Profiles.md @@ -1,56 +1,59 @@ -# Profiles +# Control Profiles A profile is a set of configuration settings. -Currently three profiles are supported. The default profile is profile `1`. +Currently, INAV gives you three control profiles. The default control profile is `1`. -## Changing profiles +## Changing contorl profiles ### Stick Commands -Profiles can be selected using a GUI or the following stick combinations: +Control profiles can be selected using a GUI or the following stick combinations: -| Profile | Throttle | Yaw | Pitch | Roll | -| ------- | -------- | ----- | ------ | ------ | -| 1 | Down | Left | Middle | Left | -| 2 | Down | Left | Up | Middle | -| 3 | Down | Left | Middle | Right | +| Profile # | Throttle | Yaw | Pitch | Roll | +| -------- | -------- | ----- | ------ | ------ | +| 1 | Down | Left | Middle | Left | +| 2 | Down | Left | Up | Middle | +| 3 | Down | Left | Middle | Right | ### CLI -The CLI `profile` command can also be used to change profiles: +The CLI `control_profile` command can also be used to change control profiles: ``` -profile +control_profile ``` ### Programming (4.0.0 onwards) -You can change profiles using the programming frame work. This allows a lot of flexability in how you change profiles. +You can change control profiles using the programming frame work. This allows a lot of flexability in how you change profiles. For example, using a simple switch on channel 15. [![For example, using a simple switch](https://i.imgur.com/SS9CaaOl.png)](https://i.imgur.com/SS9CaaO.png) -Or using the speed to change profiles. In this example: -- when lower than 25 cm/s (basically not flying), profiles are not effected. -- Below 2682 cm/s (60 mph | 97 Km/h) use Profile 1 -- Above 5364 cm/s (120 mph | 193 Km/h) use Profile 3 -- Between 2683 and 5364 cm/s, use Profile 2 +Or using the speed to change control profiles. In this example: +- when lower than 25 cm/s (basically not flying), control profiles are not effected. +- Below 2682 cm/s (60 mph | 97 Km/h) use control profile 1 +- Above 5364 cm/s (120 mph | 193 Km/h) use control profile 3 +- Between 2683 and 5364 cm/s, use control profile 2 [![Using speed to change profiles](https://i.imgur.com/WjkuhhWl.png)](https://i.imgur.com/WjkuhhW.png) -#### Configurator use with profile changing logic. +> [!NOTE] +> From INAV 8.0, the programming framework operator is **Set Control Profile** and the **Flight** Operand is **Active Control Profile**. Pre-INAV 8.0, they were **Set Profile** and **Active Profile** respectively. -If you have logic conditions that change the profiles. You may find that if you manually change the profile using the drop down boxes in the top right of Configurator; that they switch back to a different profile. This is because the logic conditions are still running in the background. If this is the case, the simplest solutuion is to temporarily disable the switches that trigger the `set profile` operations. Remember to re-enable these switches after you have made your changes. +#### Configurator use with control profile changing logic. + +If you have logic conditions that change the profiles. You may find that if you manually change the control profile; using the drop down boxes in the top right of Configurator. That they switch back to a different control profile. This is because the logic conditions are still running in the background. If this is the case, the simplest solutuion is to temporarily disable the switches that trigger the `Set Control Profile` operations. Remember to re-enable these switches after you have made your changes. [![Disabled SET PROFILE switches](https://i.imgur.com/AeH9ll7l.png)](https://i.imgur.com/AeH9ll7.png) ## Profile Contents -The values contained within a profile can be seen by using the CLI `dump profile` command. +The values contained within a control profile can be seen by using the CLI `dump control_profile` command. e.g ``` -# dump profile +# dump control_profile -# profile -profile 1 +# control_profile +control_profile 1 set mc_p_pitch = 40 set mc_i_pitch = 30 diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 9f95fdb25d5..e0199cfd714 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -44,13 +44,13 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | Operation ID | Name | Notes | |---------------|-------------------------------|-------| -| 0 | TRUE | Always evaluates as true | -| 1 | EQUAL | Evaluates `false` if `false` or `0` | -| 2 | GREATER_THAN | `true` if `Operand A` is a higher value than `Operand B` | -| 3 | LOWER_THAN | `true` if `Operand A` is a lower value than `Operand B` | -| 4 | LOW | `true` if `<1333` | -| 5 | MID | `true` if `>=1333 and <=1666` | -| 6 | HIGH | `true` if `>1666` | +| 0 | True | Always evaluates as true | +| 1 | Equal (A=B) | Evaluates `false` if `false` or `0` | +| 2 | Greater Than (A>B) | `true` if `Operand A` is a higher value than `Operand B` | +| 3 | Lower Than (A=1333 and <=1666` | +| 6 | High | `true` if `>1666` | | 7 | AND | `true` if `Operand A` and `Operand B` are the same value or both `true` | | 8 | OR | `true` if `Operand A` and/or `OperandB` is `true` | | 9 | XOR | `true` if `Operand A` or `Operand B` is `true`, but not both | @@ -58,105 +58,109 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 11 | NOR | `true` if `Operand A` and `Operand B` are both `false` | | 12 | NOT | The boolean opposite to `Operand A` | | 13 | Sticky | `Operand A` is the activation operator, `Operand B` is the deactivation operator. After the activation is `true`, the operator will return `true` until Operand B is evaluated as `true`| -| 14 | Basic: Add | Add `Operand A` to `Operand B` and returns the result | -| 15 | Basic: Subtract | Substract `Operand B` from `Operand A` and returns the result | -| 16 | Basic: Multiply | Multiply `Operand A` by `Operand B` and returns the result | -| 17 | Basic: Divide | Divide `Operand A` by `Operand B` and returns the result | +| 14 | Basic: Add | Add `Operand A` to `Operand B` and returns the result | +| 15 | Basic: Subtract | Substract `Operand B` from `Operand A` and returns the result | +| 16 | Basic: Multiply | Multiply `Operand A` by `Operand B` and returns the result | +| 17 | Basic: Divide | Divide `Operand A` by `Operand B` and returns the result | | 18 | Set GVAR | Store value from `Operand B` into the Global Variable addressed by `Operand A`. Bear in mind, that operand `Global Variable` means: Value stored in Global Variable of an index! To store in GVAR 1 use `Value 1` not `Global Variable 1` | -| 19 | Increase GVAR | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | -| 20 | Decrease GVAR | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | +| 19 | Increase GVAR | Increase the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | +| 20 | Decrease GVAR | Decrease the GVAR indexed by `Operand A` (use `Value 1` for Global Variable 1) with value from `Operand B` | | 21 | Set IO Port | Set I2C IO Expander pin `Operand A` to value of `Operand B`. `Operand A` accepts values `0-7` and `Operand B` accepts `0` and `1` | -| 22 | OVERRIDE_ARMING_SAFETY | Allows the craft to arm on any angle even without GPS fix. WARNING: This bypasses all safety checks, even that the throttle is low, so use with caution. If you only want to check for certain conditions, such as arm without GPS fix. You will need to add logic conditions to check the throttle is low. | -| 23 | OVERRIDE_THROTTLE_SCALE | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. | -| 24 | SWAP_ROLL_YAW | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes | -| 25 | SET_VTX_POWER_LEVEL | Sets VTX power level. Accepted values are `0-3` for SmartAudio and `0-4` for Tramp protocol | -| 26 | INVERT_ROLL | Inverts ROLL axis input for PID/PIFF controller | -| 27 | INVERT_PITCH | Inverts PITCH axis input for PID/PIFF controller | -| 28 | INVERT_YAW | Inverts YAW axis input for PID/PIFF controller | -| 29 | OVERRIDE_THROTTLE | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle | -| 30 | SET_VTX_BAND | Sets VTX band. Accepted values are `1-5` | -| 31 | SET_VTX_CHANNEL | Sets VTX channel. Accepted values are `1-8` | -| 32 | SET_OSD_LAYOUT | Sets OSD layout. Accepted values are `0-3` | -| 33 | Trigonometry: Sine | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | -| 34 | Trigonometry: Cosine | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | -| 35 | Trigonometry: Tangent | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | -| 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled | -| 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled | -| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"| -| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. | -| 40 | Modulo | Modulo. Divide `Operand A` by `Operand B` and returns the remainder | -| 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. | -| 42 | SET_PROFILE | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change | -| 43 | Use Lowest Value | Finds the lowest value of `Operand A` and `Operand B` | -| 44 | Use Highest Value | Finds the highest value of `Operand A` and `Operand B` | -| 45 | FLIGHT_AXIS_ANGLE_OVERRIDE | Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees | -| 46 | FLIGHT_AXIS_RATE_OVERRIDE | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second | -| 47 | EDGE | Momentarily true when triggered by `Operand A`. `Operand A` is the activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. | -| 48 | DELAY | Delays activation after being triggered. This will return `true` when `Operand A` _is_ true, and the delay time in `Operand B` [ms] has been exceeded. | -| 49 | TIMER | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. | -| 50 | DELTA | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. | -| 51 | APPROX_EQUAL | `true` if `Operand B` is within 1% of `Operand A`. | -| 52 | LED_PIN_PWM | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes)| +| 22 | Override Arming Safety | Allows the craft to arm on any angle even without GPS fix. WARNING: This bypasses all safety checks, even that the throttle is low, so use with caution. If you only want to check for certain conditions, such as arm without GPS fix. You will need to add logic conditions to check the throttle is low. | +| 23 | Override Throttle Scale | Override throttle scale to the value defined by operand. Operand type `0` and value `50` means throttle will be scaled by 50%. | +| 24 | Swap Roll & Yaw | basically, when activated, yaw stick will control roll and roll stick will control yaw. Required for tail-sitters VTOL during vertical-horizonral transition when body frame changes | +| 25 | Set VTx Power Level | Sets VTX power level. Accepted values are `0-3` for SmartAudio and `0-4` for Tramp protocol | +| 26 | Invert Roll | Inverts ROLL axis input for PID/PIFF controller | +| 27 | Invert Pitch | Inverts PITCH axis input for PID/PIFF controller | +| 28 | Invert Yaw | Inverts YAW axis input for PID/PIFF controller | +| 29 | Override Throttlw | Override throttle value that is fed to the motors by mixer. Operand is scaled in us. `1000` means throttle cut, `1500` means half throttle | +| 30 | Set VTx Band | Sets VTX band. Accepted values are `1-5` | +| 31 | Set VTx Channel | Sets VTX channel. Accepted values are `1-8` | +| 32 | Set OSD Layout | Sets OSD layout. Accepted values are `0-3` | +| 33 | Trigonometry: Sine | Computes SIN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | +| 34 | Trigonometry: Cosine | Computes COS of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | +| 35 | Trigonometry: Tangent | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | +| 36 | Map Input | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled | +| 37 | Map Output | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled | +| 38 | Override RC Channel | Overrides channel set by `Operand A` to value of `Operand B`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"| +| 39 | Set Heading Target | Sets heading-hold target to `Operand A`, in centidegrees. Value wraps-around. | +| 40 | Modulo | Modulo. Divide `Operand A` by `Operand B` and returns the remainder | +| 41 | Override Loiter Radius | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. | +| 42 | Set Control Profile | Sets the active config profile (PIDFF/Rates/Filters/etc) to `Operand A`. `Operand A` must be a valid profile number, currently from 1 to 3. If not, the profile will not change | +| 43 | Use Lowest Value | Finds the lowest value of `Operand A` and `Operand B` | +| 44 | Use Highest Value | Finds the highest value of `Operand A` and `Operand B` | +| 45 | Flight Axis Angle Override | Sets the target attitude angle for axis. In other words, when active, it enforces Angle mode (Heading Hold for Yaw) on this axis (Angle mode does not have to be active). `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the angle in degrees | +| 46 | Flight Axis Rate Override | Sets the target rate (rotation speed) for axis. `Operand A` defines the axis: `0` - Roll, `1` - Pitch, `2` - Yaw. `Operand B` defines the rate in degrees per second | +| 47 | Edge | Momentarily true when triggered by `Operand A`. `Operand A` is the activation operator [`boolean`], `Operand B` _(Optional)_ is the time for the edge to stay active [ms]. After activation, operator will return `true` until the time in Operand B is reached. If a pure momentary edge is wanted. Just leave `Operand B` as the default `Value: 0` setting. | +| 48 | Delay | Delays activation after being triggered. This will return `true` when `Operand A` _is_ true, and the delay time in `Operand B` [ms] has been exceeded. | +| 49 | Timer | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. | +| 50 | Delta (|A| >= B) | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. | +| 51 | Approx Equals (A ~ B) | `true` if `Operand B` is within 1% of `Operand A`. | +| 52 | LED Pin PWM | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes). | +| 53 | Disable GPS Sensor Fix | Disables the GNSS sensor fix. For testing GNSS failure. | +| 54 | Mag calibration | Trigger a magnetometer calibration. | ### Operands | Operand Type | Name | Notes | |---------------|-----------------------|-------| -| 0 | VALUE | Value derived from `value` field | -| 1 | GET_RC_CHANNEL | `value` points to RC channel number, indexed from 1 | -| 2 | FLIGHT | `value` points to flight parameter table | -| 3 | FLIGHT_MODE | `value` points to flight modes table | -| 4 | LC | `value` points to other logic condition ID | -| 5 | GVAR | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 | -| 5 | PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 | - -#### FLIGHT - -| Operand Value | Name | Notes | -|---------------|-------------------------------|-------| -| 0 | ARM_TIMER | in `seconds` | -| 1 | HOME_DISTANCE | in `meters` | -| 2 | TRIP_DISTANCE | in `meters` | -| 3 | RSSI | | -| 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` | -| 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` | -| 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` | -| 7 | MAH_DRAWN | in `mAh` | -| 8 | GPS_SATS | | -| 9 | GROUD_SPEED | in `cm/s` | -| 10 | 3D_SPEED | in `cm/s` | -| 11 | AIR_SPEED | in `cm/s` | -| 12 | ALTITUDE | in `cm` | -| 13 | VERTICAL_SPEED | in `cm/s` | -| 14 | TROTTLE_POS | in `%` | -| 15 | ATTITUDE_ROLL | in `degrees` | -| 16 | ATTITUDE_PITCH | in `degrees` | -| 17 | IS_ARMED | boolean `0`/`1` | -| 18 | IS_AUTOLAUNCH | boolean `0`/`1` | -| 19 | IS_ALTITUDE_CONTROL | boolean `0`/`1` | -| 20 | IS_POSITION_CONTROL | boolean `0`/`1` | -| 21 | IS_EMERGENCY_LANDING | boolean `0`/`1` | -| 22 | IS_RTH | boolean `0`/`1` | -| 23 | IS_LANDING | boolean `0`/`1` | -| 24 | IS_FAILSAFE | boolean `0`/`1` | -| 25 | STABILIZED_ROLL | Roll PID controller output `[-500:500]` | -| 26 | STABILIZED_PITCH | Pitch PID controller output `[-500:500]` | -| 27 | STABILIZED_YAW | Yaw PID controller output `[-500:500]` | -| 28 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | -| 29 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol | -| 30 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | -| 31 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix | -| 32 | LOITER_RADIUS | The current loiter radius in cm. | -| 33 | ACTIVE_PROFILE | integer for the active config profile `[1..MAX_PROFILE_COUNT]` | -| 34 | BATT_CELLS | Number of battery cells detected | -| 35 | AGL_STATUS | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted | -| 36 | AGL | integer Above The Groud Altitude in `cm` | -| 37 | RANGEFINDER_RAW | integer raw distance provided by the rangefinder in `cm` | -| 38 | ACTIVE_MIXER_PROFILE | Which mixers are currently active (for vtol etc) | -| 39 | MIXER_TRANSITION_ACTIVE | Currently switching between mixers (quad to plane etc) | -| 40 | ATTITUDE_YAW | current heading (yaw) in `degrees` | -| 41 | FW Land Sate | integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare | +| 0 | Value | Value derived from `value` field | +| 1 | Get RC Channel | `value` points to RC channel number, indexed from 1 | +| 2 | Flight | `value` points to **Flight** Parameters table | +| 3 | Flight Mode | `value` points to **Flight_Mode** table | +| 4 | Logic Condition | `value` points to other logic condition ID | +| 5 | Get Global Variable | Value stored in Global Variable indexed by `value`. `GVAR 1` means: value in GVAR 1 | +| 5 | Programming PID | Output of a Programming PID indexed by `value`. `PID 1` means: value in PID 1 | +| 6 | Waypoints | `value` points to the **Waypoint** parameter table | + +#### Flight Parameters + +| Operand Value | Name | Notes | +|---------------|---------------------------------------|-------| +| 0 | ARM Timer [s] | Time since armed in `seconds` | +| 1 | Home Distance [m] | distance from home in `meters` | +| 2 | Trip distance [m] | Trip distance in `meters` | +| 3 | RSSI | | +| 4 | Vbat [centi-Volt] [1V = 100] | VBAT Voltage in `Volts * 100`, eg. `12.1V` is `1210` | +| 5 | Cell voltage [centi-Volt] [1V = 100] | Average cell voltage in `Volts * 100`, eg. `12.1V` is `1210` | +| 6 | Current [centi-Amp] [1A = 100] | Current in `Amps * 100`, eg. `9A` is `900` | +| 7 | Current drawn [mAh] | Total used current in `mAh` | +| 8 | GPS Sats | | +| 9 | Ground speed [cm/s] | Ground speed in `cm/s` | +| 10 | 3D speed [cm/s] | 3D speed in `cm/s` | +| 11 | Air speed [cm/s] | Air speed in `cm/s` | +| 12 | Altitude [cm] | Altitude in `cm` | +| 13 | Vertical speed [cm/s] | Vertical speed in `cm/s` | +| 14 | Throttle position [%] | Throttle position in `%` | +| 15 | Roll [deg] | Roll attitude in `degrees` | +| 16 | Pitch [deg] | Pitch attitude in `degrees` | +| 17 | Is Armed | Is the system armed? boolean `0`/`1` | +| 18 | Is Autolaunch | Is auto launch active? boolean `0`/`1` | +| 19 | Is Controlling Altitude | Is altitude being controlled? boolean `0`/`1` | +| 20 | Is Controlling Position | Is the position being controlled? boolean `0`/`1` | +| 21 | Is Emergency Landing | Is the aircraft emergency landing? boolean `0`/`1` | +| 22 | Is RTH | Is RTH active? boolean `0`/`1` | +| 23 | Is Landing | Is the aircaft automatically landing? boolean `0`/`1` | +| 24 | Is Failsafe | Is the flight controller in a failsafe? boolean `0`/`1` | +| 25 | Stabilized Roll | Roll PID controller output `[-500:500]` | +| 26 | Stabilized Pitch | Pitch PID controller output `[-500:500]` | +| 27 | Stabilized Yaw | Yaw PID controller output `[-500:500]` | +| 28 | 3D home distance [m] | 3D distance to home in `meters`. Calculated from Home distance and Altitude using Pythagorean theorem | +| 29 | CRSF LQ | Link quality as returned by the CRSF protocol | +| 30 | CRSF SNR | SNR as returned by the CRSF protocol | +| 31 | GPS Valid Fix | Boolean `0`/`1`. True when the GPS has a valid 3D Fix | +| 32 | Loiter Radius [cm] | The current loiter radius in cm. | +| 33 | Active Control Profile | Integer for the active config profile `[1..MAX_PROFILE_COUNT]` | +| 34 | Battery cells | Number of battery cells detected | +| 35 | AGL status [0/1] | Boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted | +| 36 | AGL [cm] | Integer altitude above The Groud Altitude in `cm` | +| 37 | Rangefinder [cm] | Integer raw distance provided by the rangefinder in `cm` | +| 38 | Active Mixer Profile | Which mixer is currently active (for vtol etc) | +| 39 | Mixer Transition Active | Boolean `0`/`1`. Are we currently switching between mixers (quad to plane etc) | +| 40 | Yaw [deg] | Current heading (yaw) in `degrees` | +| 41 | FW Land Sate | Integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare | +| 42 | Current battery profile | The active battery profile. Integer `[1..MAX_PROFILE_COUNT]` | #### FLIGHT_MODE @@ -164,22 +168,22 @@ The flight mode operands return `true` when the mode is active. These are modes | Operand Value | Name | Notes | |---------------|-------------------|-------| -| 0 | FAILSAFE | `true` when a **Failsafe** state has been triggered. | -| 1 | MANUAL | `true` when you are in the **Manual** flight mode. | +| 0 | Failsafe | `true` when a **Failsafe** state has been triggered. | +| 1 | Manual | `true` when you are in the **Manual** flight mode. | | 2 | RTH | `true` when you are in the **Return to Home** flight mode. | -| 3 | POSHOLD | `true` when you are in the **Position Hold** or **Loiter** flight modes. | -| 4 | CRUISE | `true` when you are in the **Cruise** flight mode. | -| 5 | ALTHOLD | `true` when you the **Altitude Hold** flight mode modifier is active. | -| 6 | ANGLE | `true` when you are in the **Angle** flight mode. | -| 7 | HORIZON | `true` when you are in the **Horizon** flight mode. | -| 8 | AIR | `true` when you the **Airmode** flight mode modifier is active. | -| 9 | USER1 | `true` when the **USER 1** mode is active. | -| 10 | USER2 | `true` when the **USER 2** mode is active. | -| 11 | COURSE_HOLD | `true` when you are in the **Course Hold** flight mode. | -| 12 | USER3 | `true` when the **USER 3** mode is active. | -| 13 | USER4 | `true` when the **USER 4** mode is active. | -| 14 | ACRO | `true` when you are in the **Acro** flight mode. | -| 15 | WAYPOINT_MISSION | `true` when you are in the **WP Mission** flight mode. | +| 3 | Position Hold | `true` when you are in the **Position Hold** or **Loiter** flight modes. | +| 4 | Cruise | `true` when you are in the **Cruise** flight mode. | +| 5 | Altitude Hold | `true` when you the **Altitude Hold** flight mode modifier is active. | +| 6 | Angle | `true` when you are in the **Angle** flight mode. | +| 7 | Horizon | `true` when you are in the **Horizon** flight mode. | +| 8 | Air | `true` when you the **Airmode** flight mode modifier is active. | +| 9 | USER 1 | `true` when the **USER 1** mode is active. | +| 10 | USER 2 | `true` when the **USER 2** mode is active. | +| 11 | Course Hold | `true` when you are in the **Course Hold** flight mode. | +| 12 | USER 3 | `true` when the **USER 3** mode is active. | +| 13 | USER 4 | `true` when the **USER 4** mode is active. | +| 14 | Acro | `true` when you are in the **Acro** flight mode. | +| 15 | Waypoint Mission | `true` when you are in the **WP Mission** flight mode. | #### WAYPOINTS @@ -335,5 +339,3 @@ choose *value* and enter the channel number. Choosing "get RC value" is a common which does something other than what you probably want. ![screenshot of override an RC channel with a value](./assets/images/ipf_set_get_rc_channel.png) - - diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 3bedf7d324d..aba1f58b905 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -293,7 +293,7 @@ static void cliPutp(void *p, char ch) typedef enum { DUMP_MASTER = (1 << 0), - DUMP_PROFILE = (1 << 1), + DUMP_CONTROL_PROFILE = (1 << 1), DUMP_BATTERY_PROFILE = (1 << 2), DUMP_MIXER_PROFILE = (1 << 3), DUMP_ALL = (1 << 4), @@ -3336,30 +3336,30 @@ static void cliPlaySound(char *cmdline) beeper(beeperModeForTableIndex(i)); } -static void cliProfile(char *cmdline) +static void cliControlProfile(char *cmdline) { // CLI profile index is 1-based if (isEmpty(cmdline)) { - cliPrintLinef("profile %d", getConfigProfile() + 1); + cliPrintLinef("control_profile %d", getConfigProfile() + 1); return; } else { const int i = fastA2I(cmdline) - 1; if (i >= 0 && i < MAX_PROFILE_COUNT) { setConfigProfileAndWriteEEPROM(i); - cliProfile(""); + cliControlProfile(""); } } } -static void cliDumpProfile(uint8_t profileIndex, uint8_t dumpMask) +static void cliDumpControlProfile(uint8_t profileIndex, uint8_t dumpMask) { if (profileIndex >= MAX_PROFILE_COUNT) { // Faulty values return; } setConfigProfile(profileIndex); - cliPrintHashLine("profile"); - cliPrintLinef("profile %d\r\n", getConfigProfile() + 1); + cliPrintHashLine("control_profile"); + cliPrintLinef("control_profile %d\r\n", getConfigProfile() + 1); dumpAllValues(PROFILE_VALUE, dumpMask); dumpAllValues(CONTROL_RATE_VALUE, dumpMask); dumpAllValues(EZ_TUNE_VALUE, dumpMask); @@ -3981,12 +3981,12 @@ static void printConfig(const char *cmdline, bool doDiff) const char *options; if ((options = checkCommand(cmdline, "master"))) { dumpMask = DUMP_MASTER; // only - } else if ((options = checkCommand(cmdline, "profile"))) { - dumpMask = DUMP_PROFILE; // only - } else if ((options = checkCommand(cmdline, "battery_profile"))) { - dumpMask = DUMP_BATTERY_PROFILE; // only + } else if ((options = checkCommand(cmdline, "control_profile"))) { + dumpMask = DUMP_CONTROL_PROFILE; // only } else if ((options = checkCommand(cmdline, "mixer_profile"))) { dumpMask = DUMP_MIXER_PROFILE; // only + } else if ((options = checkCommand(cmdline, "battery_profile"))) { + dumpMask = DUMP_BATTERY_PROFILE; // only } else if ((options = checkCommand(cmdline, "all"))) { dumpMask = DUMP_ALL; // all profiles and rates } else { @@ -3997,16 +3997,16 @@ static void printConfig(const char *cmdline, bool doDiff) dumpMask = dumpMask | DO_DIFF; } - const int currentProfileIndexSave = getConfigProfile(); - const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); + const int currentControlProfileIndexSave = getConfigProfile(); const int currentMixerProfileIndexSave = getConfigMixerProfile(); + const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); backupConfigs(); // reset all configs to defaults to do differencing resetConfigs(); // restore the profile indices, since they should not be reset for proper comparison - setConfigProfile(currentProfileIndexSave); - setConfigBatteryProfile(currentBatteryProfileIndexSave); + setConfigProfile(currentControlProfileIndexSave); setConfigMixerProfile(currentMixerProfileIndexSave); + setConfigBatteryProfile(currentBatteryProfileIndexSave); if (checkCommand(options, "showdefaults")) { dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values @@ -4128,25 +4128,25 @@ static void printConfig(const char *cmdline, bool doDiff) if (dumpMask & DUMP_ALL) { // dump all profiles - const int currentProfileIndexSave = getConfigProfile(); - const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); + const int currentControlProfileIndexSave = getConfigProfile(); const int currentMixerProfileIndexSave = getConfigMixerProfile(); + const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); + for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { + cliDumpControlProfile(ii, dumpMask); + } for (int ii = 0; ii < MAX_MIXER_PROFILE_COUNT; ++ii) { cliDumpMixerProfile(ii, dumpMask); } - for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { - cliDumpProfile(ii, dumpMask); - } for (int ii = 0; ii < MAX_BATTERY_PROFILE_COUNT; ++ii) { cliDumpBatteryProfile(ii, dumpMask); } - setConfigProfile(currentProfileIndexSave); - setConfigBatteryProfile(currentBatteryProfileIndexSave); + setConfigProfile(currentControlProfileIndexSave); setConfigMixerProfile(currentMixerProfileIndexSave); + setConfigBatteryProfile(currentBatteryProfileIndexSave); cliPrintHashLine("restore original profile selection"); + cliPrintLinef("profile %d", currentControlProfileIndexSave + 1); cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1); - cliPrintLinef("profile %d", currentProfileIndexSave + 1); cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1); #ifdef USE_CLI_BATCH @@ -4154,18 +4154,19 @@ static void printConfig(const char *cmdline, bool doDiff) #endif } else { // dump just the current profiles + cliDumpControlProfile(getConfigProfile(), dumpMask); cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); - cliDumpProfile(getConfigProfile(), dumpMask); cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask); } } + + if (dumpMask & DUMP_CONTROL_PROFILE) { + cliDumpControlProfile(getConfigProfile(), dumpMask); + } + if (dumpMask & DUMP_MIXER_PROFILE) { cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); } - - if (dumpMask & DUMP_PROFILE) { - cliDumpProfile(getConfigProfile(), dumpMask); - } if (dumpMask & DUMP_BATTERY_PROFILE) { cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask); @@ -4279,9 +4280,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("defaults", "reset to defaults and reboot", NULL, cliDefaults), CLI_COMMAND_DEF("dfu", "DFU mode on reboot", NULL, cliDfu), CLI_COMMAND_DEF("diff", "list configuration changes from default", - "[master|battery_profile|profile|rates|all] {showdefaults}", cliDiff), + "[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDiff), CLI_COMMAND_DEF("dump", "dump configuration", - "[master|battery_profile|profile|rates|all] {showdefaults}", cliDump), + "[master|battery_profile|control_profile|mixer_profile|rates|all] {showdefaults}", cliDump), #ifdef USE_RX_ELERES CLI_COMMAND_DEF("eleres_bind", NULL, NULL, cliEleresBind), #endif // USE_RX_ELERES @@ -4322,12 +4323,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("msc", "switch into msc mode", NULL, cliMsc), #endif CLI_COMMAND_DEF("play_sound", NULL, "[]\r\n", cliPlaySound), - CLI_COMMAND_DEF("profile", "change profile", - "[]", cliProfile), - CLI_COMMAND_DEF("battery_profile", "change battery profile", - "[]", cliBatteryProfile), - CLI_COMMAND_DEF("mixer_profile", "change mixer profile", - "[]", cliMixerProfile), + CLI_COMMAND_DEF("control_profile", "change control profile", "[]", cliControlProfile), + CLI_COMMAND_DEF("mixer_profile", "change mixer profile", "[]", cliMixerProfile), + CLI_COMMAND_DEF("battery_profile", "change battery profile", "[]", cliBatteryProfile), CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), #if defined(USE_SAFE_HOME) From c012de6ca6295d16ac5dc8074f9476e9e77037a9 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 2 Jun 2024 19:59:24 +0100 Subject: [PATCH 087/112] Make `battery_capacity_unit` a global setting Remove `battery_capacity_unit` from battery profiles. --- docs/Battery.md | 3 --- src/main/fc/fc_msp.c | 16 ++++++++-------- src/main/fc/settings.yaml | 12 ++++++------ src/main/flight/rth_estimator.c | 2 +- src/main/io/osd.c | 6 +++--- src/main/sensors/battery.c | 9 +++++---- src/main/sensors/battery_config_structs.h | 21 +++++++++++---------- 7 files changed, 34 insertions(+), 35 deletions(-) diff --git a/docs/Battery.md b/docs/Battery.md index f6bdbd50391..e519c325410 100644 --- a/docs/Battery.md +++ b/docs/Battery.md @@ -201,7 +201,6 @@ Up to 3 battery profiles are supported. You can select the battery profile from - `vbat_max_cell_voltage` - `vbat_warning_cell_voltage` - `vbat_min_cell_voltage` -- `battery_capacity_unit` - `battery_capacity` - `battery_capacity_warning` - `battery_capacity_critical` @@ -253,7 +252,6 @@ feature BAT_PROF_AUTOSWITCH battery_profile 1 set bat_cells = 3 -set battery_capacity_unit = MAH set battery_capacity = 2200 set battery_capacity_warning = 440 set battery_capacity_critical = 220 @@ -262,7 +260,6 @@ set battery_capacity_critical = 220 battery_profile 2 set bat_cells = 4 -set battery_capacity_unit = MAH set battery_capacity = 1500 set battery_capacity_warning = 300 set battery_capacity_critical = 150 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 4ae42184816..2109d3e08a9 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -856,7 +856,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU32(dst, currentBatteryProfile->capacity.value); sbufWriteU32(dst, currentBatteryProfile->capacity.warning); sbufWriteU32(dst, currentBatteryProfile->capacity.critical); - sbufWriteU8(dst, currentBatteryProfile->capacity.unit); + sbufWriteU8(dst, batteryMetersConfig()->capacity_unit); break; case MSP2_INAV_MISC2: @@ -895,7 +895,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU32(dst, currentBatteryProfile->capacity.value); sbufWriteU32(dst, currentBatteryProfile->capacity.warning); sbufWriteU32(dst, currentBatteryProfile->capacity.critical); - sbufWriteU8(dst, currentBatteryProfile->capacity.unit); + sbufWriteU8(dst, batteryMetersConfig()->capacity_unit); break; #ifdef USE_GPS @@ -2081,13 +2081,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) currentBatteryProfileMutable->capacity.value = sbufReadU32(src); currentBatteryProfileMutable->capacity.warning = sbufReadU32(src); currentBatteryProfileMutable->capacity.critical = sbufReadU32(src); - currentBatteryProfileMutable->capacity.unit = sbufReadU8(src); + batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src); if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) { batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; return MSP_RESULT_ERROR; } - if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) { - currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH; + if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) { + batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH; return MSP_RESULT_ERROR; } } else @@ -2120,13 +2120,13 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) currentBatteryProfileMutable->capacity.value = sbufReadU32(src); currentBatteryProfileMutable->capacity.warning = sbufReadU32(src); currentBatteryProfileMutable->capacity.critical = sbufReadU32(src); - currentBatteryProfileMutable->capacity.unit = sbufReadU8(src); + batteryMetersConfigMutable()->capacity_unit = sbufReadU8(src); if ((batteryMetersConfig()->voltageSource != BAT_VOLTAGE_RAW) && (batteryMetersConfig()->voltageSource != BAT_VOLTAGE_SAG_COMP)) { batteryMetersConfigMutable()->voltageSource = BAT_VOLTAGE_RAW; return MSP_RESULT_ERROR; } - if ((currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MAH) && (currentBatteryProfile->capacity.unit != BAT_CAPACITY_UNIT_MWH)) { - currentBatteryProfileMutable->capacity.unit = BAT_CAPACITY_UNIT_MAH; + if ((batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MAH) && (batteryMetersConfig()->capacity_unit != BAT_CAPACITY_UNIT_MWH)) { + batteryMetersConfigMutable()->capacity_unit = BAT_CAPACITY_UNIT_MAH; return MSP_RESULT_ERROR; } } else diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d9aa83c93cf..3c192495751 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -890,6 +890,12 @@ groups: condition: USE_ADC min: 0 max: 65535 + - name: battery_capacity_unit + description: "Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour)." + default_value: "MAH" + field: capacity_unit + table: bat_capacity_unit + type: uint8_t - name: current_meter_scale description: "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." default_value: :target @@ -996,12 +1002,6 @@ groups: field: capacity.critical min: 0 max: 4294967295 - - name: battery_capacity_unit - description: "Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour)." - default_value: "MAH" - field: capacity.unit - table: bat_capacity_unit - type: uint8_t - name: controlrate_profile description: "Control rate profile to switch to when the battery profile is selected, 0 to disable and keep the currently selected control rate profile" default_value: 0 diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 708c71bd1cf..e0ca0fba20d 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -222,7 +222,7 @@ float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { // check requirements const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn(); - const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &¤tBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0; + const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MWH && currentBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0; const bool isNavigationOK = navigationPositionEstimateIsHealthy() && isImuHeadingValid(); if (!(areBatterySettingsOK && areRTHEstimatorSettingsOK && isNavigationOK)) { diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 34135ce2fd7..a3a298962a3 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1780,7 +1780,7 @@ static bool osdDrawSingleElement(uint8_t item) tfp_sprintf(buff, " NA"); else if (!batteryWasFullWhenPluggedIn()) tfp_sprintf(buff, " NF"); - else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) { + else if (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH) { uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value #ifndef DISABLE_MSP_DJI_COMPAT // IF DJICOMPAT is not supported, there's no need to check for it @@ -1803,11 +1803,11 @@ static bool osdDrawSingleElement(uint8_t item) buff[mah_digits + 1] = '\0'; unitsDrawn = true; } - } else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH + } else // batteryMetersConfig()->capacityUnit == BAT_CAPACITY_UNIT_MWH osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3, false); if (!unitsDrawn) { - buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; + buff[4] = batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; buff[5] = '\0'; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 2ca7024694e..a29fa2e07cf 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -97,7 +97,7 @@ static int32_t mWhDrawn = 0; // energy (milliWatt hours) draw batteryState_e batteryState; const batteryProfile_t *currentBatteryProfile; -PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(batteryProfile_t, MAX_BATTERY_PROFILE_COUNT, batteryProfiles, PG_BATTERY_PROFILES, 3); void pgResetFn_batteryProfiles(batteryProfile_t *instance) { @@ -118,7 +118,6 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) .value = SETTING_BATTERY_CAPACITY_DEFAULT, .warning = SETTING_BATTERY_CAPACITY_WARNING_DEFAULT, .critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT, - .unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT, }, .controlRateProfile = 0, @@ -167,7 +166,7 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) } } -PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_BATTERY_METERS_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_BATTERY_METERS_CONFIG, 2); PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, @@ -186,6 +185,8 @@ PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, .voltageSource = SETTING_BAT_VOLTAGE_SRC_DEFAULT, + .capacity_unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT, + .cruise_power = SETTING_CRUISE_POWER_DEFAULT, .idle_power = SETTING_IDLE_POWER_DEFAULT, .rth_energy_margin = SETTING_RTH_ENERGY_MARGIN_DEFAULT, @@ -405,7 +406,7 @@ void batteryUpdate(timeUs_t timeDelta) if ((currentBatteryProfile->capacity.value > 0) && batteryFullWhenPluggedIn) { uint32_t capacityDiffBetweenFullAndEmpty = currentBatteryProfile->capacity.value - currentBatteryProfile->capacity.critical; - int32_t drawn = (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH ? mWhDrawn : mAhDrawn); + int32_t drawn = (batteryMetersConfig()->capacity_unit == BAT_CAPACITY_UNIT_MWH ? mWhDrawn : mAhDrawn); batteryRemainingCapacity = (drawn > (int32_t)capacityDiffBetweenFullAndEmpty ? 0 : capacityDiffBetweenFullAndEmpty - drawn); } diff --git a/src/main/sensors/battery_config_structs.h b/src/main/sensors/battery_config_structs.h index 81eafef44ad..829ac8d8794 100644 --- a/src/main/sensors/battery_config_structs.h +++ b/src/main/sensors/battery_config_structs.h @@ -61,16 +61,18 @@ typedef struct batteryMetersConfig_s { #endif struct { - int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A - int16_t offset; // offset of the current sensor in millivolt steps - currentSensor_e type; // type of current meter used, either ADC or virtual + int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A + int16_t offset; // offset of the current sensor in millivolt steps + currentSensor_e type; // type of current meter used, either ADC or virtual } current; batVoltageSource_e voltageSource; - uint32_t cruise_power; // power drawn by the motor(s) at cruise throttle/speed (cW) - uint16_t idle_power; // power drawn by the system when the motor(s) are stopped (cW) - uint8_t rth_energy_margin; // Energy that should be left after RTH (%), used for remaining time/distance before RTH + batCapacityUnit_e capacity_unit; // Describes unit of capacity.value, capacity.warning and capacity.critical + + uint32_t cruise_power; // power drawn by the motor(s) at cruise throttle/speed (cW) + uint16_t idle_power; // power drawn by the system when the motor(s) are stopped (cW) + uint8_t rth_energy_margin; // Energy that should be left after RTH (%), used for remaining time/distance before RTH float throttle_compensation_weight; @@ -90,10 +92,9 @@ typedef struct batteryProfile_s { #endif struct { - uint32_t value; // mAh or mWh (see capacity.unit) - uint32_t warning; // mAh or mWh (see capacity.unit) - uint32_t critical; // mAh or mWh (see capacity.unit) - batCapacityUnit_e unit; // Describes unit of capacity.value, capacity.warning and capacity.critical + uint32_t value; // mAh or mWh (see batteryMetersConfig()->capacity_unit) + uint32_t warning; // mAh or mWh (see batteryMetersConfig()->capacity_unit) + uint32_t critical; // mAh or mWh (see batteryMetersConfig()->capacity_unit) } capacity; uint8_t controlRateProfile; From 18c7ec44a51aedd3fcb3baae4bee381a5619772b Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sun, 2 Jun 2024 20:02:12 +0100 Subject: [PATCH 088/112] Fix missed CLI command in the diff --- src/main/fc/cli.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index aba1f58b905..18243fdefce 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -4145,7 +4145,7 @@ static void printConfig(const char *cmdline, bool doDiff) setConfigBatteryProfile(currentBatteryProfileIndexSave); cliPrintHashLine("restore original profile selection"); - cliPrintLinef("profile %d", currentControlProfileIndexSave + 1); + cliPrintLinef("control_profile %d", currentControlProfileIndexSave + 1); cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1); cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1); From b8707b76cb0abed7372b5564a3b070e966cf6758 Mon Sep 17 00:00:00 2001 From: flywoo Date: Tue, 4 Jun 2024 11:09:23 +0800 Subject: [PATCH 089/112] Update F722PRO default features --- src/main/target/FLYWOOF722PRO/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/FLYWOOF722PRO/target.h b/src/main/target/FLYWOOF722PRO/target.h index 34d6de266c4..ae12a324701 100644 --- a/src/main/target/FLYWOOF722PRO/target.h +++ b/src/main/target/FLYWOOF722PRO/target.h @@ -140,6 +140,7 @@ /*** Default settings ***/ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define CURRENT_METER_SCALE_DEFAULT 170 #define SERIALRX_UART SERIAL_PORT_USART1 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL From b68619c6c8b3b096cfafbcaac53a80332b172cef Mon Sep 17 00:00:00 2001 From: flywoo Date: Tue, 4 Jun 2024 14:49:28 +0800 Subject: [PATCH 090/112] Update F722PRO target.h --- src/main/target/FLYWOOF722PRO/config.c | 1 - src/main/target/FLYWOOF722PRO/target.h | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/target/FLYWOOF722PRO/config.c b/src/main/target/FLYWOOF722PRO/config.c index ae386d4bc0a..57ecfa64ff6 100644 --- a/src/main/target/FLYWOOF722PRO/config.c +++ b/src/main/target/FLYWOOF722PRO/config.c @@ -24,7 +24,6 @@ void targetConfiguration(void) { pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; - timerOverridesMutable(timer2id(TIM3))->outputMode = OUTPUT_MODE_MOTORS; timerOverridesMutable(timer2id(TIM2))->outputMode = OUTPUT_MODE_MOTORS; } diff --git a/src/main/target/FLYWOOF722PRO/target.h b/src/main/target/FLYWOOF722PRO/target.h index ae12a324701..3f11b44e317 100644 --- a/src/main/target/FLYWOOF722PRO/target.h +++ b/src/main/target/FLYWOOF722PRO/target.h @@ -148,7 +148,7 @@ /*** Timer/PWM output ***/ #define USE_SERIAL_4WAY_BLHELI_INTERFACE -#define MAX_PWM_OUTPUT_PORTS 6 +#define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT #define USE_ESC_SENSOR From 033869b9da0c65828c518dba24be5ae468169e1f Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 6 Jun 2024 12:04:29 +0100 Subject: [PATCH 091/112] Update settings.yaml --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3c192495751..4a22ecb8711 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2866,7 +2866,7 @@ groups: description: "Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G = 981 cm/s/s" default_value: 1863 field: fw.launch_accel_thresh - min: 1500 + min: 1350 max: 20000 - name: nav_fw_launch_max_angle description: "Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]" From 4fb7204d378c86f7cfe164097ddfe104b73f0ef8 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Thu, 6 Jun 2024 12:06:26 +0100 Subject: [PATCH 092/112] Update Settings.md --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index a1c339c1d8c..3d0758ab2a2 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2958,7 +2958,7 @@ Forward acceleration threshold for bungee launch or throw launch [cm/s/s], 1G = | Default | Min | Max | | --- | --- | --- | -| 1863 | 1500 | 20000 | +| 1863 | 1350 | 20000 | --- From eabca5e0a5dfc5cb5e072c458fb39a4f980cb29b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 7 Jun 2024 12:38:18 +0200 Subject: [PATCH 093/112] Disable adaptive filter for SITL --- src/main/target/SITL/target.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 3fbe45a5cab..048626ad8dd 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -97,6 +97,7 @@ #undef USE_BRUSHED_ESC_AUTODETECT #undef USE_SERIAL_4WAY_BLHELI_BOOTLOADER #undef USE_SERIAL_4WAY_SK_BOOTLOADER +#undef USE_ADAPTIVE_FILTER #undef USE_I2C #undef USE_SPI From f8e72b8afd20f7b914069012665169da7160cad2 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 7 Jun 2024 12:57:42 +0200 Subject: [PATCH 094/112] Enable task only when adaptive filter is enabled --- src/main/fc/fc_tasks.c | 2 +- src/main/sensors/gyro.c | 2 +- src/main/sensors/gyro.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 8da10280a9d..0fd239127c2 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -428,7 +428,7 @@ void fcTasksInit(void) #endif #ifdef USE_ADAPTIVE_FILTER - setTaskEnabled(TASK_ADAPTIVE_FILTER, true); + setTaskEnabled(TASK_ADAPTIVE_FILTER, (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE)); #endif #if defined(SITL_BUILD) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 7f7d1d171d9..2b0b7d8c9bd 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -130,8 +130,8 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .adaptiveFilterHpfHz = SETTING_GYRO_ADAPTIVE_FILTER_HPF_HZ_DEFAULT, .adaptiveFilterIntegratorThresholdHigh = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_HIGH_DEFAULT, .adaptiveFilterIntegratorThresholdLow = SETTING_GYRO_ADAPTIVE_FILTER_INTEGRATOR_THRESHOLD_LOW_DEFAULT, - .gyroFilterMode = SETTING_GYRO_FILTER_MODE_DEFAULT, #endif + .gyroFilterMode = SETTING_GYRO_FILTER_MODE_DEFAULT, ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 7f767e370d1..910dac2ea00 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -100,8 +100,8 @@ typedef struct gyroConfig_s { float adaptiveFilterHpfHz; float adaptiveFilterIntegratorThresholdHigh; float adaptiveFilterIntegratorThresholdLow; - uint8_t gyroFilterMode; #endif + uint8_t gyroFilterMode; } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); From 08d46dd4712a9f38eab43e256172a2dbf9a9f23c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 7 Jun 2024 13:39:37 +0200 Subject: [PATCH 095/112] Fix compilation error in config.c --- src/main/fc/config.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index ac98eca02cb..dc249df059b 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -191,6 +191,7 @@ uint32_t getGyroLooptime(void) void validateAndFixConfig(void) { +#ifdef USE_ADAPTIVE_FILTER // gyroConfig()->adaptiveFilterMinHz has to be at least 5 units lower than gyroConfig()->gyro_main_lpf_hz if (gyroConfig()->adaptiveFilterMinHz + 5 > gyroConfig()->gyro_main_lpf_hz) { gyroConfigMutable()->adaptiveFilterMinHz = gyroConfig()->gyro_main_lpf_hz - 5; @@ -199,6 +200,7 @@ void validateAndFixConfig(void) if (gyroConfig()->adaptiveFilterMaxHz - 5 < gyroConfig()->gyro_main_lpf_hz) { gyroConfigMutable()->adaptiveFilterMaxHz = gyroConfig()->gyro_main_lpf_hz + 5; } +#endif if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) { accelerometerConfigMutable()->acc_notch_hz = 0; From bf530ecd6fcedeebb8f178d24588df3f59d0dd1d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 8 Jun 2024 13:55:07 +0200 Subject: [PATCH 096/112] Lower new rates slightly --- docs/Settings.md | 8 ++++---- src/main/fc/settings.yaml | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 40490808c83..265500e6720 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2758,7 +2758,7 @@ Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no speci | Default | Min | Max | | --- | --- | --- | -| 600 | 10 | 2000 | +| 500 | 10 | 2000 | --- @@ -3418,7 +3418,7 @@ Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mod | Default | Min | Max | | --- | --- | --- | -| 1000 | 10 | 2000 | +| 750 | 10 | 2000 | --- @@ -3438,7 +3438,7 @@ Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor onl | Default | Min | Max | | --- | --- | --- | -| 1200 | 10 | 2000 | +| 1000 | 10 | 2000 | --- @@ -3478,7 +3478,7 @@ Maximum banking angle (deg) that multicopter navigation is allowed to set. Machi | Default | Min | Max | | --- | --- | --- | -| 40 | 15 | 45 | +| 35 | 15 | 45 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 71964891ce9..f5b52be72a8 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2510,7 +2510,7 @@ groups: table: nav_fw_wp_turn_smoothing - name: nav_auto_speed description: "Speed in fully autonomous modes (RTH, WP) [cm/s]. Used for WP mode when no specific WP speed set. [Multirotor only]" - default_value: 600 + default_value: 500 field: general.auto_speed min: 10 max: 2000 @@ -2522,13 +2522,13 @@ groups: max: 50 - name: nav_max_auto_speed description: "Maximum speed allowed in fully autonomous modes (RTH, WP) [cm/s] [Multirotor only]" - default_value: 1200 + default_value: 1000 field: general.max_auto_speed min: 10 max: 2000 - name: nav_manual_speed description: "Maximum speed allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]" - default_value: 1000 + default_value: 750 field: general.max_manual_speed min: 10 max: 2000 @@ -2704,7 +2704,7 @@ groups: max: 120 - name: nav_mc_bank_angle description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude" - default_value: 40 + default_value: 35 field: mc.max_bank_angle min: 15 max: 45 From 58faed455603d7c8bcc7c8744146ef0808d58df5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Tue, 11 Jun 2024 22:07:56 +0200 Subject: [PATCH 097/112] Min and max values made defaults invalid Old values failed checks for +/-5Hz from main gyro lpf frequency --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 7177d4a8ed1..2367cc8f1ab 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1618,7 +1618,7 @@ Maximum frequency for adaptive filter | Default | Min | Max | | --- | --- | --- | -| 150 | 100 | 500 | +| 150 | 0 | 505 | --- @@ -1628,7 +1628,7 @@ Minimum frequency for adaptive filter | Default | Min | Max | | --- | --- | --- | -| 50 | 30 | 250 | +| 50 | 0 | 250 | --- diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 886fbfad5f4..cfe40bb400f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -344,15 +344,15 @@ groups: description: "Minimum frequency for adaptive filter" default_value: 50 field: adaptiveFilterMinHz - min: 30 + min: 0 max: 250 condition: USE_ADAPTIVE_FILTER - name: gyro_adaptive_filter_max_hz description: "Maximum frequency for adaptive filter" default_value: 150 field: adaptiveFilterMaxHz - min: 100 - max: 500 + min: 0 + max: 505 condition: USE_ADAPTIVE_FILTER - name: gyro_adaptive_filter_std_lpf_hz description: "Standard deviation low pass filter cutoff frequency" From 1123e59198b9af28c0aabebb10d476cca8f1a1fc Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 12 Jun 2024 09:30:48 +0200 Subject: [PATCH 098/112] Enable adaptive filter only when min and max are set --- src/main/fc/fc_tasks.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 0fd239127c2..1230f6f01d7 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -428,7 +428,11 @@ void fcTasksInit(void) #endif #ifdef USE_ADAPTIVE_FILTER - setTaskEnabled(TASK_ADAPTIVE_FILTER, (gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE)); + setTaskEnabled(TASK_ADAPTIVE_FILTER, ( + gyroConfig()->gyroFilterMode == GYRO_FILTER_MODE_ADAPTIVE && + gyroConfig()->adaptiveFilterMinHz > 0 && + gyroConfig()->adaptiveFilterMaxHz > 0 + )); #endif #if defined(SITL_BUILD) From 5a50f95f0822f8f7da67be77ee24b6468fef0b5e Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 14:47:44 +0200 Subject: [PATCH 099/112] Increase max number of pwm outputs by 1, if a LED pin is defined --- src/main/drivers/pwm_mapping.c | 4 ++-- src/main/drivers/pwm_output.c | 4 ++-- src/main/drivers/pwm_output.h | 6 ++++++ 3 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index e953c86b96e..5ed17d7bb9a 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -53,8 +53,8 @@ enum { typedef struct { int maxTimMotorCount; int maxTimServoCount; - const timerHardware_t * timMotors[MAX_PWM_OUTPUT_PORTS]; - const timerHardware_t * timServos[MAX_PWM_OUTPUT_PORTS]; + const timerHardware_t * timMotors[MAX_PWM_OUTPUTS]; + const timerHardware_t * timServos[MAX_PWM_OUTPUTS]; } timMotorServoHardware_t; static pwmInitError_e pwmInitError = PWM_INIT_ERROR_NONE; diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index d48f38679e9..faa9cd373d9 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -98,7 +98,7 @@ typedef struct { bool requestTelemetry; } pwmOutputMotor_t; -static DMA_RAM pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUT_PORTS]; +static DMA_RAM pwmOutputPort_t pwmOutputPorts[MAX_PWM_OUTPUTS]; static pwmOutputMotor_t motors[MAX_MOTORS]; static motorPwmProtocolTypes_e initMotorProtocol; @@ -142,7 +142,7 @@ static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uin static pwmOutputPort_t *pwmOutAllocatePort(void) { - if (allocatedOutputPortCount >= MAX_PWM_OUTPUT_PORTS) { + if (allocatedOutputPortCount >= MAX_PWM_OUTPUTS) { LOG_ERROR(PWM, "Attempt to allocate PWM output beyond MAX_PWM_OUTPUT_PORTS"); return NULL; } diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index b3c0fa6be0e..1041ace04fa 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -20,6 +20,12 @@ #include "drivers/io_types.h" #include "drivers/time.h" + +#if defined(WS2811_PIN) +#define MAX_PWM_OUTPUTS (MAX_PWM_OUTPUT_PORTS + 1) +#else +#define MAX_PWM_OUTPUTS (MAX_PWM_OUTPUT_PORTS) +#endif typedef enum { DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20, DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21, From 1c6499354edc1cf18a5c58b62da4f09a329c740b Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 16:48:11 +0200 Subject: [PATCH 100/112] We already support 4 page fonts bump max number of chars for displayport_msp --- src/main/io/displayport_msp_osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index 2e425e6aecb..0b9be8c6d86 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -368,7 +368,7 @@ static uint32_t txBytesFree(const displayPort_t *displayPort) static bool getFontMetadata(displayFontMetadata_t *metadata, const displayPort_t *displayPort) { UNUSED(displayPort); - metadata->charCount = 512; + metadata->charCount = 1024; metadata->version = FONT_VERSION; return true; } From ff137b73e87002f771b6c1f69eba5304bace6b06 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 17:32:54 +0200 Subject: [PATCH 101/112] Add -j4 to allow parallel builds. Based on info available at https://docs.github.com/en/actions/using-github-hosted-runners/about-github-hosted-runners/about-github-hosted-runners#standard-github-hosted-runners-for-public-repositories --- .github/workflows/ci.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 111a0cc35c0..6129d4b4da8 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -36,7 +36,7 @@ jobs: path: downloads key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}} - name: Build targets (${{ matrix.id }}) - run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci + run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja -j4 ci - name: Upload artifacts uses: actions/upload-artifact@v3 with: @@ -66,7 +66,7 @@ jobs: echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV - name: Build SITL - run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja + run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4 - name: Upload artifacts uses: actions/upload-artifact@v3 with: @@ -101,7 +101,7 @@ jobs: run: | mkdir -p build_SITL && cd build_SITL cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -DCMAKE_OSX_ARCHITECTURES="arm64;x86_64" -G Ninja .. - ninja + ninja -j3 - name: Upload artifacts uses: actions/upload-artifact@v3 @@ -137,7 +137,7 @@ jobs: echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV - name: Build SITL - run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja + run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja -j4 - name: Upload artifacts uses: actions/upload-artifact@v3 with: From 2e6fa1593b4e03a0c66d3faf442f361cfe3f1876 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 17:50:33 +0200 Subject: [PATCH 102/112] Switch to make, ninja seems ot be ignoring the -j4 --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 6129d4b4da8..4cf4d7f68ea 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -36,7 +36,7 @@ jobs: path: downloads key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}} - name: Build targets (${{ matrix.id }}) - run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja -j4 ci + run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} .. && make -j4 ci - name: Upload artifacts uses: actions/upload-artifact@v3 with: From dad4c304c746455c420630d102bae55b12850fb5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 18:12:03 +0200 Subject: [PATCH 103/112] Try -pipe --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4cf4d7f68ea..8a4c1bb4748 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -36,7 +36,7 @@ jobs: path: downloads key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}} - name: Build targets (${{ matrix.id }}) - run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} .. && make -j4 ci + run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DMAIN_COMPILE_OPTIONS=-pipe .. && make -j4 ci - name: Upload artifacts uses: actions/upload-artifact@v3 with: From 75d85eb49525c8f01a120b601f55aed7b46d74bb Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 18:20:21 +0200 Subject: [PATCH 104/112] Back to ninja, bump number of runners --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8a4c1bb4748..272918902fd 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -9,7 +9,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9] + id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14] steps: - uses: actions/checkout@v3 @@ -36,7 +36,7 @@ jobs: path: downloads key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}} - name: Build targets (${{ matrix.id }}) - run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DMAIN_COMPILE_OPTIONS=-pipe .. && make -j4 ci + run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -DMAIN_COMPILE_OPTIONS=-pipe -G Ninja .. && ninja -j4 ci - name: Upload artifacts uses: actions/upload-artifact@v3 with: From 3da647d6dd4467d32e29937db74ea5d7f0821d90 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 18:29:31 +0200 Subject: [PATCH 105/112] Tests don't really depend on build. --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 272918902fd..187150695ac 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -146,7 +146,7 @@ jobs: test: - needs: [build] + #needs: [build] runs-on: ubuntu-latest steps: - uses: actions/checkout@v3 From 0e5f7716acd7e495acec6ff8f33dbf99fb4e87e5 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 18:38:49 +0200 Subject: [PATCH 106/112] Bump parallel task once more --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 187150695ac..b98e46ec570 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -9,7 +9,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14] + id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19] steps: - uses: actions/checkout@v3 From 556a0fadd9dd21b2ef7eb4027841b7199db8ac9d Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 18:48:36 +0200 Subject: [PATCH 107/112] 18 jobs. --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b98e46ec570..019d6b87271 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -9,7 +9,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19] + id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17] steps: - uses: actions/checkout@v3 From bbad573eff936fdbad3a4d687d301fbeeaae1e48 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 18:51:42 +0200 Subject: [PATCH 108/112] Strange things happening --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 019d6b87271..0330041607d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -9,7 +9,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17] + id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16] steps: - uses: actions/checkout@v3 From 2e69f3746e591f10e31fffb53c69e4c6afb4c256 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 19:41:04 +0200 Subject: [PATCH 109/112] Update actions --- .github/workflows/ci.yml | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 111a0cc35c0..481e6ad2858 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -12,7 +12,7 @@ jobs: id: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9] steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Install dependencies run: sudo apt-get update && sudo apt-get -y install ninja-build - name: Setup environment @@ -38,7 +38,7 @@ jobs: - name: Build targets (${{ matrix.id }}) run: mkdir -p build && cd build && cmake -DWARNINGS_AS_ERRORS=ON -DCI_JOB_INDEX=${{ matrix.id }} -DCI_JOB_COUNT=${{ strategy.job-total }} -DBUILD_SUFFIX=${{ env.BUILD_SUFFIX }} -G Ninja .. && ninja ci - name: Upload artifacts - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: name: ${{ env.BUILD_NAME }} path: ./build/*.hex @@ -46,7 +46,7 @@ jobs: build-SITL-Linux: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Install dependencies run: sudo apt-get update && sudo apt-get -y install ninja-build - name: Setup environment @@ -68,15 +68,15 @@ jobs: - name: Build SITL run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja - name: Upload artifacts - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: - name: ${{ env.BUILD_NAME }}_SITL + name: ${{ env.BUILD_NAME }}_SITL-Linux path: ./build_SITL/*_SITL build-SITL-Mac: runs-on: macos-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Install dependencies run: | brew install cmake ninja ruby @@ -104,7 +104,7 @@ jobs: ninja - name: Upload artifacts - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: name: ${{ env.BUILD_NAME }}_SITL-MacOS path: ./build_SITL/*_SITL @@ -115,7 +115,7 @@ jobs: run: shell: C:\tools\cygwin\bin\bash.exe -o igncr '{0}' steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup Cygwin uses: egor-tensin/setup-cygwin@v4 with: @@ -139,7 +139,7 @@ jobs: - name: Build SITL run: mkdir -p build_SITL && cd build_SITL && cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -G Ninja .. && ninja - name: Upload artifacts - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: name: ${{ env.BUILD_NAME }}_SITL-WIN path: ./build_SITL/*.exe @@ -149,7 +149,7 @@ jobs: needs: [build] runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Install dependencies run: sudo apt-get update && sudo apt-get -y install ninja-build - name: Run Tests From 6250e0bd94137e2fe4f1ef11eaa120d934f187a3 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 19:43:15 +0200 Subject: [PATCH 110/112] Update cache action --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 481e6ad2858..9caa077b82e 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -31,7 +31,7 @@ jobs: VERSION=$(grep project CMakeLists.txt|awk -F VERSION '{ gsub(/^[ \t]+|[ \t\)]+$/, "", $2); print $2 }') echo "BUILD_SUFFIX=${BUILD_SUFFIX}" >> $GITHUB_ENV echo "BUILD_NAME=inav-${VERSION}-${BUILD_SUFFIX}" >> $GITHUB_ENV - - uses: actions/cache@v1 + - uses: actions/cache@v4 with: path: downloads key: ${{ runner.os }}-downloads-${{ hashFiles('CMakeLists.txt') }}-${{ hashFiles('**/cmake/*')}} From 31208fe2f7edeab43644a588e15f48df20f9d49b Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 19:44:35 +0200 Subject: [PATCH 111/112] Update checkout action version --- .github/workflows/docs.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/docs.yml b/.github/workflows/docs.yml index 624a129c35d..edaaecdf3bb 100644 --- a/.github/workflows/docs.yml +++ b/.github/workflows/docs.yml @@ -14,7 +14,7 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Install dependencies run: sudo apt-get update && sudo apt-get -y install python3-yaml - name: Check that Settings.md is up to date From df9805236886d68d889679712b491b3f50ed7222 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Wed, 12 Jun 2024 20:10:32 +0200 Subject: [PATCH 112/112] Break .zip file by matrix id, for now. --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4c989894188..c389884008d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -40,7 +40,7 @@ jobs: - name: Upload artifacts uses: actions/upload-artifact@v4 with: - name: ${{ env.BUILD_NAME }} + name: ${{ env.BUILD_NAME }}.${{ matrix.id }} path: ./build/*.hex build-SITL-Linux: