Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'origin/master' into sh_mixer_confi

Browse source
This commit is contained in:
shota 2023-10-24 19:19:24 +09:00
commit 0651d44b92
38 changed files with 1271 additions and 349 deletions
Download patch file Download diff file Expand all files Collapse all files

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

8
Dockerfile
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

4
cmake/docker.sh
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 "$@"

7
cmake/docker_build_sitl.sh Normal file
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

5
cmake/sitl.cmake
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

124
docs/Settings.md
View file

@ -258,7 +258,7 @@ Inertia force compensation method when gps is avaliable, VELNED use the acclerat
| Default | Min | Max |
| --- | --- | --- |
| VELNED | | |
| ADAPTIVE | | |
---
@ -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).
@ -3438,7 +3528,7 @@ Multicopter hover throttle hint for altitude controller. Should be set to approx
| Default | Min | Max |
| --- | --- | --- |
| 1500 | 1000 | 2000 |
| 1300 | 1000 | 2000 |
---
@ -4012,6 +4102,16 @@ Value above which to make the OSD relative altitude indicator blink (meters)
---
### osd_arm_screen_display_time
Amount of time to display the arm screen [ms]
| Default | Min | Max |
| --- | --- | --- |
| 1500 | 1000 | 5000 |
---
### osd_baro_temp_alarm_max
Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade)
@ -4342,6 +4442,16 @@ Temperature under which the IMU temperature OSD element will start blinking (dec
---
### osd_inav_to_pilot_logo_spacing
The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen.
| Default | Min | Max |
| --- | --- | --- |
| 8 | 0 | 20 |
---
### osd_left_sidebar_scroll
_// TODO_
@ -4762,6 +4872,16 @@ IMPERIAL, METRIC, UK
---
### osd_use_pilot_logo
Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511
| Default | Min | Max |
| --- | --- | --- |
| OFF | OFF | ON |
---
### osd_video_system
Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF', 'AVATAR' and `BF43COMPAT`

15
docs/development/Building in Docker.md
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.

2
src/main/CMakeLists.txt
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

6
src/main/cms/cms.h
View file

@ -37,9 +37,9 @@ void cmsYieldDisplay(displayPort_t *pPort, timeMs_t duration);
void cmsUpdate(uint32_t currentTimeUs);
void cmsSetExternKey(cms_key_e extKey);
#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID"
#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT"
#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP"
#define CMS_STARTUP_HELP_TEXT1 "MENU: THR MID"
#define CMS_STARTUP_HELP_TEXT2 "+ YAW LEFT"
#define CMS_STARTUP_HELP_TEXT3 "+ PITCH UP"
// cmsMenuExit special ptr values
#define CMS_EXIT (0)

3
src/main/config/parameter_group_ids.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

16
src/main/drivers/osd_symbols.h
View file

@ -58,7 +58,7 @@
// 0x26 // 038 ASCII &
#define SYM_VTX_POWER 0x27 // 039 VTx Power
// 0x28 // 040 to 062 ASCII
#define SYM_AH_NM 0x3F // 063 Ah/NM
#define SYM_AH_NM 0x3F // 063 Ah/NM
// 0x40 // 064 to 095 ASCII
#define SYM_MAH_NM_0 0x60 // 096 mAh/NM left
#define SYM_MAH_NM_1 0x61 // 097 mAh/NM right
@ -78,7 +78,7 @@
#define SYM_WH 0x6D // 109 WH
#define SYM_WH_KM 0x6E // 110 WH/KM
#define SYM_WH_MI 0x6F // 111 WH/MI
#define SYM_WH_NM 0x70 // 112 Wh/NM
#define SYM_WH_NM 0x70 // 112 Wh/NM
#define SYM_WATT 0x71 // 113 WATTS
#define SYM_MW 0x72 // 114 mW
#define SYM_KILOWATT 0x73 // 115 kW
@ -159,6 +159,7 @@
#define SYM_HEADING_W 0xCB // 203 Heading Graphic west
#define SYM_HEADING_DIVIDED_LINE 0xCC // 204 Heading Graphic
#define SYM_HEADING_LINE 0xCD // 205 Heading Graphic
#define SYM_MAX 0xCE // 206 MAX symbol
#define SYM_PROFILE 0xCF // 207 Profile symbol
#define SYM_SWITCH_INDICATOR_LOW 0xD0 // 208 Switch High
@ -175,10 +176,10 @@
#define SYM_FLIGHT_HOURS_REMAINING 0xDB // 219 Flight time (hours) remaining
#define SYM_GROUND_COURSE 0xDC // 220 Ground course
#define SYM_ALERT 0xDD // 221 General alert symbol
#define SYM_TERRAIN_FOLLOWING 0xFB // 251 Terrain following (also Alt adjust)
#define SYM_CROSS_TRACK_ERROR 0xFC // 252 Cross track error
#define SYM_LOGO_START 0x101 // 257 to 297, INAV logo
#define SYM_LOGO_WIDTH 10
#define SYM_LOGO_HEIGHT 4
@ -225,7 +226,7 @@
#define SYM_HUD_SIGNAL_3 0x163 // 355 Hud signal icon 75%
#define SYM_HUD_SIGNAL_4 0x164 // 356 Hud signal icon 100%
#define SYM_HOME_DIST 0x165 // 357 DIST
#define SYM_HOME_DIST 0x165 // 357 DIST
#define SYM_AH_CH_CENTER 0x166 // 358 Crossair center
#define SYM_FLIGHT_DIST_REMAINING 0x167 // 359 Flight distance reminaing
#define SYM_ODOMETER 0x168 // 360 Odometer
@ -262,8 +263,13 @@
#define SYM_SERVO_PAN_IS_OFFSET_L 0x1C7 // 455 Pan servo is offset left
#define SYM_SERVO_PAN_IS_OFFSET_R 0x1C8 // 456 Pan servo is offset right
#define SYM_PILOT_LOGO_SML_L 0x1D5 // 469 small Pilot logo Left
#define SYM_PILOT_LOGO_SML_C 0x1D6 // 470 small Pilot logo Centre
#define SYM_PILOT_LOGO_SML_R 0x1D7 // 471 small Pilot logo Right
#define SYM_PILOT_LOGO_LRG_START 0x1D8 // 472 to 511, Pilot logo
#else
#define TEMP_SENSOR_SYM_COUNT 0
#define TEMP_SENSOR_SYM_COUNT 0
#endif // USE_OSD

14
src/main/drivers/timer_impl_stdperiph.c
View file

@ -301,7 +301,12 @@ bool impl_timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBuf
TIM_CtrlPWMOutputs(timer, ENABLE);
TIM_ARRPreloadConfig(timer, ENABLE);
TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) {
TIM_CCxNCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCxN_Enable);
} else {
TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
}
TIM_Cmd(timer, ENABLE);
dmaInit(tch->dma, OWNER_TIMER, 0);
@ -382,7 +387,12 @@ bool impl_timerPWMConfigDMABurst(burstDmaTimer_t *burstDmaTimer, TCH_t * tch, vo
TIM_CtrlPWMOutputs(timer, ENABLE);
TIM_ARRPreloadConfig(timer, ENABLE);
TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
if (tch->timHw->output & TIMER_OUTPUT_N_CHANNEL) {
TIM_CCxNCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCxN_Enable);
} else {
TIM_CCxCmd(timer, lookupTIMChannelTable[tch->timHw->channelIndex], TIM_CCx_Enable);
}
TIM_Cmd(timer, ENABLE);
if (!tch->timCtx->dmaBurstRef) {

4
src/main/fc/config.c
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;
}

