1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00

Merge remote-tracking branch 'upstream/master' into abo_acro_attitude_hold

This commit is contained in:
breadoven 2023-10-24 11:19:55 +01:00
commit c775c29256
30 changed files with 710 additions and 71 deletions

66
.vscode/c_cpp_properties.json vendored Executable file
View file

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

41
.vscode/tasks.json vendored Executable file
View file

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

View file

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

View file

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

View file

@ -0,0 +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 -DDEBUG=ON -DWARNINGS_AS_ERRORS=ON -GNinja -B build_SITL ..
cd build_SITL
ninja

8
cmake/docker_run_sitl.sh Normal file
View file

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

View file

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

View file

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

View file

@ -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 <PATH_TO_REPO>:/src inav-build <TARGET>`
+ Where `<PATH_TO_REPO>` must be replaced with the absolute path of where you cloned this repo (see above), and `<TARGET>` 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 <PATH_TO_REPO>:/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 <PATH_TO_REPO>:/src inav-build`
4. Building SITL:
`docker run --rm --entrypoint /src/cmake/docker_build_sitl.sh -it -u root -v <PATH_TO_REPO>:/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 <PATH_TO_REPO>:/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.

View file

@ -341,6 +341,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

View file

@ -120,7 +120,8 @@
#define PG_POWER_LIMITS_CONFIG 1030
#define PG_OSD_COMMON_CONFIG 1031
#define PG_TIMER_OVERRIDE_CONFIG 1032
#define PG_INAV_END 1032
#define PG_EZ_TUNE 1033
#define PG_INAV_END PG_EZ_TUNE
// OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047

View file

@ -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"
@ -426,6 +427,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;
}

View file

@ -106,6 +106,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;
@ -120,6 +121,7 @@ uint8_t motorControlEnable = false;
static bool isRXDataNew;
static disarmReason_t lastDisarmReason = DISARM_NONE;
timeUs_t lastDisarmTimeUs = 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;
@ -315,11 +317,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 */
@ -435,6 +437,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)) {
@ -505,14 +508,27 @@ bool emergencyArmingUpdate(bool armingSwitchIsOn, bool forceArm)
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)
bool emergInflightRearmEnabled(void)
{
/* Emergency rearm allowed within 5s timeout period after disarm if craft still flying */
timeMs_t currentTimeMs = millis();
emergRearmStabiliseTimeout = 0;
void releaseSharedTelemetryPorts(void) {
serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
while (sharedPort) {
mspSerialReleasePortIfAllocated(sharedPort);
sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP);
if ((lastDisarmReason != DISARM_SWITCH && lastDisarmReason != DISARM_KILLSWITCH) ||
(currentTimeMs > US2MS(lastDisarmTimeUs) + EMERGENCY_INFLIGHT_REARM_TIME_WINDOW_MS)) {
return false;
}
// 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
ENABLE_STATE(IN_FLIGHT_EMERG_REARM);
return true;
}
return false; // craft doesn't appear to be flying, don't allow emergency rearm
}
void tryArm(void)
@ -538,9 +554,10 @@ void tryArm(void)
#endif
#ifdef USE_PROGRAMMING_FRAMEWORK
if (!isArmingDisabled() || emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) {
if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled() ||
LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY)) {
#else
if (!isArmingDisabled() || emergencyArmingIsEnabled()) {
if (emergInflightRearmEnabled() || !isArmingDisabled() || emergencyArmingIsEnabled()) {
#endif
// If nav_extra_arming_safety was bypassed we always
// allow bypassing it even without the sticks set
@ -558,11 +575,14 @@ 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)) {
resetLandingDetectorActiveState(); // reset landing detector after arming to avoid false detection before flight
logicConditionReset();
#ifdef USE_PROGRAMMING_FRAMEWORK
programmingPidReset();
programmingPidReset();
#endif
}
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
@ -595,6 +615,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
@ -645,13 +675,17 @@ void processRx(timeUs_t currentTimeUs)
processRcAdjustments(CONST_CAST(controlRateConfig_t*, currentControlRateProfile), canUseRxData);
}
// 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;
/* Disable modes initially, will be enabled as required with priority ANGLE > HORIZON > ATTITUDE HOLD */
DISABLE_FLIGHT_MODE(ANGLE_MODE);
DISABLE_FLIGHT_MODE(HORIZON_MODE);
DISABLE_FLIGHT_MODE(ATTIHOLD_MODE);
if (sensors(SENSOR_ACC)) {
if (IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) {
if (IS_RC_MODE_ACTIVE(BOXANGLE) || autoEnableAngle) {
ENABLE_FLIGHT_MODE(ANGLE_MODE);
} else if (IS_RC_MODE_ACTIVE(BOXHORIZON)) {
ENABLE_FLIGHT_MODE(HORIZON_MODE);
@ -862,6 +896,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
@ -911,15 +949,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 (!ARMING_FLAG(SIMULATOR_MODE_HITL)) {
if (isServoOutputEnabled()) {
writeServos();
}
if (motorControlEnable) {
writeMotors();
}
}
if (motorControlEnable) {
writeMotors();
}
}
#else
if (isServoOutputEnabled()) {
writeServos();

View file

@ -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"
@ -515,6 +516,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);

View file

@ -78,6 +78,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"
@ -697,6 +698,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:
@ -1582,6 +1586,23 @@ 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
#ifdef USE_RATE_DYNAMICS
case MSP2_INAV_RATE_DYNAMICS:
@ -3055,6 +3076,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
#ifdef USE_RATE_DYNAMICS
case MSP2_INAV_SET_RATE_DYNAMICS:
@ -3259,6 +3304,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:

View file

@ -140,6 +140,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))

View file

@ -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();
case MIXER_CONFIG_VALUE:

View file

@ -35,6 +35,7 @@ typedef enum {
CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET),
BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET),
MIXER_CONFIG_VALUE = (4 << SETTING_SECTION_OFFSET),
EZ_TUNE_VALUE = (5 << SETTING_SECTION_OFFSET)
} setting_section_e;
typedef enum {

View file

@ -1492,6 +1492,65 @@ 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_enabled
description: "Enables EzTune feature"
default_value: OFF
field: enabled
type: bool
- name: ez_filter_hz
description: "EzTune filter cutoff frequency"
default_value: 110
field: filterHz
min: 10
max: 300
- name: ez_axis_ratio
description: "EzTune axis ratio"
default_value: 110
field: axisRatio
min: 25
max: 175
- name: ez_response
description: "EzTune response"
default_value: 100
field: response
min: 0
max: 200
- name: ez_damping
description: "EzTune damping"
default_value: 100
field: damping
min: 0
max: 200
- name: ez_stability
description: "EzTune stability"
default_value: 100
field: stability
min: 0
max: 200
- 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"]
condition: USE_RPM_FILTER

143
src/main/flight/ez_tune.c Normal file
View file

@ -0,0 +1,143 @@
/*
* 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"
#include "fc/controlrate_profile.h"
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0);
PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune,
.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 }
#define EZ_TUNE_PID_YAW_DEFAULT { 45, 80, 0, 100 }
#define EZ_TUNE_YAW_SCALE 0.5f
static float computePt1FilterDelayMs(uint8_t filterHz) {
return 1000.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
*/
void ezTuneUpdate(void) {
if (ezTune()->enabled) {
// Setup filtering
//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 = 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;
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) {
gyroConfigMutable()->kalman_q = 200;
} else {
gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400);
}
#endif
//Disable dynamic LPF
gyroConfigMutable()->useDynamicLpf = 0;
//Setup PID controller
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);
//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);
}
}

43
src/main/flight/ez_tune.h Normal file
View file

@ -0,0 +1,43 @@
/*
* 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;
uint8_t axisRatio;
uint8_t response;
uint8_t damping;
uint8_t stability;
uint8_t aggressiveness;
uint8_t rate;
uint8_t expo;
} ezTuneSettings_t;
PG_DECLARE_PROFILE(ezTuneSettings_t, ezTune);
void ezTuneUpdate(void);

View file

@ -187,13 +187,9 @@ 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;
@ -334,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;

View file

@ -1201,7 +1201,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) {
@ -1911,7 +1911,7 @@ static bool osdDrawSingleElement(uint8_t item)
osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0);
break;
case OSD_ODOMETER:
case OSD_ODOMETER:
{
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_ODOMETER);
uint32_t odometerDist = (uint32_t)(getTotalTravelDistance() / 100);
@ -3957,7 +3957,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
/**
* @brief Draws the INAV and/or pilot logos on the display
*
*
* @param singular If true, only one logo will be drawn. If false, both logos will be drawn, separated by osdConfig()->inav_to_pilot_logo_spacing characters
* @param row The row number to start drawing the logos. If not singular, both logos are drawn on the same rows.
* @return uint8_t The row number after the logo(s).
@ -4007,7 +4007,7 @@ uint8_t drawLogos(bool singular, uint8_t row) {
} else {
logo_x = logoColOffset + SYM_LOGO_WIDTH + logoSpacing;
}
for (uint8_t lRow = 0; lRow < SYM_LOGO_HEIGHT; lRow++) {
for (uint8_t lCol = 0; lCol < SYM_LOGO_WIDTH; lCol++) {
displayWriteChar(osdDisplayPort, logo_x + lCol, logoRow, logo_c++);
@ -4131,7 +4131,7 @@ static void osdCompleteAsyncInitialization(void)
if (fontHasMetadata && metadata.charCount > 256) {
hasExtendedFont = true;
y = drawLogos(false, y);
y++;
} else if (!fontHasMetadata) {
@ -4570,12 +4570,12 @@ static void osdShowHDArmScreen(void)
displayWrite(osdDisplayPort, col, armScreenRow, buf);
displayWrite(osdDisplayPort, col + strlen(buf) + gap, armScreenRow++, buf2);
int digits = osdConfig()->plus_code_digits;
olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
}
#if defined (USE_SAFE_HOME)
if (posControl.safehomeState.distance) { // safehome found during arming
if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
@ -4655,7 +4655,7 @@ static void osdShowSDArmScreen(void)
uint8_t gpsStartCol = (osdDisplayPort->cols - (strlen(buf) + strlen(buf2) + 2)) / 2;
displayWrite(osdDisplayPort, gpsStartCol, armScreenRow, buf);
displayWrite(osdDisplayPort, gpsStartCol + strlen(buf) + 2, armScreenRow++, buf2);
int digits = osdConfig()->plus_code_digits;
olc_encode(GPS_home.lat, GPS_home.lon, digits, buf, sizeof(buf));
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
@ -4808,10 +4808,14 @@ static void osdRefresh(timeUs_t currentTimeUs)
statsDisplayed = false;
osdResetStats();
osdShowArmed();
uint32_t delay = osdConfig()->arm_screen_display_time;
uint16_t delay = osdConfig()->arm_screen_display_time;
if (STATE(IN_FLIGHT_EMERG_REARM)) {
delay = 500;
}
#if defined(USE_SAFE_HOME)
if (posControl.safehomeState.distance)
delay+= 3000;
else if (posControl.safehomeState.distance) {
delay += 3000;
}
#endif
osdSetNextRefreshIn(delay);
} else {

View file

@ -94,3 +94,6 @@
#define MSP2_INAV_RATE_DYNAMICS 0x2060
#define MSP2_INAV_SET_RATE_DYNAMICS 0x2061
#define MSP2_INAV_EZ_TUNE 0x2070
#define MSP2_INAV_EZ_TUNE_SET 0x2071

View file

@ -62,6 +62,7 @@
#include "sensors/acceleration.h"
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#include "sensors/gyro.h"
#include "programming/global_variables.h"
@ -223,6 +224,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;
@ -2660,11 +2662,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;
@ -2675,24 +2681,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;
}
}
@ -2707,6 +2705,22 @@ 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());
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;
}
}
@ -2952,14 +2966,15 @@ 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)) {
if (STATE(LANDING_DETECTED)) {
landingDetectorIsActive = false;
}
resetLandingDetector();
landingDetectorIsActive = false;
if (!IS_RC_MODE_ACTIVE(BOXARM)) {
DISABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
}
@ -2996,11 +3011,28 @@ void resetLandingDetector(void)
posControl.flags.resetLandingDetector = true;
}
void resetLandingDetectorActiveState(void)
{
landingDetectorIsActive = false;
}
bool isFlightDetected(void)
{
return STATE(AIRPLANE) ? isFixedWingFlying() : isMulticopterFlying();
}
bool isProbablyStillFlying(void)
{
bool inFlightSanityCheck;
if (STATE(MULTIROTOR)) {
inFlightSanityCheck = posControl.actualState.velXY > MC_LAND_CHECK_VEL_XY_MOVING || averageAbsGyroRates() > 4.0f;
} else {
inFlightSanityCheck = isGPSHeadingValid();
}
return landingDetectorIsActive && inFlightSanityCheck;
}
/*-----------------------------------------------------------
* Z-position controller
*-----------------------------------------------------------*/
@ -3917,14 +3949,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;
}
@ -3942,7 +3974,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;

View file

@ -610,6 +610,8 @@ const char * fixedWingLaunchStateMessage(void);
float calculateAverageSpeed(void);
void updateLandingStatus(timeMs_t currentTimeMs);
bool isProbablyStillFlying(void);
void resetLandingDetectorActiveState(void);
const navigationPIDControllers_t* getNavigationPIDControllers(void);

View file

@ -695,7 +695,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;
}
/*-----------------------------------------------------------

View file

@ -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);
@ -110,6 +111,25 @@ static bool updateTimer(navigationTimer_t * tim, timeUs_t interval, timeUs_t cur
static bool shouldResetReferenceAltitude(void)
{
/* 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;
}
}
emergRearmResetCheck = !ARMING_FLAG(ARMED);
switch ((nav_reset_type_e)positionEstimationConfig()->reset_altitude_type) {
case NAV_RESET_NEVER:
return false;
@ -305,7 +325,6 @@ 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 */

View file

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

View file

@ -192,3 +192,5 @@
#else
#define MAX_MIXER_PROFILE_COUNT 1
#endif
#define USE_EZ_TUNE