From 541d589af92b04b648ea826824941fff10e8c104 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 12 Mar 2023 19:04:55 +0100 Subject: [PATCH 01/36] Filter setup via EzTune --- src/main/CMakeLists.txt | 2 + src/main/config/parameter_group_ids.h | 3 +- src/main/fc/fc_init.c | 5 ++ src/main/fc/fc_msp.c | 2 + src/main/fc/settings.c | 2 + src/main/fc/settings.h | 1 + src/main/fc/settings.yaml | 17 +++++ src/main/flight/ez_tune.c | 91 +++++++++++++++++++++++++++ src/main/flight/ez_tune.h | 36 +++++++++++ src/main/target/common.h | 2 + 10 files changed, 160 insertions(+), 1 deletion(-) create mode 100644 src/main/flight/ez_tune.c create mode 100644 src/main/flight/ez_tune.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 317da760b6e..cdc01ac3159 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -331,6 +331,8 @@ main_sources(COMMON_SRC flight/secondary_dynamic_gyro_notch.h flight/dynamic_lpf.c flight/dynamic_lpf.h + flight/ez_tune.c + flight/ez_tune.h io/beeper.c io/beeper.h diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 46293524b14..329f42b219a 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -119,7 +119,8 @@ #define PG_UNUSED_1 1029 #define PG_POWER_LIMITS_CONFIG 1030 #define PG_OSD_COMMON_CONFIG 1031 -#define PG_INAV_END 1031 +#define PG_EZ_TUNE 1032 +#define PG_INAV_END PG_EZ_TUNE // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index a8ab8146d2f..64b421c583e 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -96,6 +96,7 @@ #include "flight/power_limits.h" #include "flight/rpm_filter.h" #include "flight/servos.h" +#include "flight/ez_tune.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -509,6 +510,10 @@ void init(void) owInit(); #endif +#ifdef USE_EZ_TUNE + ezTuneUpdate(); +#endif + if (!sensorsAutodetect()) { // if gyro was not detected due to whatever reason, we give up now. failureMode(FAILURE_MISSING_ACC); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ee5c8f03976..7f8354deee6 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3164,6 +3164,8 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src) sbufWriteU8(dst, 0); sbufWriteU8(dst, 0); break; + case EZ_TUNE_VALUE: + FALLTHROUGH; case PROFILE_VALUE: FALLTHROUGH; case CONTROL_RATE_VALUE: diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 8dc17edc631..bef0750e886 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -237,6 +237,8 @@ static uint16_t getValueOffset(const setting_t *value) return value->offset + sizeof(pidProfile_t) * getConfigProfile(); case CONTROL_RATE_VALUE: return value->offset + sizeof(controlRateConfig_t) * getConfigProfile(); + case EZ_TUNE_VALUE: + return value->offset + sizeof(ezTuneSettings_t) * getConfigProfile(); case BATTERY_CONFIG_VALUE: return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile(); } diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index b14289ba27c..a6cc403842c 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -34,6 +34,7 @@ typedef enum { PROFILE_VALUE = (1 << SETTING_SECTION_OFFSET), CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), // 0x20 BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET), + EZ_TUNE_VALUE = (4 << SETTING_SECTION_OFFSET) } setting_section_e; typedef enum { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c0df0e204cf..de52e9a7a58 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1477,6 +1477,23 @@ groups: min: 0 max: 99 + - name: PG_EZ_TUNE + headers: ["flight/ez_tune.h"] + type: ezTuneSettings_t + value_type: EZ_TUNE_VALUE + members: + - name: ez_tune_enabled + description: "Enables EzTune feature" + default_value: OFF + field: enabled + type: bool + - name: ez_tune_filter_hz + description: "EzTune filter cutoff frequency" + default_value: 110 + field: filterHz + min: 10 + max: 300 + - name: PG_RPM_FILTER_CONFIG headers: ["flight/rpm_filter.h"] condition: USE_RPM_FILTER diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c new file mode 100644 index 00000000000..a9656bd83f6 --- /dev/null +++ b/src/main/flight/ez_tune.c @@ -0,0 +1,91 @@ +/* + * 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 "fc/config.h" +#include "config/config_reset.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "flight/ez_tune.h" + +#include "fc/settings.h" +#include "flight/pid.h" +#include "sensors/gyro.h" + +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0); + +PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, + .enabled = SETTING_EZ_TUNE_ENABLED_DEFAULT, + .filterHz = SETTING_EZ_TUNE_FILTER_HZ_DEFAULT, +); + +static float computePt1FilterDelayMs(uint8_t filterHz) { + return 1.0f / (2.0f * M_PIf * filterHz); +} + +/** + * Update INAV settings based on current EZTune settings + * This has to be called every time control profile is changed, or EZTune settings are changed + * FIXME call on profile change + * FIXME call on EZTune settings change + */ +void ezTuneUpdate(void) { + if (ezTune()->enabled) { + + // Setup filtering + + //Enable Smith predictor + pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz) + + computePt1FilterDelayMs(gyroConfig()->gyro_anti_aliasing_lpf_hz); + + //Set Dterm LPF + pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50); + pidProfileMutable()->dterm_lpf_type = FILTER_PT2; + + //Set main gyro filter + gyroConfigMutable()->gyro_main_lpf_hz = ezTune()->filterHz; + gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1; + + //Set anti-aliasing filter + gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(ezTune()->filterHz * 2, 250); + gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; + + //Enable dynamic notch + gyroConfigMutable()->dynamicGyroNotchEnabled = 1; + gyroConfigMutable()->dynamicGyroNotchQ = 250; + gyroConfigMutable()->dynamicGyroNotchMinHz = MAX(ezTune()->filterHz * 0.667f, SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT); + gyroConfigMutable()->dynamicGyroNotchMode = DYNAMIC_NOTCH_MODE_3D; + + //Make sure Kalman filter is enabled + gyroConfigMutable()->kalmanEnabled = 1; + if (ezTune()->filterHz < 150) { + gyroConfigMutable()->kalman_q = 200; + } else { + gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400); + } + + + + } +} \ No newline at end of file diff --git a/src/main/flight/ez_tune.h b/src/main/flight/ez_tune.h new file mode 100644 index 00000000000..a2ba6f02497 --- /dev/null +++ b/src/main/flight/ez_tune.h @@ -0,0 +1,36 @@ +/* + * 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/. + */ + +#pragma once + +#include "config/parameter_group.h" + +typedef struct ezTuneSettings_s { + uint8_t enabled; + uint16_t filterHz; +} ezTuneSettings_t; + +PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune); + +void ezTuneUpdate(void); \ No newline at end of file diff --git a/src/main/target/common.h b/src/main/target/common.h index 2365be96712..26ec4ff0124 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -192,3 +192,5 @@ #define USE_HOTT_TEXTMODE #endif + +#define USE_EZ_TUNE \ No newline at end of file From a2051c1371b7689ce3a97a5e5ed5b82ff2f4ae03 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 12 Mar 2023 19:06:29 +0100 Subject: [PATCH 02/36] Disable gyro dynamic LPF for EZ-Tune --- src/main/flight/ez_tune.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index a9656bd83f6..240e64f921e 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -85,7 +85,8 @@ void ezTuneUpdate(void) { gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400); } - + //Disable dynamic LPF + gyroConfigMutable()->useDynamicLpf = 0; } } \ No newline at end of file From fb5210d0b952601f6ca9721321a4313414db065a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 12 Mar 2023 19:52:22 +0100 Subject: [PATCH 03/36] Add EzTune to the settings.yaml --- src/main/fc/settings.yaml | 30 ++++++++++++++++++++++++++++++ src/main/flight/ez_tune.c | 10 ++++++++++ src/main/flight/ez_tune.h | 8 ++++++++ 3 files changed, 48 insertions(+) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index de52e9a7a58..7dbc1d84535 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1493,6 +1493,36 @@ groups: field: filterHz min: 10 max: 300 + - name: ez_tune_axis_ratio + description: "EzTune axis ratio" + default_value: 100 + field: axisRatio + min: 25 + max: 175 + - name: ez_tune_response + description: "EzTune response" + default_value: 100 + field: response + min: 0 + max: 200 + - name: ez_tune_damping + description: "EzTune damping" + default_value: 100 + field: damping + min: 0 + max: 200 + - name: ez_tune_stability + description: "EzTune stability" + default_value: 100 + field: stability + min: 0 + max: 200 + - name: ez_tune_aggressiveness + description: "EzTune aggressiveness" + default_value: 100 + field: aggressiveness + min: 0 + max: 200 - name: PG_RPM_FILTER_CONFIG headers: ["flight/rpm_filter.h"] diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 240e64f921e..35717ed828d 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -38,6 +38,11 @@ PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0) PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, .enabled = SETTING_EZ_TUNE_ENABLED_DEFAULT, .filterHz = SETTING_EZ_TUNE_FILTER_HZ_DEFAULT, + .axisRatio = SETTING_EZ_TUNE_AXIS_RATIO_DEFAULT, + .response = SETTING_EZ_TUNE_RESPONSE_DEFAULT, + .damping = SETTING_EZ_TUNE_DAMPING_DEFAULT, + .stability = SETTING_EZ_TUNE_STABILITY_DEFAULT, + .aggressiveness = SETTING_EZ_TUNE_AGGRESSIVENESS_DEFAULT, ); static float computePt1FilterDelayMs(uint8_t filterHz) { @@ -88,5 +93,10 @@ void ezTuneUpdate(void) { //Disable dynamic LPF gyroConfigMutable()->useDynamicLpf = 0; + //Setup PID controller + + //Roll + + } } \ No newline at end of file diff --git a/src/main/flight/ez_tune.h b/src/main/flight/ez_tune.h index a2ba6f02497..98cb6926362 100644 --- a/src/main/flight/ez_tune.h +++ b/src/main/flight/ez_tune.h @@ -29,8 +29,16 @@ typedef struct ezTuneSettings_s { uint8_t enabled; uint16_t filterHz; + uint8_t axisRatio; + uint8_t response; + uint8_t damping; + uint8_t stability; + uint8_t aggressiveness; } ezTuneSettings_t; +#define EZ_TUNE_PID_RP_DEFAULT {40, 75, 23, 100} +#define EZ_TUNE_PID_YAW_DEFAULT {45, 80, 0, 100} + PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune); void ezTuneUpdate(void); \ No newline at end of file From de6c442a11028d72035ba191ab6bcaea2ed6bd96 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 13 Mar 2023 11:24:10 +0100 Subject: [PATCH 04/36] Adjust PIDs from EzTune --- src/main/fc/settings.yaml | 2 +- src/main/flight/ez_tune.c | 33 +++++++++++++++++++++++++++++++-- src/main/flight/ez_tune.h | 3 --- 3 files changed, 32 insertions(+), 6 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7dbc1d84535..0d000440491 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1495,7 +1495,7 @@ groups: max: 300 - name: ez_tune_axis_ratio description: "EzTune axis ratio" - default_value: 100 + default_value: 110 field: axisRatio min: 25 max: 175 diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 35717ed828d..c337332e1f6 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -45,10 +45,21 @@ PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, .aggressiveness = SETTING_EZ_TUNE_AGGRESSIVENESS_DEFAULT, ); +#define EZ_TUNE_PID_RP_DEFAULT { 40, 75, 23, 100 } +#define EZ_TUNE_PID_YAW_DEFAULT { 45, 80, 0, 100 } + +#define EZ_TUNE_YAW_SCALE 0.5f + static float computePt1FilterDelayMs(uint8_t filterHz) { return 1.0f / (2.0f * M_PIf * filterHz); } +static float getYawPidScale(float input) { + const float normalized = (input - 100) * 0.01f; + + return 1.0f + (normalized * 0.5f); +} + /** * Update INAV settings based on current EZTune settings * This has to be called every time control profile is changed, or EZTune settings are changed @@ -95,8 +106,26 @@ void ezTuneUpdate(void) { //Setup PID controller - //Roll - + const uint8_t pidDefaults[4] = EZ_TUNE_PID_RP_DEFAULT; + const uint8_t pidDefaultsYaw[4] = EZ_TUNE_PID_YAW_DEFAULT; + const float pitchRatio = ezTune()->axisRatio / 100.0f; + //Roll + pidProfileMutable()->bank_mc.pid[PID_ROLL].P = pidDefaults[0] * ezTune()->response / 100.0f; + pidProfileMutable()->bank_mc.pid[PID_ROLL].I = pidDefaults[1] * ezTune()->stability / 100.0f; + pidProfileMutable()->bank_mc.pid[PID_ROLL].D = pidDefaults[2] * ezTune()->damping / 100.0f; + pidProfileMutable()->bank_mc.pid[PID_ROLL].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f; + + //Pitch + pidProfileMutable()->bank_mc.pid[PID_PITCH].P = pidDefaults[0] * ezTune()->response / 100.0f * pitchRatio; + pidProfileMutable()->bank_mc.pid[PID_PITCH].I = pidDefaults[1] * ezTune()->stability / 100.0f * pitchRatio; + pidProfileMutable()->bank_mc.pid[PID_PITCH].D = pidDefaults[2] * ezTune()->damping / 100.0f * pitchRatio; + pidProfileMutable()->bank_mc.pid[PID_PITCH].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f * pitchRatio; + + //Yaw + pidProfileMutable()->bank_mc.pid[PID_YAW].P = pidDefaultsYaw[0] * getYawPidScale(ezTune()->response); + pidProfileMutable()->bank_mc.pid[PID_YAW].I = pidDefaultsYaw[1] * getYawPidScale(ezTune()->stability); + pidProfileMutable()->bank_mc.pid[PID_YAW].D = pidDefaultsYaw[2] * getYawPidScale(ezTune()->damping); + pidProfileMutable()->bank_mc.pid[PID_YAW].FF = pidDefaultsYaw[3] * getYawPidScale(ezTune()->aggressiveness); } } \ No newline at end of file diff --git a/src/main/flight/ez_tune.h b/src/main/flight/ez_tune.h index 98cb6926362..aa0088fe7f2 100644 --- a/src/main/flight/ez_tune.h +++ b/src/main/flight/ez_tune.h @@ -36,9 +36,6 @@ typedef struct ezTuneSettings_s { uint8_t aggressiveness; } ezTuneSettings_t; -#define EZ_TUNE_PID_RP_DEFAULT {40, 75, 23, 100} -#define EZ_TUNE_PID_YAW_DEFAULT {45, 80, 0, 100} - PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune); void ezTuneUpdate(void); \ No newline at end of file From 554eaeaa0ea59cd584e300b6c4fc9e3ab6d662cc Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 13 Mar 2023 17:03:41 +0100 Subject: [PATCH 05/36] Add rate and expo to EZ-Tune --- src/main/fc/settings.yaml | 26 +++++++++++++++++++------- src/main/flight/ez_tune.c | 16 +++++++++------- src/main/flight/ez_tune.h | 2 ++ 3 files changed, 30 insertions(+), 14 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0d000440491..188f1c9119d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1482,47 +1482,59 @@ groups: type: ezTuneSettings_t value_type: EZ_TUNE_VALUE members: - - name: ez_tune_enabled + - name: ez_enabled description: "Enables EzTune feature" default_value: OFF field: enabled type: bool - - name: ez_tune_filter_hz + - name: ez_filter_hz description: "EzTune filter cutoff frequency" default_value: 110 field: filterHz min: 10 max: 300 - - name: ez_tune_axis_ratio + - name: ez_axis_ratio description: "EzTune axis ratio" default_value: 110 field: axisRatio min: 25 max: 175 - - name: ez_tune_response + - name: ez_response description: "EzTune response" default_value: 100 field: response min: 0 max: 200 - - name: ez_tune_damping + - name: ez_damping description: "EzTune damping" default_value: 100 field: damping min: 0 max: 200 - - name: ez_tune_stability + - name: ez_stability description: "EzTune stability" default_value: 100 field: stability min: 0 max: 200 - - name: ez_tune_aggressiveness + - name: ez_aggressiveness description: "EzTune aggressiveness" default_value: 100 field: aggressiveness min: 0 max: 200 + - name: ez_rate + description: "EzTune rate" + default_value: 100 + field: rate + min: 0 + max: 200 + - name: ez_expo + description: "EzTune expo" + default_value: 100 + field: expo + min: 0 + max: 200 - name: PG_RPM_FILTER_CONFIG headers: ["flight/rpm_filter.h"] diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index c337332e1f6..cc1c3dab5b2 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -36,13 +36,15 @@ PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0); PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, - .enabled = SETTING_EZ_TUNE_ENABLED_DEFAULT, - .filterHz = SETTING_EZ_TUNE_FILTER_HZ_DEFAULT, - .axisRatio = SETTING_EZ_TUNE_AXIS_RATIO_DEFAULT, - .response = SETTING_EZ_TUNE_RESPONSE_DEFAULT, - .damping = SETTING_EZ_TUNE_DAMPING_DEFAULT, - .stability = SETTING_EZ_TUNE_STABILITY_DEFAULT, - .aggressiveness = SETTING_EZ_TUNE_AGGRESSIVENESS_DEFAULT, + .enabled = SETTING_EZ_ENABLED_DEFAULT, + .filterHz = SETTING_EZ_FILTER_HZ_DEFAULT, + .axisRatio = SETTING_EZ_AXIS_RATIO_DEFAULT, + .response = SETTING_EZ_RESPONSE_DEFAULT, + .damping = SETTING_EZ_DAMPING_DEFAULT, + .stability = SETTING_EZ_STABILITY_DEFAULT, + .aggressiveness = SETTING_EZ_AGGRESSIVENESS_DEFAULT, + .rate = SETTING_EZ_RATE_DEFAULT, + .expo = SETTING_EZ_EXPO_DEFAULT, ); #define EZ_TUNE_PID_RP_DEFAULT { 40, 75, 23, 100 } diff --git a/src/main/flight/ez_tune.h b/src/main/flight/ez_tune.h index aa0088fe7f2..5fcc1ef6f7f 100644 --- a/src/main/flight/ez_tune.h +++ b/src/main/flight/ez_tune.h @@ -34,6 +34,8 @@ typedef struct ezTuneSettings_s { uint8_t damping; uint8_t stability; uint8_t aggressiveness; + uint8_t rate; + uint8_t expo; } ezTuneSettings_t; PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune); From 9799ae07882822c65ced1028accd1105bcee741c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 15 Jun 2023 10:16:00 +0200 Subject: [PATCH 06/36] Cap gyro_anti_aliasing_lpf_hz and gyro nyquist frequency to 1/2 of gyro sampling rate --- src/main/flight/ez_tune.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index cc1c3dab5b2..c4846d39894 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -86,7 +86,7 @@ void ezTuneUpdate(void) { gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1; //Set anti-aliasing filter - gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(ezTune()->filterHz * 2, 250); + gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), getGyroLooptime() * 0.5f); gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; //Enable dynamic notch From 67890ed3ec392c98f259470e4a4d69f68aa26618 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 14 Aug 2023 10:56:45 +0200 Subject: [PATCH 07/36] Add files --- .vscode/c_cpp_properties.json | 66 +++++++++++++++++++++++++++++++++++ .vscode/tasks.json | 41 ++++++++++++++++++++++ 2 files changed, 107 insertions(+) create mode 100755 .vscode/c_cpp_properties.json create mode 100755 .vscode/tasks.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100755 index 00000000000..6e7d914b25a --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,66 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceRoot}/src/main/**", + "${workspaceRoot}/lib/main/**", + "/usr/include/**" + ], + "browse": { + "limitSymbolsToIncludedHeaders": false, + "path": [ + "${workspaceRoot}/src/main/**", + "${workspaceRoot}/lib/main/**" + ] + }, + "intelliSenseMode": "linux-gcc-arm", + "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", + "USE_DTERM_NOTCH", + "USE_ACC_NOTCH", + "USE_GYRO_BIQUAD_RC_FIR2", + "USE_D_BOOST", + "USE_SERIALSHOT", + "USE_ANTIGRAVITY", + "USE_ASYNC_GYRO_PROCESSING", + "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" + ], + "configurationProvider": "ms-vscode.cmake-tools" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100755 index 00000000000..3ca90b787d3 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,41 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + { + "label": "Build Matek F722-SE", + "type": "shell", + "command": "make MATEKF722SE", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + }, + { + "label": "Build Matek F722", + "type": "shell", + "command": "make MATEKF722", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + } + , + { + "label": "CMAKE Update", + "type": "shell", + "command": "cmake ..", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + } + ] +} \ No newline at end of file From 26deae54bbdf08085e85cace5d5d203ba7fb7fa1 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 21 Aug 2023 23:28:11 +0100 Subject: [PATCH 08/36] first build --- src/main/fc/fc_core.c | 20 ++++++++++++++------ src/main/fc/fc_core.h | 1 + src/main/navigation/navigation.c | 20 +++++++++++++++----- src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_fixedwing.c | 2 +- 5 files changed, 32 insertions(+), 12 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index af8d1202b17..46ef140f84a 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -118,6 +118,7 @@ uint8_t motorControlEnable = false; static bool isRXDataNew; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; +timeMs_t emergInflightRearmTimeout = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; @@ -176,7 +177,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) { int16_t stickDeflection = 0; -#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914 +#if defined(SITL_BUILD) // Workaround due to strange bug in GCC > 10.2 https://gcc.gnu.org/bugzilla/show_bug.cgi?id=108914 const int16_t value = rawData - PWM_RANGE_MIDDLE; if (value < -500) { stickDeflection = -500; @@ -186,9 +187,9 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) stickDeflection = value; } #else - stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500); + stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500); #endif - + stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500); return rcLookup(stickDeflection, rate); } @@ -432,6 +433,7 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); + emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? 5000 : 0); DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -509,6 +511,11 @@ void releaseSharedTelemetryPorts(void) { } } +bool emergInflightRearmEnabled(void) +{ + return millis() < emergInflightRearmTimeout; +} + void tryArm(void) { updateArmingStatus(); @@ -529,9 +536,10 @@ void tryArm(void) #endif #ifdef USE_PROGRAMMING_FRAMEWORK - if (!isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) { + if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled() || + LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) { #else - if (!isArmingDisabled() || emergencyArmingIsEnabled()) { + if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled()) { #endif // If nav_extra_arming_safety was bypassed we always // allow bypassing it even without the sticks set @@ -837,7 +845,7 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens void taskMainPidLoop(timeUs_t currentTimeUs) { - + cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 37d0bbda79a..1d34e31742b 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -43,6 +43,7 @@ void tryArm(void); disarmReason_t getDisarmReason(void); bool emergencyArmingUpdate(bool armingSwitchIsOn); +bool emergInflightRearmEnabled(void); bool areSensorsCalibrating(void); float getFlightTime(void); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index d8bc20554cc..04c910aa63a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -228,6 +228,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, static navWapointHeading_t wpHeadingControl; navigationPosControl_t posControl; navSystemStatus_t NAV_Status; +static bool landingDetectorIsActive; EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients; @@ -2803,14 +2804,14 @@ void updateLandingStatus(timeMs_t currentTimeMs) } lastUpdateTimeMs = currentTimeMs; - static bool landingDetectorIsActive; - DEBUG_SET(DEBUG_LANDING, 0, landingDetectorIsActive); DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - resetLandingDetector(); - landingDetectorIsActive = false; + if (!emergInflightRearmEnabled()) { + resetLandingDetector(); + landingDetectorIsActive = false; + } if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } @@ -2852,6 +2853,15 @@ bool isFlightDetected(void) return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying(); } +bool isProbablyStillFlying(void) +{ + bool inFlightSanityCheck = false; + if (STATE(AIRPLANE)) { + inFlightSanityCheck = isGPSHeadingValid(); + } + return landingDetectorIsActive && !STATE(LANDING_DETECTED) && inFlightSanityCheck; +} + /*----------------------------------------------------------- * Z-position controller *-----------------------------------------------------------*/ @@ -3801,7 +3811,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) canActivateWaypoint = false; // Launch mode can be activated if feature FW_LAUNCH is enabled or BOX is turned on prior to arming (avoid switching to LAUNCH in flight) - canActivateLaunchMode = isNavLaunchEnabled(); + canActivateLaunchMode = isNavLaunchEnabled() && (!sensors(SENSOR_GPS) || (sensors(SENSOR_GPS) && !isGPSHeadingValid())); } return NAV_FSM_EVENT_SWITCH_TO_IDLE; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index cf4289e14a1..9ffe532e6c5 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -610,6 +610,7 @@ const char * fixedWingLaunchStateMessage(void); float calculateAverageSpeed(void); void updateLandingStatus(timeMs_t currentTimeMs); +bool isProbablyStillFlying(void); const navigationPIDControllers_t* getNavigationPIDControllers(void); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 91354d97aed..7b55a48e6a0 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -692,7 +692,7 @@ bool isFixedWingFlying(void) bool velCondition = posControl.actualState.velXY > 250.0f || airspeed > 250.0f; bool launchCondition = isNavLaunchEnabled() && fixedWingLaunchStatus() == FW_LAUNCH_FLYING; - return (isImuHeadingValid() && throttleCondition && velCondition) || launchCondition; + return (isGPSHeadingValid() && throttleCondition && velCondition) || launchCondition; } /*----------------------------------------------------------- From 3b9e2a24ff26e697af412c000741bf3998466989 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 23 Aug 2023 15:07:20 +0100 Subject: [PATCH 09/36] add MR flight sanity and forced Angle --- src/main/fc/fc_core.c | 69 +++++++++++++++++++------------- src/main/navigation/navigation.c | 7 ++-- 2 files changed, 45 insertions(+), 31 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 46ef140f84a..187613a9ee4 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -104,6 +104,7 @@ enum { #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000 #define EMERGENCY_ARMING_COUNTER_STEP_MS 1000 #define EMERGENCY_ARMING_MIN_ARM_COUNT 10 +#define EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS 5000 timeDelta_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop static timeUs_t flightTime = 0; @@ -119,6 +120,7 @@ static bool isRXDataNew; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; timeMs_t emergInflightRearmTimeout = 0; +timeMs_t mcEmergRearmStabiliseTimeout = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; @@ -314,11 +316,11 @@ static void updateArmingStatus(void) /* CHECK: Do not allow arming if Servo AutoTrim is enabled */ if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { - ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); - } + ENABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); + } else { - DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); - } + DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); + } #ifdef USE_DSHOT /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */ @@ -433,7 +435,7 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); - emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? 5000 : 0); + emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0); DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -501,19 +503,20 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) return counter >= EMERGENCY_ARMING_MIN_ARM_COUNT; } -#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) - -void releaseSharedTelemetryPorts(void) { - serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); - while (sharedPort) { - mspSerialReleasePortIfAllocated(sharedPort); - sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); - } -} - bool emergInflightRearmEnabled(void) { - return millis() < emergInflightRearmTimeout; + /* Emergency rearm allowed within emergInflightRearmTimeout window. + * On MR emergency rearm only allowed after 1.5s if MR dropping after disarm, i.e. still in flight */ + timeMs_t currentTimeMs = millis(); + if (STATE(MULTIROTOR)) { + uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 2 sec after disarm + if (getEstimatedActualVelocity(Z) > -100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { + emergInflightRearmTimeout = currentTimeMs; // MR doesn't appear to be flying so don't allow emergency rearm + } else { + mcEmergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise MR + } + } + return currentTimeMs < emergInflightRearmTimeout; } void tryArm(void) @@ -594,6 +597,16 @@ void tryArm(void) } } +#define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_SMARTPORT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_MAVLINK | FUNCTION_TELEMETRY_IBUS) + +void releaseSharedTelemetryPorts(void) { + serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); + while (sharedPort) { + mspSerialReleasePortIfAllocated(sharedPort); + sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); + } +} + void processRx(timeUs_t currentTimeUs) { // Calculate RPY channel data @@ -644,9 +657,12 @@ void processRx(timeUs_t currentTimeUs) processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); } + // Angle mode forced on briefly after multirotor emergency in flight rearm to help stabilise attitude + bool mcEmergRearmAngleEnforce = STATE(MULTIROTOR) && mcEmergRearmStabiliseTimeout > millis(); + bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mcEmergRearmAngleEnforce; bool canUseHorizonMode = true; - if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) { + if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) { // bumpless transfer to Level mode canUseHorizonMode = false; @@ -813,7 +829,6 @@ void processRx(timeUs_t currentTimeUs) } } #endif - } // Function for loop trigger @@ -929,15 +944,15 @@ void taskMainPidLoop(timeUs_t currentTimeUs) //Servos should be filtered or written only when mixer is using servos or special feaures are enabled #ifdef USE_SIMULATOR - if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { - if (isServoOutputEnabled()) { - writeServos(); - } - - if (motorControlEnable) { - writeMotors(); - } - } + if (!ARMING_FLAG(SIMULATOR_MODE_HITL)) { + if (isServoOutputEnabled()) { + writeServos(); + } + + if (motorControlEnable) { + writeMotors(); + } + } #else if (isServoOutputEnabled()) { writeServos(); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 04c910aa63a..96d99ebf3b5 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2855,10 +2855,9 @@ bool isFlightDetected(void) bool isProbablyStillFlying(void) { - bool inFlightSanityCheck = false; - if (STATE(AIRPLANE)) { - inFlightSanityCheck = isGPSHeadingValid(); - } + // Multirotor flight sanity checked after disarm so always true here + bool inFlightSanityCheck = STATE(MULTIROTOR) || (STATE(AIRPLANE) && isGPSHeadingValid()); + return landingDetectorIsActive && !STATE(LANDING_DETECTED) && inFlightSanityCheck; } From 7bb9b69a5f5b89e5d75534248b0931929d0b990f Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 23 Aug 2023 22:16:45 +0100 Subject: [PATCH 10/36] Fix logic --- src/main/fc/fc_core.c | 24 +++++++++++++++++------- src/main/navigation/navigation.c | 7 +++---- 2 files changed, 20 insertions(+), 11 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 187613a9ee4..60c658809ef 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -435,7 +435,10 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); - emergInflightRearmTimeout = US2MS(lastDisarmTimeUs) + (isProbablyStillFlying() ? EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0); + if (disarmReason == DISARM_SWITCH || disarmReason == DISARM_KILLSWITCH) { + emergInflightRearmTimeout = isProbablyStillFlying() ? US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0; + } + DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -506,17 +509,23 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) bool emergInflightRearmEnabled(void) { /* Emergency rearm allowed within emergInflightRearmTimeout window. - * On MR emergency rearm only allowed after 1.5s if MR dropping after disarm, i.e. still in flight */ + * On MR emergency rearm only allowed after 1.5s if MR dropping or climbing after disarm, i.e. still in flight */ + timeMs_t currentTimeMs = millis(); + if (!emergInflightRearmTimeout || currentTimeMs > emergInflightRearmTimeout) { + return false; + } + if (STATE(MULTIROTOR)) { - uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 2 sec after disarm - if (getEstimatedActualVelocity(Z) > -100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { - emergInflightRearmTimeout = currentTimeMs; // MR doesn't appear to be flying so don't allow emergency rearm + uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 1.5 sec after disarm + if (fabsf(getEstimatedActualVelocity(Z)) < 100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { + return false; // MR doesn't appear to be flying so don't allow emergency rearm } else { mcEmergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise MR } } - return currentTimeMs < emergInflightRearmTimeout; + + return true; } void tryArm(void) @@ -555,6 +564,7 @@ void tryArm(void) } lastDisarmReason = DISARM_NONE; + emergInflightRearmTimeout = 0; ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); @@ -657,7 +667,7 @@ void processRx(timeUs_t currentTimeUs) processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); } - // Angle mode forced on briefly after multirotor emergency in flight rearm to help stabilise attitude + // Angle mode forced on briefly after multirotor emergency inflight rearm to help stabilise attitude bool mcEmergRearmAngleEnforce = STATE(MULTIROTOR) && mcEmergRearmStabiliseTimeout > millis(); bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mcEmergRearmAngleEnforce; bool canUseHorizonMode = true; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 96d99ebf3b5..759ddd42f3f 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2808,10 +2808,9 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - if (!emergInflightRearmEnabled()) { - resetLandingDetector(); - landingDetectorIsActive = false; - } + resetLandingDetector(); + landingDetectorIsActive = false; + if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } From 6e58a94cf3a9230096cffb21c6816b92eebcd47a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 27 Aug 2023 23:28:04 +0100 Subject: [PATCH 11/36] Change logic + add gyro check for MR --- src/main/fc/fc_core.c | 38 ++++++++++++++------------------ src/main/navigation/navigation.c | 20 ++++++++++++----- 2 files changed, 30 insertions(+), 28 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 60c658809ef..a00e7972423 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -119,8 +119,7 @@ uint8_t motorControlEnable = false; static bool isRXDataNew; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; -timeMs_t emergInflightRearmTimeout = 0; -timeMs_t mcEmergRearmStabiliseTimeout = 0; +timeMs_t emergRearmStabiliseTimeout = 0; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; @@ -435,10 +434,6 @@ void disarm(disarmReason_t disarmReason) if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); - if (disarmReason == DISARM_SWITCH || disarmReason == DISARM_KILLSWITCH) { - emergInflightRearmTimeout = isProbablyStillFlying() ? US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS : 0; - } - DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -508,24 +503,24 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn) bool emergInflightRearmEnabled(void) { - /* Emergency rearm allowed within emergInflightRearmTimeout window. - * On MR emergency rearm only allowed after 1.5s if MR dropping or climbing after disarm, i.e. still in flight */ - + /* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */ timeMs_t currentTimeMs = millis(); - if (!emergInflightRearmTimeout || currentTimeMs > emergInflightRearmTimeout) { + emergRearmStabiliseTimeout = 0; + + if ((lastDisarmReason != DISARM_SWITCH && lastDisarmReason != DISARM_KILLSWITCH) || + (currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) { return false; } - if (STATE(MULTIROTOR)) { - uint16_t mcFlightSanityCheckTime = EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS - 1500; // check MR vertical speed at least 1.5 sec after disarm - if (fabsf(getEstimatedActualVelocity(Z)) < 100.0f && (emergInflightRearmTimeout - currentTimeMs < mcFlightSanityCheckTime)) { - return false; // MR doesn't appear to be flying so don't allow emergency rearm - } else { - mcEmergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise MR - } + if (isProbablyStillFlying()) { + emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft + return true; + } else if (STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f) { + // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying + return true; } - return true; + return false; // craft doesn't appear to be flying, don't allow emergency rearm } void tryArm(void) @@ -564,7 +559,6 @@ void tryArm(void) } lastDisarmReason = DISARM_NONE; - emergInflightRearmTimeout = 0; ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); @@ -667,9 +661,9 @@ void processRx(timeUs_t currentTimeUs) processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData); } - // Angle mode forced on briefly after multirotor emergency inflight rearm to help stabilise attitude - bool mcEmergRearmAngleEnforce = STATE(MULTIROTOR) && mcEmergRearmStabiliseTimeout > millis(); - bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mcEmergRearmAngleEnforce; + // Angle mode forced on briefly after emergency inflight rearm to help stabilise attitude (currently limited to MR) + bool emergRearmAngleEnforce = STATE(MULTIROTOR) && emergRearmStabiliseTimeout > US2MS(currentTimeUs); + bool autoEnableAngle = failsafeRequiresAngleMode() || navigationRequiresAngleMode() || emergRearmAngleEnforce; bool canUseHorizonMode = true; if (sensors(SENSOR_ACC) && (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle)) { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 759ddd42f3f..eba38c68b10 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -61,6 +61,7 @@ #include "sensors/acceleration.h" #include "sensors/boardalignment.h" #include "sensors/battery.h" +#include "sensors/gyro.h" #include "programming/global_variables.h" @@ -2808,13 +2809,16 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - resetLandingDetector(); - landingDetectorIsActive = false; - + if (STATE(LANDING_DETECTED)) { + resetLandingDetector(); + landingDetectorIsActive = false; + } if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } return; + } else if (getArmTime() < 0.25f && landingDetectorIsActive) { // force reset landing detector immediately after arming + landingDetectorIsActive = false; } if (!landingDetectorIsActive) { @@ -2854,10 +2858,14 @@ bool isFlightDetected(void) bool isProbablyStillFlying(void) { - // Multirotor flight sanity checked after disarm so always true here - bool inFlightSanityCheck = STATE(MULTIROTOR) || (STATE(AIRPLANE) && isGPSHeadingValid()); + bool inFlightSanityCheck; + if (STATE(MULTIROTOR)) { + inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f; + } else { + inFlightSanityCheck = isGPSHeadingValid(); + } - return landingDetectorIsActive && !STATE(LANDING_DETECTED) && inFlightSanityCheck; + return landingDetectorIsActive && inFlightSanityCheck; } /*----------------------------------------------------------- From 1338fb4a54fcf891a175c5285ca48bfe991b34dd Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 28 Aug 2023 12:47:11 +0100 Subject: [PATCH 12/36] Update fc_core.h --- src/main/fc/fc_core.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 1d34e31742b..37d0bbda79a 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -43,7 +43,6 @@ void tryArm(void); disarmReason_t getDisarmReason(void); bool emergencyArmingUpdate(bool armingSwitchIsOn); -bool emergInflightRearmEnabled(void); bool areSensorsCalibrating(void); float getFlightTime(void); From 2d2195d841b31ace59b94831595a9e22cd366f25 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 29 Aug 2023 11:41:39 +0100 Subject: [PATCH 13/36] fix landing detector active state logic --- src/main/fc/fc_core.c | 2 ++ src/main/navigation/navigation.c | 13 +++++++------ src/main/navigation/navigation.h | 1 + 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a00e7972423..92705ab6910 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -558,6 +558,8 @@ void tryArm(void) ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED); } + resetLandingDetectorActiveState(); // reset landing detector after arming to avoid false detection before flight + lastDisarmReason = DISARM_NONE; ENABLE_ARMING_FLAG(ARMED); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index eba38c68b10..63c5b9884a9 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2809,16 +2809,12 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { - if (STATE(LANDING_DETECTED)) { - resetLandingDetector(); - landingDetectorIsActive = false; - } + resetLandingDetector(); + if (!IS_RC_MODE_ACTIVE(BOXARM)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); } return; - } else if (getArmTime() < 0.25f && landingDetectorIsActive) { // force reset landing detector immediately after arming - landingDetectorIsActive = false; } if (!landingDetectorIsActive) { @@ -2851,6 +2847,11 @@ void resetLandingDetector(void) posControl.flags.resetLandingDetector = true; } +void resetLandingDetectorActiveState(void) +{ + landingDetectorIsActive = false; +} + bool isFlightDetected(void) { return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 9ffe532e6c5..3fd213a2450 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -611,6 +611,7 @@ float calculateAverageSpeed(void); void updateLandingStatus(timeMs_t currentTimeMs); bool isProbablyStillFlying(void); +void resetLandingDetectorActiveState(void); const navigationPIDControllers_t* getNavigationPIDControllers(void); From fa60cfda81a4d50591bea9d74a85378f75844e81 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 6 Sep 2023 23:25:13 +0100 Subject: [PATCH 14/36] fixes --- src/main/fc/fc_core.c | 10 +++++----- src/main/navigation/navigation.c | 3 +++ 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 92705ab6910..19015989ca6 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -512,11 +512,11 @@ bool emergInflightRearmEnabled(void) return false; } - if (isProbablyStillFlying()) { - emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft - return true; - } else if (STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f) { - // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying + // allow emergency rearm if MR has vertical speed at least 1.5 sec after disarm indicating still flying + bool mcDisarmVertVelCheck = STATE(MULTIROTOR) && (currentTimeMs > US2MS(lastDisarmTimeUs) + 1500) && fabsf(getEstimatedActualVelocity(Z)) > 100.0f; + + if (isProbablyStillFlying() || mcDisarmVertVelCheck) { + emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft return true; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 63c5b9884a9..5b3f17bb2ab 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2809,6 +2809,9 @@ void updateLandingStatus(timeMs_t currentTimeMs) DEBUG_SET(DEBUG_LANDING, 1, STATE(LANDING_DETECTED)); if (!ARMING_FLAG(ARMED)) { + if (STATE(LANDING_DETECTED)) { + landingDetectorIsActive = false; + } resetLandingDetector(); if (!IS_RC_MODE_ACTIVE(BOXARM)) { From 0d3216ef07177c925f783ef9a4f886be5df70873 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 19 Sep 2023 20:55:00 +0200 Subject: [PATCH 15/36] Set EZ Tune on profile change --- src/main/fc/config.c | 3 +++ src/main/fc/fc_msp.c | 3 +++ src/main/flight/ez_tune.c | 1 - 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3b884db7474..65bca8f9ef4 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -423,6 +423,9 @@ bool setConfigProfile(uint8_t profileIndex) systemConfigMutable()->current_profile_index = profileIndex; // set the control rate profile to match setControlRateProfile(profileIndex); +#ifdef USE_EZ_TUNE + ezTuneUpdate(); +#endif return ret; } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b2c04e9dc1e..a60975b9dc4 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -696,6 +696,9 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, constrain(pidBank()->pid[i].D, 0, 255)); sbufWriteU8(dst, constrain(pidBank()->pid[i].FF, 0, 255)); } + #ifdef USE_EZ_TUNE + ezTuneUpdate(); + #endif break; case MSP_PIDNAMES: diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index c4846d39894..2703fe870e1 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -65,7 +65,6 @@ static float getYawPidScale(float input) { /** * Update INAV settings based on current EZTune settings * This has to be called every time control profile is changed, or EZTune settings are changed - * FIXME call on profile change * FIXME call on EZTune settings change */ void ezTuneUpdate(void) { From e772d085cdbdfecd0250f431cf5894b257ec797a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 19 Sep 2023 21:14:38 +0200 Subject: [PATCH 16/36] Add EZ Tune MSP frames --- src/main/fc/config.c | 1 + src/main/fc/fc_msp.c | 43 +++++++++++++++++++++++++++++ src/main/msp/msp_protocol_v2_inav.h | 2 ++ 3 files changed, 46 insertions(+) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 65bca8f9ef4..fa7d8936554 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -64,6 +64,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" +#include "flight/ez_tune.h" #include "fc/config.h" #include "fc/controlrate_profile.h" diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a60975b9dc4..f2c80bfd953 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -77,6 +77,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/servos.h" +#include "flight/ez_tune.h" #include "config/config_eeprom.h" #include "config/feature.h" @@ -1583,6 +1584,24 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; #endif +#ifdef USE_EZ_TUNE + + case MSP2_INAV_EZ_TUNE: + { + sbufWriteU8(dst, ezTune()->enabled); + sbufWriteU16(dst, ezTune()->filterHz); + sbufWriteU8(dst, ezTune()->axisRatio); + sbufWriteU8(dst, ezTune()->response); + sbufWriteU8(dst, ezTune()->damping); + sbufWriteU8(dst, ezTune()->stability); + sbufWriteU8(dst, ezTune()->aggressiveness); + sbufWriteU8(dst, ezTune()->rate); + sbufWriteU8(dst, ezTune()->expo); + } + break; + +#endif + default: return false; } @@ -3040,6 +3059,30 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) break; #endif +#ifdef USE_EZ_TUNE + + case MSP2_INAV_EZ_TUNE_SET: + { + if (dataSize == 10) { + ezTuneMutable()->enabled = sbufReadU8(src); + ezTuneMutable()->filterHz = sbufReadU16(src); + ezTuneMutable()->axisRatio = sbufReadU8(src); + ezTuneMutable()->response = sbufReadU8(src); + ezTuneMutable()->damping = sbufReadU8(src); + ezTuneMutable()->stability = sbufReadU8(src); + ezTuneMutable()->aggressiveness = sbufReadU8(src); + ezTuneMutable()->rate = sbufReadU8(src); + ezTuneMutable()->expo = sbufReadU8(src); + + ezTuneUpdate(); + } else { + return MSP_RESULT_ERROR; + } + } + break; + +#endif + default: return MSP_RESULT_ERROR; } diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index f746f5c8ff3..3c38c8ffd47 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -92,3 +92,5 @@ #define MSP2_INAV_LED_STRIP_CONFIG_EX 0x2048 #define MSP2_INAV_SET_LED_STRIP_CONFIG_EX 0x2049 +#define MSP2_INAV_EZ_TUNE 0x2050 +#define MSP2_INAV_EZ_TUNE_SET 0x2051 \ No newline at end of file From 52f5cffc43e73283f599054236ff5b460ea146ed Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 26 Sep 2023 23:33:56 +0100 Subject: [PATCH 17/36] prevent unwanted resets on disarm --- src/main/fc/fc_core.c | 12 +++++- src/main/fc/runtime_config.h | 1 + src/main/io/osd.c | 6 ++- src/main/navigation/navigation.c | 34 ++++++++++------ .../navigation/navigation_pos_estimator.c | 40 ++++++++++++++----- 5 files changed, 67 insertions(+), 26 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 76d1a2626c5..66c275366b4 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -436,6 +436,7 @@ void disarm(disarmReason_t disarmReason) lastDisarmReason = disarmReason; lastDisarmTimeUs = micros(); DISABLE_ARMING_FLAG(ARMED); + DISABLE_STATE(IN_FLIGHT_EMERG_REARM); #ifdef USE_BLACKBOX if (feature(FEATURE_BLACKBOX)) { @@ -522,6 +523,7 @@ bool emergInflightRearmEnabled(void) if (isProbablyStillFlying() || mcDisarmVertVelCheck) { emergRearmStabiliseTimeout = currentTimeMs + 5000; // activate Angle mode for 5s after rearm to help stabilise craft + ENABLE_STATE(IN_FLIGHT_EMERG_REARM); return true; } @@ -574,11 +576,13 @@ void tryArm(void) ENABLE_ARMING_FLAG(WAS_EVER_ARMED); //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); - logicConditionReset(); + if (!STATE(IN_FLIGHT_EMERG_REARM)) { + logicConditionReset(); #ifdef USE_PROGRAMMING_FRAMEWORK - programmingPidReset(); + programmingPidReset(); #endif + } headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); @@ -902,6 +906,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs) processDelayedSave(); } + if (armTime > 1 * USECS_PER_SEC) { // reset in flight emerg rearm flag 1 sec after arming once it's served its purpose + DISABLE_STATE(IN_FLIGHT_EMERG_REARM); + } + #if defined(SITL_BUILD) if (lockMainPID()) { #endif diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 16d1b411df8..241e5f7e258 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -139,6 +139,7 @@ typedef enum { FW_HEADING_USE_YAW = (1 << 24), ANTI_WINDUP_DEACTIVATED = (1 << 25), LANDING_DETECTED = (1 << 26), + IN_FLIGHT_EMERG_REARM = (1 << 27), } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f5daeaf63c2..b34751096eb 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -4579,9 +4579,13 @@ static void osdRefresh(timeUs_t currentTimeUs) osdResetStats(); osdShowArmed(); uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; + if (STATE(IN_FLIGHT_EMERG_REARM)) { + delay /= 3; + } #if defined(USE_SAFE_HOME) - if (posControl.safehomeState.distance) + else if (posControl.safehomeState.distance) { delay *= 3; + } #endif osdSetNextRefreshIn(delay); } else { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b6cf9583c7c..0465eceab6c 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2530,11 +2530,15 @@ bool findNearestSafeHome(void) *-----------------------------------------------------------*/ void updateHomePosition(void) { - // Disarmed and have a valid position, constantly update home + // Disarmed and have a valid position, constantly update home before first arm (depending on setting) + // Update immediately after arming thereafter if reset on each arm (required to avoid home reset after emerg in flight rearm) + static bool setHome = false; + navSetWaypointFlags_t homeUpdateFlags = NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING; + if (!ARMING_FLAG(ARMED)) { if (posControl.flags.estPosStatus >= EST_USABLE) { const navigationHomeFlags_t validHomeFlags = NAV_HOME_VALID_XY | NAV_HOME_VALID_Z; - bool setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags; + setHome = (posControl.rthState.homeFlags & validHomeFlags) != validHomeFlags; switch ((nav_reset_type_e)positionEstimationConfig()->reset_home_type) { case NAV_RESET_NEVER: break; @@ -2545,24 +2549,16 @@ void updateHomePosition(void) setHome = true; break; } - if (setHome) { -#if defined(USE_SAFE_HOME) - findNearestSafeHome(); -#endif - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - // save the current location in case it is replaced by a safehome or HOME_RESET - posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos; - } } } else { static bool isHomeResetAllowed = false; - // If pilot so desires he may reset home position to current position if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) { if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) { - const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity()); + homeUpdateFlags = 0; + homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + setHome = true; isHomeResetAllowed = false; } } @@ -2577,6 +2573,18 @@ void updateHomePosition(void) posControl.homeDirection = calculateBearingToDestination(tmpHomePos); updateHomePositionCompatibility(); } + + setHome &= !STATE(IN_FLIGHT_EMERG_REARM); // prevent reset following emerg in flight rearm + } + + if (setHome && (!ARMING_FLAG(WAS_EVER_ARMED) || ARMING_FLAG(ARMED))) { +#if defined(USE_SAFE_HOME) + findNearestSafeHome(); +#endif + setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity()); + // save the current location in case it is replaced by a safehome or HOME_RESET + posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos; + setHome = false; } } diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 5c314bfddda..220ff277c4e 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -108,15 +108,35 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur } } -static bool shouldResetReferenceAltitude(void) +static bool shouldResetReferenceAltitude(uint8_t updateSource) { - switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { - case NAV_RESET_NEVER: - return false; - case NAV_RESET_ON_FIRST_ARM: - return !ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED); - case NAV_RESET_ON_EACH_ARM: - return !ARMING_FLAG(ARMED); + /* Altitude reference reset constantly before first arm. + * After first arm altitude ref only reset immediately after arming (to avoid reset after emerg in flight rearm) + * Need to check reset status for both altitude sources immediately after rearming before reset complete */ + + static bool resetAltitudeOnArm = false; + if (ARMING_FLAG(ARMED) && resetAltitudeOnArm) { + static uint8_t sourceCheck = 0; + sourceCheck |= updateSource; + bool allAltitudeSources = STATE(GPS_FIX) && sensors(SENSOR_BARO); + + if ((allAltitudeSources && sourceCheck > SENSOR_GPS) || (!allAltitudeSources && sourceCheck)) { + resetAltitudeOnArm = false; + sourceCheck = 0; + } + return !STATE(IN_FLIGHT_EMERG_REARM); + } + + if (!ARMING_FLAG(ARMED)) { + switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { + case NAV_RESET_NEVER: + return false; + case NAV_RESET_ON_FIRST_ARM: + break; + case NAV_RESET_ON_EACH_ARM: + resetAltitudeOnArm = true; + } + return !ARMING_FLAG(WAS_EVER_ARMED); } return false; @@ -226,7 +246,7 @@ void onNewGPSData(void) if (!posControl.gpsOrigin.valid) { geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_SET); } - else if (shouldResetReferenceAltitude()) { + else if (shouldResetReferenceAltitude(SENSOR_GPS)) { /* If we were never armed - keep altitude at zero */ geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_RESET_ALTITUDE); } @@ -309,7 +329,7 @@ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) float newBaroAlt = baroCalculateAltitude(); /* If we are required - keep altitude at zero */ - if (shouldResetReferenceAltitude()) { + if (shouldResetReferenceAltitude(SENSOR_BARO)) { initialBaroAltitudeOffset = newBaroAlt; } From a181fc7f4eca15e8ff5e29fe54d3dd22dbb12eac Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 28 Sep 2023 13:19:10 +0100 Subject: [PATCH 18/36] fixes - rearm check sequence + altitude reset --- src/main/fc/fc_core.c | 7 +++---- src/main/io/osd.c | 2 +- src/main/navigation/navigation_pos_estimator.c | 3 +++ 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 66c275366b4..1272ad3aa11 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -553,10 +553,10 @@ void tryArm(void) #endif #ifdef USE_PROGRAMMING_FRAMEWORK - if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled() || + if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) { #else - if (!isArmingDisabled() || emergencyArmingIsEnabled() || emergInflightRearmEnabled()) { + if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled()) { #endif // If nav_extra_arming_safety was bypassed we always // allow bypassing it even without the sticks set @@ -568,8 +568,6 @@ void tryArm(void) ENABLE_STATE(NAV_EXTRA_ARMING_SAFETY_BYPASSED); } - resetLandingDetectorActiveState(); // reset landing detector after arming to avoid false detection before flight - lastDisarmReason = DISARM_NONE; ENABLE_ARMING_FLAG(ARMED); @@ -578,6 +576,7 @@ void tryArm(void) ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); if (!STATE(IN_FLIGHT_EMERG_REARM)) { + resetLandingDetectorActiveState(); // reset landing detector after arming to avoid false detection before flight logicConditionReset(); #ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b34751096eb..3e464f2d764 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1202,7 +1202,7 @@ int32_t osdGetAltitude(void) static inline int32_t osdGetAltitudeMsl(void) { - return getEstimatedActualPosition(Z)+GPS_home.alt; + return getEstimatedActualPosition(Z) + posControl.gpsOrigin.alt; } uint16_t osdGetRemainingGlideTime(void) { diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 220ff277c4e..4422d6fe9ef 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -121,6 +121,9 @@ static bool shouldResetReferenceAltitude(uint8_t updateSource) bool allAltitudeSources = STATE(GPS_FIX) && sensors(SENSOR_BARO); if ((allAltitudeSources && sourceCheck > SENSOR_GPS) || (!allAltitudeSources && sourceCheck)) { + if (positionEstimationConfig()->reset_home_type == NAV_RESET_ON_EACH_ARM) { + posControl.rthState.homePosition.pos.z = 0; + } resetAltitudeOnArm = false; sourceCheck = 0; } From 3c6128e8eb0bd55349eaa21b89e9d9f9f5413298 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 1 Oct 2023 15:30:35 +0100 Subject: [PATCH 19/36] simplify alt ref reset + fix 0 home alt --- src/main/navigation/navigation.c | 4 ++ .../navigation/navigation_pos_estimator.c | 58 +++++++++---------- 2 files changed, 31 insertions(+), 31 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0465eceab6c..81c9b4b85c8 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2582,6 +2582,10 @@ void updateHomePosition(void) findNearestSafeHome(); #endif setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, homeUpdateFlags, navigationActualStateHomeValidity()); + + if (ARMING_FLAG(ARMED) && positionEstimationConfig()->reset_altitude_type == NAV_RESET_ON_EACH_ARM) { + posControl.rthState.homePosition.pos.z = 0; // force to 0 if reference altitude also reset every arm + } // save the current location in case it is replaced by a safehome or HOME_RESET posControl.rthState.originalHomePosition = posControl.rthState.homePosition.pos; setHome = false; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 4422d6fe9ef..d7ef73c9382 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -54,6 +54,7 @@ #include "sensors/opflow.h" navigationPosEstimator_t posEstimator; +static float initialBaroAltitudeOffset = 0.0f; PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 5); @@ -108,38 +109,34 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur } } -static bool shouldResetReferenceAltitude(uint8_t updateSource) +static bool shouldResetReferenceAltitude(void) { - /* Altitude reference reset constantly before first arm. - * After first arm altitude ref only reset immediately after arming (to avoid reset after emerg in flight rearm) - * Need to check reset status for both altitude sources immediately after rearming before reset complete */ - - static bool resetAltitudeOnArm = false; - if (ARMING_FLAG(ARMED) && resetAltitudeOnArm) { - static uint8_t sourceCheck = 0; - sourceCheck |= updateSource; - bool allAltitudeSources = STATE(GPS_FIX) && sensors(SENSOR_BARO); - - if ((allAltitudeSources && sourceCheck > SENSOR_GPS) || (!allAltitudeSources && sourceCheck)) { - if (positionEstimationConfig()->reset_home_type == NAV_RESET_ON_EACH_ARM) { - posControl.rthState.homePosition.pos.z = 0; - } - resetAltitudeOnArm = false; - sourceCheck = 0; + /* Reference altitudes reset constantly when disarmed. + * On arming ref altitudes saved as backup in case of emerg in flight rearm + * If emerg in flight rearm active ref altitudes reset to backup values to avoid unwanted altitude reset */ + + static float backupInitialBaroAltitudeOffset = 0.0f; + static int32_t backupGpsOriginAltitude = 0; + static bool emergRearmResetCheck = false; + + if (ARMING_FLAG(ARMED) && emergRearmResetCheck) { + if (STATE(IN_FLIGHT_EMERG_REARM)) { + initialBaroAltitudeOffset = backupInitialBaroAltitudeOffset; + posControl.gpsOrigin.alt = backupGpsOriginAltitude; + } else { + backupInitialBaroAltitudeOffset = initialBaroAltitudeOffset; + backupGpsOriginAltitude = posControl.gpsOrigin.alt; } - return !STATE(IN_FLIGHT_EMERG_REARM); } + emergRearmResetCheck = !ARMING_FLAG(ARMED); - if (!ARMING_FLAG(ARMED)) { - switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { - case NAV_RESET_NEVER: - return false; - case NAV_RESET_ON_FIRST_ARM: - break; - case NAV_RESET_ON_EACH_ARM: - resetAltitudeOnArm = true; - } - return !ARMING_FLAG(WAS_EVER_ARMED); + switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) { + case NAV_RESET_NEVER: + return false; + case NAV_RESET_ON_FIRST_ARM: + return !ARMING_FLAG(ARMED) && !ARMING_FLAG(WAS_EVER_ARMED); + case NAV_RESET_ON_EACH_ARM: + return !ARMING_FLAG(ARMED); } return false; @@ -249,7 +246,7 @@ void onNewGPSData(void) if (!posControl.gpsOrigin.valid) { geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_SET); } - else if (shouldResetReferenceAltitude(SENSOR_GPS)) { + else if (shouldResetReferenceAltitude()) { /* If we were never armed - keep altitude at zero */ geoSetOrigin(&posControl.gpsOrigin, &newLLH, GEO_ORIGIN_RESET_ALTITUDE); } @@ -328,11 +325,10 @@ void onNewGPSData(void) */ void updatePositionEstimator_BaroTopic(timeUs_t currentTimeUs) { - static float initialBaroAltitudeOffset = 0.0f; float newBaroAlt = baroCalculateAltitude(); /* If we are required - keep altitude at zero */ - if (shouldResetReferenceAltitude(SENSOR_BARO)) { + if (shouldResetReferenceAltitude()) { initialBaroAltitudeOffset = newBaroAlt; } From ae6b6aa7c0ea070cc5ebf452cfba007039c0a96c Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 3 Oct 2023 21:32:13 +0200 Subject: [PATCH 20/36] docker build: do not run user commands --- Dockerfile | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Dockerfile b/Dockerfile index 9c816b0c183..e0a3949517f 100644 --- a/Dockerfile +++ b/Dockerfile @@ -4,15 +4,15 @@ ARG USER_ID ARG GROUP_ID ENV DEBIAN_FRONTEND noninteractive -RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi +RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build RUN pip install pyyaml # if either of these are already set the same as the user's machine, leave them be and ignore the error -RUN addgroup --gid $GROUP_ID inav; exit 0; -RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID inav; exit 0; +RUN if [ -n "$USER_ID" ]; then RUN addgroup --gid $GROUP_ID inav; exit 0; fi +RUN if [ -n "$USER_ID" ]; then RUN adduser --disabled-password --gecos '' --uid $USER_ID --gid $GROUP_ID inav; exit 0; fi -USER inav +RUN if [ -n "$USER_ID" ]; then USER inav; fi RUN git config --global --add safe.directory /src VOLUME /src From 55c1abea376acb8958c06939b4800b53a012fa09 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 3 Oct 2023 21:32:35 +0200 Subject: [PATCH 21/36] docked build: build with ninja --- cmake/docker.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/docker.sh b/cmake/docker.sh index 3c10ebc8e1d..843e03a48a2 100755 --- a/cmake/docker.sh +++ b/cmake/docker.sh @@ -6,7 +6,7 @@ CURR_REV="$(git rev-parse HEAD)" initialize_cmake() { echo -e "*** CMake was not initialized yet, doing it now.\n" - cmake .. + cmake -GNinja .. echo "$CURR_REV" > "$LAST_CMAKE_AT_REV_FILE" } @@ -26,4 +26,4 @@ else fi # Let Make handle the arguments coming from the build script -make "$@" +ninja "$@" From ff947cb7439f945b7fe532d66090ebfed179f743 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Tue, 3 Oct 2023 21:33:30 +0200 Subject: [PATCH 22/36] docked build: added scripts for sitl build and run --- cmake/docker_build_sitl.sh | 6 ++++++ cmake/docker_run_sitl.sh | 8 ++++++++ 2 files changed, 14 insertions(+) create mode 100644 cmake/docker_build_sitl.sh create mode 100644 cmake/docker_run_sitl.sh diff --git a/cmake/docker_build_sitl.sh b/cmake/docker_build_sitl.sh new file mode 100644 index 00000000000..b1a4ada5ca2 --- /dev/null +++ b/cmake/docker_build_sitl.sh @@ -0,0 +1,6 @@ +#!/bin/bash +rm -r build_SITL +mkdir -p build_SITL +cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL .. +cd build_SITL +ninja \ No newline at end of file diff --git a/cmake/docker_run_sitl.sh b/cmake/docker_run_sitl.sh new file mode 100644 index 00000000000..b2089137ccb --- /dev/null +++ b/cmake/docker_run_sitl.sh @@ -0,0 +1,8 @@ +#!/bin/bash +cd build_SITL + +#Lauch SITL - configurator only mode +./inav_7.0.0_SITL + +#Launch SITL - connect to X-Plane. IP address should be host IP address, not 127.0.0.1. Can be found in X-Plane "Network" tab. +#./inav_7.0.0_SITL --sim=xp --simip=192.168.2.105 --simport=49000 \ No newline at end of file From a61a175107b83e0cce0c2e81a491a43cd84da4ea Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Fri, 6 Oct 2023 02:11:54 +0300 Subject: [PATCH 23/36] Update Building in Docker.md --- docs/development/Building in Docker.md | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/docs/development/Building in Docker.md b/docs/development/Building in Docker.md index 80d661ecf37..39cd887003a 100755 --- a/docs/development/Building in Docker.md +++ b/docs/development/Building in Docker.md @@ -37,8 +37,19 @@ You'll have to manually execute the same steps that the build script does: + This step is only needed the first time. 2. `docker run --rm -it -u root -v :/src inav-build ` + Where `` must be replaced with the absolute path of where you cloned this repo (see above), and `` with the name of the target that you want to build. - + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above, or by removing "USER inav" line from the .\DockerFile before building image. + + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above. -3. If you need to update `Settings.md`, run `docker run --entrypoint /src/cmake/docker_docs.sh --rm -it -u root -v :/src inav-build` +3. If you need to update `Settings.md`, run: + +`docker run --entrypoint /src/cmake/docker_docs.sh --rm -it -u root -v :/src inav-build` + +4. Building SITL: + +`docker run --rm --entrypoint /src/cmake/docker_build_sitl.sh -it -u root -v :/src inav-build` + +5. Running SITL: + +`docker run -p 5760:5760 -p 5761:5761 -p 5762:5762 -p 5763:5763 -p 5764:5764 -p 5765:5765 -p 5766:5766 -p 5767:5767 --entrypoint /src/cmake/docker_run_sitl.sh --rm -it -u root -v :/src inav-build`. + + SITL command line parameters can be adjusted in `cmake/docker_run_sitl.sh`. Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details. From 9239b5df06e7eda275b7306d678391cb14976515 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 7 Oct 2023 17:51:58 +0200 Subject: [PATCH 24/36] add gdb to allow debugging in dev containers --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index e0a3949517f..71077c8ed96 100644 --- a/Dockerfile +++ b/Dockerfile @@ -4,7 +4,7 @@ ARG USER_ID ARG GROUP_ID ENV DEBIAN_FRONTEND noninteractive -RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build +RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build gdb RUN pip install pyyaml From 79e14749d5ba369bb4272acc2ddc4decd3a93657 Mon Sep 17 00:00:00 2001 From: Roman Lut <11955117+RomanLut@users.noreply.github.com> Date: Sat, 7 Oct 2023 17:52:37 +0200 Subject: [PATCH 25/36] add option to build sitl with debug information --- cmake/docker_build_sitl.sh | 3 ++- cmake/sitl.cmake | 5 +++++ 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/cmake/docker_build_sitl.sh b/cmake/docker_build_sitl.sh index b1a4ada5ca2..a79742ae0ff 100644 --- a/cmake/docker_build_sitl.sh +++ b/cmake/docker_build_sitl.sh @@ -1,6 +1,7 @@ #!/bin/bash rm -r build_SITL mkdir -p build_SITL -cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL .. +#cmake -DSITL=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL .. +cmake -DSITL=ON -DDEBUG=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL .. cd build_SITL ninja \ No newline at end of file diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 24a4ae9b27e..78697f98a9d 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -53,6 +53,11 @@ set(SITL_COMPILE_OPTIONS -funsigned-char ) +if(DEBUG) + message(STATUS "Debug mode enabled. Adding -g to SITL_COMPILE_OPTIONS.") + list(APPEND SITL_COMPILE_OPTIONS -g) +endif() + if(NOT MACOSX) set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} -Wno-return-local-addr From 6453c3b43f65b281f21474e8f37fa0fdca0bf69b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 8 Oct 2023 13:35:46 +0200 Subject: [PATCH 26/36] Remove not needed comment --- src/main/flight/ez_tune.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 2703fe870e1..ef56143cf97 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -65,7 +65,6 @@ static float getYawPidScale(float input) { /** * Update INAV settings based on current EZTune settings * This has to be called every time control profile is changed, or EZTune settings are changed - * FIXME call on EZTune settings change */ void ezTuneUpdate(void) { if (ezTune()->enabled) { From f364d3bb60d53e5bb0bd9cecab91d4e12fec3ea9 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 8 Oct 2023 14:27:53 +0200 Subject: [PATCH 27/36] Compute rate from EZ Tune --- src/main/flight/ez_tune.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index ef56143cf97..b08e7cf9ca5 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -32,6 +32,7 @@ #include "fc/settings.h" #include "flight/pid.h" #include "sensors/gyro.h" +#include "fc/controlrate_profile.h" PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0); @@ -127,5 +128,14 @@ void ezTuneUpdate(void) { pidProfileMutable()->bank_mc.pid[PID_YAW].I = pidDefaultsYaw[1] * getYawPidScale(ezTune()->stability); pidProfileMutable()->bank_mc.pid[PID_YAW].D = pidDefaultsYaw[2] * getYawPidScale(ezTune()->damping); pidProfileMutable()->bank_mc.pid[PID_YAW].FF = pidDefaultsYaw[3] * getYawPidScale(ezTune()->aggressiveness); + + //Setup rates + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_ROLL] = scaleRange(ezTune()->rate, 0, 200, 30, 90); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_PITCH] = scaleRange(ezTune()->rate, 0, 200, 30, 90); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_YAW] = scaleRange(ezTune()->rate, 0, 200, 30, 90) - 10; + + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100); + } } \ No newline at end of file From 059a46daad1551e39fca49ee8f60567a9972efd0 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 8 Oct 2023 17:39:54 +0200 Subject: [PATCH 28/36] Docs --- docs/Settings.md | 90 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index c919327194b..ac5df304b4a 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -872,6 +872,96 @@ Enable when BLHeli32 Auto Telemetry function is used. Disable in every other cas --- +### ez_aggressiveness + +EzTune aggressiveness + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### ez_axis_ratio + +EzTune axis ratio + +| Default | Min | Max | +| --- | --- | --- | +| 110 | 25 | 175 | + +--- + +### ez_damping + +EzTune damping + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### ez_enabled + +Enables EzTune feature + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### ez_expo + +EzTune expo + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### ez_filter_hz + +EzTune filter cutoff frequency + +| Default | Min | Max | +| --- | --- | --- | +| 110 | 10 | 300 | + +--- + +### ez_rate + +EzTune rate + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### ez_response + +EzTune response + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + +### ez_stability + +EzTune stability + +| Default | Min | Max | +| --- | --- | --- | +| 100 | 0 | 200 | + +--- + ### failsafe_delay Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). From 37e1faced5a3ce859afa27e961be6905f1c0fa9c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 8 Oct 2023 17:42:46 +0200 Subject: [PATCH 29/36] Make features aware --- src/main/flight/ez_tune.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index b08e7cf9ca5..918c00a1221 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -88,12 +88,15 @@ void ezTuneUpdate(void) { gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), getGyroLooptime() * 0.5f); gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; +#ifdef USE_DYNAMIC_FILTERS //Enable dynamic notch gyroConfigMutable()->dynamicGyroNotchEnabled = 1; gyroConfigMutable()->dynamicGyroNotchQ = 250; gyroConfigMutable()->dynamicGyroNotchMinHz = MAX(ezTune()->filterHz * 0.667f, SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT); gyroConfigMutable()->dynamicGyroNotchMode = DYNAMIC_NOTCH_MODE_3D; +#endif +#ifdef USE_GYRO_KALMAN //Make sure Kalman filter is enabled gyroConfigMutable()->kalmanEnabled = 1; if (ezTune()->filterHz < 150) { @@ -101,6 +104,7 @@ void ezTuneUpdate(void) { } else { gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400); } +#endif //Disable dynamic LPF gyroConfigMutable()->useDynamicLpf = 0; From afd2848d791baf733ea39446ce62d85857a3cd8a Mon Sep 17 00:00:00 2001 From: Hugo Chiang <31283897+DusKing1@users.noreply.github.com> Date: Sat, 14 Oct 2023 08:37:50 +0800 Subject: [PATCH 30/36] add icm42605 driver for SKYSTARSF405HD --- src/main/target/SKYSTARSF405HD/target.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/target/SKYSTARSF405HD/target.h b/src/main/target/SKYSTARSF405HD/target.h index ac88a720847..b8a696e14d4 100644 --- a/src/main/target/SKYSTARSF405HD/target.h +++ b/src/main/target/SKYSTARSF405HD/target.h @@ -45,6 +45,11 @@ #define BMI270_SPI_BUS BUS_SPI1 #define BMI270_CS_PIN PA4 +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + // *************** M25P256 flash ******************** #define USE_FLASHFS #define USE_FLASH_M25P16 From 5e608b7f66af313b5e95aa229010dd89b15c2484 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 19 Oct 2023 17:18:45 +0100 Subject: [PATCH 31/36] Update navigation.c --- src/main/navigation/navigation.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 4a871de084e..69a9c0479a9 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -949,7 +949,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, - + /** MIXER AUTOMATED TRANSITION mode, alternated althod ***************************************************/ [NAV_STATE_MIXERAT_INITIALIZE] = { .persistentId = NAV_PERSISTENT_ID_MIXERAT_INITIALIZE, @@ -992,7 +992,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .onEvent = { [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - + } }, }; @@ -1514,7 +1514,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - + if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){ return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } @@ -3919,14 +3919,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } // CRUISE has priority over COURSE_HOLD and AH - if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE) && STATE(AIRPLANE)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) { if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)) return NAV_FSM_EVENT_SWITCH_TO_CRUISE; } // PH has priority over COURSE_HOLD // CRUISE has priority on AH - if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) && STATE(AIRPLANE)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) { if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) { return NAV_FSM_EVENT_SWITCH_TO_CRUISE; } From 3f5b320dd6ad708c983ce83c5f5d4fcf1755bb45 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 20 Oct 2023 11:31:54 +0200 Subject: [PATCH 32/36] Fix antialiasing filter freqency calculation --- src/main/flight/ez_tune.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 918c00a1221..0f38b392118 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -71,11 +71,10 @@ void ezTuneUpdate(void) { if (ezTune()->enabled) { // Setup filtering - //Enable Smith predictor pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz) + computePt1FilterDelayMs(gyroConfig()->gyro_anti_aliasing_lpf_hz); - + //Set Dterm LPF pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50); pidProfileMutable()->dterm_lpf_type = FILTER_PT2; @@ -85,7 +84,8 @@ void ezTuneUpdate(void) { gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1; //Set anti-aliasing filter - gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), getGyroLooptime() * 0.5f); + const int gyroNyquist = 1000000 / TASK_GYRO_LOOPTIME; + gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), (int)(gyroNyquist * 0.5f)); gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; #ifdef USE_DYNAMIC_FILTERS From 927597c3c903196faca0e65f2fa3916efa668121 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 20 Oct 2023 12:09:42 +0200 Subject: [PATCH 33/36] Imrove processing --- src/main/flight/ez_tune.c | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c index 0f38b392118..c6c57afbfe5 100644 --- a/src/main/flight/ez_tune.c +++ b/src/main/flight/ez_tune.c @@ -54,7 +54,7 @@ PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune, #define EZ_TUNE_YAW_SCALE 0.5f static float computePt1FilterDelayMs(uint8_t filterHz) { - return 1.0f / (2.0f * M_PIf * filterHz); + return 1000.0f / (2.0f * M_PIf * filterHz); } static float getYawPidScale(float input) { @@ -71,10 +71,6 @@ void ezTuneUpdate(void) { if (ezTune()->enabled) { // Setup filtering - //Enable Smith predictor - pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz) - + computePt1FilterDelayMs(gyroConfig()->gyro_anti_aliasing_lpf_hz); - //Set Dterm LPF pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50); pidProfileMutable()->dterm_lpf_type = FILTER_PT2; @@ -84,10 +80,12 @@ void ezTuneUpdate(void) { gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1; //Set anti-aliasing filter - const int gyroNyquist = 1000000 / TASK_GYRO_LOOPTIME; - gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), (int)(gyroNyquist * 0.5f)); + gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT; gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1; + //Enable Smith predictor + pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz); + #ifdef USE_DYNAMIC_FILTERS //Enable dynamic notch gyroConfigMutable()->dynamicGyroNotchEnabled = 1; From 3456d60e9a4125859eded11d5a05e5fdcb0002c3 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 21 Oct 2023 17:30:18 +0200 Subject: [PATCH 34/36] Add SYM_TOTAL -> BF_SYM_TOTAL_DISTANCE mapping --- src/main/io/displayport_msp_bf_compat.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index 7cfa8f1bf63..b22c4affe9c 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -187,13 +187,10 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_ALT_M: return BF_SYM_M; -/* case SYM_TRIP_DIST: - return BF_SYM_TRIP_DIST; - case SYM_TOTAL: - return BF_SYM_TOTAL; - + return BF_SYM_TOTAL_DISTANCE; +/* case SYM_ALT_KM: return BF_SYM_ALT_KM; From 201398e7517178de03a421c43933787ec171b71a Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 21 Oct 2023 17:34:54 +0200 Subject: [PATCH 35/36] SYM_TOTAL and SYM_TRIP_DISTANCE are the same symbol --- src/main/io/displayport_msp_bf_compat.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index b22c4affe9c..a9831929a7e 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -187,7 +187,6 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_ALT_M: return BF_SYM_M; - case SYM_TRIP_DIST: case SYM_TOTAL: return BF_SYM_TOTAL_DISTANCE; /* From ab20702dff3c342b31e9a24daecfb272b0e47fd7 Mon Sep 17 00:00:00 2001 From: Marcelo Bezerra <23555060+mmosca@users.noreply.github.com> Date: Sat, 21 Oct 2023 22:48:22 +0200 Subject: [PATCH 36/36] Add G_FORCE symbol as letter G --- src/main/io/displayport_msp_bf_compat.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index a9831929a7e..315ac9a90c0 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -330,10 +330,12 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_PITCH_DOWN: return BF_SYM_PITCH_DOWN; + */ case SYM_GFORCE: - return BF_SYM_GFORCE; + return 'G'; +/* case SYM_GFORCE_X: return BF_SYM_GFORCE_X;