84
src/main/fc/fc_core.c
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,9 +675,12 @@ 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;
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;
@ -816,7 +849,6 @@ void processRx(timeUs_t currentTimeUs)
}
}
#endif
}
// Function for loop trigger
@ -876,6 +908,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
@ -925,15 +961,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();

5
src/main/fc/fc_init.c
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);

47
src/main/fc/fc_msp.c
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"
@ -717,6 +718,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:
@ -1602,6 +1606,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:
@ -3083,6 +3104,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:
@ -3287,6 +3332,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:

1
src/main/fc/runtime_config.h
View file

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

2
src/main/fc/settings.c
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:

1
src/main/fc/settings.h
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 {

92
src/main/fc/settings.yaml
View file

@ -1045,7 +1045,7 @@ groups:
max: PWM_RANGE_MAX
- name: nav_mc_hover_thr
description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering."
default_value: 1500
default_value: 1300
field: nav.mc.hover_throttle
min: 1000
max: 2000
@ -1452,7 +1452,7 @@ groups:
type: bool
- name: ahrs_inertia_comp_method
description: "Inertia force compensation method when gps is avaliable, VELNED use the accleration from gps, TURNRATE calculates accleration by turnrate multiplied by speed, ADAPTIVE choose best result from two in each ahrs loop"
default_value: VELNED
default_value: ADAPTIVE
field: inertia_comp_method
table: imu_inertia_comp_method
@ -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
@ -3500,59 +3559,88 @@ groups:
max: 6
default_value: 4
- name: osd_use_pilot_logo
description: Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511
field: use_pilot_logo
type: bool
default_value: OFF
- name: osd_inav_to_pilot_logo_spacing
description: The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen.
field: inav_to_pilot_logo_spacing
min: 0
max: 20
default_value: 8
- name: osd_arm_screen_display_time
description: Amount of time to display the arm screen [ms]
field: arm_screen_display_time
min: 1000
max: 5000
default_value: 1500
- name: osd_switch_indicator_zero_name
description: "Character to use for OSD switch incicator 0."
field: osd_switch_indicator0_name
type: string
max: 5
default_value: "FLAP"
- name: osd_switch_indicator_one_name
description: "Character to use for OSD switch incicator 1."
field: osd_switch_indicator1_name
type: string
max: 5
default_value: "GEAR"
- name: osd_switch_indicator_two_name
description: "Character to use for OSD switch incicator 2."
field: osd_switch_indicator2_name
type: string
max: 5
default_value: "CAM"
- name: osd_switch_indicator_three_name
description: "Character to use for OSD switch incicator 3."
field: osd_switch_indicator3_name
type: string
max: 5
default_value: "LIGT"
- name: osd_switch_indicator_zero_channel
description: "RC Channel to use for OSD switch indicator 0."
field: osd_switch_indicator0_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicator_one_channel
description: "RC Channel to use for OSD switch indicator 1."
field: osd_switch_indicator1_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicator_two_channel
description: "RC Channel to use for OSD switch indicator 2."
field: osd_switch_indicator2_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicator_three_channel
description: "RC Channel to use for OSD switch indicator 3."
field: osd_switch_indicator3_channel
min: 5
max: MAX_SUPPORTED_RC_CHANNEL_COUNT
default_value: 5
- name: osd_switch_indicators_align_left
description: "Align text to left of switch indicators"
field: osd_switch_indicators_align_left
type: bool
default_value: ON
- name: osd_system_msg_display_time
description: System message display cycle time for multiple messages (milliseconds).
field: system_msg_display_time

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);

19
src/main/flight/imu.c
View file

