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:
commit
c775c29256
30 changed files with 710 additions and 71 deletions
66
.vscode/c_cpp_properties.json
vendored
Executable file
66
.vscode/c_cpp_properties.json
vendored
Executable 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
41
.vscode/tasks.json
vendored
Executable 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"
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
|
@ -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
|
||||
|
|
|
@ -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 "$@"
|
||||
|
|
7
cmake/docker_build_sitl.sh
Normal file
7
cmake/docker_build_sitl.sh
Normal 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
8
cmake/docker_run_sitl.sh
Normal 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
|
|
@ -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
|
||||
|
|
|
@ -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).
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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))
|
||||
|
|
|
@ -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:
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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
143
src/main/flight/ez_tune.c
Normal 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
43
src/main/flight/ez_tune.h
Normal 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);
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -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 {
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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;
|
||||
|
|
|
@ -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);
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
|
@ -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 */
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -192,3 +192,5 @@
|
|||
#else
|
||||
#define MAX_MIXER_PROFILE_COUNT 1
|
||||
#endif
|
||||
|
||||
#define USE_EZ_TUNE
|
Loading…
Add table
Add a link
Reference in a new issue