diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100755 index 0000000000..6e7d914b25 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,66 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceRoot}/src/main/**", + "${workspaceRoot}/lib/main/**", + "/usr/include/**" + ], + "browse": { + "limitSymbolsToIncludedHeaders": false, + "path": [ + "${workspaceRoot}/src/main/**", + "${workspaceRoot}/lib/main/**" + ] + }, + "intelliSenseMode": "linux-gcc-arm", + "cStandard": "c11", + "cppStandard": "c++17", + "defines": [ + "MCU_FLASH_SIZE 512", + "USE_NAV", + "NAV_FIXED_WING_LANDING", + "USE_OSD", + "USE_GYRO_NOTCH_1", + "USE_GYRO_NOTCH_2", + "USE_DTERM_NOTCH", + "USE_ACC_NOTCH", + "USE_GYRO_BIQUAD_RC_FIR2", + "USE_D_BOOST", + "USE_SERIALSHOT", + "USE_ANTIGRAVITY", + "USE_ASYNC_GYRO_PROCESSING", + "USE_RPM_FILTER", + "USE_GLOBAL_FUNCTIONS", + "USE_DYNAMIC_FILTERS", + "USE_IMU_BNO055", + "USE_SECONDARY_IMU", + "USE_DSHOT", + "FLASH_SIZE 480", + "USE_I2C_IO_EXPANDER", + "USE_PCF8574", + "USE_ESC_SENSOR", + "USE_PROGRAMMING_FRAMEWORK", + "USE_SERIALRX_GHST", + "USE_TELEMETRY_GHST", + "USE_CMS", + "USE_DJI_HD_OSD", + "USE_GYRO_KALMAN", + "USE_RANGEFINDER", + "USE_RATE_DYNAMICS", + "USE_SMITH_PREDICTOR", + "USE_ALPHA_BETA_GAMMA_FILTER", + "USE_MAG_VCM5883", + "USE_TELEMETRY_JETIEXBUS", + "USE_NAV", + "USE_SDCARD_SDIO", + "USE_SDCARD", + "USE_Q_TUNE", + "USE_GYRO_FFT_FILTER" + ], + "configurationProvider": "ms-vscode.cmake-tools" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100755 index 0000000000..3ca90b787d --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,41 @@ +{ + // See https://go.microsoft.com/fwlink/?LinkId=733558 + // for the documentation about the tasks.json format + "version": "2.0.0", + "tasks": [ + { + "label": "Build Matek F722-SE", + "type": "shell", + "command": "make MATEKF722SE", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + }, + { + "label": "Build Matek F722", + "type": "shell", + "command": "make MATEKF722", + "group": { + "kind": "build", + "isDefault": true + }, + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + } + , + { + "label": "CMAKE Update", + "type": "shell", + "command": "cmake ..", + "group": "build", + "problemMatcher": [], + "options": { + "cwd": "${workspaceFolder}/build" + } + } + ] +} \ No newline at end of file diff --git a/Dockerfile b/Dockerfile index 9c816b0c18..71077c8ed9 100644 --- a/Dockerfile +++ b/Dockerfile @@ -4,15 +4,15 @@ ARG USER_ID ARG GROUP_ID ENV DEBIAN_FRONTEND noninteractive -RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi +RUN apt-get update && apt-get install -y git cmake make ruby gcc python3 python3-pip gcc-arm-none-eabi ninja-build 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 diff --git a/cmake/docker.sh b/cmake/docker.sh index 3c10ebc8e1..843e03a48a 100755 --- a/cmake/docker.sh +++ b/cmake/docker.sh @@ -6,7 +6,7 @@ CURR_REV="$(git rev-parse HEAD)" initialize_cmake() { echo -e "*** CMake was not initialized yet, doing it now.\n" - cmake .. + cmake -GNinja .. echo "$CURR_REV" > "$LAST_CMAKE_AT_REV_FILE" } @@ -26,4 +26,4 @@ else fi # Let Make handle the arguments coming from the build script -make "$@" +ninja "$@" diff --git a/cmake/docker_build_sitl.sh b/cmake/docker_build_sitl.sh new file mode 100644 index 0000000000..a79742ae0f --- /dev/null +++ b/cmake/docker_build_sitl.sh @@ -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 \ No newline at end of file diff --git a/cmake/docker_run_sitl.sh b/cmake/docker_run_sitl.sh new file mode 100644 index 0000000000..b2089137cc --- /dev/null +++ b/cmake/docker_run_sitl.sh @@ -0,0 +1,8 @@ +#!/bin/bash +cd build_SITL + +#Lauch SITL - configurator only mode +./inav_7.0.0_SITL + +#Launch SITL - connect to X-Plane. IP address should be host IP address, not 127.0.0.1. Can be found in X-Plane "Network" tab. +#./inav_7.0.0_SITL --sim=xp --simip=192.168.2.105 --simport=49000 \ No newline at end of file diff --git a/cmake/sitl.cmake b/cmake/sitl.cmake index 24a4ae9b27..78697f98a9 100644 --- a/cmake/sitl.cmake +++ b/cmake/sitl.cmake @@ -53,6 +53,11 @@ set(SITL_COMPILE_OPTIONS -funsigned-char ) +if(DEBUG) + message(STATUS "Debug mode enabled. Adding -g to SITL_COMPILE_OPTIONS.") + list(APPEND SITL_COMPILE_OPTIONS -g) +endif() + if(NOT MACOSX) set(SITL_COMPILE_OPTIONS ${SITL_COMPILE_OPTIONS} -Wno-return-local-addr diff --git a/docs/Controls.md b/docs/Controls.md index 1b63f90e2a..3cc62b4e74 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -44,9 +44,9 @@ The stick positions are combined to activate different functions: | Bypass Nav Arm disable | LOW | HIGH | CENTER | CENTER | | Save setting | LOW | LOW | LOW | HIGH | | Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER | -| Enter Camera OSD(RuncamDevice)| RIGHT | CENTER | CENTER | CENTER | -| Exit Camera OSD (RuncamDevice)| LEFT | CENTER | CENTER | CENTER | -| Confirm - Camera OSD | RIGHT | CENTER | CENTER | CENTER | +| Enter Camera OSD(RuncamDevice)| CENTER | HIGH | CENTER | CENTER | +| Exit Camera OSD (RuncamDevice)| CENTER | LOW | CENTER | CENTER | +| Confirm - Camera OSD | CENTER | HIGH | CENTER | CENTER | | Navigation - Camera OSD | CENTER | CENTER | * | * | For graphical stick position in all transmitter modes, check out [this page](https://www.mrd-rc.com/tutorials-tools-and-testing/inav-flight/inav-stick-commands-for-all-transmitter-modes/). diff --git a/docs/LED pin PWM.md b/docs/LED pin PWM.md new file mode 100644 index 0000000000..2aead4cd7a --- /dev/null +++ b/docs/LED pin PWM.md @@ -0,0 +1,90 @@ +# LED pin PWM + +Normally LED pin is used to drive WS2812 led strip. LED pin is held low, and every 10ms or 20ms a set of pulses is sent to change color of the 32 LEDs: + +![alt text](/docs/assets/images/ws2811_packets.png "ws2811 packets") +![alt text](/docs/assets/images/ws2811_data.png "ws2811 data") + +As alternative function, it is possible to generate PWM signal with specified duty ratio on the LED pin. + +Feature can be used to drive external devices. It is also used to simulate [OSD joystick](OSD%20Joystick.md) to control cameras. + +PWM frequency is fixed to 24kHz with duty ratio between 0 and 100%: + +![alt text](/docs/assets/images/led_pin_pwm.png "led pin pwm") + +There are four modes of operation: +- low +- high +- shared_low +- shared_high + +Mode is configured using ```led_pin_pwm_mode``` setting: ```LOW```, ```HIGH```, ```SHARED_LOW```, ```SHARED_HIGH``` + +*Note that in any mode, there will be ~2 seconds LOW pulse on boot.* + +## LOW +LED Pin is initialized to output low level by default and can be used to generate PWM signal. + +ws2812 strip can not be controlled. + +## HIGH +LED Pin is initialized to output high level by default and can be used to generate PWM signal. + +ws2812 strip can not be controlled. + +## SHARED_LOW (default) +LED Pin is used to drive WS2812 strip. Pauses between pulses are low: + +![alt text](/docs/assets/images/ws2811_packets.png "ws2811 packets") + +It is possible to generate PWM signal with duty ratio >0...100%. + +While PWM signal is generated, ws2811 strip is not updated. + +When PWM generation is disabled, LED pin is used to drive ws2812 strip. + +Total ws2812 pulses duration is ~1ms with ~9ms pauses. Thus connected device should ignore PWM signal with duty ratio < ~10%. + +## SHARED_HIGH +LED Pin is used to drive WS2812 strip. Pauses between pulses are high. ws2812 pulses are prefixed with 50us low 'reset' pulse: + +![alt text](/docs/assets/images/ws2811_packets_high.png "ws2811 packets_high") +![alt text](/docs/assets/images/ws2811_data_high.png "ws2811 data_high") + + It is possible to generate PWM signal with duty ratio 0...<100%. + + While PWM signal is generated, ws2811 strip is not updated. + + When PWM generation is disabled, LED pin is used to drive ws2812 strip. Total ws2812 pulses duration is ~1ms with ~9ms pauses. Thus connected device should ignore PWM signal with duty ratio > ~90%. + + After sending ws2812 protocol pulses for 32 LEDS, we held line high for 9ms, then send 50us low 'reset' pulse. Datasheet for ws2812 protocol does not describe behavior for long high pulse, but in practice it works the same as 'reset' pulse. To be safe, we also send correct low 'reset' pulse before starting next LEDs update sequence. + + This mode is used to simulate OSD joystick. It is Ok that effectively voltage level is held >90% while driving LEDs, because OSD joystick keypress voltages are below 90%. + + See [OSD Joystick](OSD%20Joystick.md) for more information. + +# Generating PWM signal with programming framework + +See "LED Pin PWM" operation in [Programming Framework](Programming%20Framework.md) + + +# Generating PWM signal from CLI + +```ledpinpwm ``` - value = 0...100 - enable PWM generation with specified duty cycle + +```ledpinpwm``` - disable PWM generation ( disable to allow ws2812 LEDs updates in shared modes ) + + +# Example of driving LED + +It is possible to drive single color LED with brightness control. Current consumption should not be greater then 1-2ma, thus LED can be used for indication only. + +![alt text](/docs/assets/images/ledpinpwmled.png "led pin pwm led") + +# Example of driving powerfull white LED + +To drive power LED with brightness control, Mosfet should be used: + +![alt text](/docs/assets/images/ledpinpwmpowerled.png "led pin pwm power_led") + diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 052298e73f..55aedd3a30 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -1,80 +1,30 @@ # MixerProfile -A MixerProfile is a set of motor mixer, servo-mixer and platform type configuration settings to enable VTOL transitions. +A MixerProfile is a set of motor mixer, servo-mixer and platform type configuration settings. It is designed for experienced inav users. -Currently two profiles are supported on targets other than F411 and F722 (due to resource constraints on these FC). i.e VTOL transition is not available on F411 and F722 targets. +### For a tutorial of vtol setup, Read https://github.com/iNavFlight/inav/blob/master/docs/VTOL.md -By default, switching between profiles requires reboot to take affect. However, when all conditions are met, and a suitable [configuration](#configuration) has been applied, `mixer_profile` also allows in-flight profile [switching](#rc-mode-settings) to allow things like VTOL operation. This is the recommended operating mode. +Not limited to VTOL. air/land/sea mixed vehicle is also achievable with this feature. Model behaves according to current mixer_profile's platform_type and configured custom motor/servo mixer + +Currently two profiles are supported on targets other than F411(due to resource constraints on F411). i.e VTOL transition is not available on F411. + +For VTOL setup. one mixer_profile is used for multi-rotor(MR) and the other is used for fixed-wing(FW) +By default, switching between profiles requires reboot to take affect. However, using the RC mode: `MIXER PROFILE 2` will allow in flight switching for things like VTOL operation +. And will re-initialize pid and navigation controllers for current MC or FW flying mode. Please note that this is an emerging / experimental capability that will require some effort by the pilot to implement. -## Setup for VTOL -- A VTOL specific FC target or `timer_output_mode` overrides was required in the early stage of the development, But since unified mapping introduced in INAV 7.0 It is not needed anymore. -- ~~For mixer profile switching it is necessary to keep motor and servo PWM mapping consistent between Fixed-Wing (FW) and Multi-rotor (MR) profiles~~ -- ~~Traditionally, FW and MR have had different enumerations to define the PWM mappings. For VTOL operation it is necessary to set the `timer_output_mode` overrides to allow a consistent enumeration and thus mapping between MR and FW modes.~~ -- ~~In operation, it is necessary to set the `mixer_profile` and the `pid_profile` separately and to set a [RC mode](#rc-mode-settings) to switch between them.~~ -## Configuration -### Timer overrides -Set the timer overrides for the outputs that you are intended to use. -For SITL builds, is not necessary to set timer overrides. -Please note that there are some display issues on the configurator that will show wrong mapping on the mixer_profile which has less motor/servo compared with the another -### Profile Switch - -Setup the FW mode and MR mode separately in two different mixer profiles: - -In this example, FW mode is `mixer_profile` 1 and MR mode is `mixer_profile` 2. - -Currently, the INAV Configurator does not fully support `mixer_profile`, so some of the settings have to be done in CLI. - -Add `set mixer_pid_profile_linking = ON` in order to enable `pid_profile` auto handling. It will change the `pid profile` index according to the `mixer_profile` index on FC boot and allow `mixer_profile` hot switching (this is recommended usage). - -The following 2 `mixer_profile` sections are added in the CLI: - -``` -#switch to mixer_profile by cli -mixer_profile 1 - -set platform_type = AIRPLANE -set model_preview_type = 26 -# motor stop feature have been moved to here -set motorstop_on_low = ON -# enable pid_profile auto handling (recommended). -set mixer_pid_profile_linking = ON -save -``` -Then finish the aeroplane setting on mixer_profile 1 - -``` -mixer_profile 2 - -set platform_type = TRICOPTER -set model_preview_type = 1 -# also enable pid_profile auto handling -set mixer_pid_profile_linking = ON -save -``` -Then finish the multi-rotor setting on `mixer_profile` 2. - -Note that default profile is profile `1`. - -You can use `MAX` servo input to set a fixed input for the tilting servo. Speed setting for `MAX` input is available in the CLI. - -It is recommended to have some amount of control surface (elevon / elevator) mapped for stabilization even in MR mode to get improved authority when airspeed is high. - -**Double check all settings in CLI with the `diff all` command**; make sure you have set the correct settings. Also check what will change with `mixer_profile`. For example servo output min / max range will not change. But `smix` and `mmix` will change. - -### Mixer Transition input +## Mixer Transition input Typically, 'transition input' will be useful in MR mode to gain airspeed. -Both the servo mixer and motor mixer can accept transition mode as an input. The associated motor or servo will then move accordingly when transition mode is activated. Transition input is disabled when navigation mode is activate The use of Transition Mode is recommended to enable further features and future developments like fail-safe support. Mapping motor to servo output, or servo with logic conditions is **not** recommended -#### Servo +## Servo -38 is the input source for transition input; use this to tilt motor to gain airspeed. +`Mixer Transition` is the input source for transition input; use this to tilt motor to gain airspeed. Example: Increase servo 1 output by +45 with speed of 150 when transition mode is activated for tilted motor setup: @@ -82,15 +32,14 @@ Example: Increase servo 1 output by +45 with speed of 150 when transition mode i # rule no; servo index; input source; rate; speed; activate logic function number smix 6 1 38 45 150 -1 ``` -Please note there will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. More forward tilting servo position on transition input(you can use 'speed' in servo rules to slowly move to this position), A faster tilting servo speed on `MAX` servo input will reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect. -#### Motor +## Motor -The default `mmix` throttle value is 0.0, It will not show in `diff` command when throttle value is 0.0 (unused); this causes the motor to stop. +The default `mmix` throttle value is 0.0, It will not show in `diff` command when throttle value is 0.0 (unused); - 0.0```, value = 0...100 to find out PWM values for each voltage. +5. Specify PWM values in configuration and save: + +```set osd_joystick_down=0``` + +```set osd_joystick_up=48``` + +```set osd_joystick_left=63``` + +```set osd_joystick_right=28``` + +```set osd_joystick_enter=75``` + +```save``` + +# Entering OSD Joystick emulation mode + +Emulation can be enabled in unarmed state only. + +OSD Joystick emulation mode is enabled using the following stick combination: + +```Throttle:CENTER Yaw:RIGHT``` + + +Than camera OSD can be navigated using right stick. See [Controls](Controls.md) for all stick combinations. + +*Note that the same stick combination is used to enable 5keys joystick emulation with RuncamDevice protocol.* + +Mode is exited using stick combination: + +```Throttle:CENTER Yaw:LEFT``` + +# RC Box + +There are 3 RC Boxes which can be used in armed and unarmed state: +- Camera 1 - Enter +- Camera 2 - Up +- Camera 3 - Down + +Other keys can be emulated using Programming framework ( see [LED pin PWM](LED%20pin%20PWM.md) for more details ). + +# Behavior on boot + +There is ~2 seconds LOW pulse during boot sequence, which corresponds to DOWN key. Fortunately, cameras seem to ignore any key events few seconds after statup. diff --git a/docs/OSD.md b/docs/OSD.md index 64530f0e2c..b3526d0d76 100644 --- a/docs/OSD.md +++ b/docs/OSD.md @@ -5,163 +5,190 @@ The On Screen Display, or OSD, is a feature that overlays flight data over the v ## Features and Limitations Not all OSDs are created equally. This table shows the differences between the different systems available. -| OSD System | Character grid | Character | Canvas | MSP DisplayPort | All elements supported | -|---------------|-------------------|-----------|-----------|-------------------|---------------------------| -| Analogue PAL | 30 x 16 | X | | | YES | -| Analogue NTSC | 30 x 13 | X | | | YES | -| PixelOSD | As PAL or NTSC | | X | | YES | -| DJI OSD | 30 x 16 | X | | | NO - BF Characters only | -| DJI WTFOS | 60 x 22 | X | | X | YES | -| HDZero | 50 x 18 | X | | X | YES | -| Avatar | 53 x 20 | X | | X | YES | -| DJI O3 | 53 x 20 (HD) | X | | X (partial) | NO - BF Characters only | +| OSD System | Character grid | Character | Canvas | MSP DisplayPort | All elements supported | +|---------------|----------------|-----------|--------|-----------------|-------------------------| +| Analogue PAL | 30 x 16 | X | | | YES | +| Analogue NTSC | 30 x 13 | X | | | YES | +| PixelOSD | As PAL or NTSC | | X | | YES | +| DJI OSD | 30 x 16 | X | | | NO - BF Characters only | +| DJI WTFOS | 60 x 22 | X | | X | YES | +| HDZero | 50 x 18 | X | | X | YES | +| Avatar | 53 x 20 | X | | X | YES | +| DJI O3 | 53 x 20 (HD) | X | | X (partial) | NO - BF Characters only | ## OSD Elements Here are the OSD Elements provided by INAV. -| ID | Element | Added | -|-------|-----------------------------------------------|-------| -| 0 | OSD_RSSI_VALUE | 1.0.0 | -| 1 | OSD_MAIN_BATT_VOLTAGE | 1.0.0 | -| 2 | OSD_CROSSHAIRS | 1.0.0 | -| 3 | OSD_ARTIFICIAL_HORIZON | 1.0.0 | -| 4 | OSD_HORIZON_SIDEBARS | 1.0.0 | -| 5 | OSD_ONTIME | 1.0.0 | -| 6 | OSD_FLYTIME | 1.0.0 | -| 7 | OSD_FLYMODE | 1.0.0 | -| 8 | OSD_CRAFT_NAME | 1.0.0 | -| 9 | OSD_THROTTLE_POS | 1.0.0 | -| 10 | OSD_VTX_CHANNEL | 1.0.0 | -| 11 | OSD_CURRENT_DRAW | 1.0.0 | -| 12 | OSD_MAH_DRAWN | 1.0.0 | -| 13 | OSD_GPS_SPEED | 1.0.0 | -| 14 | OSD_GPS_SATS | 1.0.0 | -| 15 | OSD_ALTITUDE | 1.0.0 | -| 16 | OSD_ROLL_PIDS | 1.6.0 | -| 17 | OSD_PITCH_PIDS | 1.6.0 | -| 18 | OSD_YAW_PIDS | 1.6.0 | -| 19 | OSD_POWER | 1.6.0 | -| 20 | OSD_GPS_LON | 1.6.0 | -| 21 | OSD_GPS_LAT | 1.6.0 | -| 22 | OSD_HOME_DIR | 1.6.0 | -| 23 | OSD_HOME_DIST | 1.6.0 | -| 24 | OSD_HEADING | 1.6.0 | -| 25 | OSD_VARIO | 1.6.0 | -| 26 | OSD_VARIO_NUM | 1.6.0 | -| 27 | OSD_AIR_SPEED | 1.7.3 | -| 28 | OSD_ONTIME_FLYTIME | 1.8.0 | -| 29 | OSD_RTC_TIME | 1.8.0 | -| 30 | OSD_MESSAGES | 1.8.0 | -| 31 | OSD_GPS_HDOP | 1.8.0 | -| 32 | OSD_MAIN_BATT_CELL_VOLTAGE | 1.8.0 | -| 33 | OSD_SCALED_THROTTLE_POS | 1.8.0 | -| 34 | OSD_HEADING_GRAPH | 1.8.0 | -| 35 | OSD_EFFICIENCY_MAH_PER_KM | 1.9.0 | -| 36 | OSD_WH_DRAWN | 1.9.0 | -| 37 | OSD_BATTERY_REMAINING_CAPACITY | 1.9.0 | -| 38 | OSD_BATTERY_REMAINING_PERCENT | 1.9.0 | -| 39 | OSD_EFFICIENCY_WH_PER_KM | 1.9.0 | -| 40 | OSD_TRIP_DIST | 1.9.1 | -| 41 | OSD_ATTITUDE_PITCH | 2.0.0 | -| 42 | OSD_ATTITUDE_ROLL | 2.0.0 | -| 43 | OSD_MAP_NORTH | 2.0.0 | -| 44 | OSD_MAP_TAKEOFF | 2.0.0 | -| 45 | OSD_RADAR | 2.0.0 | -| 46 | OSD_WIND_SPEED_HORIZONTAL | 2.0.0 | -| 47 | OSD_WIND_SPEED_VERTICAL | 2.0.0 | -| 48 | OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH | 2.0.0 | -| 49 | OSD_REMAINING_DISTANCE_BEFORE_RTH | 2.0.0 | -| 50 | OSD_HOME_HEADING_ERROR | 2.0.0 | -| 51 | OSD_COURSE_HOLD_ERROR | 2.0.0 | -| 52 | OSD_COURSE_HOLD_ADJUSTMENT | 2.0.0 | -| 53 | OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE | 2.0.0 | -| 54 | OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE | 2.0.0 | -| 55 | OSD_POWER_SUPPLY_IMPEDANCE | 2.0.0 | -| 56 | OSD_LEVEL_PIDS | 2.0.0 | -| 57 | OSD_POS_XY_PIDS | 2.0.0 | -| 58 | OSD_POS_Z_PIDS | 2.0.0 | -| 59 | OSD_VEL_XY_PIDS | 2.0.0 | -| 60 | OSD_VEL_Z_PIDS | 2.0.0 | -| 61 | OSD_HEADING_P | 2.0.0 | -| 62 | OSD_BOARD_ALIGN_ROLL | 2.0.0 | -| 63 | OSD_BOARD_ALIGN_PITCH | 2.0.0 | -| 64 | OSD_RC_EXPO | 2.0.0 | -| 65 | OSD_RC_YAW_EXPO | 2.0.0 | -| 66 | OSD_THROTTLE_EXPO | 2.0.0 | -| 67 | OSD_PITCH_RATE | 2.0.0 | -| 68 | OSD_ROLL_RATE | 2.0.0 | -| 69 | OSD_YAW_RATE | 2.0.0 | -| 70 | OSD_MANUAL_RC_EXPO | 2.0.0 | -| 71 | OSD_MANUAL_RC_YAW_EXPO | 2.0.0 | -| 72 | OSD_MANUAL_PITCH_RATE | 2.0.0 | -| 73 | OSD_MANUAL_ROLL_RATE | 2.0.0 | -| 74 | OSD_MANUAL_YAW_RATE | 2.0.0 | -| 75 | OSD_NAV_FW_CRUISE_THR | 2.0.0 | -| 76 | OSD_NAV_FW_PITCH2THR | 2.0.0 | -| 77 | OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE | 2.0.0 | -| 78 | OSD_DEBUG | 2.0.0 | -| 79 | OSD_FW_ALT_PID_OUTPUTS | 2.0.0 | -| 80 | OSD_FW_POS_PID_OUTPUTS | 2.0.0 | -| 81 | OSD_MC_VEL_X_PID_OUTPUTS | 2.0.0 | -| 82 | OSD_MC_VEL_Y_PID_OUTPUTS | 2.0.0 | -| 83 | OSD_MC_VEL_Z_PID_OUTPUTS | 2.0.0 | -| 84 | OSD_MC_POS_XYZ_P_OUTPUTS | 2.0.0 | -| 85 | OSD_3D_SPEED | 2.1.0 | -| 86 | OSD_IMU_TEMPERATURE | 2.1.0 | -| 87 | OSD_BARO_TEMPERATURE | 2.1.0 | -| 88 | OSD_TEMP_SENSOR_0_TEMPERATURE | 2.1.0 | -| 89 | OSD_TEMP_SENSOR_1_TEMPERATURE | 2.1.0 | -| 90 | OSD_TEMP_SENSOR_2_TEMPERATURE | 2.1.0 | -| 91 | OSD_TEMP_SENSOR_3_TEMPERATURE | 2.1.0 | -| 92 | OSD_TEMP_SENSOR_4_TEMPERATURE | 2.1.0 | -| 93 | OSD_TEMP_SENSOR_5_TEMPERATURE | 2.1.0 | -| 94 | OSD_TEMP_SENSOR_6_TEMPERATURE | 2.1.0 | -| 95 | OSD_TEMP_SENSOR_7_TEMPERATURE | 2.1.0 | -| 96 | OSD_ALTITUDE_MSL | 2.1.0 | -| 97 | OSD_PLUS_CODE | 2.1.0 | -| 98 | OSD_MAP_SCALE | 2.2.0 | -| 99 | OSD_MAP_REFERENCE | 2.2.0 | -| 100 | OSD_GFORCE | 2.2.0 | -| 101 | OSD_GFORCE_X | 2.2.0 | -| 102 | OSD_GFORCE_Y | 2.2.0 | -| 103 | OSD_GFORCE_Z | 2.2.0 | -| 104 | OSD_RC_SOURCE | 2.2.0 | -| 105 | OSD_VTX_POWER | 2.2.0 | -| 106 | OSD_ESC_RPM | 2.3.0 | -| 107 | OSD_ESC_TEMPERATURE | 2.5.0 | -| 108 | OSD_AZIMUTH | 2.6.0 | -| 109 | OSD_CRSF_RSSI_DBM | 2.6.0 | -| 110 | OSD_CRSF_LQ | 2.6.0 | -| 111 | OSD_CRSF_SNR_DB | 2.6.0 | -| 112 | OSD_CRSF_TX_POWER | 2.6.0 | -| 113 | OSD_GVAR_0 | 2.6.0 | -| 114 | OSD_GVAR_1 | 2.6.0 | -| 115 | OSD_GVAR_2 | 2.6.0 | -| 116 | OSD_GVAR_3 | 2.6.0 | -| 117 | OSD_TPA | 2.6.0 | -| 118 | OSD_NAV_FW_CONTROL_SMOOTHNESS | 2.6.0 | -| 119 | OSD_VERSION | 3.0.0 | -| 120 | OSD_RANGEFINDER | 3.0.0 | -| 121 | OSD_PLIMIT_REMAINING_BURST_TIME | 3.0.0 | -| 122 | OSD_PLIMIT_ACTIVE_CURRENT_LIMIT | 3.0.0 | -| 123 | OSD_PLIMIT_ACTIVE_POWER_LIMIT | 3.0.0 | -| 124 | OSD_GLIDESLOPE | 3.0.1 | -| 125 | OSD_GPS_MAX_SPEED | 4.0.0 | -| 126 | OSD_3D_MAX_SPEED | 4.0.0 | -| 127 | OSD_AIR_MAX_SPEED | 4.0.0 | -| 128 | OSD_ACTIVE_PROFILE | 4.0.0 | -| 129 | OSD_MISSION | 4.0.0 | -| 130 | OSD_SWITCH_INDICATOR_0 | 5.0.0 | -| 131 | OSD_SWITCH_INDICATOR_1 | 5.0.0 | -| 132 | OSD_SWITCH_INDICATOR_2 | 5.0.0 | -| 133 | OSD_SWITCH_INDICATOR_3 | 5.0.0 | -| 134 | OSD_TPA_TIME_CONSTANT | 5.0.0 | -| 135 | OSD_FW_LEVEL_TRIM | 5.0.0 | -| 136 | OSD_GLIDE_TIME_REMAINING | 6.0.0 | -| 137 | OSD_GLIDE_RANGE | 6.0.0 | -| 138 | OSD_CLIMB_EFFICIENCY | 6.0.0 | -| 139 | OSD_NAV_WP_MULTI_MISSION_INDEX | 6.0.0 | -| 140 | OSD_GROUND_COURSE | 6.0.0 | -| 141 | OSD_CROSS_TRACK_ERROR | 6.0.0 | -| 142 | OSD_PILOT_NAME | 6.0.0 | -| 143 | OSD_PAN_SERVO_CENTRED | 6.0.0 | \ No newline at end of file +| ID | Element | Added | +|-----|--------------------------------------------------|--------| +| 0 | OSD_RSSI_VALUE | 1.0.0 | +| 1 | OSD_MAIN_BATT_VOLTAGE | 1.0.0 | +| 2 | OSD_CROSSHAIRS | 1.0.0 | +| 3 | OSD_ARTIFICIAL_HORIZON | 1.0.0 | +| 4 | OSD_HORIZON_SIDEBARS | 1.0.0 | +| 5 | OSD_ONTIME | 1.0.0 | +| 6 | OSD_FLYTIME | 1.0.0 | +| 7 | OSD_FLYMODE | 1.0.0 | +| 8 | OSD_CRAFT_NAME | 1.0.0 | +| 9 | OSD_THROTTLE_POS | 1.0.0 | +| 10 | OSD_VTX_CHANNEL | 1.0.0 | +| 11 | OSD_CURRENT_DRAW | 1.0.0 | +| 12 | OSD_MAH_DRAWN | 1.0.0 | +| 13 | OSD_GPS_SPEED | 1.0.0 | +| 14 | OSD_GPS_SATS | 1.0.0 | +| 15 | OSD_ALTITUDE | 1.0.0 | +| 16 | OSD_ROLL_PIDS | 1.6.0 | +| 17 | OSD_PITCH_PIDS | 1.6.0 | +| 18 | OSD_YAW_PIDS | 1.6.0 | +| 19 | OSD_POWER | 1.6.0 | +| 20 | OSD_GPS_LON | 1.6.0 | +| 21 | OSD_GPS_LAT | 1.6.0 | +| 22 | OSD_HOME_DIR | 1.6.0 | +| 23 | OSD_HOME_DIST | 1.6.0 | +| 24 | OSD_HEADING | 1.6.0 | +| 25 | OSD_VARIO | 1.6.0 | +| 26 | OSD_VARIO_NUM | 1.6.0 | +| 27 | OSD_AIR_SPEED | 1.7.3 | +| 28 | OSD_ONTIME_FLYTIME | 1.8.0 | +| 29 | OSD_RTC_TIME | 1.8.0 | +| 30 | OSD_MESSAGES | 1.8.0 | +| 31 | OSD_GPS_HDOP | 1.8.0 | +| 32 | OSD_MAIN_BATT_CELL_VOLTAGE | 1.8.0 | +| 33 | OSD_SCALED_THROTTLE_POS | 1.8.0 | +| 34 | OSD_HEADING_GRAPH | 1.8.0 | +| 35 | OSD_EFFICIENCY_MAH_PER_KM | 1.9.0 | +| 36 | OSD_WH_DRAWN | 1.9.0 | +| 37 | OSD_BATTERY_REMAINING_CAPACITY | 1.9.0 | +| 38 | OSD_BATTERY_REMAINING_PERCENT | 1.9.0 | +| 39 | OSD_EFFICIENCY_WH_PER_KM | 1.9.0 | +| 40 | OSD_TRIP_DIST | 1.9.1 | +| 41 | OSD_ATTITUDE_PITCH | 2.0.0 | +| 42 | OSD_ATTITUDE_ROLL | 2.0.0 | +| 43 | OSD_MAP_NORTH | 2.0.0 | +| 44 | OSD_MAP_TAKEOFF | 2.0.0 | +| 45 | OSD_RADAR | 2.0.0 | +| 46 | OSD_WIND_SPEED_HORIZONTAL | 2.0.0 | +| 47 | OSD_WIND_SPEED_VERTICAL | 2.0.0 | +| 48 | OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH | 2.0.0 | +| 49 | OSD_REMAINING_DISTANCE_BEFORE_RTH | 2.0.0 | +| 50 | OSD_HOME_HEADING_ERROR | 2.0.0 | +| 51 | OSD_COURSE_HOLD_ERROR | 2.0.0 | +| 52 | OSD_COURSE_HOLD_ADJUSTMENT | 2.0.0 | +| 53 | OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE | 2.0.0 | +| 54 | OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE | 2.0.0 | +| 55 | OSD_POWER_SUPPLY_IMPEDANCE | 2.0.0 | +| 56 | OSD_LEVEL_PIDS | 2.0.0 | +| 57 | OSD_POS_XY_PIDS | 2.0.0 | +| 58 | OSD_POS_Z_PIDS | 2.0.0 | +| 59 | OSD_VEL_XY_PIDS | 2.0.0 | +| 60 | OSD_VEL_Z_PIDS | 2.0.0 | +| 61 | OSD_HEADING_P | 2.0.0 | +| 62 | OSD_BOARD_ALIGN_ROLL | 2.0.0 | +| 63 | OSD_BOARD_ALIGN_PITCH | 2.0.0 | +| 64 | OSD_RC_EXPO | 2.0.0 | +| 65 | OSD_RC_YAW_EXPO | 2.0.0 | +| 66 | OSD_THROTTLE_EXPO | 2.0.0 | +| 67 | OSD_PITCH_RATE | 2.0.0 | +| 68 | OSD_ROLL_RATE | 2.0.0 | +| 69 | OSD_YAW_RATE | 2.0.0 | +| 70 | OSD_MANUAL_RC_EXPO | 2.0.0 | +| 71 | OSD_MANUAL_RC_YAW_EXPO | 2.0.0 | +| 72 | OSD_MANUAL_PITCH_RATE | 2.0.0 | +| 73 | OSD_MANUAL_ROLL_RATE | 2.0.0 | +| 74 | OSD_MANUAL_YAW_RATE | 2.0.0 | +| 75 | OSD_NAV_FW_CRUISE_THR | 2.0.0 | +| 76 | OSD_NAV_FW_PITCH2THR | 2.0.0 | +| 77 | OSD_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE | 2.0.0 | +| 78 | OSD_DEBUG | 2.0.0 | +| 79 | OSD_FW_ALT_PID_OUTPUTS | 2.0.0 | +| 80 | OSD_FW_POS_PID_OUTPUTS | 2.0.0 | +| 81 | OSD_MC_VEL_X_PID_OUTPUTS | 2.0.0 | +| 82 | OSD_MC_VEL_Y_PID_OUTPUTS | 2.0.0 | +| 83 | OSD_MC_VEL_Z_PID_OUTPUTS | 2.0.0 | +| 84 | OSD_MC_POS_XYZ_P_OUTPUTS | 2.0.0 | +| 85 | OSD_3D_SPEED | 2.1.0 | +| 86 | OSD_IMU_TEMPERATURE | 2.1.0 | +| 87 | OSD_BARO_TEMPERATURE | 2.1.0 | +| 88 | OSD_TEMP_SENSOR_0_TEMPERATURE | 2.1.0 | +| 89 | OSD_TEMP_SENSOR_1_TEMPERATURE | 2.1.0 | +| 90 | OSD_TEMP_SENSOR_2_TEMPERATURE | 2.1.0 | +| 91 | OSD_TEMP_SENSOR_3_TEMPERATURE | 2.1.0 | +| 92 | OSD_TEMP_SENSOR_4_TEMPERATURE | 2.1.0 | +| 93 | OSD_TEMP_SENSOR_5_TEMPERATURE | 2.1.0 | +| 94 | OSD_TEMP_SENSOR_6_TEMPERATURE | 2.1.0 | +| 95 | OSD_TEMP_SENSOR_7_TEMPERATURE | 2.1.0 | +| 96 | OSD_ALTITUDE_MSL | 2.1.0 | +| 97 | OSD_PLUS_CODE | 2.1.0 | +| 98 | OSD_MAP_SCALE | 2.2.0 | +| 99 | OSD_MAP_REFERENCE | 2.2.0 | +| 100 | OSD_GFORCE | 2.2.0 | +| 101 | OSD_GFORCE_X | 2.2.0 | +| 102 | OSD_GFORCE_Y | 2.2.0 | +| 103 | OSD_GFORCE_Z | 2.2.0 | +| 104 | OSD_RC_SOURCE | 2.2.0 | +| 105 | OSD_VTX_POWER | 2.2.0 | +| 106 | OSD_ESC_RPM | 2.3.0 | +| 107 | OSD_ESC_TEMPERATURE | 2.5.0 | +| 108 | OSD_AZIMUTH | 2.6.0 | +| 109 | OSD_CRSF_RSSI_DBM | 2.6.0 | +| 110 | OSD_CRSF_LQ | 2.6.0 | +| 111 | OSD_CRSF_SNR_DB | 2.6.0 | +| 112 | OSD_CRSF_TX_POWER | 2.6.0 | +| 113 | OSD_GVAR_0 | 2.6.0 | +| 114 | OSD_GVAR_1 | 2.6.0 | +| 115 | OSD_GVAR_2 | 2.6.0 | +| 116 | OSD_GVAR_3 | 2.6.0 | +| 117 | OSD_TPA | 2.6.0 | +| 118 | OSD_NAV_FW_CONTROL_SMOOTHNESS | 2.6.0 | +| 119 | OSD_VERSION | 3.0.0 | +| 120 | OSD_RANGEFINDER | 3.0.0 | +| 121 | OSD_PLIMIT_REMAINING_BURST_TIME | 3.0.0 | +| 122 | OSD_PLIMIT_ACTIVE_CURRENT_LIMIT | 3.0.0 | +| 123 | OSD_PLIMIT_ACTIVE_POWER_LIMIT | 3.0.0 | +| 124 | OSD_GLIDESLOPE | 3.0.1 | +| 125 | OSD_GPS_MAX_SPEED | 4.0.0 | +| 126 | OSD_3D_MAX_SPEED | 4.0.0 | +| 127 | OSD_AIR_MAX_SPEED | 4.0.0 | +| 128 | OSD_ACTIVE_PROFILE | 4.0.0 | +| 129 | OSD_MISSION | 4.0.0 | +| 130 | OSD_SWITCH_INDICATOR_0 | 5.0.0 | +| 131 | OSD_SWITCH_INDICATOR_1 | 5.0.0 | +| 132 | OSD_SWITCH_INDICATOR_2 | 5.0.0 | +| 133 | OSD_SWITCH_INDICATOR_3 | 5.0.0 | +| 134 | OSD_TPA_TIME_CONSTANT | 5.0.0 | +| 135 | OSD_FW_LEVEL_TRIM | 5.0.0 | +| 136 | OSD_GLIDE_TIME_REMAINING | 6.0.0 | +| 137 | OSD_GLIDE_RANGE | 6.0.0 | +| 138 | OSD_CLIMB_EFFICIENCY | 6.0.0 | +| 139 | OSD_NAV_WP_MULTI_MISSION_INDEX | 6.0.0 | +| 140 | OSD_GROUND_COURSE | 6.0.0 | +| 141 | OSD_CROSS_TRACK_ERROR | 6.0.0 | +| 142 | OSD_PILOT_NAME | 6.0.0 | +| 143 | OSD_PAN_SERVO_CENTRED | 6.0.0 | +| 144 | OSD_MULTI_FUNCTION | 7.0.0 | +| 145 | OSD_ODOMETER | 7.0.0 | +| 146 | OSD_PILOT_LOGO | 7.0.0 | + +# Pilot Logos + +From INAV 7.0.0, pilots can add their own logos to the OSD. These can appear in 2 places: the power on/arming screens or as an element on the standard OSD. Please note that the power on/arming screen large pilot logos are only available on HD systems. + +To use the pilot logos, you will need to make a custom font for your OSD system. Base fonts and information can be found in the [OSD folder](https://github.com/iNavFlight/inav-configurator/tree/master/resources/osd) in the Configurator resources. Each system will need a specific method to create the font image files. So they will not be covered here. There are two pilot logos. + +Default small INAV Pilot logoThe small pilot logo appears on standard OSD layouts, when you add the elemement to the OSD screen. This is a 3 character wide symbol (characters 469-471). + +Default large INAV Pilot logoThe large pilot logo appears on the power on and arming screens, when you enable the feature in the CLI. To do this, set the `osd_use_pilot_logo` parameter to `on`. This is a 10 character wide, 4 character high symbol (characters 472-511). + +## Settings + +* `osd_arm_screen_display_time` The amount of time the arming screen is displayed. +* `osd_inav_to_pilot_logo_spacing` The spacing between two logos. This can be set to `0`, so the original INAV logo and Pilot Logo can be combined in to a larger logo. Any non-0 value will be adjusted to best align the logos. For example, the Avatar system has an odd number of columns. If you set the spacing to 8, the logos would look misaligned. So the even number will be changed to an odd number for better alignment. +* `osd_use_pilot_logo` Enable to use the large pilot logo. + +## Examples + +This is an example of the arming screen with the pilot logo enabled. This is using the default settings. +![Arming screen example using default settings with osd_use_pilot_logo set to on](https://user-images.githubusercontent.com/17590174/271817487-eb18da4d-0911-44b2-b670-ea5940f79176.png) + +This is an example of setting the `osd_inav_to_pilot_logo_spacing` to 0. This will allow a larger, single logo. +![Power on screen example with 0 spacing between logos](https://user-images.githubusercontent.com/17590174/271817352-6206402c-9da4-4682-9d83-790cc2396b00.png) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 42a4f2a83f..1d0f4776b7 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -83,7 +83,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 35 | Trigonometry: Tangent | Computes TAN of `Operand A` value in degrees. Output is multiplied by `Operand B` value. If `Operand B` is `0`, result is multiplied by `500` | | 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled | | 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled | -| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` | +| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B`. Note operand A should normally be set as a "Value", NOT as "Get RC Channel"| | 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. | | 40 | Modulo | Modulo. Divide `Operand A` by `Operand B` and returns the remainder | | 41 | LOITER_RADIUS_OVERRIDE | Sets the loiter radius to `Operand A` [`0` : `100000`] in cm. If the value is lower than the loiter radius set in the **Advanced Tuning**, that will be used. | @@ -97,6 +97,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 49 | TIMER | A simple on - off timer. `true` for the duration of `Operand A` [ms]. Then `false` for the duration of `Operand B` [ms]. | | 50 | DELTA | This returns `true` when the value of `Operand A` has changed by the value of `Operand B` or greater within 100ms. | | 51 | APPROX_EQUAL | `true` if `Operand B` is within 1% of `Operand A`. | +| 52 | LED_PIN_PWM | Value `Operand A` from [`0` : `100`] starts PWM generation on LED Pin. See [LED pin PWM](LED%20pin%20PWM.md). Any other value stops PWM generation (stop to allow ws2812 LEDs updates in shared modes)| ### Operands @@ -152,6 +153,9 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn | 35 | AGL_STATUS | boolean `1` when AGL can be trusted, `0` when AGL estimate can not be trusted | | 36 | AGL | integer Above The Groud Altitude in `cm` | | 37 | RANGEFINDER_RAW | integer raw distance provided by the rangefinder in `cm` | +| 38 | ACTIVE_MIXER_PROFILE | Which mixers are currently active (for vtol etc) | +| 39 | MIXER_TRANSITION_ACTIVE | Currently switching between mixers (quad to plane etc) | +| 40 | ATTITUDE_YAW | current heading (yaw) in `degrees` | #### FLIGHT_MODE diff --git a/docs/Runcam device.md b/docs/Runcam device.md new file mode 100644 index 0000000000..b8c6ef80fd --- /dev/null +++ b/docs/Runcam device.md @@ -0,0 +1,32 @@ +# Runcam device + +Cameras which support [Runcam device protocol](https://support.runcam.com/hc/en-us/articles/360014537794-RunCam-Device-Protocol), can be configured using sticks. + +Note that for cameras which has OSD pin, there is alternative functionality: [OSD Joystick](OSD%20Joystick.md). + +Camera's RX/TX should be connected to FC's UART, which has "Runcam device" option selected. + +# Entering Joystick emulation mode + +Emulation can be enabled in unarmed state only. + +Joystick emulation mode is enabled using the following stick combination: + +```RIGHT CENTER``` + + +Than camera OSD can be navigated using right stick. See [Controls](Controls.md) for all stick combinations. + +*Note that the same stick combination is used to enable [OSD Joystick](OSD%20Joystick.md).* + +Mode is exited using stick combination: + +```LEFT CENTER``` + +# RC Box + +There are 3 RC Boxes which can be used in armed and unarmed state: +- Camera 1 - Simulate Wifi button +- Camera 2 - Simulate POWER button +- Camera 3 - Simulate Change Mode button. + diff --git a/docs/Screenshots/mixerprofile_4puls1_mix.png b/docs/Screenshots/mixerprofile_4puls1_mix.png new file mode 100644 index 0000000000..9fdae07077 Binary files /dev/null and b/docs/Screenshots/mixerprofile_4puls1_mix.png differ diff --git a/docs/Screenshots/mixerprofile_flow.png b/docs/Screenshots/mixerprofile_flow.png new file mode 100644 index 0000000000..292949b23b Binary files /dev/null and b/docs/Screenshots/mixerprofile_flow.png differ diff --git a/docs/Screenshots/mixerprofile_fw_mixer.png b/docs/Screenshots/mixerprofile_fw_mixer.png new file mode 100644 index 0000000000..b27b1b4b78 Binary files /dev/null and b/docs/Screenshots/mixerprofile_fw_mixer.png differ diff --git a/docs/Screenshots/mixerprofile_mc_mixer.png b/docs/Screenshots/mixerprofile_mc_mixer.png new file mode 100644 index 0000000000..fe1766665a Binary files /dev/null and b/docs/Screenshots/mixerprofile_mc_mixer.png differ diff --git a/docs/Screenshots/mixerprofile_servo_transition_mix.png b/docs/Screenshots/mixerprofile_servo_transition_mix.png new file mode 100644 index 0000000000..31e40ff548 Binary files /dev/null and b/docs/Screenshots/mixerprofile_servo_transition_mix.png differ diff --git a/docs/Settings.md b/docs/Settings.md index 968f2e49f5..8d4b027e8b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -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). @@ -1222,16 +1312,6 @@ Iterm is not allowed to grow when stick position is above threshold. This solves --- -### fw_iterm_throw_limit - -Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. - -| Default | Min | Max | -| --- | --- | --- | -| 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX | - ---- - ### fw_level_pitch_gain I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations @@ -1932,6 +2012,16 @@ Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened w --- +### led_pin_pwm_mode + +PWM mode of LED pin. + +| Default | Min | Max | +| --- | --- | --- | +| SHARED_LOW | | | + +--- + ### ledstrip_visual_beeper _// TODO_ @@ -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,76 @@ 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_joystick_down + +PWM value for DOWN key + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 100 | + +--- + +### osd_joystick_enabled + +Enable OSD Joystick emulation + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### osd_joystick_enter + +PWM value for ENTER key + +| Default | Min | Max | +| --- | --- | --- | +| 75 | 0 | 100 | + +--- + +### osd_joystick_left + +PWM value for LEFT key + +| Default | Min | Max | +| --- | --- | --- | +| 63 | 0 | 100 | + +--- + +### osd_joystick_right + +PWM value for RIGHT key + +| Default | Min | Max | +| --- | --- | --- | +| 28 | 0 | 100 | + +--- + +### osd_joystick_up + +PWM value for UP key + +| Default | Min | Max | +| --- | --- | --- | +| 48 | 0 | 100 | + +--- + ### osd_left_sidebar_scroll _// TODO_ @@ -4372,9 +4542,9 @@ LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50 --- -### osd_mah_used_precision +### osd_mah_precision -Number of digits used to display mAh used. +Number of digits used for mAh precision. Currently used by mAh Used and Battery Remaining Capacity | Default | Min | Max | | --- | --- | --- | @@ -4762,6 +4932,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` @@ -4772,6 +4952,16 @@ Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF' --- +### pid_iterm_limit_percent + +Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 200 | + +--- + ### pid_type Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` @@ -5682,9 +5872,19 @@ See tpa_rate. --- +### tpa_on_yaw + +Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### tpa_rate -Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. +Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | Default | Min | Max | | --- | --- | --- | diff --git a/docs/VTOL.md b/docs/VTOL.md new file mode 100644 index 0000000000..64afb333d1 --- /dev/null +++ b/docs/VTOL.md @@ -0,0 +1,236 @@ +# Welcome to INAV VTOL + +Thank you for trying the INAV VTOL. Read every line in this tutorial. Your patience can save both time and potential repair costs for the model. + +## Who Should Use This Tutorial? + +This tutorial is designed for individuals who +- have prior experience with **both INAV multi-rotor and INAV fixed-wing configurations/operations.** +- know how to create a custom mixer for their model. +- know basic physics of vtol operation + +## Firmware Status + +The firmware is in a flyable state, but it hasn't undergone extensive testing yet. This means there may be potential issues that have not yet been discovered. + +## Future Changes + +Please be aware that both the setup procedure and firmware may change in response to user feedback and testing results. +## Your Feedback Matters + +We highly value your feedback as it plays a crucial role in the development and refinement of INAV VTOL capabilities. Please share your experiences, suggestions, and any issues you encounter during testing. Your insights are invaluable in making INAV VTOL better for everyone. + +# VTOL Configuration Steps + +### The VTOL functionality is achieved by switching/transitioning between two configurations stored in the FC. VTOL specific configurations are Mixer Profiles with associated PID profiles. One profile set is for fixed-wing(FW) mode, One is for multi-copter(MC) mode. Configuration/Settings other than Mixer/PID profiles are shared among two modes +![Alt text](Screenshots/mixerprofile_flow.png) + +0. **Find a DIFF ALL file for your model and start from there if possible** + - Be aware that `MIXER PROFILE 2` RC mode setting introduced by diff file can get your stuck in a mixer_profile. remove or change channel to proceed +1. **Setup Profile 1:** + - Configure it as a normal fixed-wing/multi-copter. + +2. **Setup Profile 2:** + - Configure it as a normal multi-copter/fixed-wing. + +3. **Mode Tab Settings:** + - Set up switching in the mode tab. + +4. *(Recommended)* **Transition Mixing (Multi-Rotor Profile):** + - Configure transition mixing to gain airspeed in the multi-rotor profile. + +5. *(Optional)* **Automated Switching (RTH):** + - Optionally, set up automated switching in case of failsafe. + +# STEP0: Load parameter preset/templates +Find a working diff file if you can and start from there. If not, select keep current settings and apply following parameter in cli but read description about which one to apply. + +``` +set small_angle = 180 +set gyro_main_lpf_hz = 80 +set dynamic_gyro_notch_min_hz = 50 +set dynamic_gyro_notch_mode = 3D +set motor_pwm_protocol = DSHOT300 #Try dshot first and see if it works +set airmode_type = STICK_CENTER_ONCE + + +set nav_disarm_on_landing = OFF #band-aid for false landing detection in NAV landing of multi-copter +set nav_rth_allow_landing = FS_ONLY +set nav_wp_max_safe_distance = 500 +set nav_fw_control_smoothness = 2 +set nav_fw_launch_max_altitude = 5000 + +set servo_pwm_rate = 160 #If model using servo for stabilization in MC mode and servo can tolerate it +set servo_lpf_hz = 30 #If model using servo for stabilization in MC mode + + +## profile 1 as airplane and profile 2 as multi rotor +mixer_profile 1 + +set platform_type = AIRPLANE +set model_preview_type = 26 +set motorstop_on_low = ON +set mixer_pid_profile_linking = ON + +mixer_profile 2 + +set platform_type = TRICOPTER +set model_preview_type = 1 +set mixer_pid_profile_linking = ON + +profile 1 #pid profile +set dterm_lpf_hz = 10 +set d_boost_min = 1.000 +set d_boost_max = 1.000 +set fw_level_pitch_trim = 5.000 +set roll_rate = 18 +set pitch_rate = 9 +set yaw_rate = 3 +set fw_turn_assist_pitch_gain = 0.4 +set max_angle_inclination_rll = 450 +set fw_ff_pitch = 80 +set fw_ff_roll = 50 +set fw_p_pitch = 15 +set fw_p_roll = 15 + +profile 2 +set dterm_lpf_hz = 60 +set dterm_lpf_type = PT3 +set d_boost_min = 0.800 +set d_boost_max = 1.200 +set d_boost_gyro_delta_lpf_hz = 60 +set antigravity_gain = 2.000 +set antigravity_accelerator = 5.000 +set smith_predictor_delay = 1.500 +set tpa_rate = 20 +set tpa_breakpoint = 1200 +set tpa_on_yaw = ON #If model using control surface/tilt mechanism for stabilization in MC mode +set roll_rate = 18 +set pitch_rate = 18 +set yaw_rate = 9 +set mc_iterm_relax = RPY + +save +``` + +# STEP1&2: Configuring as a Normal fixed-wing/Multi-Copter in two profiles separately + +1. **Select the fisrt Mixer Profile and PID Profile:** + - In the CLI, switch to the mixer_profile and pid_profile you wish to set first. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded. + ``` + mixer_profile 1 #in this example, we set profile 1 first + set mixer_pid_profile_linking = ON # Let the mixer_profile handle the pid_profile switch on this mixer_profile + set platform_type = AIRPLANE + save + ``` + +2. **Configure the fixed-wing/Multi-Copter:** + - Configure your fixed-wing/Multi-Copter as you normally would, or you can copy and paste default settings to expedite the process. + - Dshot esc protocol availability might be limited depends on outputs and fc board you are using. change the motor wiring or use oneshot/multishot esc protocol and calibrate throttle range. + - You can use throttle = -1 as a placeholder for the motor you wish to stop if the motor isn't the last motor + - Consider conducting a test flight to ensure that everything operates as expected. And tune the settings, trim the servos. + +![Alt text](Screenshots/mixerprofile_fw_mixer.png) + +3. **Switch to Another Mixer Profile with PID Profile:** + - In the CLI, switch to another mixer_profile along with the appropriate pid_profile. You can also switch mixer_profile/pid_profile through gui if with aforementioned presets loaded. + ``` + mixer_profile 2 + set mixer_pid_profile_linking = ON + set platform_type = MULTIROTOR/TRICOPTER + save + ``` + +4. **Configure the Multi-Copter/fixed-wing:** + - Set up your multi-copter/fixed-wing as usual, this time for mixer_profile 2 and pid_profile 2. + - Utilize the 'MAX' input in the servo mixer to tilt the motors without altering the servo midpoint. + - At this stage, focus on configuring profile-specific settings. You can streamline this process by copying and pasting the default PID settings. + - you can set -1 in motor mixer throttle as a place holder: disable that motor but will load following motor rules + - compass is required to enable navigation modes for multi-rotor profile. + - Consider conducting a test flight to ensure that everything operates as expected. And tune the settings. + - It is advisable to have a certain degree of control surface (elevon / elevator) mapping for stabilization even in multi-copter mode. This helps improve control authority when airspeed is high. It might be unable to recover from a dive without them. + +![Alt text](Screenshots/mixerprofile_mc_mixer.png) + +5. **Tailsitters:planned for INAV 7.1** + - Configure the fixed-wing mode/profile sets normally. Use MultiCopter platform type for tail_sitting flying mode/profile sets. + - The baseline board aliment is FW mode(ROLL axis is the trust axis). So set `tailsitter_orientation_offset = ON ` in the tail_sitting MC mode. + - Configure mixer ROLL/YAW mixing according to tail_sitting orientation in the tail_sitting MC mode. YAW axis is the trust axis. + - Conduct a bench test and see the orientation of the model changes in inav-configurator setup tab + +# STEP3: Mode Tab Settings: +### We recommend using an 3-pos switch on you radio to activate these modes, So pilot can jump in or bell out at any moment. +### Here is a example, in the bottom of inav-configurator Modes tab: +![Alt text](Screenshots/mixer_profile.png) +| 1000~1300 | 1300~1700 | 1700~2000 | +| :-- | :-- | :-- | +| Profile1(FW) with transition off | Profile2(MC) with transition on | Profile2(MC) with transition off | + +- Profile file switching becomes available after completing the runtime sensor calibration(15-30s after booting). And It is **not available** when a navigation mode or position hold is active. + +- By default, `mixer_profile 1` is used. `mixer_profile 2` is used when the `MIXER PROFILE 2` mode is activate. Once configured successfully, you will notice that the profiles and model preview changes accordingly when you refresh the relevant INAV Configurator tabs. + +- Use the `MIXER TRANSITION` mode to gain airspeed in MC profile, set `MIXER TRANSITION` accordingly. + +Conduct a bench test on the model (without props attached). The model can now switch between fixed-wing and multi-copter modes while armed. Furthermore, it is capable of mid-air switching, resulting in an immediate stall upon entering fixed-wing profile + +# STEP4: Transition Mixing (Multi-Rotor Profile)(Recommended) +### Transition Mixing is typically useful in multi-copter profile to gain airspeed in prior to entering the fixed-wing profile. When the `MIXER TRANSITION` mode is activated, the associated motor or servo will move according to your configured Transition Mixing. + +Please note that transition input is disabled when a navigation mode is activated. The use of Transition Mixing is necessary to enable additional features such as VTOL RTH with out stalling. +## Servo 'Transition Mixing': Tilting rotor configuration. +Add new servo mixer rules, and select 'Mixer Transition' in input. Set the weight/rate according to your desired angle. This will allow tilting the motor for tilting rotor model. + +![Alt text](Screenshots/mixerprofile_servo_transition_mix.png) + +## Motor 'Transition Mixing': Dedicated forward motor configuration +In motor mixer set: +- -2.0 < throttle < -1.0: The motor will spin regardless of the radio's throttle position at a speed of `abs(throttle) - 1` multiplied by throttle range only when Mixer Transition is activated. + +![Alt text](Screenshots/mixerprofile_4puls1_mix.png) + +## TailSitter 'Transition Mixing': +No additional settings needed, 45deg off set will be added to target pitch angle for angle mode in the firmware. + +### With aforementioned settings, your model should be able to enter fixed-wing profile without stalling. + +# Automated Switching (RTH) (Optional): +### This is one of the least tested features. This feature is primarily designed for Return to Home (RTH) in the event of a failsafe. +When configured correctly, the model will use the Fixed-Wing (FW) mode to efficiently return home and then transition to Multi-Copter (MC) mode for easier landing. + +To enable this feature, type following command in cli + +1. In your MC mode mixer profile (e.g., mixer_profile 2), set `mixer_automated_switch` to `ON`. leave it to `OFF` if burning remaining battery capacity on the way home is acceptable. +``` +mixer_profile 2or1 +set mixer_automated_switch= ON +``` + +2. Set `mixer_switch_trans_timer` ds in cli in the MC mode mixer profile to specify the time required for your model to gain sufficient airspeed before transitioning to FW mode. +``` +mixer_profile 2or1 +set mixer_switch_trans_timer = 30 # 3s, 3000ms +``` +3. In your FW mode mixer profile (e.g., mixer_profile 1), also set `mixer_automated_switch` to `ON`. leave it to `OFF` if automated landing in fixed-wing is acceptable. +``` +mixer_profile 1or2 +set mixer_automated_switch = ON +``` +4. Save your settings. type `save` in cli. + +If you set `mixer_automated_switch` to `OFF` for all mixer profiles (the default setting), the model will not perform automated transitions. You can always enable navigation modes after performing a manual transition. + + +# Notes and Experiences +## General +- VTOL model operating in multi-copter (MC) mode may encounter challenges in windy conditions. Please exercise caution when testing in such conditions. +- Make sure you can recover from a complete stall before trying the mid air transition +- It will be much safer if you can understand every line in diff all, read your diff all before maiden + +## Tilting-rotor +- In some tilting motor models, you may experience roll/yaw coupled oscillations when `MIXER TRANSITION` is activated. To address this issue, you can try the following: + 1. Use prop blade meets at top/rear prop direction for tilting motors to balance the effects of torque and P-factor. + 2. In addition to 1. Add a little yaw mixing(about 0.2) in tilt motors. +- There will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. Use the max speed or faster speed in tiling servo to reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect. +## Dedicated forward motor +- Easiest way to setup a vtol. and efficiency can be improved by using different motor/prop for hover and forward flight \ No newline at end of file diff --git a/docs/assets/images/led_pin_pwm.png b/docs/assets/images/led_pin_pwm.png new file mode 100644 index 0000000000..93bab8d1df Binary files /dev/null and b/docs/assets/images/led_pin_pwm.png differ diff --git a/docs/assets/images/ledpinpwmfilter.png b/docs/assets/images/ledpinpwmfilter.png new file mode 100644 index 0000000000..d27acea6bc Binary files /dev/null and b/docs/assets/images/ledpinpwmfilter.png differ diff --git a/docs/assets/images/ledpinpwmled.png b/docs/assets/images/ledpinpwmled.png new file mode 100644 index 0000000000..3286e51ae3 Binary files /dev/null and b/docs/assets/images/ledpinpwmled.png differ diff --git a/docs/assets/images/ledpinpwmpowerled.png b/docs/assets/images/ledpinpwmpowerled.png new file mode 100644 index 0000000000..2cc0d4bd1c Binary files /dev/null and b/docs/assets/images/ledpinpwmpowerled.png differ diff --git a/docs/assets/images/osd_joystick.jpg b/docs/assets/images/osd_joystick.jpg new file mode 100644 index 0000000000..9b2ad0bac0 Binary files /dev/null and b/docs/assets/images/osd_joystick.jpg differ diff --git a/docs/assets/images/osd_joystick_keys.png b/docs/assets/images/osd_joystick_keys.png new file mode 100644 index 0000000000..d21b7b7d65 Binary files /dev/null and b/docs/assets/images/osd_joystick_keys.png differ diff --git a/docs/assets/images/ws2811_data.png b/docs/assets/images/ws2811_data.png new file mode 100644 index 0000000000..ce95ef947f Binary files /dev/null and b/docs/assets/images/ws2811_data.png differ diff --git a/docs/assets/images/ws2811_data_high.png b/docs/assets/images/ws2811_data_high.png new file mode 100644 index 0000000000..c77e89e5ec Binary files /dev/null and b/docs/assets/images/ws2811_data_high.png differ diff --git a/docs/assets/images/ws2811_packets.png b/docs/assets/images/ws2811_packets.png new file mode 100644 index 0000000000..ef5cdca72b Binary files /dev/null and b/docs/assets/images/ws2811_packets.png differ diff --git a/docs/assets/images/ws2811_packets_high.png b/docs/assets/images/ws2811_packets_high.png new file mode 100644 index 0000000000..49c4711e41 Binary files /dev/null and b/docs/assets/images/ws2811_packets_high.png differ diff --git a/docs/development/Building in Docker.md b/docs/development/Building in Docker.md index 80d661ecf3..39cd887003 100755 --- a/docs/development/Building in Docker.md +++ b/docs/development/Building in Docker.md @@ -37,8 +37,19 @@ You'll have to manually execute the same steps that the build script does: + This step is only needed the first time. 2. `docker run --rm -it -u root -v :/src inav-build ` + Where `` must be replaced with the absolute path of where you cloned this repo (see above), and `` with the name of the target that you want to build. - + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above, or by removing "USER inav" line from the .\DockerFile before building image. + + Note that on Windows/WSL 2 mounted /src folder is writeable for root user only. You have to run build under root user. You can achieve this by using `-u root` option in the command line above. -3. If you need to update `Settings.md`, run `docker run --entrypoint /src/cmake/docker_docs.sh --rm -it -u root -v :/src inav-build` +3. If you need to update `Settings.md`, run: + +`docker run --entrypoint /src/cmake/docker_docs.sh --rm -it -u root -v :/src inav-build` + +4. Building SITL: + +`docker run --rm --entrypoint /src/cmake/docker_build_sitl.sh -it -u root -v :/src inav-build` + +5. Running SITL: + +`docker run -p 5760:5760 -p 5761:5761 -p 5762:5762 -p 5763:5763 -p 5764:5764 -p 5765:5765 -p 5766:5766 -p 5767:5767 --entrypoint /src/cmake/docker_run_sitl.sh --rm -it -u root -v :/src inav-build`. + + SITL command line parameters can be adjusted in `cmake/docker_run_sitl.sh`. Refer to the [Linux](#Linux) instructions or the [build script](/build.sh) for more details. diff --git a/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md index a2c5894e72..9cee0ede25 100644 --- a/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md +++ b/docs/development/Building in Windows 10 or 11 with Linux Subsystem.md @@ -36,7 +36,9 @@ To run `cmake` in the latest version you will need to update from Ubuntu `18_04 Mount MS windows C drive and clone INAV 1. `cd /mnt/c` -1. `git clone https://github.com/iNavFlight/inav.git` +2. `git clone https://github.com/iNavFlight/inav.git` +3. `git checkout 6.1.1` (to switch to a specific release tag, for this example INAV version 6.1.1) +4. `git checkout -b my-branch` (to create own branch) You are ready! You now have a folder called inav in the root of C drive that you can edit in windows diff --git a/docs/development/Software In The Loop (HITL) plugin for X-Plane 11.md b/docs/development/Hardware In The Loop (HITL) plugin for X-Plane.md similarity index 83% rename from docs/development/Software In The Loop (HITL) plugin for X-Plane 11.md rename to docs/development/Hardware In The Loop (HITL) plugin for X-Plane.md index 0b7d106e0d..c166c5117a 100644 --- a/docs/development/Software In The Loop (HITL) plugin for X-Plane 11.md +++ b/docs/development/Hardware In The Loop (HITL) plugin for X-Plane.md @@ -1,4 +1,4 @@ -# Software In The Loop (HITL) plugin for X-Plane 11 +# Hardware In The Loop (HITL) plugin for X-Plane 11/12 **Hardware-in-the-loop (HITL) simulation**, is a technique that is used in the development and testing of complex real-time embedded systems. @@ -6,6 +6,6 @@ **INAV-X-Plane-HITL** is plugin for **X-Plane** for testing and developing flight controllers with **INAV flight controller firmware** -https://github.com/iNavFlight/inav. +https://github.com/RomanLut/INAV-X-Plane-HITL HITL technique can be used to test features during development. Please check page above for installation instructions. diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index b6e433d97d..13ac4a9291 100755 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -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 @@ -523,6 +525,8 @@ main_sources(COMMON_SRC io/osd_grid.h io/osd_hud.c io/osd_hud.h + io/osd_joystick.c + io/osd_joystick.h io/smartport_master.c io/smartport_master.h io/vtx.c diff --git a/src/main/cms/cms.h b/src/main/cms/cms.h index f1f8d8017c..7084df8b50 100644 --- a/src/main/cms/cms.h +++ b/src/main/cms/cms.h @@ -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) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index cc3d1bc517..ff0505fb53 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -25,15 +25,15 @@ #endif // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations -#define FAST_MATH // order 9 approximation -//#define VERY_FAST_MATH // order 7 approximation +#define FAST_MATH // order 9 approximation +//#define VERY_FAST_MATH // order 7 approximation // Use floating point M_PI instead explicitly. -#define M_PIf 3.14159265358979323846f -#define M_LN2f 0.69314718055994530942f -#define M_Ef 2.71828182845904523536f +#define M_PIf 3.14159265358979323846f +#define M_LN2f 0.69314718055994530942f +#define M_Ef 2.71828182845904523536f -#define RAD (M_PIf / 180.0f) +#define RAD (M_PIf / 180.0f) #define DEGREES_TO_CENTIDEGREES(angle) ((angle) * 100) #define CENTIDEGREES_TO_DEGREES(angle) ((angle) / 100.0f) @@ -56,46 +56,46 @@ #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD) #define CENTIDEGREES_TO_RADIANS(angle) (((angle) / 100.0f) * RAD) -#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) -#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) -#define CENTIMETERS_TO_METERS(cm) (cm / 100.0f) +#define CENTIMETERS_TO_CENTIFEET(cm) (cm / 0.3048f) +#define CENTIMETERS_TO_FEET(cm) (cm / 30.48f) +#define CENTIMETERS_TO_METERS(cm) (cm / 100.0f) -#define METERS_TO_CENTIMETERS(m) (m * 100) +#define METERS_TO_CENTIMETERS(m) (m * 100) -#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f) -#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f) -#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f) +#define CMSEC_TO_CENTIMPH(cms) (cms * 2.2369363f) +#define CMSEC_TO_CENTIKPH(cms) (cms * 3.6f) +#define CMSEC_TO_CENTIKNOTS(cms) (cms * 1.943845f) #define C_TO_KELVIN(temp) (temp + 273.15f) // Standard Sea Level values // Ref:https://en.wikipedia.org/wiki/Standard_sea_level -#define SSL_AIR_DENSITY 1.225f // kg/m^3 -#define SSL_AIR_PRESSURE 101325.01576f // Pascal -#define SSL_AIR_TEMPERATURE 288.15f // K +#define SSL_AIR_DENSITY 1.225f // kg/m^3 +#define SSL_AIR_PRESSURE 101325.01576f // Pascal +#define SSL_AIR_TEMPERATURE 288.15f // K // copied from https://code.google.com/p/cxutil/source/browse/include/cxutil/utility.h#70 -#define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \ - ( __extension__ ({ \ - __typeof__(lexpr) lvar = (lexpr); \ - __typeof__(rexpr) rvar = (rexpr); \ - lvar binoper rvar ? lvar : rvar; \ - })) +#define _CHOOSE2(binoper, lexpr, lvar, rexpr, rvar) \ + ( __extension__ ({ \ + __typeof__(lexpr) lvar = (lexpr); \ + __typeof__(rexpr) rvar = (rexpr); \ + lvar binoper rvar ? lvar : rvar; \ + })) #define _CHOOSE_VAR2(prefix, unique) prefix##unique #define _CHOOSE_VAR(prefix, unique) _CHOOSE_VAR2(prefix, unique) -#define _CHOOSE(binoper, lexpr, rexpr) \ - _CHOOSE2( \ - binoper, \ - lexpr, _CHOOSE_VAR(_left, __COUNTER__), \ - rexpr, _CHOOSE_VAR(_right, __COUNTER__) \ +#define _CHOOSE(binoper, lexpr, rexpr) \ + _CHOOSE2( \ + binoper, \ + lexpr, _CHOOSE_VAR(_left, __COUNTER__), \ + rexpr, _CHOOSE_VAR(_right, __COUNTER__) \ ) #define MIN(a, b) _CHOOSE(<, a, b) #define MAX(a, b) _CHOOSE(>, a, b) -#define _ABS_II(x, var) \ - ( __extension__ ({ \ - __typeof__(x) var = (x); \ - var < 0 ? -var : var; \ +#define _ABS_II(x, var) \ + ( __extension__ ({ \ + __typeof__(x) var = (x); \ + var < 0 ? -var : var; \ })) #define _ABS_I(x, var) _ABS_II(x, var) #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__)) @@ -114,8 +114,7 @@ typedef union { fp_angles_def angles; } fp_angles_t; -typedef struct stdev_s -{ +typedef struct stdev_s { float m_oldM, m_newM, m_oldS, m_newS; int m_n; } stdev_t; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 16423639fb..eb5a07ddfb 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -120,7 +120,11 @@ #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_LEDPIN_CONFIG 1034 +#define PG_OSD_JOYSTICK_CONFIG 1035 +#define PG_INAV_END PG_OSD_JOYSTICK_CONFIG + // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/drivers/accgyro/accgyro_bmi270.c b/src/main/drivers/accgyro/accgyro_bmi270.c index 0b37518a3c..3e1db22253 100644 --- a/src/main/drivers/accgyro/accgyro_bmi270.c +++ b/src/main/drivers/accgyro/accgyro_bmi270.c @@ -214,7 +214,7 @@ static void bmi270AccAndGyroInit(gyroDev_t *gyro) delay(1); // Configure the accelerometer full-scale range - busWrite(busDev, BMI270_REG_ACC_RANGE, BMI270_ACC_RANGE_8G); + busWrite(busDev, BMI270_REG_ACC_RANGE, BMI270_ACC_RANGE_16G); delay(1); // Configure the gyro @@ -301,7 +301,7 @@ static void bmi270GyroInit(gyroDev_t *gyro) static void bmi270AccInit(accDev_t *acc) { // sensor is configured during gyro init - acc->acc_1G = 4096; // 8G sensor scale + acc->acc_1G = 2048; // 16G sensor scale } bool bmi270AccDetect(accDev_t *acc) diff --git a/src/main/drivers/accgyro/accgyro_icm42605.c b/src/main/drivers/accgyro/accgyro_icm42605.c index 5f536527c7..33cdfc368d 100644 --- a/src/main/drivers/accgyro/accgyro_icm42605.c +++ b/src/main/drivers/accgyro/accgyro_icm42605.c @@ -78,11 +78,13 @@ #define ICM42605_INT_TPULSE_DURATION_100 (0 << ICM42605_INT_TPULSE_DURATION_BIT) #define ICM42605_INT_TPULSE_DURATION_8 (1 << ICM42605_INT_TPULSE_DURATION_BIT) - #define ICM42605_RA_INT_SOURCE0 0x65 #define ICM42605_UI_DRDY_INT1_EN_DISABLED (0 << 3) #define ICM42605_UI_DRDY_INT1_EN_ENABLED (1 << 3) +#define ICM42605_INTF_CONFIG1 0x4D +#define ICM42605_INTF_CONFIG1_AFSR_MASK 0xC0 +#define ICM42605_INTF_CONFIG1_AFSR_DISABLE 0x40 static void icm42605AccInit(accDev_t *acc) { @@ -190,6 +192,15 @@ static void icm42605AccAndGyroInit(gyroDev_t *gyro) busWrite(dev, ICM42605_RA_INT_CONFIG1, intConfig1Value); delay(15); + //Disable AFSR as in BF and Ardupilot + uint8_t intfConfig1Value; + busRead(dev, ICM42605_INTF_CONFIG1, &intfConfig1Value); + intfConfig1Value &= ~ICM42605_INTF_CONFIG1_AFSR_MASK; + intfConfig1Value |= ICM42605_INTF_CONFIG1_AFSR_DISABLE; + busWrite(dev, ICM42605_INTF_CONFIG1, intfConfig1Value); + + delay(15); + busSetSpeed(dev, BUS_SPEED_FAST); } diff --git a/src/main/drivers/bus_i2c_at32f43x.c b/src/main/drivers/bus_i2c_at32f43x.c index 97036b6186..98fdd94246 100644 --- a/src/main/drivers/bus_i2c_at32f43x.c +++ b/src/main/drivers/bus_i2c_at32f43x.c @@ -70,7 +70,7 @@ static void i2cUnstick(IO_t scl, IO_t sda); //Define thi I2C hardware map static i2cDevice_t i2cHardwareMap[I2CDEV_COUNT] = { - { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C1_EVT_IRQn, .er_irq = I2C1_ERR_IRQn, .af = GPIO_MUX_8 }, + { .dev = I2C1, .scl = IO_TAG(I2C1_SCL), .sda = IO_TAG(I2C1_SDA), .rcc = RCC_APB1(I2C1), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C1_EVT_IRQn, .er_irq = I2C1_ERR_IRQn, .af = GPIO_MUX_4 }, { .dev = I2C2, .scl = IO_TAG(I2C2_SCL), .sda = IO_TAG(I2C2_SDA), .rcc = RCC_APB1(I2C2), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C2_EVT_IRQn, .er_irq = I2C2_ERR_IRQn, .af = GPIO_MUX_4 }, { .dev = I2C3, .scl = IO_TAG(I2C3_SCL), .sda = IO_TAG(I2C3_SDA), .rcc = RCC_APB1(I2C3), .speed = I2C_SPEED_400KHZ, .ev_irq = I2C3_EVT_IRQn, .er_irq = I2C3_ERR_IRQn, .af = GPIO_MUX_4 }, }; diff --git a/src/main/drivers/light_ws2811strip.c b/src/main/drivers/light_ws2811strip.c index 718c96ca8a..cb56a7fe79 100644 --- a/src/main/drivers/light_ws2811strip.c +++ b/src/main/drivers/light_ws2811strip.c @@ -43,15 +43,26 @@ #include "drivers/timer.h" #include "drivers/light_ws2811strip.h" +#include "config/parameter_group_ids.h" +#include "fc/settings.h" +#include "fc/runtime_config.h" + #define WS2811_PERIOD (WS2811_TIMER_HZ / WS2811_CARRIER_HZ) #define WS2811_BIT_COMPARE_1 ((WS2811_PERIOD * 2) / 3) #define WS2811_BIT_COMPARE_0 (WS2811_PERIOD / 3) +PG_REGISTER_WITH_RESET_TEMPLATE(ledPinConfig_t, ledPinConfig, PG_LEDPIN_CONFIG, 0); + +PG_RESET_TEMPLATE(ledPinConfig_t, ledPinConfig, + .led_pin_pwm_mode = SETTING_LED_PIN_PWM_MODE_DEFAULT +); + static DMA_RAM timerDMASafeType_t ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE]; static IO_t ws2811IO = IO_NONE; static TCH_t * ws2811TCH = NULL; static bool ws2811Initialised = false; +static bool pwmMode = false; static hsvColor_t ledColorBuffer[WS2811_LED_STRIP_LENGTH]; @@ -91,6 +102,24 @@ void setStripColors(const hsvColor_t *colors) } } +bool ledConfigureDMA(void) { + /* Compute the prescaler value */ + uint8_t period = WS2811_TIMER_HZ / WS2811_CARRIER_HZ; + + timerConfigBase(ws2811TCH, period, WS2811_TIMER_HZ); + timerPWMConfigChannel(ws2811TCH, 0); + + return timerPWMConfigChannelDMA(ws2811TCH, ledStripDMABuffer, sizeof(ledStripDMABuffer[0]), WS2811_DMA_BUFFER_SIZE); +} + +void ledConfigurePWM(void) { + timerConfigBase(ws2811TCH, 100, WS2811_TIMER_HZ ); + timerPWMConfigChannel(ws2811TCH, 0); + timerPWMStart(ws2811TCH); + timerEnable(ws2811TCH); + pwmMode = true; +} + void ws2811LedStripInit(void) { const timerHardware_t * timHw = timerGetByTag(IO_TAG(WS2811_PIN), TIM_USE_ANY); @@ -104,27 +133,32 @@ void ws2811LedStripInit(void) return; } - /* Compute the prescaler value */ - uint8_t period = WS2811_TIMER_HZ / WS2811_CARRIER_HZ; - ws2811IO = IOGetByTag(IO_TAG(WS2811_PIN)); IOInit(ws2811IO, OWNER_LED_STRIP, RESOURCE_OUTPUT, 0); IOConfigGPIOAF(ws2811IO, IOCFG_AF_PP_FAST, timHw->alternateFunction); - timerConfigBase(ws2811TCH, period, WS2811_TIMER_HZ); - timerPWMConfigChannel(ws2811TCH, 0); + if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW ) { + ledConfigurePWM(); + *timerCCR(ws2811TCH) = 0; + } else if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH ) { + ledConfigurePWM(); + *timerCCR(ws2811TCH) = 100; + } else { + if (!ledConfigureDMA()) { + // If DMA failed - abort + ws2811Initialised = false; + return; + } - // If DMA failed - abort - if (!timerPWMConfigChannelDMA(ws2811TCH, ledStripDMABuffer, sizeof(ledStripDMABuffer[0]), WS2811_DMA_BUFFER_SIZE)) { - ws2811Initialised = false; - return; + // Zero out DMA buffer + memset(&ledStripDMABuffer, 0, sizeof(ledStripDMABuffer)); + if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_SHARED_HIGH ) { + ledStripDMABuffer[WS2811_DMA_BUFFER_SIZE-1] = 255; + } + ws2811Initialised = true; + + ws2811UpdateStrip(); } - - // Zero out DMA buffer - memset(&ledStripDMABuffer, 0, sizeof(ledStripDMABuffer)); - ws2811Initialised = true; - - ws2811UpdateStrip(); } bool isWS2811LedStripReady(void) @@ -140,7 +174,7 @@ STATIC_UNIT_TESTED void fastUpdateLEDDMABuffer(rgbColor24bpp_t *color) uint32_t grb = (color->rgb.g << 16) | (color->rgb.r << 8) | (color->rgb.b); for (int8_t index = 23; index >= 0; index--) { - ledStripDMABuffer[dmaBufferOffset++] = (grb & (1 << index)) ? WS2811_BIT_COMPARE_1 : WS2811_BIT_COMPARE_0; + ledStripDMABuffer[WS2811_DELAY_BUFFER_LENGTH + dmaBufferOffset++] = (grb & (1 << index)) ? WS2811_BIT_COMPARE_1 : WS2811_BIT_COMPARE_0; } } @@ -153,7 +187,7 @@ void ws2811UpdateStrip(void) static rgbColor24bpp_t *rgb24; // don't wait - risk of infinite block, just get an update next time round - if (timerPWMDMAInProgress(ws2811TCH)) { + if (pwmMode || timerPWMDMAInProgress(ws2811TCH)) { return; } @@ -178,4 +212,40 @@ void ws2811UpdateStrip(void) timerPWMStartDMA(ws2811TCH); } +//value +void ledPinStartPWM(uint16_t value) { + if (ws2811TCH == NULL) { + return; + } + + if ( !pwmMode ) { + timerPWMStopDMA(ws2811TCH); + //FIXME: implement method to release DMA + ws2811TCH->dma->owner = OWNER_FREE; + + ledConfigurePWM(); + } + *timerCCR(ws2811TCH) = value; +} + +void ledPinStopPWM(void) { + if (ws2811TCH == NULL || !pwmMode ) { + return; + } + + if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_HIGH ) { + *timerCCR(ws2811TCH) = 100; + return; + } else if ( ledPinConfig()->led_pin_pwm_mode == LED_PIN_PWM_MODE_LOW ) { + *timerCCR(ws2811TCH) = 0; + return; + } + pwmMode = false; + + if (!ledConfigureDMA()) { + ws2811Initialised = false; + } +} + + #endif diff --git a/src/main/drivers/light_ws2811strip.h b/src/main/drivers/light_ws2811strip.h index d70a255d88..829ba93509 100644 --- a/src/main/drivers/light_ws2811strip.h +++ b/src/main/drivers/light_ws2811strip.h @@ -17,23 +17,41 @@ #pragma once #include "common/color.h" +#include "config/parameter_group.h" #define WS2811_LED_STRIP_LENGTH 32 #define WS2811_BITS_PER_LED 24 -#define WS2811_DELAY_BUFFER_LENGTH 42 // for 50us delay +#define WS2811_DELAY_BUFFER_LENGTH 42 // for 50us delay #define WS2811_DATA_BUFFER_SIZE (WS2811_BITS_PER_LED * WS2811_LED_STRIP_LENGTH) -#define WS2811_DMA_BUFFER_SIZE (WS2811_DATA_BUFFER_SIZE + WS2811_DELAY_BUFFER_LENGTH) // number of bytes needed is #LEDs * 24 bytes + 42 trailing bytes) +#define WS2811_DMA_BUFFER_SIZE (WS2811_DELAY_BUFFER_LENGTH + WS2811_DATA_BUFFER_SIZE + 1) // leading bytes (reset low 302us) + data bytes LEDS*3 + 1 byte(keep line high optionally) #define WS2811_TIMER_HZ 2400000 #define WS2811_CARRIER_HZ 800000 +typedef enum { + LED_PIN_PWM_MODE_SHARED_LOW = 0, + LED_PIN_PWM_MODE_SHARED_HIGH = 1, + LED_PIN_PWM_MODE_LOW = 2, + LED_PIN_PWM_MODE_HIGH = 3 +} led_pin_pwm_mode_e; + +typedef struct ledPinConfig_s { + uint8_t led_pin_pwm_mode; //led_pin_pwm_mode_e +} ledPinConfig_t; + +PG_DECLARE(ledPinConfig_t, ledPinConfig); + void ws2811LedStripInit(void); void ws2811LedStripHardwareInit(void); void ws2811LedStripDMAEnable(void); bool ws2811LedStripDMAInProgress(void); +//value 0...100 +void ledPinStartPWM(uint16_t value); +void ledPinStopPWM(void); + void ws2811UpdateStrip(void); void setLedHsv(uint16_t index, const hsvColor_t *color); diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 147725ab46..d645184e5b 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -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,9 +226,10 @@ #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 #define SYM_AH_CH_TYPE3 0x190 // 400 to 402, crosshair 3 #define SYM_AH_CH_TYPE4 0x193 // 403 to 405, crosshair 4 @@ -261,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 diff --git a/src/main/drivers/timer_impl_hal.c b/src/main/drivers/timer_impl_hal.c index e800a94235..8df0f7024d 100644 --- a/src/main/drivers/timer_impl_hal.c +++ b/src/main/drivers/timer_impl_hal.c @@ -81,13 +81,16 @@ void impl_timerConfigBase(TCH_t * tch, uint16_t period, uint32_t hz) TIM_HandleTypeDef * timHandle = tch->timCtx->timHandle; TIM_TypeDef * timer = tch->timCtx->timDef->tim; - if (timHandle->Instance == timer) { + uint16_t period1 = (period - 1) & 0xffff; + uint16_t prescaler1 = lrintf((float)timerGetBaseClock(tch) / hz + 0.01f) - 1; + + if (timHandle->Instance == timer && timHandle->Init.Prescaler == prescaler1 && timHandle->Init.Period == period1) { return; } timHandle->Instance = timer; - timHandle->Init.Prescaler = lrintf((float)timerGetBaseClock(tch) / hz + 0.01f) - 1; - timHandle->Init.Period = (period - 1) & 0xffff; // AKA TIMx_ARR + timHandle->Init.Prescaler = prescaler1; + timHandle->Init.Period = period1; // AKA TIMx_ARR timHandle->Init.RepetitionCounter = 0; timHandle->Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; timHandle->Init.CounterMode = TIM_COUNTERMODE_UP; @@ -565,6 +568,15 @@ void impl_timerPWMStartDMA(TCH_t * tch) void impl_timerPWMStopDMA(TCH_t * tch) { - (void)tch; - // FIXME + const uint32_t streamLL = lookupDMALLStreamTable[DMATAG_GET_STREAM(tch->timHw->dmaTag)]; + DMA_TypeDef *dmaBase = tch->dma->dma; + + ATOMIC_BLOCK(NVIC_PRIO_MAX) { + LL_TIM_DisableDMAReq_CCx(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex]); + LL_DMA_DisableStream(dmaBase, streamLL); + DMA_CLEAR_FLAG(tch->dma, DMA_IT_TCIF); + } + tch->dmaState = TCH_DMA_IDLE; + + HAL_TIM_Base_Start(tch->timCtx->timHandle); } diff --git a/src/main/drivers/timer_impl_stdperiph.c b/src/main/drivers/timer_impl_stdperiph.c index 5cb3457c77..d2bd35dd52 100644 --- a/src/main/drivers/timer_impl_stdperiph.c +++ b/src/main/drivers/timer_impl_stdperiph.c @@ -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) { @@ -506,5 +516,6 @@ void impl_timerPWMStopDMA(TCH_t * tch) { DMA_Cmd(tch->dma->ref, DISABLE); TIM_DMACmd(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], DISABLE); + tch->dmaState = TCH_DMA_IDLE; TIM_Cmd(tch->timHw->tim, ENABLE); } diff --git a/src/main/drivers/timer_impl_stdperiph_at32.c b/src/main/drivers/timer_impl_stdperiph_at32.c index 4579db2bb7..0cc194897d 100644 --- a/src/main/drivers/timer_impl_stdperiph_at32.c +++ b/src/main/drivers/timer_impl_stdperiph_at32.c @@ -404,6 +404,6 @@ void impl_timerPWMStopDMA(TCH_t * tch) { dma_channel_enable(tch->dma->ref,FALSE); tmr_dma_request_enable(tch->timHw->tim, lookupDMASourceTable[tch->timHw->channelIndex], FALSE); + tch->dmaState = TCH_DMA_IDLE; tmr_counter_enable(tch->timHw->tim, TRUE); - } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index f2ccec9ec0..6e904b593f 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -68,6 +68,7 @@ bool cliMode = false; #include "drivers/time.h" #include "drivers/usb_msc.h" #include "drivers/vtx_common.h" +#include "drivers/light_ws2811strip.h" #include "fc/fc_core.h" #include "fc/cli.h" @@ -1688,6 +1689,20 @@ static void cliModeColor(char *cmdline) cliPrintLinef("mode_color %u %u %u", modeIdx, funIdx, color); } } + +static void cliLedPinPWM(char *cmdline) +{ + int i; + + if (isEmpty(cmdline)) { + ledPinStopPWM(); + cliPrintLine("PWM stopped"); + } else { + i = fastA2I(cmdline); + ledPinStartPWM(i); + cliPrintLinef("PWM started: %d%%",i); + } +} #endif static void cliDelay(char* cmdLine) { @@ -4035,6 +4050,7 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("help", NULL, NULL, cliHelp), #ifdef USE_LED_STRIP CLI_COMMAND_DEF("led", "configure leds", NULL, cliLed), + CLI_COMMAND_DEF("ledpinpwm", "start/stop PWM on LED pin, 0..100 duty ratio", "[]\r\n", cliLedPinPWM), #endif CLI_COMMAND_DEF("map", "configure rc channel order", "[]", cliMap), CLI_COMMAND_DEF("memory", "view memory usage", NULL, cliMemory), diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3950d76376..52cb06f81a 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -64,6 +64,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/failsafe.h" +#include "flight/ez_tune.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -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; } diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 18f5bd343b..c8b144e136 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -44,6 +44,7 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) .rcMid8 = SETTING_THR_MID_DEFAULT, .rcExpo8 = SETTING_THR_EXPO_DEFAULT, .dynPID = SETTING_TPA_RATE_DEFAULT, + .dynPID_on_YAW = SETTING_TPA_ON_YAW_DEFAULT, .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT, .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT }, diff --git a/src/main/fc/controlrate_profile_config_struct.h b/src/main/fc/controlrate_profile_config_struct.h index e07c328560..e403853760 100644 --- a/src/main/fc/controlrate_profile_config_struct.h +++ b/src/main/fc/controlrate_profile_config_struct.h @@ -29,6 +29,7 @@ typedef struct controlRateConfig_s { uint8_t rcMid8; uint8_t rcExpo8; uint8_t dynPID; + bool dynPID_on_YAW; uint16_t pa_breakpoint; // Breakpoint where TPA is activated uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter } throttle; diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 87d9fbe746..7fa07a2a83 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -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; @@ -727,6 +760,8 @@ void processRx(timeUs_t currentTimeUs) } else { DISABLE_FLIGHT_MODE(MANUAL_MODE); } + }else{ + DISABLE_FLIGHT_MODE(MANUAL_MODE); } /* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered. @@ -814,7 +849,6 @@ void processRx(timeUs_t currentTimeUs) } } #endif - } // Function for loop trigger @@ -871,7 +905,14 @@ void taskMainPidLoop(timeUs_t currentTimeUs) if (!ARMING_FLAG(ARMED)) { armTime = 0; - processDelayedSave(); + // Delay saving for 0.5s to allow other functions to process save actions on disarm + if (currentTimeUs - lastDisarmTimeUs > USECS_PER_SEC / 2) { + 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) @@ -923,15 +964,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(); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 211c6f0704..849cdc2b7c 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -96,6 +96,7 @@ #include "flight/power_limits.h" #include "flight/rpm_filter.h" #include "flight/servos.h" +#include "flight/ez_tune.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -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); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0f3d80829a..fddcde00eb 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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" @@ -463,6 +464,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, (getConfigBatteryProfile() << 4) | getConfigProfile()); sbufWriteU32(dst, armingFlags); sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags)); + sbufWriteU8(dst, getConfigMixerProfile()); } break; @@ -522,6 +524,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, -1); #endif } + if(MAX_MIXER_PROFILE_COUNT==1) break; + for (int i = 0; i < MAX_SERVO_RULES; i++) { + sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].targetChannel); + sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].inputSource); + sbufWriteU16(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].rate); + sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].speed); + #ifdef USE_PROGRAMMING_FRAMEWORK + sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].conditionId); + #else + sbufWriteU8(dst, -1); + #endif + } break; #ifdef USE_PROGRAMMING_FRAMEWORK case MSP2_INAV_LOGIC_CONDITIONS: @@ -567,11 +581,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif case MSP2_COMMON_MOTOR_MIXER: for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - sbufWriteU16(dst, primaryMotorMixer(i)->throttle * 1000); + sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->throttle + 2.0f, 0.0f, 4.0f) * 1000); sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->roll + 2.0f, 0.0f, 4.0f) * 1000); sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->pitch + 2.0f, 0.0f, 4.0f) * 1000); sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->yaw + 2.0f, 0.0f, 4.0f) * 1000); } + if (MAX_MIXER_PROFILE_COUNT==1) break; + for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].throttle + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].roll + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].pitch + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].yaw + 2.0f, 0.0f, 4.0f) * 1000); + } break; case MSP_MOTOR: @@ -697,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: @@ -1582,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: @@ -2100,7 +2141,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_COMMON_SET_MOTOR_MIXER: sbufReadU8Safe(&tmp_u8, src); if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) { - primaryMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 1.0f); + primaryMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f; primaryMotorMixerMutable(tmp_u8)->roll = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f; primaryMotorMixerMutable(tmp_u8)->pitch = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f; primaryMotorMixerMutable(tmp_u8)->yaw = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f; @@ -2994,6 +3035,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; + case MSP2_INAV_SELECT_MIXER_PROFILE: + if (!ARMING_FLAG(ARMED) && sbufReadU8Safe(&tmp_u8, src)) { + setConfigMixerProfileAndWriteEEPROM(tmp_u8); + } else { + return MSP_RESULT_ERROR; + } + break; + #ifdef USE_TEMPERATURE_SENSOR case MSP2_INAV_SET_TEMP_SENSOR_CONFIG: if (dataSize == sizeof(tempSensorConfig_t) * MAX_TEMP_SENSORS) { @@ -3055,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: @@ -3259,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: @@ -3322,7 +3397,7 @@ bool isOSDTypeSupportedBySimulator(void) { #ifdef USE_OSD displayPort_t *osdDisplayPort = osdGetDisplayPort(); - return (osdDisplayPort && osdDisplayPort->cols == 30 && (osdDisplayPort->rows == 13 || osdDisplayPort->rows == 16)); + return (!!osdDisplayPort && !!osdDisplayPort->vTable->readChar); #else return false; #endif @@ -3334,18 +3409,25 @@ void mspWriteSimulatorOSD(sbuf_t *dst) //scan displayBuffer iteratively //no more than 80+3+2 bytes output in single run //0 and 255 are special symbols - //255 - font bank switch - //0 - font bank switch, blink switch and character repeat + //255 [char] - font bank switch + //0 [flags,count] [char] - font bank switch, blink switch and character repeat + //original 0 is sent as 32 + //original 0xff, 0x100 and 0x1ff are forcibly sent inside command 0 static uint8_t osdPos_y = 0; static uint8_t osdPos_x = 0; + //indicate new format hitl 1.4.0 + sbufWriteU8(dst, 255); if (isOSDTypeSupportedBySimulator()) { displayPort_t *osdDisplayPort = osdGetDisplayPort(); - sbufWriteU8(dst, osdPos_y | (osdDisplayPort->rows == 16 ? 128: 0)); + sbufWriteU8(dst, osdDisplayPort->rows); + sbufWriteU8(dst, osdDisplayPort->cols); + + sbufWriteU8(dst, osdPos_y); sbufWriteU8(dst, osdPos_x); int bytesCount = 0; @@ -3356,7 +3438,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst) bool blink = false; int count = 0; - int processedRows = 16; + int processedRows = osdDisplayPort->rows; while (bytesCount < 80) //whole response should be less 155 bytes at worst. { @@ -3367,7 +3449,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst) while ( true ) { displayReadCharWithAttr(osdDisplayPort, osdPos_x, osdPos_y, &c, &attr); - if (c == 0 || c == 255) c = 32; + if (c == 0) c = 32; //REVIEW: displayReadCharWithAttr() should return mode with _TEXT_ATTRIBUTES_BLINK_BIT ! //for max7456 it returns mode with MAX7456_MODE_BLINK instead (wrong) @@ -3382,7 +3464,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst) lastChar = c; blink1 = blink2; } - else if (lastChar != c || blink2 != blink1 || count == 63) + else if ((lastChar != c) || (blink2 != blink1) || (count == 63)) { break; } @@ -3390,12 +3472,12 @@ void mspWriteSimulatorOSD(sbuf_t *dst) count++; osdPos_x++; - if (osdPos_x == 30) + if (osdPos_x == osdDisplayPort->cols) { osdPos_x = 0; osdPos_y++; processedRows--; - if (osdPos_y == 16) + if (osdPos_y == osdDisplayPort->rows) { osdPos_y = 0; } @@ -3403,6 +3485,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst) } uint8_t cmd = 0; + uint8_t lastCharLow = (uint8_t)(lastChar & 0xff); if (blink1 != blink) { cmd |= 128;//switch blink attr @@ -3418,27 +3501,27 @@ void mspWriteSimulatorOSD(sbuf_t *dst) if (count == 1 && cmd == 64) { - sbufWriteU8(dst, 255); //short command for bank switch + sbufWriteU8(dst, 255); //short command for bank switch with char following sbufWriteU8(dst, lastChar & 0xff); bytesCount += 2; } - else if (count > 2 || cmd !=0 ) + else if ((count > 2) || (cmd !=0) || (lastChar == 255) || (lastChar == 0x100) || (lastChar == 0x1ff)) { cmd |= count; //long command for blink/bank switch and symbol repeat sbufWriteU8(dst, 0); sbufWriteU8(dst, cmd); - sbufWriteU8(dst, lastChar & 0xff); + sbufWriteU8(dst, lastCharLow); bytesCount += 3; } else if (count == 2) //cmd == 0 here { - sbufWriteU8(dst, lastChar & 0xff); - sbufWriteU8(dst, lastChar & 0xff); + sbufWriteU8(dst, lastCharLow); + sbufWriteU8(dst, lastCharLow); bytesCount+=2; } else { - sbufWriteU8(dst, lastChar & 0xff); + sbufWriteU8(dst, lastCharLow); bytesCount++; } @@ -3452,7 +3535,7 @@ void mspWriteSimulatorOSD(sbuf_t *dst) } else { - sbufWriteU8(dst, 255); + sbufWriteU8(dst, 0); } } #endif @@ -3586,6 +3669,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu } #endif ENABLE_ARMING_FLAG(SIMULATOR_MODE_HITL); + ENABLE_STATE(ACCELEROMETER_CALIBRATED); LOG_DEBUG(SYSTEM, "Simulator enabled"); } @@ -3662,15 +3746,11 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu sbufAdvance(src, sizeof(uint16_t) * XYZ_AXIS_COUNT); } -#if defined(USE_FAKE_BATT_SENSOR) if (SIMULATOR_HAS_OPTION(HITL_EXT_BATTERY_VOLTAGE)) { - fakeBattSensorSetVbat(sbufReadU8(src) * 10); + simulatorData.vbat = sbufReadU8(src); } else { -#endif - fakeBattSensorSetVbat((uint16_t)(SIMULATOR_FULL_BATTERY * 10.0f)); -#if defined(USE_FAKE_BATT_SENSOR) + simulatorData.vbat = SIMULATOR_FULL_BATTERY; } -#endif if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) { simulatorData.airSpeed = sbufReadU16(src); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index f7c88d6e5b..5b6b8656d8 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -202,7 +202,7 @@ void initActiveBoxIds(void) //Camstab mode is enabled always ADD_ACTIVE_BOX(BOXCAMSTAB); - if (STATE(MULTIROTOR)) { + if (STATE(MULTIROTOR) || platformTypeConfigured(PLATFORM_MULTIROTOR) || platformTypeConfigured(PLATFORM_TRICOPTER)) { if ((sensors(SENSOR_ACC) || sensors(SENSOR_MAG))) { ADD_ACTIVE_BOX(BOXHEADFREE); ADD_ACTIVE_BOX(BOXHEADADJ); @@ -244,13 +244,13 @@ void initActiveBoxIds(void) #endif } - if (STATE(AIRPLANE)) { + if (STATE(AIRPLANE) || platformTypeConfigured(PLATFORM_AIRPLANE)) { ADD_ACTIVE_BOX(BOXSOARING); } } #ifdef USE_MR_BRAKING_MODE - if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) { + if (mixerConfig()->platformType == PLATFORM_MULTIROTOR || platformTypeConfigured(PLATFORM_MULTIROTOR)) { ADD_ACTIVE_BOX(BOXBRAKING); } #endif @@ -259,11 +259,12 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXNAVALTHOLD); } - if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) { + if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT) || + platformTypeConfigured(PLATFORM_AIRPLANE) || platformTypeConfigured(PLATFORM_ROVER) || platformTypeConfigured(PLATFORM_BOAT)) { ADD_ACTIVE_BOX(BOXMANUAL); } - if (STATE(AIRPLANE)) { + if (STATE(AIRPLANE) || platformTypeConfigured(PLATFORM_AIRPLANE)) { if (!feature(FEATURE_FW_LAUNCH)) { ADD_ACTIVE_BOX(BOXNAVLAUNCH); } diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index af5f27b527..607a814bed 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -62,6 +62,7 @@ #include "io/osd.h" #include "io/serial.h" #include "io/rcdevice_cam.h" +#include "io/osd_joystick.h" #include "io/smartport_master.h" #include "io/vtx.h" #include "io/vtx_msp.h" @@ -393,8 +394,12 @@ void fcTasksInit(void) #endif #endif #ifdef USE_RCDEVICE +#ifdef USE_LED_STRIP + setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled() || osdJoystickEnabled()); +#else setTaskEnabled(TASK_RCDEVICE, rcdeviceIsEnabled()); #endif +#endif #ifdef USE_PROGRAMMING_FRAMEWORK setTaskEnabled(TASK_PROGRAMMING_FRAMEWORK, true); #endif diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 15dadd1c59..175de20c48 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -179,7 +179,8 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) #ifdef USE_SIMULATOR simulatorData_t simulatorData = { - .flags = 0, - .debugIndex = 0 + .flags = 0, + .debugIndex = 0, + .vbat = 0 }; #endif diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 94eb23bd6a..6430e38ec3 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -139,7 +139,8 @@ typedef enum { FW_HEADING_USE_YAW = (1 << 24), ANTI_WINDUP_DEACTIVATED = (1 << 25), LANDING_DETECTED = (1 << 26), - TAILSITTER = (1 << 27), //offset the pitch angle by 90 degrees + IN_FLIGHT_EMERG_REARM = (1 << 27), + TAILSITTER = (1 << 28), //offset the pitch angle by 90 degrees } stateFlags_t; #define DISABLE_STATE(mask) (stateFlags &= ~(mask)) @@ -171,7 +172,7 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void); #define SIMULATOR_MSP_VERSION 2 // Simulator MSP version #define SIMULATOR_BARO_TEMP 25 // °C -#define SIMULATOR_FULL_BATTERY 12.6f // Volts +#define SIMULATOR_FULL_BATTERY 126 // Volts*10 #define SIMULATOR_HAS_OPTION(flag) ((simulatorData.flags & flag) != 0) typedef enum { diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 8ea69b82f0..109f430f84 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -237,6 +237,8 @@ static uint16_t getValueOffset(const setting_t *value) return value->offset + sizeof(pidProfile_t) * getConfigProfile(); case CONTROL_RATE_VALUE: return value->offset + sizeof(controlRateConfig_t) * getConfigProfile(); + case EZ_TUNE_VALUE: + return value->offset + sizeof(ezTuneSettings_t) * getConfigProfile(); case BATTERY_CONFIG_VALUE: return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile(); case MIXER_CONFIG_VALUE: diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 7fdfd66bf2..75e8bdbef3 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -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 { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml old mode 100755 new mode 100644 index bc5429b338..a8d73210e3 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -193,6 +193,9 @@ tables: - name: nav_mc_althold_throttle values: ["STICK", "MID_STICK", "HOVER"] enum: navMcAltHoldThrottle_e + - name: led_pin_pwm_mode + values: ["SHARED_LOW", "SHARED_HIGH", "LOW", "HIGH"] + enum: led_pin_pwm_mode_e constants: RPYL_PID_MIN: 0 @@ -1045,7 +1048,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 @@ -1272,7 +1275,7 @@ groups: min: 0 max: 100 - name: tpa_rate - description: "Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate." + description: "Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate." default_value: 0 field: throttle.dynPID min: 0 @@ -1283,6 +1286,11 @@ groups: field: throttle.pa_breakpoint min: PWM_RANGE_MIN max: PWM_RANGE_MAX + - name: tpa_on_yaw + description: "Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors." + type: bool + field: throttle.dynPID_on_YAW + default_value: OFF - name: fw_tpa_time_constant description: "TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. See **PID Attenuation and scaling** Wiki for full details." default_value: 1500 @@ -1457,7 +1465,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 @@ -1497,6 +1505,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 @@ -1856,12 +1923,6 @@ groups: default_value: 0 min: 0 max: 200 - - name: fw_iterm_throw_limit - description: "Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely." - default_value: 165 - field: fixedWingItermThrowLimit - min: FW_ITERM_THROW_LIMIT_MIN - max: FW_ITERM_THROW_LIMIT_MAX - name: fw_reference_airspeed description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present." default_value: 1500 @@ -1910,6 +1971,12 @@ groups: field: itermWindupPointPercent min: 0 max: 90 + - name: pid_iterm_limit_percent + description: "Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely." + default_value: 33 + field: pidItermLimitPercent + min: 0 + max: 200 - name: rate_accel_limit_roll_pitch description: "Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting." default_value: 0 @@ -3498,66 +3565,95 @@ groups: max: 6 default_value: 3 - - name: osd_mah_used_precision - description: Number of digits used to display mAh used. - field: mAh_used_precision + - name: osd_mah_precision + description: Number of digits used for mAh precision. Currently used by mAh Used and Battery Remaining Capacity + field: mAh_precision min: 4 max: 6 default_value: 4 + - name: osd_use_pilot_logo + description: Use custom pilot logo with/instead of the INAV logo. The pilot logo must be characters 473 to 511 + field: use_pilot_logo + type: bool + default_value: OFF + + - name: osd_inav_to_pilot_logo_spacing + description: The space between the INAV and pilot logos, if `osd_use_pilot_logo` is `ON`. This number may be adjusted so that it fits the odd/even col width displays. For example, if using an odd column width display, such as Walksnail, and this is set to 4. 1 will be added so that the logos are equally spaced from the centre of the screen. + field: inav_to_pilot_logo_spacing + min: 0 + max: 20 + default_value: 8 + + - name: osd_arm_screen_display_time + description: Amount of time to display the arm screen [ms] + field: arm_screen_display_time + min: 1000 + max: 5000 + default_value: 1500 + - name: osd_switch_indicator_zero_name description: "Character to use for OSD switch incicator 0." field: osd_switch_indicator0_name type: string max: 5 default_value: "FLAP" + - name: osd_switch_indicator_one_name description: "Character to use for OSD switch incicator 1." field: osd_switch_indicator1_name type: string max: 5 default_value: "GEAR" + - name: osd_switch_indicator_two_name description: "Character to use for OSD switch incicator 2." field: osd_switch_indicator2_name type: string max: 5 default_value: "CAM" + - name: osd_switch_indicator_three_name description: "Character to use for OSD switch incicator 3." field: osd_switch_indicator3_name type: string max: 5 default_value: "LIGT" + - name: osd_switch_indicator_zero_channel description: "RC Channel to use for OSD switch indicator 0." field: osd_switch_indicator0_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicator_one_channel description: "RC Channel to use for OSD switch indicator 1." field: osd_switch_indicator1_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicator_two_channel description: "RC Channel to use for OSD switch indicator 2." field: osd_switch_indicator2_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicator_three_channel description: "RC Channel to use for OSD switch indicator 3." field: osd_switch_indicator3_channel min: 5 max: MAX_SUPPORTED_RC_CHANNEL_COUNT default_value: 5 + - name: osd_switch_indicators_align_left description: "Align text to left of switch indicators" field: osd_switch_indicators_align_left type: bool default_value: ON + - name: osd_system_msg_display_time description: System message display cycle time for multiple messages (milliseconds). field: system_msg_display_time @@ -3931,3 +4027,55 @@ groups: default_value: 1.2 field: attnFilterCutoff max: 100 + + - name: PG_LEDPIN_CONFIG + type: ledPinConfig_t + headers: ["drivers/light_ws2811strip.h"] + members: + - name: led_pin_pwm_mode + condition: USE_LED_STRIP + description: "PWM mode of LED pin." + default_value: "SHARED_LOW" + field: led_pin_pwm_mode + table: led_pin_pwm_mode + + - name: PG_OSD_JOYSTICK_CONFIG + type: osdJoystickConfig_t + headers: ["io/osd_joystick.h"] + condition: USE_RCDEVICE && USE_LED_STRIP + members: + - name: osd_joystick_enabled + description: "Enable OSD Joystick emulation" + default_value: OFF + field: osd_joystick_enabled + type: bool + - name: osd_joystick_down + description: "PWM value for DOWN key" + default_value: 0 + field: osd_joystick_down + min: 0 + max: 100 + - name: osd_joystick_up + description: "PWM value for UP key" + default_value: 48 + field: osd_joystick_up + min: 0 + max: 100 + - name: osd_joystick_left + description: "PWM value for LEFT key" + default_value: 63 + field: osd_joystick_left + min: 0 + max: 100 + - name: osd_joystick_right + description: "PWM value for RIGHT key" + default_value: 28 + field: osd_joystick_right + min: 0 + max: 100 + - name: osd_joystick_enter + description: "PWM value for ENTER key" + default_value: 75 + field: osd_joystick_enter + min: 0 + max: 100 diff --git a/src/main/fc/stats.h b/src/main/fc/stats.h index 275460643c..54b676f657 100644 --- a/src/main/fc/stats.h +++ b/src/main/fc/stats.h @@ -3,8 +3,8 @@ #ifdef USE_STATS typedef struct statsConfig_s { - uint32_t stats_total_time; // [s] - uint32_t stats_total_dist; // [m] + uint32_t stats_total_time; // [Seconds] + uint32_t stats_total_dist; // [Metres] #ifdef USE_ADC uint32_t stats_total_energy; // deciWatt hour (x0.1Wh) #endif diff --git a/src/main/flight/ez_tune.c b/src/main/flight/ez_tune.c new file mode 100644 index 0000000000..c6c57afbfe --- /dev/null +++ b/src/main/flight/ez_tune.c @@ -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); + + } +} \ No newline at end of file diff --git a/src/main/flight/ez_tune.h b/src/main/flight/ez_tune.h new file mode 100644 index 0000000000..5fcc1ef6f7 --- /dev/null +++ b/src/main/flight/ez_tune.h @@ -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); \ No newline at end of file diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 939f9c4ee7..585097b88a 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -746,28 +746,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 diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 421dff1763..7b2590ff70 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -23,6 +23,7 @@ #include "fc/runtime_config.h" #include "fc/settings.h" #include "fc/rc_modes.h" +#include "fc/cli.h" #include "programming/logic_condition.h" #include "navigation/navigation.h" @@ -34,7 +35,7 @@ int currentMixerProfileIndex; bool isMixerTransitionMixing; bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; -int nextProfileIndex; +int nextMixerProfileIndex; PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); @@ -81,7 +82,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) void activateMixerConfig(){ currentMixerProfileIndex = getConfigMixerProfile(); currentMixerConfig = *mixerConfig(); - nextProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; + nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; } void mixerConfigInit(void) @@ -108,6 +109,14 @@ void setMixerProfileAT(void) mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; } +bool platformTypeConfigured(flyingPlatformType_e platformType) +{ + if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){ + return false; + } + return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType; +} + bool checkMixerATRequired(mixerProfileATRequest_e required_action) { //return false if mixerAT condition is not required or setting is not valid @@ -163,7 +172,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) isMixerTransitionMixing_requested = true; if (millis() > mixerProfileAT.transitionTransEndTime){ isMixerTransitionMixing_requested = false; - outputProfileHotSwitch(nextProfileIndex); + outputProfileHotSwitch(nextMixerProfileIndex); mixerProfileAT.phase = MIXERAT_PHASE_IDLE; reprocessState = true; //transition is done @@ -188,8 +197,9 @@ bool checkMixerProfileHotSwitchAvalibility(void) } void outputProfileUpdateTask(timeUs_t currentTimeUs) -{ +{ UNUSED(currentTimeUs); + if(cliMode) return; bool mixerAT_inuse = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; // transition mode input for servo mix and motor mix if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index a709309685..947ca70155 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -55,6 +55,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action); extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; +extern int nextMixerProfileIndex; extern bool isMixerTransitionMixing; #define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config)) #define mixerConfigMutable() ((mixerConfig_t *) mixerConfig()) @@ -72,6 +73,7 @@ static inline const mixerProfile_t* mixerProfiles_CopyArray_by_index(int _index) #define mixerMotorMixersByIndex(index) (mixerProfiles(index)->MotorMixers) #define mixerServoMixersByIndex(index) (mixerProfiles(index)->ServoMixers) +bool platformTypeConfigured(flyingPlatformType_e platformType); bool outputProfileHotSwitch(int profile_index); bool checkMixerProfileHotSwitchAvalibility(void); void activateMixerConfig(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fdea10dc43..b55e6c6ad8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -161,7 +161,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; #define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand -#define FIXED_WING_LEVEL_TRIM_DIVIDER 500.0f +#define FIXED_WING_LEVEL_TRIM_DIVIDER 50.0f #define FIXED_WING_LEVEL_TRIM_MULTIPLIER 1.0f / FIXED_WING_LEVEL_TRIM_DIVIDER #define FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT FIXED_WING_LEVEL_TRIM_DIVIDER * FIXED_WING_LEVEL_TRIM_MAX_ANGLE @@ -264,8 +264,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT, .pidSumLimit = SETTING_PIDSUM_LIMIT_DEFAULT, .pidSumLimitYaw = SETTING_PIDSUM_LIMIT_YAW_DEFAULT, + .pidItermLimitPercent = SETTING_PID_ITERM_LIMIT_PERCENT_DEFAULT, - .fixedWingItermThrowLimit = SETTING_FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT, .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT, .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT, @@ -531,7 +531,7 @@ void updatePIDCoefficients(void) pidState[axis].kT = 0.0f; } else { - const float axisTPA = (axis == FD_YAW) ? 1.0f : tpaFactor; + const float axisTPA = (axis == FD_YAW && (!currentControlRateProfile->throttle.dynPID_on_YAW)) ? 1.0f : tpaFactor; pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER; pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA; @@ -766,8 +766,9 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh applyItermLimiting(pidState); - if (pidProfile()->fixedWingItermThrowLimit != 0) { - pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); + if (pidProfile()->pidItermLimitPercent != 0){ + float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f; + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); } axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); @@ -841,6 +842,12 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + + if (pidProfile()->pidItermLimitPercent != 0){ + float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f; + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); + } + // Don't grow I-term if motors are at their limit applyItermLimiting(pidState); @@ -1037,7 +1044,7 @@ void checkItermLimitingActive(pidState_t *pidState) shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else { - shouldActivate = mixerIsOutputSaturated(); + shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent } pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; @@ -1076,7 +1083,7 @@ void FAST_CODE pidController(float dT) // In case Yaw override is active, we engage the Heading Hold state if (isFlightAxisAngleOverrideActive(FD_YAW)) { headingHoldState = HEADING_HOLD_ENABLED; - headingHoldTarget = getFlightAxisAngleOverride(FD_YAW, 0); + headingHoldTarget = DECIDEGREES_TO_DEGREES(getFlightAxisAngleOverride(FD_YAW, 0)); } if (headingHoldState == HEADING_HOLD_UPDATE_HEADING) { @@ -1138,7 +1145,7 @@ void FAST_CODE pidController(float dT) canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } - if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees) { + if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) { pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); } @@ -1251,7 +1258,7 @@ void pidInit(void) navPidInit( &fixedWingLevelTrimController, 0.0f, - (float)pidProfile()->fixedWingLevelTrimGain / 100000.0f, + (float)pidProfile()->fixedWingLevelTrimGain / 100.0f, 0.0f, 0.0f, 2.0f, @@ -1263,47 +1270,52 @@ void pidInit(void) const pidBank_t * pidBank(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; } + pidBank_t * pidBankMutable(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } +bool isFixedWingLevelTrimActive(void) +{ + return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() && + (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && + !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) && + !navigationIsControllingAltitude(); +} + void updateFixedWingLevelTrim(timeUs_t currentTimeUs) { if (!STATE(AIRPLANE)) { return; } - static timeUs_t previousUpdateTimeUs; - static bool previousArmingState; - const float dT = US2S(currentTimeUs - previousUpdateTimeUs); + static bool previousArmingState = false; - /* - * On every ARM reset the controller - */ - if (ARMING_FLAG(ARMED) && !previousArmingState) { - navPidReset(&fixedWingLevelTrimController); - } - - /* - * On disarm update the default value - */ - if (!ARMING_FLAG(ARMED) && previousArmingState) { + if (ARMING_FLAG(ARMED)) { + if (!previousArmingState) { // On every ARM reset the controller + navPidReset(&fixedWingLevelTrimController); + } + } else if (previousArmingState) { // On disarm update the default value pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE); } + previousArmingState = ARMING_FLAG(ARMED); + + // return if not active or disarmed + if (!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) { + return; + } + + static timeUs_t previousUpdateTimeUs; + const float dT = US2S(currentTimeUs - previousUpdateTimeUs); + previousUpdateTimeUs = currentTimeUs; /* * Prepare flags for the PID controller */ pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR; - //Iterm should freeze when sticks are deflected - if ( - !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || - areSticksDeflected() || - (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) || - FLIGHT_MODE(SOARING_MODE) || - navigationIsControllingAltitude() - ) { + // Iterm should freeze when conditions for setting level trim aren't met + if (!isFixedWingLevelTrimActive()) { flags |= PID_FREEZE_INTEGRATOR; } @@ -1321,8 +1333,6 @@ void updateFixedWingLevelTrim(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_AUTOLEVEL, 4, output); fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER); - - previousArmingState = !!ARMING_FLAG(ARMED); } float getFixedWingLevelTrim(void) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index de0e3126b7..48f1352eae 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -31,10 +31,6 @@ #define HEADING_HOLD_RATE_LIMIT_MAX 250 #define HEADING_HOLD_RATE_LIMIT_DEFAULT 90 -#define FW_ITERM_THROW_LIMIT_DEFAULT 165 -#define FW_ITERM_THROW_LIMIT_MIN 0 -#define FW_ITERM_THROW_LIMIT_MAX 500 - #define AXIS_ACCEL_MIN_LIMIT 50 #define HEADING_HOLD_ERROR_LPF_FREQ 2 @@ -106,8 +102,8 @@ typedef struct pidProfile_s { pidBank_t bank_mc; uint8_t dterm_lpf_type; // Dterm LPF type: PT1, BIQUAD - uint16_t dterm_lpf_hz; - + uint16_t dterm_lpf_hz; + uint8_t yaw_lpf_hz; uint8_t heading_hold_rate_limit; // Maximum rotation rate HEADING_HOLD mode can feed to yaw rate PID controller @@ -121,15 +117,15 @@ typedef struct pidProfile_s { uint16_t pidSumLimit; uint16_t pidSumLimitYaw; + uint16_t pidItermLimitPercent; // Airplane-specific parameters - uint16_t fixedWingItermThrowLimit; float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point uint16_t fixedWingYawItermBankFreeze; // Freeze yaw Iterm when bank angle is more than this many degrees - + float navVelXyDTermLpfHz; uint8_t navVelXyDtermAttenuation; // VEL_XY dynamic Dterm scale: Dterm will be attenuatedby this value (in percent) when UAV is traveling with more than navVelXyDtermAttenuationStart percents of max velocity uint8_t navVelXyDtermAttenuationStart; // VEL_XY dynamic Dterm scale: Dterm attenuation will begin at this percent of max velocity @@ -221,5 +217,6 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa pidType_e pidIndexGetType(pidIndex_e pidIndex); +bool isFixedWingLevelTrimActive(void); void updateFixedWingLevelTrim(timeUs_t currentTimeUs); float getFixedWingLevelTrim(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 4d4bb497d1..f38a4ea108 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -103,8 +103,14 @@ void pgResetFn_servoParams(servoParam_t *instance) int16_t servo[MAX_SUPPORTED_SERVOS]; static uint8_t servoRuleCount = 0; +static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; + +/* +//Was used to keep track of servo rules in all mixer_profile, In order to Apply mixer speed limit when rules turn off static servoMixer_t currentServoMixer[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; -static bool currentServoMixerActivative[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT];// if true, the rule is used by current servo mixer +static bool currentServoMixerActivative[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; // if true, the rule is used by current servo mixer +*/ + static bool servoOutputEnabled; static bool mixerUsesServos; @@ -115,7 +121,7 @@ static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS]; static bool servoFilterIsSet; static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS]; -static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; +static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES]; STATIC_FASTRAM pt1Filter_t rotRateFilter; STATIC_FASTRAM pt1Filter_t targetRateFilter; @@ -137,6 +143,33 @@ void servoComputeScalingFactors(uint8_t servoIndex) { servoMetadata[servoIndex].scaleMin = (servoParams(servoIndex)->middle - servoParams(servoIndex)->min) / 500.0f; } +void computeServoCount(void) +{ + static bool firstRun = true; + if (!firstRun) { + return; + } + 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 + if (mixerServoMixersByIndex(j)[i].rate == 0){ + break; + } + if (mixerServoMixersByIndex(j)[i].targetChannel < minServoIndex) { + minServoIndex = mixerServoMixersByIndex(j)[i].targetChannel; + } + + if (mixerServoMixersByIndex(j)[i].targetChannel > maxServoIndex) { + maxServoIndex = mixerServoMixersByIndex(j)[i].targetChannel; + } + mixerUsesServos = true; + } + } + firstRun = false; +} + void servosInit(void) { // give all servos a default command @@ -147,12 +180,12 @@ void servosInit(void) /* * load mixer */ + computeServoCount(); loadCustomServoMixer(); // If there are servo rules after all, update variables - if (servoRuleCount > 0) { + if (mixerUsesServos) { servoOutputEnabled = true; - mixerUsesServos = true; } for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { @@ -161,8 +194,8 @@ void servosInit(void) } int getServoCount(void) -{ - if (servoRuleCount) { +{ + if (mixerUsesServos) { return 1 + maxServoIndex - minServoIndex; } else { @@ -173,30 +206,17 @@ int getServoCount(void) void loadCustomServoMixer(void) { servoRuleCount = 0; - minServoIndex = 255; - maxServoIndex = 0; memset(currentServoMixer, 0, sizeof(currentServoMixer)); - for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) { - const servoMixer_t* tmp_customServoMixers = &mixerServoMixersByIndex(j)[0]; - // load custom mixer into currentServoMixer - for (int i = 0; i < MAX_SERVO_RULES; i++) { - // check if done - if (tmp_customServoMixers[i].rate == 0) - break; - - if (tmp_customServoMixers[i].targetChannel < minServoIndex) { - minServoIndex = tmp_customServoMixers[i].targetChannel; - } - - if (tmp_customServoMixers[i].targetChannel > maxServoIndex) { - maxServoIndex = tmp_customServoMixers[i].targetChannel; - } - - memcpy(¤tServoMixer[servoRuleCount], &tmp_customServoMixers[i], sizeof(servoMixer_t)); - currentServoMixerActivative[servoRuleCount] = j==currentMixerProfileIndex; - servoRuleCount++; + // load custom mixer into currentServoMixer + for (int i = 0; i < MAX_SERVO_RULES; i++) { + // check if done + if (customServoMixers(i)->rate == 0){ + break; } + currentServoMixer[servoRuleCount] = *customServoMixers(i); + servoSpeedLimitFilter[servoRuleCount].state = 0; + servoRuleCount++; } } @@ -353,9 +373,6 @@ void servoMixer(float dT) inputRaw = 0; } #endif - if (!currentServoMixerActivative[i]) { - inputRaw = 0; - } /* * Apply mixer speed limit. 1 [one] speed unit is defined as 10us/s: * 0 = no limiting @@ -368,20 +385,6 @@ void servoMixer(float dT) servo[target] += ((int32_t)inputLimited * currentServoMixer[i].rate) / 100; } - /* - * When not armed, apply servo low position to all outputs that include a throttle or stabilizet throttle in the mix - */ - if (!ARMING_FLAG(ARMED)) { - for (int i = 0; i < servoRuleCount; i++) { - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - - if (from == INPUT_STABILIZED_THROTTLE || from == INPUT_RC_THROTTLE) { - servo[target] = motorConfig()->mincommand; - } - } - } - for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { /* @@ -412,6 +415,20 @@ void servoMixer(float dT) */ servo[i] = constrain(servo[i], servoParams(i)->min, servoParams(i)->max); } + + /* + * When not armed, apply servo low position to all outputs that include a throttle or stabilizet throttle in the mix + */ + if (!ARMING_FLAG(ARMED)) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + + if (from == INPUT_STABILIZED_THROTTLE || from == INPUT_RC_THROTTLE) { + servo[target] = motorConfig()->mincommand; + } + } + } } #define SERVO_AUTOTRIM_TIMER_MS 2000 @@ -438,7 +455,6 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (!currentServoMixerActivative[i]) {continue;} // Reset servo middle accumulator const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; @@ -461,7 +477,6 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -474,7 +489,6 @@ void processServoAutotrimMode(void) if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -508,7 +522,6 @@ void processServoAutotrimMode(void) if (trimState == AUTOTRIM_SAVE_PENDING) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { diff --git a/src/main/io/displayport_msp_bf_compat.c b/src/main/io/displayport_msp_bf_compat.c index 7cfa8f1bf6..4229e035b0 100644 --- a/src/main/io/displayport_msp_bf_compat.c +++ b/src/main/io/displayport_msp_bf_compat.c @@ -96,10 +96,31 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) case SYM_AH_DECORATION_DOWN: return BF_SYM_AH_DECORATION_DOWN; - - case SYM_DIRECTION: - return BF_SYM_DIRECTION; */ + case SYM_DIRECTION: + return BF_SYM_ARROW_NORTH; + + case SYM_DIRECTION + 1: // NE pointing arrow + return BF_SYM_ARROW_7; + + case SYM_DIRECTION + 2: // E pointing arrow + return BF_SYM_ARROW_EAST; + + case SYM_DIRECTION + 3: // SE pointing arrow + return BF_SYM_ARROW_3; + + case SYM_DIRECTION + 4: // S pointing arrow + return BF_SYM_ARROW_SOUTH; + + case SYM_DIRECTION + 5: // SW pointing arrow + return BF_SYM_ARROW_15; + + case SYM_DIRECTION + 6: // W pointing arrow + return BF_SYM_ARROW_WEST; + + case SYM_DIRECTION + 7: // NW pointing arrow + return BF_SYM_ARROW_11; + case SYM_VOLT: return BF_SYM_VOLT; @@ -187,13 +208,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; @@ -226,20 +243,19 @@ uint8_t getBfCharacter(uint8_t ch, uint8_t page) /* case SYM_NM: return BF_SYM_NM; - +*/ case SYM_WIND_HORIZONTAL: - return BF_SYM_WIND_HORIZONTAL; + return 'W'; // W for wind +/* case SYM_WIND_VERTICAL: return BF_SYM_WIND_VERTICAL; case SYM_3D_KT: return BF_SYM_3D_KT; - - - case SYM_AIR: - return BF_SYM_AIR; */ + case SYM_AIR: + return 'A'; // A for airspeed case SYM_3D_KMH: return BF_SYM_KPH; @@ -334,10 +350,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; diff --git a/src/main/io/displayport_msp_osd.c b/src/main/io/displayport_msp_osd.c index 17f49c8b78..4269b2a561 100644 --- a/src/main/io/displayport_msp_osd.c +++ b/src/main/io/displayport_msp_osd.c @@ -42,6 +42,7 @@ #include "drivers/osd_symbols.h" #include "fc/rc_modes.h" +#include "fc/runtime_config.h" #include "io/osd.h" #include "io/displayport_msp.h" @@ -113,6 +114,10 @@ static void checkVtxPresent(void) if (vtxActive && (millis()-vtxHeartbeat) > VTX_TIMEOUT) { vtxActive = false; } + + if (ARMING_FLAG(SIMULATOR_MODE_HITL)) { + vtxActive = true; + } } static int output(displayPort_t *displayPort, uint8_t cmd, uint8_t *subcmd, int len) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f13d86d1ed..1662094cab 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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) { @@ -316,7 +315,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, digits)) { + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, digits, false)) { buff[sym_index] = symbol_mi; } else { buff[sym_index] = symbol_ft; @@ -326,7 +325,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, digits)) { + if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, digits, false)) { buff[sym_index] = symbol_km; } else { buff[sym_index] = symbol_m; @@ -334,7 +333,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) buff[sym_index + 1] = '\0'; break; case OSD_UNIT_GA: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), (uint32_t)FEET_PER_NAUTICALMILE, decimals, 3, digits)) { + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), (uint32_t)FEET_PER_NAUTICALMILE, decimals, 3, digits, false)) { buff[sym_index] = symbol_nm; } else { buff[sym_index] = symbol_ft; @@ -489,7 +488,7 @@ static void osdFormatWindSpeedStr(char *buff, int32_t ws, bool isValid) break; } - osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3); + osdFormatCentiNumber(buff, centivalue, 0, 2, 0, 3, false); if (!isValid && ((millis() / 1000) % 4 < 2)) suffix = '*'; @@ -562,7 +561,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) case OSD_UNIT_GA: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff + totalDigits - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits)) { + if (osdFormatCentiNumber(buff + totalDigits - digits, CENTIMETERS_TO_CENTIFEET(alt), 1000, 0, 2, digits, false)) { // Scaled to kft buff[symbolIndex++] = symbolKFt; } else { @@ -575,7 +574,7 @@ void osdFormatAltitudeSymbol(char *buff, int32_t alt) FALLTHROUGH; case OSD_UNIT_METRIC: // alt is alredy in cm - if (osdFormatCentiNumber(buff + totalDigits - digits, alt, 1000, 0, 2, digits)) { + if (osdFormatCentiNumber(buff + totalDigits - digits, alt, 1000, 0, 2, digits, false)) { // Scaled to km buff[symbolIndex++] = SYM_ALT_KM; } else { @@ -1151,7 +1150,7 @@ static void osdFormatGVar(char *buff, uint8_t index) buff[1] = '0'+index; buff[2] = ':'; #ifdef USE_PROGRAMMING_FRAMEWORK - osdFormatCentiNumber(buff + 3, (int32_t)gvGet(index)*(int32_t)100, 1, 0, 0, 5); + osdFormatCentiNumber(buff + 3, (int32_t)gvGet(index)*(int32_t)100, 1, 0, 0, 5, false); #endif } @@ -1162,7 +1161,7 @@ static void osdFormatRpm(char *buff, uint32_t rpm) if (rpm) { if ( digitCount(rpm) > osdConfig()->esc_rpm_precision) { uint8_t rpmMaxDecimals = (osdConfig()->esc_rpm_precision - 3); - osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1); + osdFormatCentiNumber(buff + 1, rpm / 10, 0, rpmMaxDecimals, rpmMaxDecimals, osdConfig()->esc_rpm_precision-1, false); buff[osdConfig()->esc_rpm_precision] = 'K'; buff[osdConfig()->esc_rpm_precision+1] = '\0'; } @@ -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) { @@ -1486,13 +1485,13 @@ static void osdFormatPidControllerOutput(char *buff, const char *label, const pi strcpy(buff, label); for (uint8_t i = strlen(label); i < 5; ++i) buff[i] = ' '; uint8_t decimals = showDecimal ? 1 : 0; - osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4); + osdFormatCentiNumber(buff + 5, pidController->proportional * scale, 0, decimals, 0, 4, false); buff[9] = ' '; - osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4); + osdFormatCentiNumber(buff + 10, pidController->integrator * scale, 0, decimals, 0, 4, false); buff[14] = ' '; - osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4); + osdFormatCentiNumber(buff + 15, pidController->derivative * scale, 0, decimals, 0, 4, false); buff[19] = ' '; - osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4); + osdFormatCentiNumber(buff + 20, pidController->output_constrained * scale, 0, decimals, 0, 4, false); buff[24] = '\0'; } @@ -1508,7 +1507,7 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_ elemAttr = TEXT_ATTRIBUTES_NONE; digits = MIN(digits, 5); - osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits); + osdFormatCentiNumber(buff, voltage, 0, decimals, 0, digits, false); buff[digits] = SYM_VOLT; buff[digits+1] = '\0'; const batteryState_e batteryVoltageState = checkBatteryVoltageState(); @@ -1602,7 +1601,7 @@ static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY, displayWrite(osdDisplayPort, elemPosX, elemPosY, str); elemAttr = TEXT_ATTRIBUTES_NONE; - osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8)); + osdFormatCentiNumber(buff, value * 100, 0, maxDecimals, 0, MIN(valueLength, 8), false); if (isAdjustmentFunctionSelected(adjFunc)) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr); @@ -1707,7 +1706,7 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_CURRENT_DRAW: { - osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3); + osdFormatCentiNumber(buff, getAmperage(), 0, 2, 0, 3, false); buff[3] = SYM_AMP; buff[4] = '\0'; @@ -1719,22 +1718,18 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_MAH_DRAWN: { - uint8_t mah_digits = osdConfig()->mAh_used_precision; // Initialize to config value - bool bfcompat = false; // Assume BFCOMPAT is off + uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value #ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it if (isBfCompatibleVideoSystem(osdConfig())) { - bfcompat = true; - } -#endif - - if (bfcompat) { //BFcompat is unable to work with scaled values and it only has mAh symbol to work with - tfp_sprintf(buff, "%5d", (int)getMAhDrawn()); // Use 5 digits to allow 10Ah+ packs + tfp_sprintf(buff, "%5d", (int)getMAhDrawn()); // Use 5 digits to allow packs below 100Ah buff[5] = SYM_MAH; buff[6] = '\0'; - } else { - if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits)) { + } else +#endif + { + if (osdFormatCentiNumber(buff, getMAhDrawn() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) { // Shown in Ah buff[mah_digits] = SYM_AH; } else { @@ -1749,32 +1744,57 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_WH_DRAWN: - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3, false); osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); buff[3] = SYM_WH; buff[4] = '\0'; break; case OSD_BATTERY_REMAINING_CAPACITY: + { + bool unitsDrawn = false; if (currentBatteryProfile->capacity.value == 0) tfp_sprintf(buff, " NA"); else if (!batteryWasFullWhenPluggedIn()) tfp_sprintf(buff, " NF"); - else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) - tfp_sprintf(buff, "%4lu", (unsigned long)getBatteryRemainingCapacity()); - else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH - osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3); + else if (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH) { + uint8_t mah_digits = osdConfig()->mAh_precision; // Initialize to config value + +#ifndef DISABLE_MSP_BF_COMPAT // IF BFCOMPAT is not supported, there's no need to check for it + if (isBfCompatibleVideoSystem(osdConfig())) { + //BFcompat is unable to work with scaled values and it only has mAh symbol to work with + tfp_sprintf(buff, "%5d", (int)getBatteryRemainingCapacity()); // Use 5 digits to allow packs below 100Ah + buff[5] = SYM_MAH; + buff[6] = '\0'; + unitsDrawn = true; + } else +#endif + { + if (osdFormatCentiNumber(buff, getBatteryRemainingCapacity() * 100, 1000, 0, (mah_digits - 2), mah_digits, false)) { + // Shown in Ah + buff[mah_digits] = SYM_AH; + } else { + // Shown in mAh + buff[mah_digits] = SYM_MAH; + } + buff[mah_digits + 1] = '\0'; + unitsDrawn = true; + } + } else // currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH + osdFormatCentiNumber(buff + 1, getBatteryRemainingCapacity() / 10, 0, 2, 0, 3, false); - buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; - buff[5] = '\0'; + if (!unitsDrawn) { + buff[4] = currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MAH ? SYM_MAH : SYM_WH; + buff[5] = '\0'; + } if (batteryUsesCapacityThresholds()) { osdUpdateBatteryCapacityOrVoltageTextAttributes(&elemAttr); } break; - + } case OSD_BATTERY_REMAINING_PERCENT: osdFormatBatteryChargeSymbol(buff); tfp_sprintf(buff + 1, "%3d%%", calculateBatteryPercentage()); @@ -1834,7 +1854,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_GLIDESLOPE; if (glideSlope > 0.0f && glideSlope < 100.0f) { - osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3); + osdFormatCentiNumber(buff + 1, glideSlope * 100.0f, 0, 2, 0, 3, false); } else { buff[1] = buff[2] = buff[3] = '-'; } @@ -1912,6 +1932,39 @@ static bool osdDrawSingleElement(uint8_t item) osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0); break; + case OSD_ODOMETER: + { + displayWriteChar(osdDisplayPort, elemPosX, elemPosY, SYM_ODOMETER); + uint32_t odometerDist = (uint32_t)(getTotalTravelDistance() / 100); +#ifdef USE_STATS + odometerDist+= statsConfig()->stats_total_dist; +#endif + odometerDist = odometerDist / 10; + + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(odometerDist), FEET_PER_MILE, 1, 0, 6, true); + buff[6] = SYM_MI; + break; + default: + case OSD_UNIT_GA: + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(odometerDist), (uint32_t)FEET_PER_NAUTICALMILE, 1, 0, 6, true); + buff[6] = SYM_NM; + break; + case OSD_UNIT_METRIC_MPH: + FALLTHROUGH; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(buff, odometerDist, METERS_PER_KILOMETER, 1, 0, 6, true); + buff[6] = SYM_KM; + break; + } + buff[7] = '\0'; + elemPosX++; + } + break; + case OSD_GROUND_COURSE: { buff[0] = SYM_GROUND_COURSE; @@ -1994,7 +2047,7 @@ static bool osdDrawSingleElement(uint8_t item) digits = 3U; } #endif - osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits); + osdFormatCentiNumber(&buff[2], centiHDOP, 0, 1, 0, digits, false); break; } @@ -2226,6 +2279,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); @@ -2424,7 +2483,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_ROLL_LEVEL; if (ABS(attitude.values.roll) >= 1) buff[0] += (attitude.values.roll < 0 ? -1 : 1); - osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3); + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.roll)), 0, 1, 0, 3, false); break; case OSD_ATTITUDE_PITCH: @@ -2434,7 +2493,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_PITCH_DOWN; else if (attitude.values.pitch < 0) buff[0] = SYM_PITCH_UP; - osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3); + osdFormatCentiNumber(buff + 1, DECIDEGREES_TO_CENTIDEGREES(ABS(attitude.values.pitch)), 0, 1, 0, 3, false); break; case OSD_ARTIFICIAL_HORIZON: @@ -2495,7 +2554,7 @@ static bool osdDrawSingleElement(uint8_t item) break; } - osdFormatCentiNumber(buff, value, 0, 1, 0, 3); + osdFormatCentiNumber(buff, value, 0, 1, 0, 3, false); buff[3] = sym; buff[4] = '\0'; break; @@ -2528,7 +2587,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_IMPERIAL: // mAh/foot if (efficiencyValid) { - osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3); + osdFormatCentiNumber(buff, (value * METERS_PER_FOOT), 1, 2, 2, 3, false); tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_FT_0, SYM_AH_V_FT_1); } else { buff[0] = buff[1] = buff[2] = '-'; @@ -2542,7 +2601,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_METRIC: // mAh/metre if (efficiencyValid) { - osdFormatCentiNumber(buff, value, 1, 2, 2, 3); + osdFormatCentiNumber(buff, value, 1, 2, 2, 3, false); tfp_sprintf(buff, "%s%c%c", buff, SYM_AH_V_M_0, SYM_AH_V_M_1); } else { buff[0] = buff[1] = buff[2] = '-'; @@ -2837,7 +2896,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_POWER: { - bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3); + bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3, false); buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[4] = '\0'; @@ -3000,7 +3059,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); } else { @@ -3014,7 +3073,7 @@ static bool osdDrawSingleElement(uint8_t item) } break; case OSD_UNIT_GA: - moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10, 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); } else { @@ -3030,7 +3089,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); } else { @@ -3071,17 +3130,17 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_IMPERIAL: - osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3); + osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3, false); buff[3] = SYM_WH_MI; break; case OSD_UNIT_GA: - osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10000, 0, 2, 0, 3); + osdFormatCentiNumber(buff, value * METERS_PER_NAUTICALMILE / 10000, 0, 2, 0, 3, false); buff[3] = SYM_WH_NM; break; case OSD_UNIT_METRIC_MPH: FALLTHROUGH; case OSD_UNIT_METRIC: - osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); + osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3, false); buff[3] = SYM_WH_KM; break; } @@ -3095,7 +3154,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_GFORCE: { buff[0] = SYM_GFORCE; - osdFormatCentiNumber(buff + 1, GForce, 0, 2, 0, 3); + osdFormatCentiNumber(buff + 1, GForce, 0, 2, 0, 3, false); if (GForce > osdConfig()->gforce_alarm * 100) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -3108,7 +3167,7 @@ static bool osdDrawSingleElement(uint8_t item) { float GForceValue = GForceAxis[item - OSD_GFORCE_X]; buff[0] = SYM_GFORCE_X + item - OSD_GFORCE_X; - osdFormatCentiNumber(buff + 1, GForceValue, 0, 2, 0, 4); + osdFormatCentiNumber(buff + 1, GForceValue, 0, 2, 0, 4, false); if ((GForceValue < osdConfig()->gforce_axis_alarm_min * 100) || (GForceValue > osdConfig()->gforce_axis_alarm_max * 100)) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } @@ -3284,7 +3343,7 @@ static bool osdDrawSingleElement(uint8_t item) } buff[0] = SYM_SCALE; if (osdMapData.scale > 0) { - bool scaled = osdFormatCentiNumber(&buff[1], osdMapData.scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3); + bool scaled = osdFormatCentiNumber(&buff[1], osdMapData.scale * scaleToUnit, scaleUnitDivisor, maxDecimals, 2, 3, false); buff[4] = scaled ? symScaled : symUnscaled; // Make sure this is cleared if the map stops being drawn osdMapData.scale = 0; @@ -3453,14 +3512,14 @@ static bool osdDrawSingleElement(uint8_t item) #ifdef USE_POWER_LIMITS case OSD_PLIMIT_REMAINING_BURST_TIME: - osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3); + osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3, false); buff[3] = 'S'; buff[4] = '\0'; break; case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT: if (currentBatteryProfile->powerLimits.continuousCurrent) { - osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3); + osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3, false); buff[3] = SYM_AMP; buff[4] = '\0'; @@ -3474,7 +3533,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_PLIMIT_ACTIVE_POWER_LIMIT: { if (currentBatteryProfile->powerLimits.continuousPower) { - bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3); + bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3, false); buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[4] = '\0'; @@ -3703,7 +3762,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .pan_servo_offcentre_warning = SETTING_OSD_PAN_SERVO_OFFCENTRE_WARNING_DEFAULT, .pan_servo_indicator_show_degrees = SETTING_OSD_PAN_SERVO_INDICATOR_SHOW_DEGREES_DEFAULT, .esc_rpm_precision = SETTING_OSD_ESC_RPM_PRECISION_DEFAULT, - .mAh_used_precision = SETTING_OSD_MAH_USED_PRECISION_DEFAULT, + .mAh_precision = SETTING_OSD_MAH_PRECISION_DEFAULT, .osd_switch_indicator0_name = SETTING_OSD_SWITCH_INDICATOR_ZERO_NAME_DEFAULT, .osd_switch_indicator0_channel = SETTING_OSD_SWITCH_INDICATOR_ZERO_CHANNEL_DEFAULT, .osd_switch_indicator1_name = SETTING_OSD_SWITCH_INDICATOR_ONE_NAME_DEFAULT, @@ -3716,6 +3775,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, @@ -3751,6 +3813,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) //line 2 osdLayoutsConfig->item_pos[0][OSD_HOME_DIST] = OSD_POS(1, 1); osdLayoutsConfig->item_pos[0][OSD_TRIP_DIST] = OSD_POS(1, 2); + osdLayoutsConfig->item_pos[0][OSD_ODOMETER] = OSD_POS(1, 3); osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_CELL_VOLTAGE] = OSD_POS(12, 1); osdLayoutsConfig->item_pos[0][OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE] = OSD_POS(12, 1); osdLayoutsConfig->item_pos[0][OSD_GPS_SPEED] = OSD_POS(23, 1); @@ -3788,6 +3851,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 @@ -3910,6 +3974,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; @@ -3946,19 +4150,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) { @@ -3968,85 +4165,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); - 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); - string_buffer[3] = SYM_WH_MI; - break; - case OSD_UNIT_GA: - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); - 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); - 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); @@ -4250,22 +4378,22 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) if (isSinglePageStatsCompatible || page == 1) { if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); - osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); + osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2, false); } else { displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); - osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); + osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3, false); } tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); displayWrite(osdDisplayPort, statValuesX, top++, buff); if (feature(FEATURE_CURRENT_METER)) { displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); - osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); + osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3, false); tfp_sprintf(buff, "%s%c", buff, SYM_AMP); displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); - bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); + bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3, false); buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[4] = '\0'; displayWrite(osdDisplayPort, statValuesX, top++, buff); @@ -4274,7 +4402,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); } else { - osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); + osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3, false); tfp_sprintf(buff, "%s%c", buff, SYM_WH); } displayWrite(osdDisplayPort, statValuesX, top++, buff); @@ -4296,7 +4424,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) FALLTHROUGH; case OSD_UNIT_IMPERIAL: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); } else { @@ -4309,7 +4437,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) buff[5] = '\0'; } } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, digits, false); tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; @@ -4318,7 +4446,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) break; case OSD_UNIT_GA: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_NAUTICALMILE / totalDistance), 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_NM_0, SYM_MAH_NM_1); } else { @@ -4331,7 +4459,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) buff[5] = '\0'; } } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_NAUTICALMILE / totalDistance), 0, 2, 0, digits, false); tfp_sprintf(buff, "%s%c", buff, SYM_WH_NM); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; @@ -4342,7 +4470,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) FALLTHROUGH; case OSD_UNIT_METRIC: if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits); + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, digits, false); if (!moreThanAh) { tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); } else { @@ -4355,7 +4483,7 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) buff[5] = '\0'; } } else { - osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits); + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, digits, false); tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; @@ -4370,19 +4498,19 @@ static void osdShowStats(bool isSinglePageStatsCompatible, uint8_t page) const float max_gforce = accGetMeasuredMaxG(); displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); - osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); + osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3, false); displayWrite(osdDisplayPort, statValuesX, top++, buff); const acc_extremes_t *acc_extremes = accGetMeasuredExtremes(); const float acc_extremes_min = acc_extremes[Z].min; const float acc_extremes_max = acc_extremes[Z].max; displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); - osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4); + osdFormatCentiNumber(buff, acc_extremes_min * 100, 0, 2, 0, 4, false); osdLeftAlignString(buff); strcat(osdFormatTrimWhiteSpace(buff),"/"); multiValueLengthOffset = strlen(buff); displayWrite(osdDisplayPort, statValuesX, top, buff); - osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3); + osdFormatCentiNumber(buff, acc_extremes_max * 100, 0, 2, 0, 3, false); osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + multiValueLengthOffset, top++, buff); } @@ -4400,85 +4528,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) { @@ -4579,10 +4827,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 { @@ -4929,8 +5181,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTUNE_ACRO); } } - if (IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || (navigationRequiresAngleMode() && !navigationIsControllingAltitude()))) { - messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL); + if (isFixedWingLevelTrimActive()) { + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLEVEL); } if (FLIGHT_MODE(HEADFREE_MODE)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE); @@ -5199,4 +5451,5 @@ static textAttributes_t osdGetMultiFunctionMessage(char *buff) return elemAttr; } + #endif // OSD diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 04794c6cec..f09c9d049b 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -274,15 +274,17 @@ typedef enum { OSD_PILOT_NAME, 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; @@ -343,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_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); @@ -483,7 +486,7 @@ void osdStartedSaveProcess(void); void osdShowEEPROMSavedNotification(void); void osdCrosshairPosition(uint8_t *x, uint8_t *y); -bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length); +bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros); void osdFormatAltitudeSymbol(char *buff, int32_t alt); void osdFormatVelocityStr(char* buff, int32_t vel, bool _3D, bool _max); // Returns a heading angle in degrees normalized to [0, 360). diff --git a/src/main/io/osd_hud.c b/src/main/io/osd_hud.c index 2e209fd282..47cc96f834 100644 --- a/src/main/io/osd_hud.c +++ b/src/main/io/osd_hud.c @@ -256,18 +256,18 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu case OSD_UNIT_IMPERIAL: { if (poiType == 1) { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 4, 4); + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 4, 4, false); } else { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3); + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), FEET_PER_MILE, 0, 3, 3, false); } } break; case OSD_UNIT_GA: { if (poiType == 1) { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4); + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 4, 4, false); } else { - osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 3, 3); + osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(poiDistance * 100), (uint32_t)FEET_PER_NAUTICALMILE, 0, 3, 3, false); } } break; @@ -278,9 +278,9 @@ void osdHudDrawPoi(uint32_t poiDistance, int16_t poiDirection, int32_t poiAltitu case OSD_UNIT_METRIC: { if (poiType == 1) { - osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 4, 4); + osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 4, 4, false); } else { - osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3); + osdFormatCentiNumber(buff, poiDistance * 100, METERS_PER_KILOMETER, 0, 3, 3, false); } } break; diff --git a/src/main/io/osd_joystick.c b/src/main/io/osd_joystick.c new file mode 100644 index 0000000000..c1d9dee5a5 --- /dev/null +++ b/src/main/io/osd_joystick.c @@ -0,0 +1,74 @@ +#include +#include +#include + +#include "platform.h" + +#include "common/crc.h" +#include "common/maths.h" +#include "common/streambuf.h" +#include "common/utils.h" + +#include "build/build_config.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "fc/settings.h" +#include "fc/runtime_config.h" + +#include "drivers/time.h" +#include "drivers/light_ws2811strip.h" + +#include "io/serial.h" +#include "io/rcdevice.h" + +#include "osd_joystick.h" + +#ifdef USE_RCDEVICE +#ifdef USE_LED_STRIP + + +PG_REGISTER_WITH_RESET_TEMPLATE(osdJoystickConfig_t, osdJoystickConfig, PG_OSD_JOYSTICK_CONFIG, 0); + +PG_RESET_TEMPLATE(osdJoystickConfig_t, osdJoystickConfig, + .osd_joystick_enabled = SETTING_OSD_JOYSTICK_ENABLED_DEFAULT, + .osd_joystick_down = SETTING_OSD_JOYSTICK_DOWN_DEFAULT, + .osd_joystick_up = SETTING_OSD_JOYSTICK_UP_DEFAULT, + .osd_joystick_left = SETTING_OSD_JOYSTICK_LEFT_DEFAULT, + .osd_joystick_right = SETTING_OSD_JOYSTICK_RIGHT_DEFAULT, + .osd_joystick_enter = SETTING_OSD_JOYSTICK_ENTER_DEFAULT +); + +bool osdJoystickEnabled(void) { + return osdJoystickConfig()->osd_joystick_enabled; +} + + +void osdJoystickSimulate5KeyButtonPress(uint8_t operation) { + switch (operation) { + case RCDEVICE_CAM_KEY_ENTER: + ledPinStartPWM( osdJoystickConfig()->osd_joystick_enter ); + break; + case RCDEVICE_CAM_KEY_LEFT: + ledPinStartPWM( osdJoystickConfig()->osd_joystick_left ); + break; + case RCDEVICE_CAM_KEY_UP: + ledPinStartPWM( osdJoystickConfig()->osd_joystick_up ); + break; + case RCDEVICE_CAM_KEY_RIGHT: + ledPinStartPWM( osdJoystickConfig()->osd_joystick_right ); + break; + case RCDEVICE_CAM_KEY_DOWN: + ledPinStartPWM( osdJoystickConfig()->osd_joystick_down ); + break; + } +} + + +void osdJoystickSimulate5KeyButtonRelease(void) { + ledPinStopPWM(); +} + + +#endif +#endif diff --git a/src/main/io/osd_joystick.h b/src/main/io/osd_joystick.h new file mode 100644 index 0000000000..574b8e3b77 --- /dev/null +++ b/src/main/io/osd_joystick.h @@ -0,0 +1,26 @@ +#pragma once + +#include "config/parameter_group.h" + +#ifdef USE_RCDEVICE +#ifdef USE_LED_STRIP + +typedef struct osdJoystickConfig_s { + bool osd_joystick_enabled; + uint8_t osd_joystick_down; + uint8_t osd_joystick_up; + uint8_t osd_joystick_left; + uint8_t osd_joystick_right; + uint8_t osd_joystick_enter; +} osdJoystickConfig_t; + +PG_DECLARE(osdJoystickConfig_t, osdJoystickConfig); + +bool osdJoystickEnabled(void); + +// 5 key osd cable simulation +void osdJoystickSimulate5KeyButtonPress(uint8_t operation); +void osdJoystickSimulate5KeyButtonRelease(void); + +#endif +#endif \ No newline at end of file diff --git a/src/main/io/osd_utils.c b/src/main/io/osd_utils.c index 194be36f95..6675be8783 100644 --- a/src/main/io/osd_utils.c +++ b/src/main/io/osd_utils.c @@ -38,7 +38,7 @@ int digitCount(int32_t value) } -bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length) +bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros) { char *ptr = buff; char *dec; @@ -86,7 +86,11 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma // Done counting. Time to write the characters. // Write spaces at the start while (remaining > 0) { - *ptr = SYM_BLANK; + if (leadingZeros) + *ptr = '0'; + else + *ptr = SYM_BLANK; + ptr++; remaining--; } @@ -98,7 +102,11 @@ bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int ma // Add any needed remaining leading spaces while(rem_spaces > 0) { - *ptr = SYM_BLANK; + if (leadingZeros) + *ptr = '0'; + else + *ptr = SYM_BLANK; + ptr++; remaining--; rem_spaces--; diff --git a/src/main/io/osd_utils.h b/src/main/io/osd_utils.h index 2f9c61a320..7f10f2bf8f 100644 --- a/src/main/io/osd_utils.h +++ b/src/main/io/osd_utils.h @@ -33,6 +33,6 @@ int digitCount(int32_t value); * of the same length. If the value doesn't fit into the provided length * it will be divided by scale and true will be returned. */ -bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length); +bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length, bool leadingZeros); #endif diff --git a/src/main/io/rcdevice_cam.c b/src/main/io/rcdevice_cam.c index 36a0f99869..0edbc8bf97 100644 --- a/src/main/io/rcdevice_cam.c +++ b/src/main/io/rcdevice_cam.c @@ -29,6 +29,7 @@ #include "io/beeper.h" #include "io/rcdevice_cam.h" +#include "io/osd_joystick.h" #include "rx/rx.h" @@ -47,6 +48,14 @@ bool waitingDeviceResponse = false; static bool isFeatureSupported(uint8_t feature) { +#ifndef UNIT_TEST +#ifdef USE_LED_STRIP + if (!rcdeviceIsEnabled() && osdJoystickEnabled() ) { + return true; + } +#endif +#endif + if (camDevice->info.features & feature) { return true; } @@ -72,6 +81,7 @@ static void rcdeviceCameraControlProcess(void) } uint8_t behavior = RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION; + uint8_t behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION; switch (i) { case BOXCAMERA1: if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_WIFI_BUTTON)) { @@ -81,11 +91,13 @@ static void rcdeviceCameraControlProcess(void) if (!ARMING_FLAG(ARMED)) { behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN; } + behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN; } break; case BOXCAMERA2: if (isFeatureSupported(RCDEVICE_PROTOCOL_FEATURE_SIMULATE_POWER_BUTTON)) { behavior = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN; + behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN; } break; case BOXCAMERA3: @@ -94,16 +106,43 @@ static void rcdeviceCameraControlProcess(void) if (!ARMING_FLAG(ARMED)) { behavior = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE; } + behavior1 = RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE; } break; default: break; } - if (behavior != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) { + if ((behavior != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) && rcdeviceIsEnabled()) { runcamDeviceSimulateCameraButton(camDevice, behavior); switchStates[switchIndex].isActivated = true; } +#ifndef UNIT_TEST +#ifdef USE_LED_STRIP + else if ((behavior1 != RCDEVICE_PROTOCOL_CAM_CTRL_UNKNOWN_CAMERA_OPERATION) && osdJoystickEnabled()) { + switch (behavior1) { + case RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_WIFI_BTN: + osdJoystickSimulate5KeyButtonPress(RCDEVICE_CAM_KEY_ENTER); + break; + case RCDEVICE_PROTOCOL_CAM_CTRL_SIMULATE_POWER_BTN: + osdJoystickSimulate5KeyButtonPress(RCDEVICE_CAM_KEY_UP); + break; + case RCDEVICE_PROTOCOL_CAM_CTRL_CHANGE_MODE: + osdJoystickSimulate5KeyButtonPress(RCDEVICE_CAM_KEY_DOWN); + break; + } + switchStates[switchIndex].isActivated = true; + } +#endif +#endif + UNUSED(behavior1); } else { +#ifndef UNIT_TEST +#ifdef USE_LED_STRIP + if (osdJoystickEnabled() && switchStates[switchIndex].isActivated) { + osdJoystickSimulate5KeyButtonRelease(); + } +#endif +#endif switchStates[switchIndex].isActivated = false; } } @@ -225,14 +264,24 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) } #endif - if (camDevice->serialPort == 0 || ARMING_FLAG(ARMED)) { + if (ARMING_FLAG(ARMED)) { return; } if (isButtonPressed) { if (IS_MID(YAW) && IS_MID(PITCH) && IS_MID(ROLL)) { - rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); - waitingDeviceResponse = true; + if ( rcdeviceIsEnabled() ) { + rcdeviceSend5KeyOSDCableSimualtionEvent(RCDEVICE_CAM_KEY_RELEASE); + waitingDeviceResponse = true; + } +#ifndef UNIT_TEST +#ifdef USE_LED_STRIP + else if (osdJoystickEnabled()) { + osdJoystickSimulate5KeyButtonRelease(); + isButtonPressed = false; + } +#endif +#endif } } else { if (waitingDeviceResponse) { @@ -266,16 +315,33 @@ static void rcdevice5KeySimulationProcess(timeUs_t currentTimeUs) } if (key != RCDEVICE_CAM_KEY_NONE) { - rcdeviceSend5KeyOSDCableSimualtionEvent(key); + if ( rcdeviceIsEnabled() ) { + rcdeviceSend5KeyOSDCableSimualtionEvent(key); + waitingDeviceResponse = true; + } +#ifndef UNIT_TEST +#ifdef USE_LED_STRIP + else if (osdJoystickEnabled()) { + if ( key == RCDEVICE_CAM_KEY_CONNECTION_OPEN ) { + rcdeviceInMenu = true; + } else if ( key == RCDEVICE_CAM_KEY_CONNECTION_CLOSE ) { + rcdeviceInMenu = false; + } else { + osdJoystickSimulate5KeyButtonPress(key); + } + } +#endif +#endif isButtonPressed = true; - waitingDeviceResponse = true; } } } void rcdeviceUpdate(timeUs_t currentTimeUs) { - rcdeviceReceive(currentTimeUs); + if ( rcdeviceIsEnabled() ) { + rcdeviceReceive(currentTimeUs); + } rcdeviceCameraControlProcess(); diff --git a/src/main/io/vtx_tramp.c b/src/main/io/vtx_tramp.c index 2791ada294..540c9c9f22 100644 --- a/src/main/io/vtx_tramp.c +++ b/src/main/io/vtx_tramp.c @@ -575,16 +575,27 @@ const char * const trampPowerNames_5G8_800[VTX_TRAMP_5G8_MAX_POWER_COUNT + 1] = const uint16_t trampPowerTable_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT] = { 25, 200, 800 }; const char * const trampPowerNames_1G3_800[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "800" }; +const uint16_t trampPowerTable_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT] = { 25, 200, 2000 }; +const char * const trampPowerNames_1G3_2000[VTX_TRAMP_1G3_MAX_POWER_COUNT + 1] = { "---", "25 ", "200", "2000" }; + static void vtxProtoUpdatePowerMetadata(uint16_t maxPower) { switch (vtxSettingsConfig()->frequencyGroup) { case FREQUENCYGROUP_1G3: - vtxState.metadata.powerTablePtr = trampPowerTable_1G3_800; - vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; + if (maxPower >= 2000) { + vtxState.metadata.powerTablePtr = trampPowerTable_1G3_2000; + vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; - impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_800; - impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; + impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_2000; + impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; + } + else { + vtxState.metadata.powerTablePtr = trampPowerTable_1G3_800; + vtxState.metadata.powerTableCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; + impl_vtxDevice.capability.powerNames = (char **)trampPowerNames_1G3_800; + impl_vtxDevice.capability.powerCount = VTX_TRAMP_1G3_MAX_POWER_COUNT; + } impl_vtxDevice.capability.bandCount = VTX_TRAMP_1G3_BAND_COUNT; impl_vtxDevice.capability.channelCount = VTX_TRAMP_1G3_CHANNEL_COUNT; impl_vtxDevice.capability.bandNames = (char **)vtx1G3BandNames; diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 9cf1881c73..eba96690a2 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -94,3 +94,8 @@ #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 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 51105f24ff..c991531f32 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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; } } @@ -2879,6 +2893,9 @@ static void updateNavigationFlightStatistics(void) } } +/* + * Total travel distance in cm + */ uint32_t getTotalTravelDistance(void) { return lrintf(posControl.totalTripDistance); @@ -2949,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); } @@ -2993,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 *-----------------------------------------------------------*/ @@ -3916,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; } @@ -3937,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; @@ -4021,7 +4055,8 @@ bool navigationPositionEstimateIsHealthy(void) navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) { - const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE))); + const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || + IS_RC_MODE_ACTIVE(BOXNAVCRUISE) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)); if (usedBypass) { *usedBypass = false; @@ -4051,13 +4086,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){ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 3a9223ff63..ecc487cc9e 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -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); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 3a88e4ea36..e6084e0972 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -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; } /*----------------------------------------------------------- diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 5c314bfddd..d7ef73c938 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -54,6 +54,7 @@ #include "sensors/opflow.h" navigationPosEstimator_t posEstimator; +static float initialBaroAltitudeOffset = 0.0f; PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 5); @@ -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 */ diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index d3b45453c9..1e3ce0eb61 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -56,6 +56,7 @@ #include "io/vtx.h" #include "drivers/vtx_common.h" +#include "drivers/light_ws2811strip.h" PG_REGISTER_ARRAY_WITH_RESET_FN(logicCondition_t, MAX_LOGIC_CONDITIONS, logicConditions, PG_LOGIC_CONDITIONS, 4); @@ -474,6 +475,17 @@ static int logicConditionCompute( return false; } break; + +#ifdef LED_PIN + case LOGIC_CONDITION_LED_PIN_PWM: + if (operandA >=0 && operandA <= 100) { + ledPinStartPWM((uint8_t)operandA); + } else { + ledPinStopPWM(); + } + return operandA; + break; +#endif default: return false; @@ -704,6 +716,10 @@ static int logicConditionGetFlightOperandValue(int operand) { return constrain(attitude.values.pitch / 10, -180, 180); break; + case LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW: // deg + return constrain(attitude.values.yaw / 10, 0, 360); + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_IS_ARMED: // 0/1 return ARMING_FLAG(ARMED) ? 1 : 0; break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 941e47f8d0..09163b62ba 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -81,7 +81,8 @@ typedef enum { LOGIC_CONDITION_TIMER = 49, LOGIC_CONDITION_DELTA = 50, LOGIC_CONDITION_APPROX_EQUAL = 51, - LOGIC_CONDITION_LAST = 52, + LOGIC_CONDITION_LED_PIN_PWM = 52, + LOGIC_CONDITION_LAST = 53, } logicOperation_e; typedef enum logicOperandType_s { @@ -135,8 +136,9 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2 // 35 LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2 // 36 LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int // 37 - LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int // 39 - LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1 // 40 + LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int // 38 + LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1 // 39 + LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg // 40 } logicFlightOperands_e; typedef enum { diff --git a/src/main/rx/jetiexbus.c b/src/main/rx/jetiexbus.c index d9b009342e..3556483399 100644 --- a/src/main/rx/jetiexbus.c +++ b/src/main/rx/jetiexbus.c @@ -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) @@ -153,6 +158,7 @@ static void jetiExBusDataReceive(uint16_t c, void *data) static timeUs_t jetiExBusTimeLast = 0; static uint8_t *jetiExBusFrame; + static uint8_t jetiExBusFrameMaxSize; const timeUs_t now = microsISR(); // Check if we shall reset frame position due to time @@ -169,11 +175,13 @@ static void jetiExBusDataReceive(uint16_t c, void *data) case EXBUS_START_CHANNEL_FRAME: jetiExBusFrameState = EXBUS_STATE_IN_PROGRESS; jetiExBusFrame = jetiExBusChannelFrame; + jetiExBusFrameMaxSize = EXBUS_MAX_CHANNEL_FRAME_SIZE; break; case EXBUS_START_REQUEST_FRAME: jetiExBusRequestState = EXBUS_STATE_IN_PROGRESS; jetiExBusFrame = jetiExBusRequestFrame; + jetiExBusFrameMaxSize = EXBUS_MAX_CHANNEL_FRAME_SIZE; break; default: @@ -181,6 +189,15 @@ static void jetiExBusDataReceive(uint16_t c, void *data) } } + if (jetiExBusFramePosition == jetiExBusFrameMaxSize) { + // frame overrun + jetiExBusFrameReset(); + jetiExBusFrameState = EXBUS_STATE_ZERO; + jetiExBusRequestState = EXBUS_STATE_ZERO; + + return; + } + // Store in frame copy jetiExBusFrame[jetiExBusFramePosition] = (uint8_t)c; jetiExBusFramePosition++; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 4d5e2b135f..1aee04ec08 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -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) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 78e52f75c2..3eaeecc5f0 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -84,7 +84,7 @@ static bool batteryUseCapacityThresholds = false; static bool batteryFullWhenPluggedIn = false; static bool profileAutoswitchDisable = false; -static uint16_t vbat = 0; // battery voltage in 0.1V steps (filtered) +static uint16_t vbat = 0; // battery voltage in 0.01V steps (filtered) static uint16_t powerSupplyImpedance = 0; // calculated impedance in milliohm static uint16_t sagCompensatedVBat = 0; // calculated no load vbat static bool powerSupplyImpedanceIsValid = false; @@ -297,6 +297,14 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) vbat = 0; break; } + +#ifdef USE_SIMULATOR + if (ARMING_FLAG(SIMULATOR_MODE_HITL) && SIMULATOR_HAS_OPTION(HITL_SIMULATE_BATTERY)) { + vbat = ((uint16_t)simulatorData.vbat)*10; + return; + } +#endif + if (justConnected) { pt1FilterReset(&vbatFilterState, vbat); } else { diff --git a/src/main/target/AIKONF4/target.h b/src/main/target/AIKONF4/target.h index 22ee824e5f..cd057f8b87 100644 --- a/src/main/target/AIKONF4/target.h +++ b/src/main/target/AIKONF4/target.h @@ -86,14 +86,7 @@ #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h index db9c2ba7e1..9d5b0eef8b 100644 --- a/src/main/target/AIRBOTF4/target.h +++ b/src/main/target/AIRBOTF4/target.h @@ -41,14 +41,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_MAG3110 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 @@ -139,4 +132,4 @@ #define TARGET_IO_PORTD 0xffff #define USE_DSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR diff --git a/src/main/target/AIRBOTF7/target.h b/src/main/target/AIRBOTF7/target.h index d77529ed39..affa3ed435 100644 --- a/src/main/target/AIRBOTF7/target.h +++ b/src/main/target/AIRBOTF7/target.h @@ -70,12 +70,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 5527b45a1d..52cd4cf4a9 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -46,13 +46,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_MPU9250 -#define USE_MAG_HMC5883 -#define USE_MAG_MAG3110 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 5971f3d848..1c8e0f901e 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -44,14 +44,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8963 -#define USE_MAG_MPU9250 -#define USE_MAG_HMC5883 -#define USE_MAG_MAG3110 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define AK8963_CS_PIN PC15 #define AK8963_SPI_BUS BUS_SPI3 diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index 1bda97600d..4e1abcc0d0 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -35,12 +35,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_MAG3110 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index b09890a7ef..179a7211f0 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -35,12 +35,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index e058ed7a28..ce6cb0e63a 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -35,12 +35,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/AOCODARCF405AIO/target.h b/src/main/target/AOCODARCF405AIO/target.h index 859caa51f4..6a95a867cc 100644 --- a/src/main/target/AOCODARCF405AIO/target.h +++ b/src/main/target/AOCODARCF405AIO/target.h @@ -98,13 +98,7 @@ // Mag #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_MLX90393 +#define USE_MAG_ALL /*** Onboard Flash ***/ #define USE_SPI_DEVICE_3 diff --git a/src/main/target/AOCODARCF4V2/target.h b/src/main/target/AOCODARCF4V2/target.h index ed524edc90..104ac193af 100644 --- a/src/main/target/AOCODARCF4V2/target.h +++ b/src/main/target/AOCODARCF4V2/target.h @@ -60,13 +60,7 @@ //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS - -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 +#define USE_MAG_ALL // ******* SERIAL ******** #define USE_VCP diff --git a/src/main/target/AOCODARCF722AIO/CMakeLists.txt b/src/main/target/AOCODARCF722AIO/CMakeLists.txt new file mode 100644 index 0000000000..87553f152f --- /dev/null +++ b/src/main/target/AOCODARCF722AIO/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(AOCODARCF722AIO) diff --git a/src/main/target/AOCODARCF722AIO/config.c b/src/main/target/AOCODARCF722AIO/config.c new file mode 100644 index 0000000000..8996c3c5d3 --- /dev/null +++ b/src/main/target/AOCODARCF722AIO/config.c @@ -0,0 +1,44 @@ +/* + * @Author: g05047 + * @Date: 2023-03-22 17:15:53 + * @LastEditors: g05047 + * @LastEditTime: 2023-03-23 16:21:45 + * @Description: file content + */ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" +#include "sensors/boardalignment.h" +#include "sensors/barometer.h" +#include "sensors/compass.h" +#include "sensors/acceleration.h" + +void targetConfiguration(void) +{ + + compassConfigMutable()->mag_align = CW90_DEG; + + // barometerConfigMutable()->baro_hardware = BARO_DPS310; + + boardAlignmentMutable()->rollDeciDegrees = -450; + +} diff --git a/src/main/target/AOCODARCF722AIO/target.c b/src/main/target/AOCODARCF722AIO/target.c new file mode 100644 index 0000000000..90d21bbf2f --- /dev/null +++ b/src/main/target/AOCODARCF722AIO/target.c @@ -0,0 +1,40 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/sensor.h" + +BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, MPU6500_CS_PIN, NONE, 0, DEVFLAGS_NONE, IMU_MPU6500_ALIGN); + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM + + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MOTOR, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), // S3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // S4 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/AOCODARCF722AIO/target.h b/src/main/target/AOCODARCF722AIO/target.h new file mode 100644 index 0000000000..409bc03bfd --- /dev/null +++ b/src/main/target/AOCODARCF722AIO/target.h @@ -0,0 +1,170 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "F722AIO" +#define USBD_PRODUCT_STRING "AocodaRCF722AIO" + +#define LED0 PA4 + +#define USE_BEEPER +#define BEEPER PC13 +#define BEEPER_INVERTED + +/*** UART ***/ +#define USB_IO +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART6 +#define UART6_RX_PIN PC7 +#define UART6_TX_PIN PC6 + +#define SERIAL_PORT_COUNT 6 + +/*** Gyro & Acc ***/ +#define USE_TARGET_IMU_HARDWARE_DESCRIPTORS + +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_MPU6500 +#define MPU6500_CS_PIN PB2 +#define MPU6500_SPI_BUS BUS_SPI1 +#define MPU6500_EXTI_PIN PC4 + +#define IMU_MPU6500_ALIGN CW180_DEG + +#define USE_IMU_MPU6000 +#define MPU6000_CS_PIN PB2 +#define MPU6000_SPI_BUS BUS_SPI1 +#define MPU6000_EXTI_PIN PC4 + +#define IMU_MPU6000_ALIGN CW180_DEG + +#define USE_IMU_ICM42605 +#define ICM42605_CS_PIN PB2 +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_EXTI_PIN PC4 + +#define IMU_ICM42605_ALIGN CW180_DEG + +/*** I2C (Baro & Mag) ***/ +#define USE_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// Baro +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_SPL06 +#define USE_BARO_DPS310 + +// Mag +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_ALL + +/*** Onboard Flash ***/ +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PC12 + +#define M25P16_SPI_BUS BUS_SPI3 +#define M25P16_CS_PIN PD2 + +#define W25N01G_SPI_BUS BUS_SPI3 +#define W25N01G_CS_PIN PD2 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +/*** OSD ***/ +#define USE_OSD +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +/*** ADC ***/ +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC2 +#define ADC_CHANNEL_2_PIN PC0 +#define ADC_CHANNEL_3_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 +#define RSSI_ADC_CHANNEL ADC_CHN_2 + +/*** LED ***/ +#define USE_LED_STRIP +#define WS2811_PIN PA8 + +/*** Optical Flow & Lidar ***/ +#define USE_RANGEFINDER +#define USE_RANGEFINDER_MSP +#define USE_OPFLOW +#define USE_OPFLOW_MSP + +/*** Misc ***/ +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_BLACKBOX) +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define SERIALRX_UART SERIAL_PORT_USART2 +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_CRSF + +#define MAX_PWM_OUTPUT_PORTS 8 + +#define CURRENT_METER_SCALE 250 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/AOCODARCF7DUAL/target.h b/src/main/target/AOCODARCF7DUAL/target.h index 71d38f174d..5d4b0fda52 100644 --- a/src/main/target/AOCODARCF7DUAL/target.h +++ b/src/main/target/AOCODARCF7DUAL/target.h @@ -67,13 +67,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/AOCODARCF7MINI/target.h b/src/main/target/AOCODARCF7MINI/target.h index ff52565f58..9f628963d8 100644 --- a/src/main/target/AOCODARCF7MINI/target.h +++ b/src/main/target/AOCODARCF7MINI/target.h @@ -64,13 +64,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/AOCODARCH7DUAL/target.h b/src/main/target/AOCODARCH7DUAL/target.h index 58d9c4893e..1c12f198a0 100644 --- a/src/main/target/AOCODARCH7DUAL/target.h +++ b/src/main/target/AOCODARCH7DUAL/target.h @@ -116,13 +116,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_VCM5883 +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/ASGARD32F7/target.h b/src/main/target/ASGARD32F7/target.h index ddfc11f15b..0c5084db5f 100644 --- a/src/main/target/ASGARD32F7/target.h +++ b/src/main/target/ASGARD32F7/target.h @@ -42,11 +42,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ATOMRCF405NAVI/target.h b/src/main/target/ATOMRCF405NAVI/target.h index b8ae0ab6a3..5e9c9b1bd4 100644 --- a/src/main/target/ATOMRCF405NAVI/target.h +++ b/src/main/target/ATOMRCF405NAVI/target.h @@ -74,12 +74,7 @@ */ #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS - -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL /* * Barometer diff --git a/src/main/target/ATOMRCF405V2/CMakeLists.txt b/src/main/target/ATOMRCF405V2/CMakeLists.txt new file mode 100644 index 0000000000..940c2a80fa --- /dev/null +++ b/src/main/target/ATOMRCF405V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(ATOMRCF405V2 SKIP_RELEASES) diff --git a/src/main/target/ATOMRCF405V2/target.c b/src/main/target/ATOMRCF405V2/target.c new file mode 100644 index 0000000000..848bd0a80e --- /dev/null +++ b/src/main/target/ATOMRCF405V2/target.c @@ -0,0 +1,47 @@ +/* + * 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 + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S5 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6 + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED_STRIP +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ATOMRCF405V2/target.h b/src/main/target/ATOMRCF405V2/target.h new file mode 100644 index 0000000000..b831565389 --- /dev/null +++ b/src/main/target/ATOMRCF405V2/target.h @@ -0,0 +1,194 @@ +/* + * 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 +#define TARGET_BOARD_IDENTIFIER "ARF4" + +#define USBD_PRODUCT_STRING "AtomRCF405V2" + +#define LED0 PC14 +#define LED1 PB2 + +#define BEEPER PC13 +#define BEEPER_INVERTED + +/* + * SPI defines + */ +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PC3 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +/* + * I2C defines + */ +#define USE_I2C +#define DEFAULT_I2C_BUS BUS_I2C1 + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +//MPU6000 +//#define USE_IMU_MPU6000 +//#define IMU_MPU6000_ALIGN CW180_DEG +//#define MPU6000_SPI_BUS BUS_SPI1 +//#define MPU6000_CS_PIN PA4 + +//BMI270 +#define USE_IMU_BMI270 +#define IMU_BMI270_ALIGN CW180_DEG +#define BMI270_SPI_BUS BUS_SPI1 +#define BMI270_CS_PIN PA4 + + +/* + * Magnetometer + */ +#define USE_MAG +#define MAG_I2C_BUS DEFAULT_I2C_BUS + +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +/* + * Barometer + */ +#define USE_BARO +#define BARO_I2C_BUS DEFAULT_I2C_BUS +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_BMP085 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +/* + * Serial ports + */ +#define USE_VCP +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define SERIAL_PORT_COUNT 6 + +/* + * ADC + */ +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream4 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC2 + +#define VBAT_ADC_CHANNEL ADC_CHN_3 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_1 + +#define CURRENT_METER_SCALE 320 + +/* + * OSD + */ +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 + +/* + * Blackbox + */ +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +/* + * LED Strip + */ +#define USE_LED_STRIP +#define WS2811_PIN PB6 + +/* + * Other configs + */ +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT ) +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// #define USE_RANGEFINDER +// #define USE_RANGEFINDER_MSP +// #define USE_OPFLOW +// #define USE_OPFLOW_MSP + +// #define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS +// #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +// #define PITOT_I2C_BUS DEFAULT_I2C_BUS + +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define MAX_PWM_OUTPUT_PORTS 6 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) diff --git a/src/main/target/AXISFLYINGF7PRO/target.h b/src/main/target/AXISFLYINGF7PRO/target.h index 4a4b95657e..39c94e47e9 100644 --- a/src/main/target/AXISFLYINGF7PRO/target.h +++ b/src/main/target/AXISFLYINGF7PRO/target.h @@ -134,11 +134,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 4ebde46875..1ff901b870 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -37,14 +37,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 13e7dd3265..05f3587298 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -112,14 +112,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/BETAFPVF435/target.h b/src/main/target/BETAFPVF435/target.h index c8e17bd7c4..2376afbcfe 100644 --- a/src/main/target/BETAFPVF435/target.h +++ b/src/main/target/BETAFPVF435/target.h @@ -116,14 +116,12 @@ #define MAX7456_CS_PIN PA15 #endif -#if 0 // I2C #define USE_I2C #define USE_I2C_DEVICE_2 #define I2C2_SCL PB10 // SCL pad #define I2C2_SDA PB11 // SDA pad #define USE_I2C_PULLUP -#endif #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 @@ -198,4 +196,4 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE //#define USE_DSHOT //#define USE_ESC_SENSOR -#define USE_ESCSERIAL \ No newline at end of file +#define USE_ESCSERIAL diff --git a/src/main/target/BETAFPVF722/target.h b/src/main/target/BETAFPVF722/target.h index 9d1258385d..cab6d2ff14 100755 --- a/src/main/target/BETAFPVF722/target.h +++ b/src/main/target/BETAFPVF722/target.h @@ -70,12 +70,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 diff --git a/src/main/target/BLACKPILL_F411/target.h b/src/main/target/BLACKPILL_F411/target.h index f425de423e..b6f937bc9b 100644 --- a/src/main/target/BLACKPILL_F411/target.h +++ b/src/main/target/BLACKPILL_F411/target.h @@ -113,13 +113,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_AK8975 +#define USE_MAG_ALL #define PITOT_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 689dba9282..e62120bdc8 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -41,12 +41,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index c76b91b499..6898448809 100755 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -42,12 +42,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C3 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C3 diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h index 449be6a715..6a9a5494ff 100644 --- a/src/main/target/DALRCF405/target.h +++ b/src/main/target/DALRCF405/target.h @@ -82,13 +82,7 @@ #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h index b7d2b4a0d1..5c6dc15a3d 100644 --- a/src/main/target/DALRCF722DUAL/target.h +++ b/src/main/target/DALRCF722DUAL/target.h @@ -63,19 +63,12 @@ #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP280 -#define USE_BARO_MS5611 -#define USE_BARO_BMP085 +#define USE_BARO_ALL #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index f72f78f20e..21f91c1d49 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -41,12 +41,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h index ce8831f4a9..a2f69ae49d 100644 --- a/src/main/target/FIREWORKSV2/target.h +++ b/src/main/target/FIREWORKSV2/target.h @@ -87,11 +87,7 @@ #else #define MAG_I2C_BUS BUS_I2C2 #endif -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #if defined(OMNIBUSF4V6) #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 741a8aded8..10e325202e 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -47,13 +47,7 @@ // *************** Compass ***************************** #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_MPU9250 -#define USE_MAG_MAG3110 -#define USE_MAG_HMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_QMC5883 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // *************** Temperature sensor ***************** #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/FLYCOLORF7MINI/target.h b/src/main/target/FLYCOLORF7MINI/target.h index ece951b3c4..938913d648 100644 --- a/src/main/target/FLYCOLORF7MINI/target.h +++ b/src/main/target/FLYCOLORF7MINI/target.h @@ -58,13 +58,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 diff --git a/src/main/target/FLYWOOF405PRO/target.h b/src/main/target/FLYWOOF405PRO/target.h index 39a3339c10..14b5e9e03c 100644 --- a/src/main/target/FLYWOOF405PRO/target.h +++ b/src/main/target/FLYWOOF405PRO/target.h @@ -68,13 +68,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04_I2C diff --git a/src/main/target/FLYWOOF411/target.h b/src/main/target/FLYWOOF411/target.h index 286b986373..ed263d5ee3 100644 --- a/src/main/target/FLYWOOF411/target.h +++ b/src/main/target/FLYWOOF411/target.h @@ -70,12 +70,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // *************** SPI OSD ***************************** #define USE_MAX7456 diff --git a/src/main/target/FLYWOOF745/target.h b/src/main/target/FLYWOOF745/target.h index e0e012aa60..3b57ccc4fa 100644 --- a/src/main/target/FLYWOOF745/target.h +++ b/src/main/target/FLYWOOF745/target.h @@ -143,12 +143,7 @@ #define BARO_I2C_BUS BUS_I2C1 #define USE_MAG -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_MAG3110 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define USE_ADC #define ADC_CHANNEL_1_PIN PC2 diff --git a/src/main/target/FLYWOOF7DUAL/target.h b/src/main/target/FLYWOOF7DUAL/target.h index 58630efe7e..1a23650521 100644 --- a/src/main/target/FLYWOOF7DUAL/target.h +++ b/src/main/target/FLYWOOF7DUAL/target.h @@ -109,11 +109,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL /*** ADC ***/ #define USE_ADC diff --git a/src/main/target/FOXEERF405/target.h b/src/main/target/FOXEERF405/target.h index 8ee53eeb70..7f97e832b9 100644 --- a/src/main/target/FOXEERF405/target.h +++ b/src/main/target/FOXEERF405/target.h @@ -104,11 +104,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL /*** ADC ***/ #define USE_ADC diff --git a/src/main/target/FOXEERF722DUAL/target.h b/src/main/target/FOXEERF722DUAL/target.h index 7698fe1abf..250a88a307 100644 --- a/src/main/target/FOXEERF722DUAL/target.h +++ b/src/main/target/FOXEERF722DUAL/target.h @@ -112,11 +112,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL /*** ADC ***/ #define USE_ADC diff --git a/src/main/target/FOXEERF722V4/target.h b/src/main/target/FOXEERF722V4/target.h index 3c6ab0b2a7..4d92f85cde 100644 --- a/src/main/target/FOXEERF722V4/target.h +++ b/src/main/target/FOXEERF722V4/target.h @@ -124,11 +124,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define BNO055_I2C_BUS BUS_I2C1 diff --git a/src/main/target/FOXEERF745AIO/target.h b/src/main/target/FOXEERF745AIO/target.h index 60666b7611..860ea18d86 100644 --- a/src/main/target/FOXEERF745AIO/target.h +++ b/src/main/target/FOXEERF745AIO/target.h @@ -112,8 +112,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 +#define USE_MAG_ALL /*** ADC ***/ #define USE_ADC diff --git a/src/main/target/FOXEERH743/target.h b/src/main/target/FOXEERH743/target.h index ffb582ea9a..f2e2f83eb4 100644 --- a/src/main/target/FOXEERH743/target.h +++ b/src/main/target/FOXEERH743/target.h @@ -83,13 +83,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_VCM5883 +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 @@ -162,4 +156,4 @@ #define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR diff --git a/src/main/target/FRSKYPILOT/target.h b/src/main/target/FRSKYPILOT/target.h index d8100f2610..19944ac24a 100644 --- a/src/main/target/FRSKYPILOT/target.h +++ b/src/main/target/FRSKYPILOT/target.h @@ -103,12 +103,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C3 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C3 #define PITOT_I2C_BUS BUS_I2C3 diff --git a/src/main/target/FRSKY_ROVERF7/target.h b/src/main/target/FRSKY_ROVERF7/target.h index ea1c62e0d1..ff9672c742 100755 --- a/src/main/target/FRSKY_ROVERF7/target.h +++ b/src/main/target/FRSKY_ROVERF7/target.h @@ -53,12 +53,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h index 46eec2422f..79c26d178a 100644 --- a/src/main/target/FURYF4OSD/target.h +++ b/src/main/target/FURYF4OSD/target.h @@ -119,14 +119,7 @@ #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/GEPRCF405/target.h b/src/main/target/GEPRCF405/target.h index 3a628ffe59..7f641643e8 100644 --- a/src/main/target/GEPRCF405/target.h +++ b/src/main/target/GEPRCF405/target.h @@ -70,12 +70,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/GEPRCF722/target.h b/src/main/target/GEPRCF722/target.h index bc08477c45..df7cc6d3ec 100644 --- a/src/main/target/GEPRCF722/target.h +++ b/src/main/target/GEPRCF722/target.h @@ -67,12 +67,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/GEPRCF722_BT_HD/target.h b/src/main/target/GEPRCF722_BT_HD/target.h index ca32c210c6..5494ed8300 100644 --- a/src/main/target/GEPRCF722_BT_HD/target.h +++ b/src/main/target/GEPRCF722_BT_HD/target.h @@ -65,12 +65,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/GEPRC_F722_AIO/target.h b/src/main/target/GEPRC_F722_AIO/target.h index ce2ebde6a5..f7ed46bd46 100644 --- a/src/main/target/GEPRC_F722_AIO/target.h +++ b/src/main/target/GEPRC_F722_AIO/target.h @@ -65,12 +65,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/HAKRCF405D/target.h b/src/main/target/HAKRCF405D/target.h index 002fd191c3..d47ec61dc9 100644 --- a/src/main/target/HAKRCF405D/target.h +++ b/src/main/target/HAKRCF405D/target.h @@ -127,11 +127,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL /*** ADC ***/ #define USE_ADC diff --git a/src/main/target/HAKRCF405V2/target.h b/src/main/target/HAKRCF405V2/target.h index 3e7b1420d1..85cc164230 100644 --- a/src/main/target/HAKRCF405V2/target.h +++ b/src/main/target/HAKRCF405V2/target.h @@ -134,11 +134,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL /*** ADC ***/ #define USE_ADC diff --git a/src/main/target/HAKRCF411D/target.h b/src/main/target/HAKRCF411D/target.h index 96ea1a7c77..5b3a479ed1 100644 --- a/src/main/target/HAKRCF411D/target.h +++ b/src/main/target/HAKRCF411D/target.h @@ -113,11 +113,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/HAKRCF722V2/target.h b/src/main/target/HAKRCF722V2/target.h index 809f6ddb10..65325ebe41 100644 --- a/src/main/target/HAKRCF722V2/target.h +++ b/src/main/target/HAKRCF722V2/target.h @@ -137,11 +137,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/HAKRCKD722/target.h b/src/main/target/HAKRCKD722/target.h index 5b4869951d..0058b75d15 100644 --- a/src/main/target/HAKRCKD722/target.h +++ b/src/main/target/HAKRCKD722/target.h @@ -102,12 +102,7 @@ // Mag #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL /*** Onboard Flash ***/ #define USE_SPI_DEVICE_3 diff --git a/src/main/target/HGLRCF722/target.h b/src/main/target/HGLRCF722/target.h index 3cf2986f75..348f30d241 100644 --- a/src/main/target/HGLRCF722/target.h +++ b/src/main/target/HGLRCF722/target.h @@ -72,13 +72,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/IFLIGHTF4_SUCCEXD/target.h b/src/main/target/IFLIGHTF4_SUCCEXD/target.h index 2d9d7bb535..6ba059bb63 100644 --- a/src/main/target/IFLIGHTF4_SUCCEXD/target.h +++ b/src/main/target/IFLIGHTF4_SUCCEXD/target.h @@ -97,13 +97,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_AK8975 +#define USE_MAG_ALL #define PITOT_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/IFLIGHTF4_TWING/target.h b/src/main/target/IFLIGHTF4_TWING/target.h index 77dd3992e0..e0afa9aab5 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.h +++ b/src/main/target/IFLIGHTF4_TWING/target.h @@ -59,12 +59,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h index 7bfbb0bbc5..1240089c76 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.h +++ b/src/main/target/IFLIGHTF7_TWING/target.h @@ -56,17 +56,12 @@ #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 -#define USE_BARO_BMP280 -#define USE_BARO_DPS310 +#define USE_BARO_ALL + #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/IFLIGHT_BLITZ_F722/target.h b/src/main/target/IFLIGHT_BLITZ_F722/target.h index 370cea2267..0ced96ad5f 100644 --- a/src/main/target/IFLIGHT_BLITZ_F722/target.h +++ b/src/main/target/IFLIGHT_BLITZ_F722/target.h @@ -56,13 +56,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.h b/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.h index c94da102b1..69ea37e535 100644 --- a/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.h +++ b/src/main/target/IFLIGHT_BLITZ_F7_AIO/target.h @@ -85,12 +85,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.h b/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.h index 03e7d0e2f4..a5361a5190 100644 --- a/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.h +++ b/src/main/target/IFLIGHT_BLITZ_F7_PRO/target.h @@ -62,12 +62,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 diff --git a/src/main/target/IFLIGHT_H743_AIO_V2/target.h b/src/main/target/IFLIGHT_H743_AIO_V2/target.h index 60a235e08c..c390037668 100644 --- a/src/main/target/IFLIGHT_H743_AIO_V2/target.h +++ b/src/main/target/IFLIGHT_H743_AIO_V2/target.h @@ -66,13 +66,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 diff --git a/src/main/target/IFLIGHT_JBF7PRO/target.h b/src/main/target/IFLIGHT_JBF7PRO/target.h index 8c2dfd5a76..570e1750e4 100644 --- a/src/main/target/IFLIGHT_JBF7PRO/target.h +++ b/src/main/target/IFLIGHT_JBF7PRO/target.h @@ -59,12 +59,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/JHEH7AIO/target.h b/src/main/target/JHEH7AIO/target.h index 2290b828de..00ecf856e2 100644 --- a/src/main/target/JHEH7AIO/target.h +++ b/src/main/target/JHEH7AIO/target.h @@ -55,13 +55,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 @@ -166,4 +160,4 @@ #define MAX_PWM_OUTPUT_PORTS 4 #define USE_DSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index bf68496933..1895b4b7e7 100644 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -62,12 +62,7 @@ # define USE_MAG # define MAG_I2C_BUS BUS_I2C1 -# define USE_MAG_HMC5883 -# define USE_MAG_QMC5883 -# define USE_MAG_MAG3110 -# define USE_MAG_IST8310 -# define USE_MAG_IST8308 -# define USE_MAG_LIS3MDL +# define USE_MAG_ALL # define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index 064032b7f4..f3f4edfed4 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -135,13 +135,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_MAG3110 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_LIS3MDL -#define USE_MAG_MLX90393 +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/KAKUTEF7MINIV3/target.h b/src/main/target/KAKUTEF7MINIV3/target.h index c3d8983725..997cad1a97 100644 --- a/src/main/target/KAKUTEF7MINIV3/target.h +++ b/src/main/target/KAKUTEF7MINIV3/target.h @@ -116,11 +116,7 @@ */ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL /* * ADC diff --git a/src/main/target/KAKUTEH7/target.h b/src/main/target/KAKUTEH7/target.h index 388515fbbb..8f88563c02 100644 --- a/src/main/target/KAKUTEH7/target.h +++ b/src/main/target/KAKUTEH7/target.h @@ -132,13 +132,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_VCM5883 +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/KAKUTEH7WING/target.h b/src/main/target/KAKUTEH7WING/target.h index 72cf6b6806..79068b8cd8 100644 --- a/src/main/target/KAKUTEH7WING/target.h +++ b/src/main/target/KAKUTEH7WING/target.h @@ -107,13 +107,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_VCM5883 +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index 9af00d2f72..ea5ee6a8e0 100755 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -36,12 +36,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h index b6b95cb12b..1b21fe60e3 100644 --- a/src/main/target/MAMBAF405US/target.h +++ b/src/main/target/MAMBAF405US/target.h @@ -74,14 +74,7 @@ //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS - -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // ******* SERIAL ******** #define USE_VCP diff --git a/src/main/target/MAMBAF405_2022A/target.h b/src/main/target/MAMBAF405_2022A/target.h index c4c20e127f..5a24771288 100644 --- a/src/main/target/MAMBAF405_2022A/target.h +++ b/src/main/target/MAMBAF405_2022A/target.h @@ -83,14 +83,7 @@ //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS - -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // ******* SERIAL ******** #define USE_VCP @@ -200,4 +193,4 @@ #define USE_PINIO #define USE_PINIOBOX #define PINIO1_PIN PC2 -#define PINIO2_PIN PC5 \ No newline at end of file +#define PINIO2_PIN PC5 diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h index c536cc6017..20e907fa87 100644 --- a/src/main/target/MAMBAF722/target.h +++ b/src/main/target/MAMBAF722/target.h @@ -74,12 +74,7 @@ //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS - -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // ******* SERIAL ******** #define USE_VCP diff --git a/src/main/target/MAMBAF722_2022A/target.h b/src/main/target/MAMBAF722_2022A/target.h index 0752bb368b..4ab5a75bbc 100644 --- a/src/main/target/MAMBAF722_2022A/target.h +++ b/src/main/target/MAMBAF722_2022A/target.h @@ -86,11 +86,7 @@ #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // ******* SERIAL ******** #define USE_VCP @@ -197,4 +193,4 @@ #define USE_PINIO #define USE_PINIOBOX #define PINIO1_PIN PC0 // VTX power switcher -#define PINIO2_PIN PC2 // WiFi Switcher \ No newline at end of file +#define PINIO2_PIN PC2 // WiFi Switcher diff --git a/src/main/target/MAMBAF722_WING/target.h b/src/main/target/MAMBAF722_WING/target.h index a877a74e65..654b9afdfb 100644 --- a/src/main/target/MAMBAF722_WING/target.h +++ b/src/main/target/MAMBAF722_WING/target.h @@ -55,12 +55,7 @@ //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS - -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // ******* SERIAL ******** #define USE_VCP @@ -168,4 +163,4 @@ #define USE_PINIO #define USE_PINIOBOX #define PINIO1_PIN PC0 -#define PINIO2_PIN PC2 \ No newline at end of file +#define PINIO2_PIN PC2 diff --git a/src/main/target/MAMBAF722_X8/target.h b/src/main/target/MAMBAF722_X8/target.h index 2c8c84e9d1..1a89d3bf31 100644 --- a/src/main/target/MAMBAF722_X8/target.h +++ b/src/main/target/MAMBAF722_X8/target.h @@ -53,12 +53,7 @@ //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS - -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // ******* SERIAL ******** #define USE_VCP diff --git a/src/main/target/MAMBAH743/target.h b/src/main/target/MAMBAH743/target.h index 27a65ded29..18070e1775 100644 --- a/src/main/target/MAMBAH743/target.h +++ b/src/main/target/MAMBAH743/target.h @@ -124,13 +124,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_VCM5883 +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 @@ -238,4 +232,4 @@ #define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index fa2af63384..667cd2b3fa 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -147,14 +147,7 @@ #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MATEKF405CAN/target.h b/src/main/target/MATEKF405CAN/target.h index 81aa91ce99..d12b5e3e1a 100644 --- a/src/main/target/MATEKF405CAN/target.h +++ b/src/main/target/MATEKF405CAN/target.h @@ -55,13 +55,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 4c0b4b8b39..38c8f56f5d 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -68,13 +68,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define USE_RANGEFINDER #define USE_RANGEFINDER_US42 diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index acaed9daf2..fc7c1ad0ce 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -92,12 +92,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index 0bf0aab198..6be5949419 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -120,11 +120,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index abb4688457..db5652e0ca 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -103,13 +103,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_AK8975 +#define USE_MAG_ALL #define PITOT_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF411TE/target.h b/src/main/target/MATEKF411TE/target.h index 7ecb2d1cb6..fd2ea8fed4 100644 --- a/src/main/target/MATEKF411TE/target.h +++ b/src/main/target/MATEKF411TE/target.h @@ -88,13 +88,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_AK8975 +#define USE_MAG_ALL #define PITOT_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index 49cc5fac09..1908c6a5b4 100644 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -56,12 +56,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 @@ -152,4 +147,4 @@ #define MAX_PWM_OUTPUT_PORTS 7 #define USE_DSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h index ce780e7c93..ae03407447 100755 --- a/src/main/target/MATEKF722PX/target.h +++ b/src/main/target/MATEKF722PX/target.h @@ -54,13 +54,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h index 04dc0d8123..0679d3f531 100644 --- a/src/main/target/MATEKF722SE/target.h +++ b/src/main/target/MATEKF722SE/target.h @@ -68,13 +68,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h index e40fe455be..8a726d56b3 100644 --- a/src/main/target/MATEKF765/target.h +++ b/src/main/target/MATEKF765/target.h @@ -87,12 +87,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 diff --git a/src/main/target/MATEKH743/CMakeLists.txt b/src/main/target/MATEKH743/CMakeLists.txt index 96a65ca5a4..dcc5019a9d 100644 --- a/src/main/target/MATEKH743/CMakeLists.txt +++ b/src/main/target/MATEKH743/CMakeLists.txt @@ -1 +1,2 @@ target_stm32h743xi(MATEKH743) +target_stm32h743xi(MATEKH743HD) diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h index 37742a88fc..f2e21b0ee8 100644 --- a/src/main/target/MATEKH743/target.h +++ b/src/main/target/MATEKH743/target.h @@ -19,7 +19,12 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "H743" -#define USBD_PRODUCT_STRING "MATEKH743" + +#if defined(MATEKH743HD) + #define USBD_PRODUCT_STRING "MATEKH743HD" +#else + #define USBD_PRODUCT_STRING "MATEKH743" +#endif #define USE_TARGET_CONFIG @@ -69,14 +74,16 @@ #define ICM42605_CS_PIN PC13 // *************** SPI2 OSD *********************** -#define USE_SPI_DEVICE_2 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 + #define USE_SPI_DEVICE_2 + #define SPI2_SCK_PIN PB13 + #define SPI2_MISO_PIN PB14 + #define SPI2_MOSI_PIN PB15 -#define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI2 -#define MAX7456_CS_PIN PB12 +#if defined(MATEKH743) + #define USE_MAX7456 + #define MAX7456_SPI_BUS BUS_SPI2 + #define MAX7456_CS_PIN PB12 +#endif // *************** SPI3 SPARE for external RM3100 *********** #define USE_SPI_DEVICE_3 @@ -108,12 +115,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 @@ -159,7 +161,7 @@ #define SERIAL_PORT_COUNT 9 #define DEFAULT_RX_TYPE RX_TYPE_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_PROVIDER SERIALRX_CRSF #define SERIALRX_UART SERIAL_PORT_USART6 // *************** SDIO SD BLACKBOX******************* diff --git a/src/main/target/NEUTRONRCF435MINI/target.h b/src/main/target/NEUTRONRCF435MINI/target.h index 27fc03ae5e..11083bd61e 100644 --- a/src/main/target/NEUTRONRCF435MINI/target.h +++ b/src/main/target/NEUTRONRCF435MINI/target.h @@ -95,8 +95,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 +#define USE_MAG_ALL #define DEFAULT_I2C_BUS BUS_I2C2 // temperature sensors diff --git a/src/main/target/NEUTRONRCF435SE/target.h b/src/main/target/NEUTRONRCF435SE/target.h index 5d00a09911..68efdf35ac 100644 --- a/src/main/target/NEUTRONRCF435SE/target.h +++ b/src/main/target/NEUTRONRCF435SE/target.h @@ -93,8 +93,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 +#define USE_MAG_ALL #define DEFAULT_I2C_BUS BUS_I2C2 // temperature sensors @@ -196,4 +195,4 @@ #define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR diff --git a/src/main/target/NEUTRONRCF435WING/target.h b/src/main/target/NEUTRONRCF435WING/target.h index 4f56d86778..2438d01ddf 100644 --- a/src/main/target/NEUTRONRCF435WING/target.h +++ b/src/main/target/NEUTRONRCF435WING/target.h @@ -95,8 +95,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 +#define USE_MAG_ALL #define DEFAULT_I2C_BUS BUS_I2C2 // temperature sensors diff --git a/src/main/target/NEUTRONRCH7BT/target.h b/src/main/target/NEUTRONRCH7BT/target.h index 78a1949e04..9f4ffd118d 100644 --- a/src/main/target/NEUTRONRCH7BT/target.h +++ b/src/main/target/NEUTRONRCH7BT/target.h @@ -87,12 +87,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 2c0bd22801..2c5a27afbb 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -91,13 +91,7 @@ #define USE_MAG #define MAG_I2C_BUS I2C_EXT_BUS -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_AK8975 +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS I2C_EXT_BUS diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index 6a5a9d6ea9..a5f1bfc6b8 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -141,12 +141,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h index 367e8b22a7..6287e83473 100644 --- a/src/main/target/OMNIBUSF7NXT/target.h +++ b/src/main/target/OMNIBUSF7NXT/target.h @@ -52,11 +52,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/PIXRACER/target.h b/src/main/target/PIXRACER/target.h index 738fc1ad91..bc3bf3a4d9 100755 --- a/src/main/target/PIXRACER/target.h +++ b/src/main/target/PIXRACER/target.h @@ -56,13 +56,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_MPU9250 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 29619624df..713c1a8ce3 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -41,12 +41,7 @@ #define USE_DUAL_MAG #define MAG_I2C_BUS_EXT BUS_I2C2 #define MAG_I2C_BUS_INT BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/RUSH_BLADE_F7/target.h b/src/main/target/RUSH_BLADE_F7/target.h index ba5915892b..2e68f1a392 100644 --- a/src/main/target/RUSH_BLADE_F7/target.h +++ b/src/main/target/RUSH_BLADE_F7/target.h @@ -75,13 +75,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // *************** Flash **************************** diff --git a/src/main/target/SAGEATF4/target.h b/src/main/target/SAGEATF4/target.h index 3ec00a37b3..afa89aebdd 100644 --- a/src/main/target/SAGEATF4/target.h +++ b/src/main/target/SAGEATF4/target.h @@ -98,8 +98,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C3 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 +#define USE_MAG_ALL // temperature sensors //#define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/SDMODELH7V1/CMakeLists.txt b/src/main/target/SDMODELH7V1/CMakeLists.txt new file mode 100644 index 0000000000..d8e1f7c3ab --- /dev/null +++ b/src/main/target/SDMODELH7V1/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32h743xi(SDMODELH7V1) diff --git a/src/main/target/SDMODELH7V1/config.c b/src/main/target/SDMODELH7V1/config.c new file mode 100644 index 0000000000..54f980fd8b --- /dev/null +++ b/src/main/target/SDMODELH7V1/config.c @@ -0,0 +1,40 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "fc/fc_msp_box.h" +#include "fc/config.h" + +#include "io/piniobox.h" +#include "drivers/serial.h" +#include "io/serial.h" + +void targetConfiguration(void) +{ + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_115200; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART2)].msp_baudrateIndex = BAUD_115200; + +} diff --git a/src/main/target/SDMODELH7V1/target.c b/src/main/target/SDMODELH7V1/target.c new file mode 100644 index 0000000000..8851f95286 --- /dev/null +++ b/src/main/target/SDMODELH7V1/target.c @@ -0,0 +1,44 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/pinio.h" +#include "drivers/sensor.h" + +timerHardware_t timerHardware[] = { + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_OUTPUT_AUTO, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_OUTPUT_AUTO, 0, 1), // S2 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_OUTPUT_AUTO, 0, 2), // S3 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_OUTPUT_AUTO, 0, 3), // S4 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_OUTPUT_AUTO, 0, 4), // S5 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_OUTPUT_AUTO, 0, 5), // S6 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_OUTPUT_AUTO, 0, 6), // S7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_OUTPUT_AUTO, 0, 7), // S8 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 9), // LED_2812 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SDMODELH7V1/target.h b/src/main/target/SDMODELH7V1/target.h new file mode 100644 index 0000000000..cc4d39cf6c --- /dev/null +++ b/src/main/target/SDMODELH7V1/target.h @@ -0,0 +1,182 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "SMH71" +#define USBD_PRODUCT_STRING "SDMODELH7V1" + +#define USE_TARGET_CONFIG + +#define LED0 PC2 + +#define BEEPER PC13 +#define BEEPER_INVERTED + +// *************** IMU generic *********************** + + + + +// *************** SPI1 **************** +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_FLASHFS +#define USE_FLASH_W25N01G +#define W25N01G_SPI_BUS BUS_SPI1 +#define W25N01G_CS_PIN PA4 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + + + +// *************** SPI2 *********************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +// *************** SPI4 *************** +#define USE_SPI_DEVICE_4 +#define SPI4_SCK_PIN PE2 +#define SPI4_MISO_PIN PE5 +#define SPI4_MOSI_PIN PE6 + +//MPU6000 +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG +#define MPU6000_SPI_BUS BUS_SPI4 +#define MPU6000_CS_PIN PE4 + +//BMI270 +#define USE_IMU_BMI270 +#define BMI270_SPI_BUS BUS_SPI4 +#define BMI270_CS_PIN PE4 + +#define IMU_BMI270_ALIGN CW0_DEG + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN PB12 + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C1 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 +#define USE_BARO_SPL06 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_VCM5883 + +#define TEMPERATURE_I2C_BUS BUS_I2C1 +#define PITOT_I2C_BUS BUS_I2C1 + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PD5 +#define UART2_RX_PIN PD6 + +#define USE_UART3 +#define UART3_TX_PIN PD8 +#define UART3_RX_PIN PD9 + +#define USE_UART4 +#define UART4_TX_PIN PD1 +#define UART4_RX_PIN PD0 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_UART7 +#define UART7_RX_PIN PE7 + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART1 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 + +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC5 +#define ADC_CHANNEL_3_PIN PC1 + +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 +#define RSSI_ADC_CHANNEL ADC_CHN_2 + +// *************** PINIO *************************** +#define USE_PINIO +#define USE_PINIOBOX + +#define PINIO1_PIN PE13 +#define PINIO2_PIN PB11 +#define PINIO1_FLAGS PINIO_FLAGS_INVERTED +#define PINIO2_FLAGS PINIO_FLAGS_INVERTED + + +// *************** LEDSTRIP ************************ +#define USE_LED_STRIP +#define WS2811_PIN PD12 + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) +#define CURRENT_METER_SCALE 250 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 8 +#define USE_DSHOT +#define USE_ESC_SENSOR + diff --git a/src/main/target/SKYSTARSF405HD/target.h b/src/main/target/SKYSTARSF405HD/target.h index ac88a72084..c5ab2d49fe 100644 --- a/src/main/target/SKYSTARSF405HD/target.h +++ b/src/main/target/SKYSTARSF405HD/target.h @@ -45,6 +45,11 @@ #define BMI270_SPI_BUS BUS_SPI1 #define BMI270_CS_PIN PA4 +#define USE_IMU_ICM42605 +#define IMU_ICM42605_ALIGN CW180_DEG_FLIP +#define ICM42605_SPI_BUS BUS_SPI1 +#define ICM42605_CS_PIN PA4 + // *************** M25P256 flash ******************** #define USE_FLASHFS #define USE_FLASH_M25P16 @@ -117,14 +122,7 @@ #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/SKYSTARSF722HD/target.h b/src/main/target/SKYSTARSF722HD/target.h index 9c2521380b..c24b849182 100644 --- a/src/main/target/SKYSTARSF722HD/target.h +++ b/src/main/target/SKYSTARSF722HD/target.h @@ -125,14 +125,7 @@ #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/SKYSTARSH743HD/target.h b/src/main/target/SKYSTARSH743HD/target.h index d0f9360517..6e830664c5 100644 --- a/src/main/target/SKYSTARSH743HD/target.h +++ b/src/main/target/SKYSTARSH743HD/target.h @@ -71,12 +71,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 @@ -198,4 +193,4 @@ #define MAX_PWM_OUTPUT_PORTS 10 #define USE_DSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index 76d6e93099..78f80cd609 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -36,15 +36,10 @@ #define MPU9250_CS_PIN PC4 #define USE_MAG -#define USE_MAG_MPU9250 #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL + +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h index ff7fc7189a..310f6e9b45 100644 --- a/src/main/target/SPEEDYBEEF4/target.h +++ b/src/main/target/SPEEDYBEEF4/target.h @@ -126,11 +126,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL /*** ADC ***/ #define USE_ADC diff --git a/src/main/target/SPEEDYBEEF405MINI/target.h b/src/main/target/SPEEDYBEEF405MINI/target.h index dd5b9f4cfb..1f25899ed9 100644 --- a/src/main/target/SPEEDYBEEF405MINI/target.h +++ b/src/main/target/SPEEDYBEEF405MINI/target.h @@ -107,13 +107,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 @@ -183,4 +177,4 @@ #define USE_DSHOT #define USE_SERIALSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR diff --git a/src/main/target/SPEEDYBEEF405V3/target.h b/src/main/target/SPEEDYBEEF405V3/target.h index 99711f82de..5553fe314f 100644 --- a/src/main/target/SPEEDYBEEF405V3/target.h +++ b/src/main/target/SPEEDYBEEF405V3/target.h @@ -100,12 +100,7 @@ // Mag #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // *************** Internal SD card ************************** #define USE_SPI_DEVICE_2 diff --git a/src/main/target/SPEEDYBEEF405V4/target.h b/src/main/target/SPEEDYBEEF405V4/target.h index 8f333db3c2..1c5c847597 100644 --- a/src/main/target/SPEEDYBEEF405V4/target.h +++ b/src/main/target/SPEEDYBEEF405V4/target.h @@ -106,14 +106,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_VCM5883 +#define USE_MAG_ALL #define USE_RANGEFINDER #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/SPEEDYBEEF405WING/target.h b/src/main/target/SPEEDYBEEF405WING/target.h index ad58668268..e384ee01ff 100644 --- a/src/main/target/SPEEDYBEEF405WING/target.h +++ b/src/main/target/SPEEDYBEEF405WING/target.h @@ -115,14 +115,7 @@ //Mag #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8963 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_VCM5883 +#define USE_MAG_ALL #define RANGEFINDER_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 @@ -175,4 +168,4 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define MAX_PWM_OUTPUT_PORTS 11 \ No newline at end of file +#define MAX_PWM_OUTPUT_PORTS 11 diff --git a/src/main/target/SPEEDYBEEF7/target.h b/src/main/target/SPEEDYBEEF7/target.h index 7f5ed75141..032f7479b5 100644 --- a/src/main/target/SPEEDYBEEF7/target.h +++ b/src/main/target/SPEEDYBEEF7/target.h @@ -109,12 +109,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 diff --git a/src/main/target/SPEEDYBEEF745AIO/target.h b/src/main/target/SPEEDYBEEF745AIO/target.h index 1cb7b7dbc1..3e82678f55 100644 --- a/src/main/target/SPEEDYBEEF745AIO/target.h +++ b/src/main/target/SPEEDYBEEF745AIO/target.h @@ -100,12 +100,7 @@ // Mag #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // *************** SPI3 SD BLACKBOX******************* #define USE_SPI_DEVICE_1 diff --git a/src/main/target/SPEEDYBEEF7MINI/target.h b/src/main/target/SPEEDYBEEF7MINI/target.h index 0dd3a6e7a8..d267ade336 100644 --- a/src/main/target/SPEEDYBEEF7MINI/target.h +++ b/src/main/target/SPEEDYBEEF7MINI/target.h @@ -70,13 +70,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/SPEEDYBEEF7V2/target.h b/src/main/target/SPEEDYBEEF7V2/target.h index 8b53b53b96..9dfcdf232b 100644 --- a/src/main/target/SPEEDYBEEF7V2/target.h +++ b/src/main/target/SPEEDYBEEF7V2/target.h @@ -95,9 +95,7 @@ // Mag #define USE_MAG -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define MAG_I2C_BUS BUS_I2C2 // *************** Flash ************************** diff --git a/src/main/target/SPEEDYBEEF7V3/target.h b/src/main/target/SPEEDYBEEF7V3/target.h index 7c1d565c37..87eeb7114f 100644 --- a/src/main/target/SPEEDYBEEF7V3/target.h +++ b/src/main/target/SPEEDYBEEF7V3/target.h @@ -100,12 +100,7 @@ // Mag #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // *************** Internal SD card ************************** diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index 6eb48977d1..8af6981e54 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -46,13 +46,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_MPU9250 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h index 20e1104720..11e1bc934d 100644 --- a/src/main/target/SPRACINGF7DUAL/target.h +++ b/src/main/target/SPRACINGF7DUAL/target.h @@ -57,12 +57,7 @@ #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/TMOTORF7/target.h b/src/main/target/TMOTORF7/target.h index 890e95b353..9e935d1dc6 100644 --- a/src/main/target/TMOTORF7/target.h +++ b/src/main/target/TMOTORF7/target.h @@ -58,15 +58,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL // *************** SPI2 OSD *********************** #define USE_SPI_DEVICE_2 diff --git a/src/main/target/TMOTORF7V2/target.h b/src/main/target/TMOTORF7V2/target.h index 5aaab2e390..525330e120 100644 --- a/src/main/target/TMOTORF7V2/target.h +++ b/src/main/target/TMOTORF7V2/target.h @@ -104,12 +104,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index ab9f07cd2c..6a7bb7d545 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -57,8 +57,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h index f494222f0f..bec1035858 100644 --- a/src/main/target/YUPIF7/target.h +++ b/src/main/target/YUPIF7/target.h @@ -36,8 +36,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 +#define USE_MAG_ALL #define TEMPERATURE_I2C_BUS BUS_I2C1 #define USE_BARO diff --git a/src/main/target/ZEEZF7/target.h b/src/main/target/ZEEZF7/target.h index 07e22f1d93..28395b274f 100755 --- a/src/main/target/ZEEZF7/target.h +++ b/src/main/target/ZEEZF7/target.h @@ -131,13 +131,7 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C3 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_AK8975 +#define USE_MAG_ALL #endif // *************** Flash **************************** diff --git a/src/main/target/common.h b/src/main/target/common.h old mode 100755 new mode 100644 index 713458002b..d2d135cfa2 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -188,6 +188,9 @@ #define USE_SERIALRX_SUMD #define USE_TELEMETRY_HOTT #define USE_HOTT_TEXTMODE -#else +#define USE_24CHANNELS +#define MAX_MIXER_PROFILE_COUNT 2 +#elif !defined(STM32F7) #define MAX_MIXER_PROFILE_COUNT 1 #endif +#define USE_EZ_TUNE diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index ec7cc83664..d3ccf280b1 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -42,10 +42,48 @@ extern uint8_t __config_end; // Enable MSP BARO & MAG drivers if BARO and MAG sensors are compiled in #if defined(USE_MAG) #define USE_MAG_MSP + +#if defined(USE_MAG_ALL) + +#define USE_MAG_HMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_LIS3MDL +#define USE_MAG_MAG3110 +#define USE_MAG_QMC5883 + +//#if (MCU_FLASH_SIZE > 512) +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 +#define USE_MAG_IST8308 +#define USE_MAG_MLX90393 + +#if defined(USE_IMU_MPU9250) +#define USE_MAG_MPU9250 #endif +#define USE_MAG_RM3100 +#define USE_MAG_VCM5883 +//#endif // MCU_FLASH_SIZE + +#endif // USE_MAG_ALL + +#endif // USE_MAG + #if defined(USE_BARO) #define USE_BARO_MSP + +#if defined(USE_BARO_ALL) +#define USE_BARO_BMP085 +#define USE_BARO_BMP280 +#define USE_BARO_BMP388 +#define USE_BARO_DPS310 +#define USE_BARO_LPS25H +#define USE_BARO_MS5607 +#define USE_BARO_MS5611 +//#define USE_BARO_SPI_BMP280 +#define USE_BARO_SPL06 +#endif + #endif #ifdef USE_ESC_SENSOR diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 6a8aa6f091..fd126c5c41 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -14,55 +14,55 @@ TEST(OSDTest, TestCentiNumber) //bool osdFormatCentiNumber(char *buff, int32_t centivalue, uint32_t scale, int maxDecimals, int maxScaledDecimals, int length); char buf[11] = "0123456789"; - osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7); + osdFormatCentiNumber(buf, 12345, 1, 2, 3, 7, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, " 123.45")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, 12345, 1, 2, 2, 6); + osdFormatCentiNumber(buf, 12345, 1, 2, 2, 6, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "123.45")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, 12345, 1, 2, 2, 5); + osdFormatCentiNumber(buf, 12345, 1, 2, 2, 5, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "123.4")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, 12345, 1, 2, 2, 4); + osdFormatCentiNumber(buf, 12345, 1, 2, 2, 4, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, " 123")); // this should be causing #8769 memset(buf, 0, 11); - osdFormatCentiNumber(buf, 12345, 1, 2, 2, 3); + osdFormatCentiNumber(buf, 12345, 1, 2, 2, 3, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "123")); std::cout << "'" << buf << "'" << std::endl; memset(buf, 0, 11); - osdFormatCentiNumber(buf, -12345, 1, 2, 2, 8); + osdFormatCentiNumber(buf, -12345, 1, 2, 2, 8, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, " -123.45")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, -12345, 1, 2, 2, 7); + osdFormatCentiNumber(buf, -12345, 1, 2, 2, 7, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "-123.45")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, -12345, 1, 2, 2, 6); + osdFormatCentiNumber(buf, -12345, 1, 2, 2, 6, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "-123.4")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, -12345, 1, 2, 2, 5); + osdFormatCentiNumber(buf, -12345, 1, 2, 2, 5, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, " -123")); memset(buf, 0, 11); - osdFormatCentiNumber(buf, -12345, 1, 2, 2, 4); + osdFormatCentiNumber(buf, -12345, 1, 2, 2, 4, false); std::cout << "'" << buf << "'" << std::endl; EXPECT_FALSE(strcmp(buf, "-123"));