@ -719,28 +719,25 @@ static void imuCalculateEstimatedAttitude(float dT)
}
if (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE && isGPSTrustworthy() && STATE(AIRPLANE))
{
//pick the best centrifugal acceleration between velned and turnrate
fpVector3_t compansatedGravityBF_velned;
vectorAdd(&compansatedGravityBF_velned, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
float velned_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS);
float velned_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_velned)) - GRAVITY_CMSS);
fpVector3_t compansatedGravityBF_turnrate;
vectorAdd(&compansatedGravityBF_turnrate, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
float turnrate_magnitude = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS);
if (velned_magnitude > turnrate_magnitude)
{
compansatedGravityBF = compansatedGravityBF_turnrate;
}
else
{
compansatedGravityBF = compansatedGravityBF_velned;
}
float turnrate_error = fabsf(fast_fsqrtf(vectorNormSquared(&compansatedGravityBF_turnrate)) - GRAVITY_CMSS);
compansatedGravityBF = velned_error > turnrate_error? compansatedGravityBF_turnrate:compansatedGravityBF_velned;
}
else if (imuConfig()->inertia_comp_method == COMPMETHOD_VELNED && isGPSTrustworthy())
else if (((imuConfig()->inertia_comp_method == COMPMETHOD_VELNED) || (imuConfig()->inertia_comp_method == COMPMETHOD_ADAPTIVE)) && isGPSTrustworthy())
{
//velned centrifugal force compensation, quad will use this method
vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_velned);
}
else if (STATE(AIRPLANE))
{
//turnrate centrifugal force compensation
vectorAdd(&compansatedGravityBF, &imuMeasuredAccelBF, &vEstcentrifugalAccelBF_turnrate);
}
else

64
src/main/flight/servos.c
View file

@ -143,34 +143,14 @@ void servoComputeScalingFactors(uint8_t servoIndex) {
servoMetadata[servoIndex].scaleMin = (servoParams(servoIndex)->middle - servoParams(servoIndex)->min) / 500.0f;
}
void servosInit(void)
void computeServoCount(void)
{
// give all servos a default command
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = servoParams(i)->middle;
static bool firstRun = true;
if (!firstRun) {
return;
}
/*
* load mixer
*/
loadCustomServoMixer();
// If there are servo rules after all, update variables
if (servoRuleCount > 0) {
servoOutputEnabled = true;
mixerUsesServos = true;
}
for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servoComputeScalingFactors(i);
}
}
int getServoCount(void)
{
bool servoRuleDetected = false;
minServoIndex = 0;
maxServoIndex = 255;
minServoIndex = 255;
maxServoIndex = 0;
for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) {
for (int i = 0; i < MAX_SERVO_RULES; i++) {
// check if done
@ -184,10 +164,38 @@ int getServoCount(void)
if (mixerServoMixersByIndex(j)[i].targetChannel > maxServoIndex) {
maxServoIndex = mixerServoMixersByIndex(j)[i].targetChannel;
}
servoRuleDetected = true;
mixerUsesServos = true;
}
}
if (servoRuleDetected) {
firstRun = false;
}
void servosInit(void)
{
// give all servos a default command
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = servoParams(i)->middle;
}
/*
* load mixer
*/
computeServoCount();
loadCustomServoMixer();
// If there are servo rules after all, update variables
if (mixerUsesServos) {
servoOutputEnabled = true;
}
for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servoComputeScalingFactors(i);
}
}
int getServoCount(void)
{
if (mixerUsesServos) {
return 1 + maxServoIndex - minServoIndex;
}
else {

12
src/main/io/displayport_msp_bf_compat.c
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;

456
src/main/io/osd.c
View file

@ -131,7 +131,6 @@
#define STATS_PAGE1 (checkStickPosition(ROL_LO))
#define SPLASH_SCREEN_DISPLAY_TIME 4000 // ms
#define ARMED_SCREEN_DISPLAY_TIME 1500 // ms
#define STATS_SCREEN_DISPLAY_TIME 60000 // ms
#define EFFICIENCY_UPDATE_INTERVAL (5 * 1000)
@ -209,7 +208,7 @@ static bool osdDisplayHasCanvas;
#define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees)
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 8);
PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 9);
PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 1);
void osdStartedSaveProcess(void) {
@ -1202,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) {
@ -1912,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);
@ -2259,6 +2258,12 @@ static bool osdDrawSingleElement(uint8_t item)
osdFormatPilotName(buff);
break;
case OSD_PILOT_LOGO:
displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_PILOT_LOGO_SML_L);
displayWriteChar(osdDisplayPort, elemPosX+1, elemPosY, SYM_PILOT_LOGO_SML_C);
displayWriteChar(osdDisplayPort, elemPosX+2, elemPosY, SYM_PILOT_LOGO_SML_R);
break;
case OSD_THROTTLE_POS:
{
osdFormatThrottlePosition(buff, false, &elemAttr);
@ -3749,6 +3754,9 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
.system_msg_display_time = SETTING_OSD_SYSTEM_MSG_DISPLAY_TIME_DEFAULT,
.units = SETTING_OSD_UNITS_DEFAULT,
.main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT,
.use_pilot_logo = SETTING_OSD_USE_PILOT_LOGO_DEFAULT,
.inav_to_pilot_logo_spacing = SETTING_OSD_INAV_TO_PILOT_LOGO_SPACING_DEFAULT,
.arm_screen_display_time = SETTING_OSD_ARM_SCREEN_DISPLAY_TIME_DEFAULT,
#ifdef USE_WIND_ESTIMATOR
.estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT,
@ -3822,6 +3830,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
osdLayoutsConfig->item_pos[0][OSD_CRAFT_NAME] = OSD_POS(20, 2);
osdLayoutsConfig->item_pos[0][OSD_PILOT_NAME] = OSD_POS(20, 3);
osdLayoutsConfig->item_pos[0][OSD_PILOT_LOGO] = OSD_POS(20, 3);
osdLayoutsConfig->item_pos[0][OSD_VTX_CHANNEL] = OSD_POS(8, 6);
#ifdef USE_SERIALRX_CRSF
@ -3944,6 +3953,146 @@ 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).
*/
uint8_t drawLogos(bool singular, uint8_t row) {
uint8_t logoRow = row;
uint8_t logoColOffset = 0;
bool usePilotLogo = (osdConfig()->use_pilot_logo && osdDisplayIsHD());
#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is in use, the pilot logo cannot be used, due to font issues.
if (isBfCompatibleVideoSystem(osdConfig()))
usePilotLogo = false;
#endif
uint8_t logoSpacing = osdConfig()->inav_to_pilot_logo_spacing;
if (logoSpacing > 0 && ((osdDisplayPort->cols % 2) != (logoSpacing % 2))) {
logoSpacing++; // Add extra 1 character space between logos, if the odd/even of the OSD cols doesn't match the odd/even of the logo spacing
}
// Draw Logo(s)
if (usePilotLogo && !singular) {
logoColOffset = ((osdDisplayPort->cols - (SYM_LOGO_WIDTH * 2)) - logoSpacing) / 2;
} else {
logoColOffset = floorf((osdDisplayPort->cols - SYM_LOGO_WIDTH) / 2.0f);
}
// Draw INAV logo
if ((singular && !usePilotLogo) || !singular) {
unsigned logo_c = SYM_LOGO_START;
uint8_t logo_x = logoColOffset;
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++);
}
logoRow++;
}
}
// Draw the pilot logo
if (usePilotLogo) {
unsigned logo_c = SYM_PILOT_LOGO_LRG_START;
uint8_t logo_x = 0;
logoRow = row;
if (singular) {
logo_x = logoColOffset;
} 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++);
}
logoRow++;
}
}
return logoRow;
}
uint8_t drawStats(uint8_t row) {
#ifdef USE_STATS
char string_buffer[30];
uint8_t statNameX = (osdDisplayPort->cols - 22) / 2;
uint8_t statValueX = statNameX + 21;
if (statsConfig()->stats_enabled) {
displayWrite(osdDisplayPort, statNameX, row, "ODOMETER:");
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE));
string_buffer[5] = SYM_MI;
break;
default:
case OSD_UNIT_GA:
tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE));
string_buffer[5] = SYM_NM;
break;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC:
tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER));
string_buffer[5] = SYM_KM;
break;
}
string_buffer[6] = '\0';
displayWrite(osdDisplayPort, statValueX-5, row, string_buffer);
displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL TIME:");
uint32_t tot_mins = statsConfig()->stats_total_time / 60;
tfp_sprintf(string_buffer, "%2d:%02dH:M", (int)(tot_mins / 60), (int)(tot_mins % 60));
displayWrite(osdDisplayPort, statValueX-7, row, string_buffer);
#ifdef USE_ADC
if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) {
displayWrite(osdDisplayPort, statNameX, ++row, "TOTAL ENERGY:");
osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4, false);
displayWrite(osdDisplayPort, statValueX-4, row, string_buffer);
displayWriteChar(osdDisplayPort, statValueX, row, SYM_WH);
displayWrite(osdDisplayPort, statNameX, ++row, "AVG EFFICIENCY:");
if (statsConfig()->stats_total_dist) {
uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3, false);
string_buffer[3] = SYM_WH_MI;
break;
case OSD_UNIT_GA:
osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3, false);
string_buffer[3] = SYM_WH_NM;
break;
default:
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC:
osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3, false);
string_buffer[3] = SYM_WH_KM;
break;
}
} else {
string_buffer[0] = string_buffer[1] = string_buffer[2] = '-';
}
string_buffer[4] = '\0';
displayWrite(osdDisplayPort, statValueX-3, row++, string_buffer);
}
#endif // USE_ADC
}
#endif // USE_STATS
return row;
}
static void osdSetNextRefreshIn(uint32_t timeMs) {
resumeRefreshAt = micros() + timeMs * 1000;
refreshWaitForResumeCmdRelease = true;
@ -3980,19 +4129,12 @@ static void osdCompleteAsyncInitialization(void)
if (fontHasMetadata && metadata.charCount > 256) {
hasExtendedFont = true;
unsigned logo_c = SYM_LOGO_START;
unsigned logo_x = OSD_CENTER_LEN(SYM_LOGO_WIDTH);
for (unsigned ii = 0; ii < SYM_LOGO_HEIGHT; ii++) {
for (unsigned jj = 0; jj < SYM_LOGO_WIDTH; jj++) {
displayWriteChar(osdDisplayPort, logo_x + jj, y, logo_c++);
}
y++;
}
y = drawLogos(false, y);
y++;
} else if (!fontHasMetadata) {
const char *m = "INVALID FONT";
displayWrite(osdDisplayPort, OSD_CENTER_S(m), 3, m);
y = 4;
displayWrite(osdDisplayPort, OSD_CENTER_S(m), y++, m);
}
if (fontHasMetadata && metadata.version < OSD_MIN_FONT_VERSION) {
@ -4002,85 +4144,16 @@ static void osdCompleteAsyncInitialization(void)
char string_buffer[30];
tfp_sprintf(string_buffer, "INAV VERSION: %s", FC_VERSION_STRING);
uint8_t xPos = osdDisplayIsHD() ? 15 : 5;
uint8_t xPos = (osdDisplayPort->cols - 19) / 2; // Automatically centre, regardless of resolution. In the case of odd number screens, bias to the left.
displayWrite(osdDisplayPort, xPos, y++, string_buffer);
#ifdef USE_CMS
displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1);
displayWrite(osdDisplayPort, xPos+2, y++, CMS_STARTUP_HELP_TEXT1);
displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT2);
displayWrite(osdDisplayPort, xPos+6, y++, CMS_STARTUP_HELP_TEXT3);
#endif
#ifdef USE_STATS
uint8_t statNameX = osdDisplayIsHD() ? 14 : 4;
uint8_t statValueX = osdDisplayIsHD() ? 34 : 24;
if (statsConfig()->stats_enabled) {
displayWrite(osdDisplayPort, statNameX, ++y, "ODOMETER:");
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_MILE));
string_buffer[5] = SYM_MI;
break;
default:
case OSD_UNIT_GA:
tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_NAUTICALMILE));
string_buffer[5] = SYM_NM;
break;
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC:
tfp_sprintf(string_buffer, "%5d", (int)(statsConfig()->stats_total_dist / METERS_PER_KILOMETER));
string_buffer[5] = SYM_KM;
break;
}
string_buffer[6] = '\0';
displayWrite(osdDisplayPort, statValueX-5, y, string_buffer);
displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL TIME:");
uint32_t tot_mins = statsConfig()->stats_total_time / 60;
tfp_sprintf(string_buffer, "%2d:%02dHM", (int)(tot_mins / 60), (int)(tot_mins % 60));
displayWrite(osdDisplayPort, statValueX-5, y, string_buffer);
#ifdef USE_ADC
if (feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER)) {
displayWrite(osdDisplayPort, statNameX, ++y, "TOTAL ENERGY:");
osdFormatCentiNumber(string_buffer, statsConfig()->stats_total_energy / 10, 0, 2, 0, 4, false);
strcat(string_buffer, "\xAB"); // SYM_WH
displayWrite(osdDisplayPort, statValueX-4, y, string_buffer);
displayWrite(osdDisplayPort, statNameX, ++y, "AVG EFFICIENCY:");
if (statsConfig()->stats_total_dist) {
uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km
switch (osdConfig()->units) {
case OSD_UNIT_UK:
FALLTHROUGH;
case OSD_UNIT_IMPERIAL:
osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3, false);
string_buffer[3] = SYM_WH_MI;
break;
case OSD_UNIT_GA:
osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3, false);
string_buffer[3] = SYM_WH_NM;
break;
default:
case OSD_UNIT_METRIC_MPH:
FALLTHROUGH;
case OSD_UNIT_METRIC:
osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3, false);
string_buffer[3] = SYM_WH_KM;
break;
}
} else {
string_buffer[0] = string_buffer[1] = string_buffer[2] = '-';
}
string_buffer[4] = '\0';
displayWrite(osdDisplayPort, statValueX-3, y, string_buffer);
}
#endif // USE_ADC
}
y = drawStats(++y);
#endif
displayCommitTransaction(osdDisplayPort);
@ -4434,85 +4507,205 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page)
displayCommitTransaction(osdDisplayPort);
}
// called when motors armed
static void osdShowArmed(void)
// HD arming screen. based on the minimum HD OSD grid size of 50 x 18
static void osdShowHDArmScreen(void)
{
dateTime_t dt;
char buf[MAX(32, FORMATTED_DATE_TIME_BUFSIZE)];
char craftNameBuf[MAX_NAME_LENGTH];
char versionBuf[30];
char *date;
char *time;
// We need 12 visible rows, start row never < first fully visible row 1
uint8_t y = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1;
dateTime_t dt;
char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
char craftNameBuf[MAX_NAME_LENGTH];
char versionBuf[osdDisplayPort->cols];
uint8_t safehomeRow = 0;
uint8_t armScreenRow = 2; // Start at row 2
displayClearScreen(osdDisplayPort);
strcpy(buf, "ARMED");
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
y += 2;
armScreenRow = drawLogos(false, armScreenRow);
armScreenRow++;
if (strlen(systemConfig()->craftName) > 0) {
osdFormatCraftName(craftNameBuf);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, y, craftNameBuf );
y += 1;
strcpy(buf2, "ARMED!");
tfp_sprintf(buf, "%s - %s", craftNameBuf, buf2);
} else {
strcpy(buf, "ARMED!");
}
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
#if defined(USE_GPS)
#if defined (USE_SAFE_HOME)
if (posControl.safehomeState.distance) {
safehomeRow = armScreenRow;
armScreenRow++;
}
#endif // USE_SAFE_HOME
#endif // USE_GPS
armScreenRow++;
if (posControl.waypointListValid && posControl.waypointCount > 0) {
#ifdef USE_MULTI_MISSION
tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
#else
strcpy(buf, "*MISSION LOADED*");
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
#endif
}
y += 1;
armScreenRow++;
#if defined(USE_GPS)
if (feature(FEATURE_GPS)) {
if (STATE(GPS_FIX_HOME)) {
if (osdConfig()->osd_home_position_arm_screen){
if (osdConfig()->osd_home_position_arm_screen) {
osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
osdFormatCoordinate(buf, SYM_LON, GPS_home.lon);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y + 1, buf);
osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon);
uint8_t gap = 1;
uint8_t col = strlen(buf) + strlen(buf2) + gap;
if ((osdDisplayPort->cols %2) != (col %2)) {
gap++;
col++;
}
col = (osdDisplayPort->cols - col) / 2;
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, y + 2, buf);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
}
y += 4;
#if defined (USE_SAFE_HOME)
if (posControl.safehomeState.distance) { // safehome found during arming
if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
strcpy(buf, "SAFEHOME FOUND; MODE OFF");
} else {
char buf2[12]; // format the distance first
osdFormatDistanceStr(buf2, posControl.safehomeState.distance);
tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, posControl.safehomeState.index);
tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, posControl.safehomeState.index, buf2);
}
textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
// write this message above the ARMED message to make it obvious
displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr);
// write this message below the ARMED message to make it obvious
displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr);
}
#endif
} else {
strcpy(buf, "!NO HOME POSITION!");
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y, buf);
y += 1;
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
}
armScreenRow++;
}
#endif
if (rtcGetDateTime(&dt)) {
dateTimeFormatLocal(buf, &dt);
dateTimeSplitFormatted(buf, &date, &time);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(date)) / 2, y, date);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(time)) / 2, y + 1, time);
y += 3;
if (rtcGetDateTimeLocal(&dt)) {
tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
armScreenRow++;
}
tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, y, versionBuf);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf);
armScreenRow++;
#ifdef USE_STATS
if (armScreenRow < (osdDisplayPort->rows - 4))
armScreenRow = drawStats(armScreenRow);
#endif // USE_STATS
}
static void osdShowSDArmScreen(void)
{
dateTime_t dt;
char buf[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
char buf2[MAX(osdDisplayPort->cols, FORMATTED_DATE_TIME_BUFSIZE)];
char craftNameBuf[MAX_NAME_LENGTH];
char versionBuf[osdDisplayPort->cols];
// We need 12 visible rows, start row never < first fully visible row 1
uint8_t armScreenRow = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1;
uint8_t safehomeRow = 0;
strcpy(buf, "ARMED!");
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
safehomeRow = armScreenRow;
armScreenRow++;
if (strlen(systemConfig()->craftName) > 0) {
osdFormatCraftName(craftNameBuf);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig()->craftName)) / 2, armScreenRow++, craftNameBuf );
}
if (posControl.waypointListValid && posControl.waypointCount > 0) {
#ifdef USE_MULTI_MISSION
tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf);
#else
strcpy(buf, "*MISSION LOADED*");
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow, buf);
#endif
}
armScreenRow++;
#if defined(USE_GPS)
if (feature(FEATURE_GPS)) {
if (STATE(GPS_FIX_HOME)) {
if (osdConfig()->osd_home_position_arm_screen) {
osdFormatCoordinate(buf, SYM_LAT, GPS_home.lat);
osdFormatCoordinate(buf2, SYM_LON, GPS_home.lon);
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);
}
#if defined (USE_SAFE_HOME)
if (posControl.safehomeState.distance) { // safehome found during arming
if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) {
strcpy(buf, "SAFEHOME FOUND; MODE OFF");
} else {
osdFormatDistanceStr(buf2, posControl.safehomeState.distance);
tfp_sprintf(buf, "%c SAFEHOME %u @ %s", SYM_HOME, posControl.safehomeState.index, buf2);
}
textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT;
// write this message below the ARMED message to make it obvious
displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, safehomeRow, buf, elemAttr);
}
#endif
} else {
strcpy(buf, "!NO HOME POSITION!");
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
}
armScreenRow++;
}
#endif
if (rtcGetDateTimeLocal(&dt)) {
tfp_sprintf(buf, "%04u-%02u-%02u %02u:%02u:%02u", dt.year, dt.month, dt.day, dt.hours, dt.minutes, dt.seconds);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, armScreenRow++, buf);
armScreenRow++;
}
tfp_sprintf(versionBuf, "INAV VERSION: %s", FC_VERSION_STRING);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(versionBuf)) / 2, armScreenRow++, versionBuf);
armScreenRow++;
#ifdef USE_STATS
if (armScreenRow < (osdDisplayPort->rows - 4))
armScreenRow = drawStats(armScreenRow);
#endif // USE_STATS
}
// called when motors armed
static void osdShowArmed(void)
{
displayClearScreen(osdDisplayPort);
if (osdDisplayIsHD()) {
osdShowHDArmScreen();
} else {
osdShowSDArmScreen();
}
}
static void osdFilterData(timeUs_t currentTimeUs) {
@ -4613,10 +4806,14 @@ static void osdRefresh(timeUs_t currentTimeUs)
statsDisplayed = false;
osdResetStats();
osdShowArmed();
uint32_t delay = ARMED_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 *= 3;
else if (posControl.safehomeState.distance) {
delay += 3000;
}
#endif
osdSetNextRefreshIn(delay);
} else {
@ -5233,4 +5430,5 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff)
return elemAttr;
}
#endif // OSD

186
src/main/io/osd.h
View file

@ -275,15 +275,16 @@ typedef enum {
OSD_PAN_SERVO_CENTRED,
OSD_MULTI_FUNCTION,
OSD_ODOMETER,
OSD_PILOT_LOGO,
OSD_ITEM_COUNT // MUST BE LAST
} osd_items_e;
typedef enum {
OSD_UNIT_IMPERIAL,
OSD_UNIT_METRIC,
OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph
OSD_UNIT_UK, // Show everything in imperial, temperature in C
OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C
OSD_UNIT_METRIC_MPH, // Old UK units, all metric except speed in mph
OSD_UNIT_UK, // Show everything in imperial, temperature in C
OSD_UNIT_GA, // General Aviation: Knots, Nautical Miles, Feet, Degrees C
OSD_UNIT_MAX = OSD_UNIT_GA,
} osd_unit_e;
@ -344,111 +345,112 @@ PG_DECLARE(osdLayoutsConfig_t, osdLayoutsConfig);
typedef struct osdConfig_s {
// Alarms
uint8_t rssi_alarm; // rssi %
uint16_t time_alarm; // fly minutes
uint16_t alt_alarm; // positive altitude in m
uint16_t dist_alarm; // home distance in m
uint16_t neg_alt_alarm; // abs(negative altitude) in m
uint8_t current_alarm; // current consumption in A
int16_t imu_temp_alarm_min;
int16_t imu_temp_alarm_max;
int16_t esc_temp_alarm_min;
int16_t esc_temp_alarm_max;
float gforce_alarm;
float gforce_axis_alarm_min;
float gforce_axis_alarm_max;
uint8_t rssi_alarm; // rssi %
uint16_t time_alarm; // fly minutes
uint16_t alt_alarm; // positive altitude in m
uint16_t dist_alarm; // home distance in m
uint16_t neg_alt_alarm; // abs(negative altitude) in m
uint8_t current_alarm; // current consumption in A
int16_t imu_temp_alarm_min;
int16_t imu_temp_alarm_max;
int16_t esc_temp_alarm_min;
int16_t esc_temp_alarm_max;
float gforce_alarm;
float gforce_axis_alarm_min;
float gforce_axis_alarm_max;
#ifdef USE_SERIALRX_CRSF
int8_t snr_alarm; //CRSF SNR alarm in dB
int8_t link_quality_alarm;
int16_t rssi_dbm_alarm; // in dBm
int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100%
int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0%
int8_t snr_alarm; //CRSF SNR alarm in dB
int8_t link_quality_alarm;
int16_t rssi_dbm_alarm; // in dBm
int16_t rssi_dbm_max; // Perfect RSSI. Set to High end of curve. RSSI at 100%
int16_t rssi_dbm_min; // Worst RSSI. Set to low end of curve or RX sensitivity level. RSSI at 0%
#endif
#ifdef USE_BARO
int16_t baro_temp_alarm_min;
int16_t baro_temp_alarm_max;
int16_t baro_temp_alarm_min;
int16_t baro_temp_alarm_max;
#endif
#ifdef USE_TEMPERATURE_SENSOR
osd_alignment_e temp_label_align;
#endif
#ifdef USE_PITOT
float airspeed_alarm_min;
float airspeed_alarm_max;
float airspeed_alarm_min;
float airspeed_alarm_max;
#endif
videoSystem_e video_system;
uint8_t row_shiftdown;
int16_t msp_displayport_fullframe_interval;
videoSystem_e video_system;
uint8_t row_shiftdown;
int16_t msp_displayport_fullframe_interval;
// Preferences
uint8_t main_voltage_decimals;
uint8_t ahi_reverse_roll;
uint8_t ahi_max_pitch;
uint8_t crosshairs_style; // from osd_crosshairs_style_e
int8_t horizon_offset;
int8_t camera_uptilt;
bool ahi_camera_uptilt_comp;
uint8_t camera_fov_h;
uint8_t camera_fov_v;
uint8_t hud_margin_h;
uint8_t hud_margin_v;
bool hud_homing;
bool hud_homepoint;
uint8_t hud_radar_disp;
uint16_t hud_radar_range_min;
uint16_t hud_radar_range_max;
uint8_t hud_radar_alt_difference_display_time;
uint8_t hud_radar_distance_display_time;
uint8_t hud_wp_disp;
uint8_t main_voltage_decimals;
uint8_t ahi_reverse_roll;
uint8_t ahi_max_pitch;
uint8_t crosshairs_style; // from osd_crosshairs_style_e
int8_t horizon_offset;
int8_t camera_uptilt;
bool ahi_camera_uptilt_comp;
uint8_t camera_fov_h;
uint8_t camera_fov_v;
uint8_t hud_margin_h;
uint8_t hud_margin_v;
bool hud_homing;
bool hud_homepoint;
uint8_t hud_radar_disp;
uint16_t hud_radar_range_min;
uint16_t hud_radar_range_max;
uint8_t hud_radar_alt_difference_display_time;
uint8_t hud_radar_distance_display_time;
uint8_t hud_wp_disp;
uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e
uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e
uint8_t sidebar_scroll_arrows;
uint8_t left_sidebar_scroll; // from osd_sidebar_scroll_e
uint8_t right_sidebar_scroll; // from osd_sidebar_scroll_e
uint8_t sidebar_scroll_arrows;
uint8_t units; // from osd_unit_e
uint8_t stats_energy_unit; // from osd_stats_energy_unit_e
uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e
uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds)
uint8_t units; // from osd_unit_e
uint8_t stats_energy_unit; // from osd_stats_energy_unit_e
uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e
uint8_t stats_page_auto_swap_time; // stats page auto swap interval time (seconds)
#ifdef USE_WIND_ESTIMATOR
bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
#endif
uint8_t coordinate_digits;
bool osd_failsafe_switch_layout;
uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE
uint8_t plus_code_short;
uint8_t ahi_style;
uint8_t force_grid; // Force a pixel based OSD to use grid mode.
uint8_t ahi_bordered; // Only used by the AHI widget
uint8_t ahi_width; // In pixels, only used by the AHI widget
uint8_t ahi_height; // In pixels, only used by the AHI widget
int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only.
int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges.
uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type.
uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar.
bool osd_home_position_arm_screen;
uint8_t pan_servo_index; // Index of the pan servo used for home direction offset
int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm
uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred
bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo
uint8_t crsf_lq_format;
uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
uint8_t telemetry; // use telemetry on displayed pixel line 0
uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers.
uint16_t system_msg_display_time; // system message display time for multiple messages (ms)
uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh
uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD)
char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0.
uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0.
char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1.
uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1.
char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2.
uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2.
char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3.
uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3.
bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch.
bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
#endif
uint8_t coordinate_digits;
bool osd_failsafe_switch_layout;
uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE
uint8_t plus_code_short;
uint8_t ahi_style;
uint8_t force_grid; // Force a pixel based OSD to use grid mode.
uint8_t ahi_bordered; // Only used by the AHI widget
uint8_t ahi_width; // In pixels, only used by the AHI widget
uint8_t ahi_height; // In pixels, only used by the AHI widget
int8_t ahi_vertical_offset; // Offset from center in pixels. Positive moves the AHI down. Widget only.
int8_t sidebar_horizontal_offset; // Horizontal offset from default position. Units are grid slots for grid OSDs, pixels for pixel based OSDs. Positive values move sidebars closer to the edges.
uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type.
uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar.
bool osd_home_position_arm_screen;
uint8_t pan_servo_index; // Index of the pan servo used for home direction offset
int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm
uint8_t pan_servo_offcentre_warning; // Degrees around the centre, that is assumed camera is wanted to be facing forwards, but isn't centred
bool pan_servo_indicator_show_degrees; // Show the degrees of offset for the pan servo
uint8_t crsf_lq_format;
uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows
uint8_t telemetry; // use telemetry on displayed pixel line 0
uint8_t esc_rpm_precision; // Number of characters used for the RPM numbers.
uint16_t system_msg_display_time; // system message display time for multiple messages (ms)
uint8_t mAh_used_precision; // Number of numbers used for mAh drawn. Plently of packs now are > 9999 mAh
uint8_t ahi_pitch_interval; // redraws AHI at set pitch interval (Not pixel OSD)
char osd_switch_indicator0_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 0.
uint8_t osd_switch_indicator0_channel; // RC Channel to use for switch indicator 0.
char osd_switch_indicator1_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 1.
uint8_t osd_switch_indicator1_channel; // RC Channel to use for switch indicator 1.
char osd_switch_indicator2_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 2.
uint8_t osd_switch_indicator2_channel; // RC Channel to use for switch indicator 2.
char osd_switch_indicator3_name[OSD_SWITCH_INDICATOR_NAME_LENGTH + 1]; // Name to use for switch indicator 3.
uint8_t osd_switch_indicator3_channel; // RC Channel to use for switch indicator 3.
bool osd_switch_indicators_align_left; // Align switch indicator name to left of the switch.
bool use_pilot_logo; // If enabled, the pilot logo (last 40 characters of page 2 font) will be used with the INAV logo.
uint8_t inav_to_pilot_logo_spacing; // The space between the INAV and pilot logos, if pilot logo is used. This number may be adjusted so that it fits the odd/even col width.
uint16_t arm_screen_display_time; // Length of time the arm screen is displayed
} osdConfig_t;
PG_DECLARE(osdConfig_t, osdConfig);

3
src/main/msp/msp_protocol_v2_inav.h
View file

@ -95,4 +95,7 @@
#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
#define MSP2_INAV_SELECT_MIXER_PROFILE 0x2080

93
src/main/navigation/navigation.c
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;
@ -949,7 +951,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 +994,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 +1516,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;
}
@ -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
*-----------------------------------------------------------*/
@ -3919,14 +3951,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;
}
@ -3940,12 +3972,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
}
}
else {
} else {
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;
@ -4054,13 +4085,13 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
}
}
/*
* Don't allow arming if any of JUMP waypoint has invalid settings
* First WP can't be JUMP
* Can't jump to immediately adjacent WPs (pointless)
* Can't jump beyond WP list
* Only jump to geo-referenced WP types
*/
/*
* Don't allow arming if any of JUMP waypoint has invalid settings
* First WP can't be JUMP
* Can't jump to immediately adjacent WPs (pointless)
* Can't jump beyond WP list
* Only jump to geo-referenced WP types
*/
if (posControl.waypointCount) {
for (uint8_t wp = posControl.startWpIndex; wp < posControl.waypointCount + posControl.startWpIndex; wp++){
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){

2
src/main/navigation/navigation.h
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);

2
src/main/navigation/navigation_fixedwing.c
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;
}
/*-----------------------------------------------------------

21
src/main/navigation/navigation_pos_estimator.c
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 */

7
src/main/rx/jetiexbus.c
View file

@ -63,7 +63,12 @@
#define JETIEXBUS_BAUDRATE 125000 // EX Bus 125000; EX Bus HS 250000 not supported
#define JETIEXBUS_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
#define JETIEXBUS_MIN_FRAME_GAP 1000
#define JETIEXBUS_CHANNEL_COUNT 16 // most Jeti TX transmit 16 channels
#ifdef USE_24CHANNELS
#define JETIEXBUS_CHANNEL_COUNT 24
#else
#define JETIEXBUS_CHANNEL_COUNT 16
#endif
#define EXBUS_START_CHANNEL_FRAME (0x3E)

6
src/main/rx/rx.h
View file

@ -83,7 +83,11 @@ typedef enum {
SERIALRX_FBUS,
} rxSerialReceiverType_e;
#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18
#ifdef USE_24CHANNELS
#define MAX_SUPPORTED_RC_CHANNEL_COUNT 26
#else
#define MAX_SUPPORTED_RC_CHANNEL_COUNT 18
#endif
#define NON_AUX_CHANNEL_COUNT 4
#define MAX_AUX_CHANNEL_COUNT (MAX_SUPPORTED_RC_CHANNEL_COUNT - NON_AUX_CHANNEL_COUNT)

5
src/main/target/SKYSTARSF405HD/target.h
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

3
src/main/target/common.h Executable file → Normal file
View file

@ -188,6 +188,9 @@
#define USE_SERIALRX_SUMD
#define USE_TELEMETRY_HOTT
#define USE_HOTT_TEXTMODE
#define USE_24CHANNELS
#else
#define MAX_MIXER_PROFILE_COUNT 1
#endif
#define USE_EZ_TUNE