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

Merge remote-tracking branch 'upstream/master' into RTH-Altitude-Override

Browse source
This commit is contained in:
breadoven 2021-04-10 13:44:02 +01:00
commit df8bccd973
223 changed files with 6996 additions and 2491 deletions
Download patch file Download diff file Expand all files Collapse all files

1
AUTHORS
View file

@ -7,6 +7,7 @@ Alberto García Hierro
Alex Gorbatchev Alex Gorbatchev
Alex Zaitsev Alex Zaitsev
Alexander Fedorov Alexander Fedorov
Alexander van Saase
Alexey Stankevich Alexey Stankevich
Andre Bernet Andre Bernet
Andreas Tacke Andreas Tacke

16
CMakeLists.txt
View file

@ -22,6 +22,14 @@ endif()
option(COMPILER_VERSION_CHECK "Ensure the compiler matches the expected version" ON) option(COMPILER_VERSION_CHECK "Ensure the compiler matches the expected version" ON)
include(GetGitRevisionDescription)
get_git_head_revision(GIT_REFSPEC GIT_SHA1)
string(SUBSTRING ${GIT_SHA1} 0 8 GIT_REV)
# Load settings related functions, so the tests can use them
include(main)
include(settings)
if(TOOLCHAIN STREQUAL none) if(TOOLCHAIN STREQUAL none)
add_subdirectory(src/test) add_subdirectory(src/test)
else() else()
@ -29,7 +37,7 @@ else()
include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake")
endif() endif()
project(INAV VERSION 2.7.0) project(INAV VERSION 3.0.0)
enable_language(ASM) enable_language(ASM)
@ -49,10 +57,6 @@ if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebI
set(IS_RELEASE_BUILD ON) set(IS_RELEASE_BUILD ON)
endif() endif()
include(GetGitRevisionDescription)
get_git_head_revision(GIT_REFSPEC GIT_SHA1)
string(SUBSTRING ${GIT_SHA1} 0 8 GIT_REV)
set(FIRMWARE_VERSION ${PROJECT_VERSION}) set(FIRMWARE_VERSION ${PROJECT_VERSION})
option(WARNINGS_AS_ERRORS "Make all warnings into errors") option(WARNINGS_AS_ERRORS "Make all warnings into errors")
@ -64,10 +68,8 @@ set(COMMON_COMPILE_DEFINITIONS
FC_VERSION_PATCH_LEVEL=${CMAKE_PROJECT_VERSION_PATCH} FC_VERSION_PATCH_LEVEL=${CMAKE_PROJECT_VERSION_PATCH}
) )
include(settings)
include(openocd) include(openocd)
include(svd) include(svd)
include(main)
include(stm32) include(stm32)
add_subdirectory(src) add_subdirectory(src)

2
README.md
View file

@ -39,7 +39,7 @@ Tool for Blackbox logs analysis is available [here](https://github.com/iNavFligh
### Telemetry screen for OpenTX ### Telemetry screen for OpenTX
Users of FrSky Taranis X9 and Q X7 can use INAV Lua Telemetry screen created by @teckel12 . Software and installation instruction are available here: [https://github.com/iNavFlight/LuaTelemetry](https://github.com/iNavFlight/LuaTelemetry) Users of OpenTX radios (Taranis, Horus, Jumper, Radiomaster, Nirvana) can use INAV OpenTX Telemetry Widget screen. Software and installation instruction are available here: [https://github.com/iNavFlight/OpenTX-Telemetry-Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget)
## Installation ## Installation

20
cmake/settings.cmake
View file

@ -4,6 +4,7 @@ set(SETTINGS_GENERATED_H "${SETTINGS_GENERATED}.h")
set(SETTINGS_FILE "${MAIN_SRC_DIR}/fc/settings.yaml") set(SETTINGS_FILE "${MAIN_SRC_DIR}/fc/settings.yaml")
set(SETTINGS_GENERATOR "${MAIN_UTILS_DIR}/settings.rb") set(SETTINGS_GENERATOR "${MAIN_UTILS_DIR}/settings.rb")
include(CMakeParseArguments)
function(enable_settings exe name) function(enable_settings exe name)
get_generated_files_dir(dir ${name}) get_generated_files_dir(dir ${name})
@ -15,11 +16,26 @@ function(enable_settings exe name)
list(APPEND cflags ${options}) list(APPEND cflags ${options})
list(APPEND cflags ${includes}) list(APPEND cflags ${includes})
list(APPEND cflags ${defs}) list(APPEND cflags ${defs})
cmake_parse_arguments(
args
# Boolean arguments
""
# Single value arguments
"OUTPUTS;SETTINGS_CXX"
# Multi-value arguments
""
# Start parsing after the known arguments
${ARGN}
)
set(output ${dir}/${SETTINGS_GENERATED_H} ${dir}/${SETTINGS_GENERATED_C})
add_custom_command( add_custom_command(
OUTPUT ${dir}/${SETTINGS_GENERATED_H} ${dir}/${SETTINGS_GENERATED_C} OUTPUT ${output}
COMMAND COMMAND
${CMAKE_COMMAND} -E env CFLAGS="${cflags}" TARGET=${name} PATH=$ENV{PATH} ${CMAKE_COMMAND} -E env CFLAGS="${cflags}" TARGET=${name} PATH=$ENV{PATH} SETTINGS_CXX=${args_SETTINGS_CXX}
${RUBY_EXECUTABLE} ${SETTINGS_GENERATOR} ${MAIN_DIR} ${SETTINGS_FILE} -o "${dir}" ${RUBY_EXECUTABLE} ${SETTINGS_GENERATOR} ${MAIN_DIR} ${SETTINGS_FILE} -o "${dir}"
DEPENDS ${SETTINGS_GENERATOR} ${SETTINGS_FILE} DEPENDS ${SETTINGS_GENERATOR} ${SETTINGS_FILE}
) )
set(${args_OUTPUTS} ${output} PARENT_SCOPE)
endfunction() endfunction()

12
docs/Programming Framework.md
View file

@ -71,6 +71,9 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 35 | TAN | 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` | | 35 | TAN | 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 | | 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 | | 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` |
| 39 | SET_HEADING_TARGET | Sets heading-hold target to `Operand A`, in degrees. Value wraps-around. |
| 40 | MOD | Divide `Operand A` by `Operand B` and returns the remainder |
### Operands ### Operands
@ -93,8 +96,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 1 | HOME_DISTANCE | in `meters` | | 1 | HOME_DISTANCE | in `meters` |
| 2 | TRIP_DISTANCE | in `meters` | | 2 | TRIP_DISTANCE | in `meters` |
| 3 | RSSI | | | 3 | RSSI | |
| 4 | VBAT | in `Volts * 10`, eg. `12.1V` is `121` | | 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` |
| 5 | CELL_VOLTAGE | in `Volts * 10`, eg. `12.1V` is `121` | | 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` |
| 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` | | 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` |
| 7 | MAH_DRAWN | in `mAh` | | 7 | MAH_DRAWN | in `mAh` |
| 8 | GPS_SATS | | | 8 | GPS_SATS | |
@ -122,7 +125,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI
| 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph | | 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph |
| 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | | 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem |
| 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol | | 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol |
| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | | 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol |
| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix |
#### ACTIVE_WAYPOINT_ACTION #### ACTIVE_WAYPOINT_ACTION
@ -258,4 +262,4 @@ Steps:
1. Normalize range [1000:2000] to [0:1000] by substracting `1000` 1. Normalize range [1000:2000] to [0:1000] by substracting `1000`
2. Scale range [0:1000] to [0:3] 2. Scale range [0:1000] to [0:3]
3. Increase range by `1` to have the range of [1:4] 3. Increase range by `1` to have the range of [1:4]
4. Assign LC#2 to VTX power function 4. Assign LC#2 to VTX power function

279
docs/Settings.md
View file

@ -12,8 +12,8 @@
| acc_hardware | AUTO | Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info | | acc_hardware | AUTO | Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info |
| acc_lpf_hz | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | | acc_lpf_hz | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
| acc_lpf_type | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | | acc_lpf_type | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
| acc_notch_cutoff | | | | acc_notch_cutoff | 1 | |
| acc_notch_hz | | | | acc_notch_hz | 0 | |
| accgain_x | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | | accgain_x | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
| accgain_y | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | | accgain_y | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
| accgain_z | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | | accgain_z | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. |
@ -22,7 +22,7 @@
| acczero_z | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | | acczero_z | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. |
| airmode_throttle_threshold | 1300 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used | | airmode_throttle_threshold | 1300 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used |
| airmode_type | STICK_CENTER | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. | | airmode_type | STICK_CENTER | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. |
| airspeed_adc_channel | 0 | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 | | airspeed_adc_channel | _target default_ | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 |
| align_acc | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | | align_acc | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. |
| align_board_pitch | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | | align_board_pitch | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc |
| align_board_roll | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | | align_board_roll | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc |
@ -32,7 +32,7 @@
| align_mag_pitch | 0 | Same as align_mag_roll, but for the pitch axis. | | align_mag_pitch | 0 | Same as align_mag_roll, but for the pitch axis. |
| align_mag_roll | 0 | Set the external mag alignment on the roll axis (in 0.1 degree steps). If this value is non-zero, the compass is assumed to be externally mounted and both the board and on-board compass alignent (align_mag) are ignored. See also align_mag_pitch and align_mag_yaw. | | align_mag_roll | 0 | Set the external mag alignment on the roll axis (in 0.1 degree steps). If this value is non-zero, the compass is assumed to be externally mounted and both the board and on-board compass alignent (align_mag) are ignored. See also align_mag_pitch and align_mag_yaw. |
| align_mag_yaw | 0 | Same as align_mag_roll, but for the yaw axis. | | align_mag_yaw | 0 | Same as align_mag_roll, but for the yaw axis. |
| align_opflow | 5 | Optical flow module alignment (default CW0_DEG_FLIP) | | align_opflow | CW0FLIP | Optical flow module alignment (default CW0_DEG_FLIP) |
| alt_hold_deadband | 50 | Defines the deadband of throttle during alt_hold [r/c points] | | alt_hold_deadband | 50 | Defines the deadband of throttle during alt_hold [r/c points] |
| antigravity_accelerator | 1 | | | antigravity_accelerator | 1 | |
| antigravity_cutoff_lpf_hz | 15 | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain | | antigravity_cutoff_lpf_hz | 15 | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain |
@ -41,47 +41,48 @@
| baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | | baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. |
| baro_hardware | AUTO | Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info | | baro_hardware | AUTO | Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info |
| baro_median_filter | ON | 3-point median filtering for barometer readouts. No reason to change this setting | | baro_median_filter | ON | 3-point median filtering for barometer readouts. No reason to change this setting |
| bat_cells | 0 | Number of cells of the battery (0 = autodetect), see battery documentation | | bat_cells | 0 | Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected. |
| bat_voltage_src | RAW | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` | | bat_voltage_src | RAW | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` |
| battery_capacity | 0 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. | | battery_capacity | 0 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. |
| battery_capacity_critical | 0 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. | | battery_capacity_critical | 0 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. |
| battery_capacity_unit | MAH | Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour). | | battery_capacity_unit | MAH | Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour). |
| battery_capacity_warning | 0 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. | | battery_capacity_warning | 0 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. |
| blackbox_device | SPIFLASH | Selection of where to write blackbox data | | blackbox_device | _target default_ | Selection of where to write blackbox data |
| blackbox_rate_denom | 1 | Blackbox logging rate denominator. See blackbox_rate_num. | | blackbox_rate_denom | 1 | Blackbox logging rate denominator. See blackbox_rate_num. |
| blackbox_rate_num | 1 | Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations | | blackbox_rate_num | 1 | Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations |
| cpu_underclock | OFF | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz | | cpu_underclock | OFF | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz |
| cruise_power | 0 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit | | cruise_power | 0 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit |
| current_adc_channel | - | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled | | current_adc_channel | _target default_ | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled |
| current_meter_offset | 0 | This sets the output offset voltage of the current sensor in millivolts. | | current_meter_offset | _target default_ | This sets the output offset voltage of the current sensor in millivolts. |
| current_meter_scale | 400 | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. | | current_meter_scale | _target default_ | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. |
| current_meter_type | ADC | ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. | | current_meter_type | ADC | ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. |
| d_boost_factor | | | | d_boost_factor | 1.25 | |
| d_boost_gyro_delta_lpf_hz | | | | d_boost_gyro_delta_lpf_hz | 80 | |
| d_boost_max_at_acceleration | | | | d_boost_max_at_acceleration | 7500 | |
| deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | | deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. |
| debug_mode | NONE | Defines debug values exposed in debug variables (developer / debugging setting) | | debug_mode | NONE | Defines debug values exposed in debug variables (developer / debugging setting) |
| disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | | disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. |
| display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | | display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON |
| dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature | | dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature |
| dji_speed_source | GROUND | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR |
| dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | | dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance |
| dji_workarounds | | Enables workarounds for different versions of MSP protocol used | | dji_workarounds | 1 | Enables workarounds for different versions of MSP protocol used |
| dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter | | dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter |
| dterm_lpf2_type | `BIQUAD` | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | | dterm_lpf2_type | BIQUAD | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
| dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | | dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value |
| dterm_lpf_type | `BIQUAD` | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | | dterm_lpf_type | BIQUAD | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
| dynamic_gyro_notch_enabled | `OFF` | Enable/disable dynamic gyro notch also known as Matrix Filter | | dynamic_gyro_notch_enabled | OFF | Enable/disable dynamic gyro notch also known as Matrix Filter |
| dynamic_gyro_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` | | dynamic_gyro_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` |
| dynamic_gyro_notch_q | 120 | Q factor for dynamic notches | | dynamic_gyro_notch_q | 120 | Q factor for dynamic notches |
| dynamic_gyro_notch_range | `MEDIUM` | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers | | dynamic_gyro_notch_range | MEDIUM | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers |
| eleres_freq | | | | eleres_freq | 435 | |
| eleres_loc_delay | | | | eleres_loc_delay | 240 | |
| eleres_loc_en | | | | eleres_loc_en | OFF | |
| eleres_loc_power | | | | eleres_loc_power | 7 | |
| eleres_signature | | | | eleres_signature | 0 | |
| eleres_telemetry_en | | | | eleres_telemetry_en | OFF | |
| eleres_telemetry_power | | | | eleres_telemetry_power | 7 | |
| esc_sensor_listen_only | | | | esc_sensor_listen_only | OFF | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case |
| failsafe_delay | 5 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | | failsafe_delay | 5 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). |
| failsafe_fw_pitch_angle | 100 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb | | failsafe_fw_pitch_angle | 100 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb |
| failsafe_fw_roll_angle | -200 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll | | failsafe_fw_roll_angle | -200 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll |
@ -97,13 +98,14 @@
| failsafe_recovery_delay | 5 | Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). | | failsafe_recovery_delay | 5 | Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). |
| failsafe_stick_threshold | 50 | Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe. | | failsafe_stick_threshold | 50 | Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe. |
| failsafe_throttle | 1000 | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | | failsafe_throttle | 1000 | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). |
| failsafe_throttle_low_delay | 100 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout | | failsafe_throttle_low_delay | 0 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout |
| fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. | | fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. |
| flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. | | flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. |
| fpv_mix_degrees | | | | flip_over_after_crash_power_factor | 65 | flip over after crash power factor |
| fpv_mix_degrees | 0 | |
| frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA | | frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA |
| frsky_default_latitude | 0.000 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | | frsky_default_latitude | 0 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |
| frsky_default_longitude | 0.000 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | | frsky_default_longitude | 0 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. |
| frsky_pitch_roll | OFF | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | | frsky_pitch_roll | OFF | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data |
| frsky_unit | METRIC | Not used? [METRIC/IMPERIAL] | | frsky_unit | METRIC | Not used? [METRIC/IMPERIAL] |
| frsky_vfas_precision | 0 | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method | | frsky_vfas_precision | 0 | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method |
@ -113,6 +115,9 @@
| fw_autotune_threshold | 50 | Threshold [%] of max rate to consider overshoot/undershoot detection | | fw_autotune_threshold | 50 | Threshold [%] of max rate to consider overshoot/undershoot detection |
| fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot | | fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot |
| fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point | | fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point |
| fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH |
| fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for ROLL |
| fw_d_yaw | 0 | Fixed wing rate stabilisation D-gain for YAW |
| fw_ff_pitch | 50 | Fixed-wing rate stabilisation FF-gain for PITCH | | fw_ff_pitch | 50 | Fixed-wing rate stabilisation FF-gain for PITCH |
| fw_ff_roll | 50 | Fixed-wing rate stabilisation FF-gain for ROLL | | fw_ff_roll | 50 | Fixed-wing rate stabilisation FF-gain for ROLL |
| fw_ff_yaw | 60 | Fixed-wing rate stabilisation FF-gain for YAW | | fw_ff_yaw | 60 | Fixed-wing rate stabilisation FF-gain for YAW |
@ -133,6 +138,7 @@
| fw_tpa_time_constant | 0 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups | | fw_tpa_time_constant | 0 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups |
| fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter |
| fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter |
| fw_yaw_iterm_freeze_bank_angle | 0 | Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled. |
| gps_auto_baud | ON | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports | | gps_auto_baud | ON | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports |
| gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. | | gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. |
| gps_dyn_model | AIR_1G | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. | | gps_dyn_model | AIR_1G | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. |
@ -143,15 +149,15 @@
| gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF | | gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
| gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF | | gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF |
| gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF | | gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF |
| gyro_hardware_lpf | 42HZ | Hardware lowpass filter for gyro. Allowed values depend on the driver - For example MPU6050 allows 10HZ,20HZ,42HZ,98HZ,188HZ,256Hz (8khz mode). If you have to set gyro lpf below 42Hz generally means the frame is vibrating too much, and that should be fixed first. | | gyro_hardware_lpf | 256HZ | Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. |
| gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | | gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. |
| gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | | gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. |
| gyro_notch_cutoff | | | | gyro_notch_cutoff | 1 | |
| gyro_notch_hz | | | | gyro_notch_hz | 0 | |
| gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) | | gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) |
| gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | | gyro_stage2_lowpass_type | BIQUAD | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. |
| gyro_sync | OFF | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf | | gyro_sync | ON | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf |
| gyro_to_use | | | | gyro_to_use | 0 | |
| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. | | gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. |
| has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot | | has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot |
| heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. | | heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
@ -159,6 +165,21 @@
| i2c_speed | 400KHZ | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) | | i2c_speed | 400KHZ | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) |
| ibus_telemetry_type | 0 | Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. | | ibus_telemetry_type | 0 | Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. |
| idle_power | 0 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit | | idle_power | 0 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit |
| imu2_align_pitch | 0 | Pitch alignment for Secondary IMU. 1/10 of a degree |
| imu2_align_roll | 0 | Roll alignment for Secondary IMU. 1/10 of a degree |
| imu2_align_yaw | 0 | Yaw alignment for Secondary IMU. 1/10 of a degree |
| imu2_gain_acc_x | 0 | Secondary IMU ACC calibration data |
| imu2_gain_acc_y | 0 | Secondary IMU ACC calibration data |
| imu2_gain_acc_z | 0 | Secondary IMU ACC calibration data |
| imu2_gain_mag_x | 0 | Secondary IMU MAG calibration data |
| imu2_gain_mag_y | 0 | Secondary IMU MAG calibration data |
| imu2_gain_mag_z | 0 | Secondary IMU MAG calibration data |
| imu2_hardware | NONE | Selection of a Secondary IMU hardware type. NONE disables this functionality |
| imu2_radius_acc | 0 | Secondary IMU MAG calibration data |
| imu2_radius_mag | 0 | Secondary IMU MAG calibration data |
| imu2_use_for_osd_ahi | OFF | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon |
| imu2_use_for_osd_heading | OFF | If set to ON, Secondary IMU data will be used for Analog OSD heading |
| imu2_use_for_stabilized | OFF | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) |
| imu_acc_ignore_rate | 0 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes | | imu_acc_ignore_rate | 0 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes |
| imu_acc_ignore_slope | 0 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) | | imu_acc_ignore_slope | 0 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) |
| imu_dcm_ki | 50 | Inertial Measurement Unit KI Gain for accelerometer measurements | | imu_dcm_ki | 50 | Inertial Measurement Unit KI Gain for accelerometer measurements |
@ -167,37 +188,37 @@
| imu_dcm_kp_mag | 10000 | Inertial Measurement Unit KP Gain for compass measurements | | imu_dcm_kp_mag | 10000 | Inertial Measurement Unit KP Gain for compass measurements |
| inav_allow_dead_reckoning | OFF | Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation | | inav_allow_dead_reckoning | OFF | Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation |
| inav_auto_mag_decl | ON | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. | | inav_auto_mag_decl | ON | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. |
| inav_baro_epv | 100.000 | Uncertainty value for barometric sensor [cm] | | inav_baro_epv | 100 | Uncertainty value for barometric sensor [cm] |
| inav_gravity_cal_tolerance | 5 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. | | inav_gravity_cal_tolerance | 5 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. |
| inav_max_eph_epv | 1000.000 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] | | inav_max_eph_epv | 1000 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] |
| inav_max_surface_altitude | 200 | Max allowed altitude for surface following mode. [cm] | | inav_max_surface_altitude | 200 | Max allowed altitude for surface following mode. [cm] |
| inav_reset_altitude | FIRST_ARM | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) | | inav_reset_altitude | FIRST_ARM | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) |
| inav_reset_home | FIRST_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM | | inav_reset_home | FIRST_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM |
| inav_use_gps_no_baro | | | | inav_use_gps_no_baro | OFF | |
| inav_use_gps_velned | ON | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. | | inav_use_gps_velned | ON | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. |
| inav_w_acc_bias | 0.010 | Weight for accelerometer drift estimation | | inav_w_acc_bias | 0.01 | Weight for accelerometer drift estimation |
| inav_w_xy_flow_p | | | | inav_w_xy_flow_p | 1.0 | |
| inav_w_xy_flow_v | | | | inav_w_xy_flow_v | 2.0 | |
| inav_w_xy_gps_p | 1.000 | Weight of GPS coordinates in estimated UAV position and speed. | | inav_w_xy_gps_p | 1.0 | Weight of GPS coordinates in estimated UAV position and speed. |
| inav_w_xy_gps_v | 2.000 | Weight of GPS velocity data in estimated UAV speed | | inav_w_xy_gps_v | 2.0 | Weight of GPS velocity data in estimated UAV speed |
| inav_w_xy_res_v | 0.500 | Decay coefficient for estimated velocity when GPS reference for position is lost | | inav_w_xy_res_v | 0.5 | Decay coefficient for estimated velocity when GPS reference for position is lost |
| inav_w_xyz_acc_p | | | | inav_w_xyz_acc_p | 1.0 | |
| inav_w_z_baro_p | 0.350 | Weight of barometer measurements in estimated altitude and climb rate | | inav_w_z_baro_p | 0.35 | Weight of barometer measurements in estimated altitude and climb rate |
| inav_w_z_gps_p | 0.200 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes | | inav_w_z_gps_p | 0.2 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes |
| inav_w_z_gps_v | 0.500 | Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero | | inav_w_z_gps_v | 0.1 | Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero |
| inav_w_z_res_v | 0.500 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost | | inav_w_z_res_v | 0.5 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost |
| inav_w_z_surface_p | | | | inav_w_z_surface_p | 3.5 | |
| inav_w_z_surface_v | | | | inav_w_z_surface_v | 6.1 | |
| iterm_windup | 50 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) | | iterm_windup | 50 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) |
| ledstrip_visual_beeper | OFF | | | ledstrip_visual_beeper | OFF | |
| log_level | `ERROR` | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. | | log_level | ERROR | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. |
| log_topics | 0 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. | | log_topics | 0 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. |
| looptime | 1000 | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. | | looptime | 1000 | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. |
| ltm_update_rate | NORMAL | Defines the LTM update rate (use of bandwidth [NORMAL/MEDIUM/SLOW]). See Telemetry.md, LTM section for details. | | ltm_update_rate | NORMAL | Defines the LTM update rate (use of bandwidth [NORMAL/MEDIUM/SLOW]). See Telemetry.md, LTM section for details. |
| mag_calibration_time | 30 | Adjust how long time the Calibration of mag will last. | | mag_calibration_time | 30 | Adjust how long time the Calibration of mag will last. |
| mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. | | mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. |
| mag_hardware | AUTO | Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info | | mag_hardware | AUTO | Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info |
| mag_to_use | | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target | | mag_to_use | 0 | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target |
| maggain_x | 1024 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed | | maggain_x | 1024 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed |
| maggain_y | 1024 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed | | maggain_y | 1024 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed |
| maggain_z | 1024 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed | | maggain_z | 1024 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed |
@ -209,12 +230,12 @@
| manual_rc_yaw_expo | 20 | Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] | | manual_rc_yaw_expo | 20 | Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] |
| manual_roll_rate | 100 | Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% | | manual_roll_rate | 100 | Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% |
| manual_yaw_rate | 100 | Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% | | manual_yaw_rate | 100 | Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% |
| mavlink_ext_status_rate | | | | mavlink_ext_status_rate | 2 | |
| mavlink_extra1_rate | | | | mavlink_extra1_rate | 10 | |
| mavlink_extra2_rate | | | | mavlink_extra2_rate | 2 | |
| mavlink_extra3_rate | | | | mavlink_extra3_rate | 1 | |
| mavlink_pos_rate | | | | mavlink_pos_rate | 2 | |
| mavlink_rc_chan_rate | | | | mavlink_rc_chan_rate | 5 | |
| max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | | max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° |
| max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | | max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° |
| max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | | max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. |
@ -231,8 +252,8 @@
| mc_i_pitch | 30 | Multicopter rate stabilisation I-gain for PITCH | | mc_i_pitch | 30 | Multicopter rate stabilisation I-gain for PITCH |
| mc_i_roll | 30 | Multicopter rate stabilisation I-gain for ROLL | | mc_i_roll | 30 | Multicopter rate stabilisation I-gain for ROLL |
| mc_i_yaw | 45 | Multicopter rate stabilisation I-gain for YAW | | mc_i_yaw | 45 | Multicopter rate stabilisation I-gain for YAW |
| mc_iterm_relax | | | | mc_iterm_relax | RP | |
| mc_iterm_relax_cutoff | | | | mc_iterm_relax_cutoff | 15 | |
| mc_p_level | 20 | Multicopter attitude stabilisation P-gain | | mc_p_level | 20 | Multicopter attitude stabilisation P-gain |
| mc_p_pitch | 40 | Multicopter rate stabilisation P-gain for PITCH | | mc_p_pitch | 40 | Multicopter rate stabilisation P-gain for PITCH |
| mc_p_roll | 40 | Multicopter rate stabilisation P-gain for ROLL | | mc_p_roll | 40 | Multicopter rate stabilisation P-gain for ROLL |
@ -245,18 +266,18 @@
| motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | | motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] |
| motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] | | motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] |
| motor_direction_inverted | OFF | Use if you need to inverse yaw motor direction. | | motor_direction_inverted | OFF | Use if you need to inverse yaw motor direction. |
| motor_poles | | | | motor_poles | 14 | The number of motor poles. Required to compute motor RPM |
| motor_pwm_protocol | ONESHOT125 | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED | | motor_pwm_protocol | ONESHOT125 | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED |
| motor_pwm_rate | 400 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. | | motor_pwm_rate | 400 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. |
| msp_override_channels | 0 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. | | msp_override_channels | 0 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. |
| name | Empty string | Craft name | | name | _empty_ | Craft name |
| nav_auto_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] | | nav_auto_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] |
| nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] | | nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] |
| nav_disarm_on_landing | OFF | If set to ON, iNav disarms the FC after landing | | nav_disarm_on_landing | OFF | If set to ON, iNav disarms the FC after landing |
| nav_emerg_landing_speed | 500 | Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] | | nav_emerg_landing_speed | 500 | Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] |
| nav_extra_arming_safety | ON | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used | | nav_extra_arming_safety | ON | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used |
| nav_fw_allow_manual_thr_increase | OFF | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing | | nav_fw_allow_manual_thr_increase | OFF | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing |
| nav_fw_bank_angle | 20 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll | | nav_fw_bank_angle | 35 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll |
| nav_fw_climb_angle | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit | | nav_fw_climb_angle | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit |
| nav_fw_control_smoothness | 0 | How smoothly the autopilot controls the airplane to correct the navigation error | | nav_fw_control_smoothness | 0 | How smoothly the autopilot controls the airplane to correct the navigation error |
| nav_fw_cruise_speed | 0 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s | | nav_fw_cruise_speed | 0 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s |
@ -268,7 +289,7 @@
| nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | | nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s |
| nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | | nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
| nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | | nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] |
| nav_fw_launch_end_time | 2000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_end_time | 3000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] |
| nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) | | nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) |
| nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. | | nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. |
| nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | | nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] |
@ -285,22 +306,22 @@
| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | | nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. |
| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] |
| nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
| nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 2 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) |
| nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 30 | P gain of heading PID controller. (Fixedwing, rovers, boats) |
| nav_fw_pos_hdg_pidsum_limit | 350 | Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_pidsum_limit | 350 | Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats) |
| nav_fw_pos_xy_d | 8 | D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero | | nav_fw_pos_xy_d | 8 | D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero |
| nav_fw_pos_xy_i | 5 | I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero | | nav_fw_pos_xy_i | 5 | I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero |
| nav_fw_pos_xy_p | 75 | P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH | | nav_fw_pos_xy_p | 75 | P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH |
| nav_fw_pos_z_d | 0 | D gain of altitude PID controller (Fixedwing) | | nav_fw_pos_z_d | 10 | D gain of altitude PID controller (Fixedwing) |
| nav_fw_pos_z_i | 0 | I gain of altitude PID controller (Fixedwing) | | nav_fw_pos_z_i | 5 | I gain of altitude PID controller (Fixedwing) |
| nav_fw_pos_z_p | 50 | P gain of altitude PID controller (Fixedwing) | | nav_fw_pos_z_p | 40 | P gain of altitude PID controller (Fixedwing) |
| nav_fw_yaw_deadband | 0 | Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course | | nav_fw_yaw_deadband | 0 | Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course |
| nav_land_slowdown_maxalt | 2000 | Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm] | | nav_land_slowdown_maxalt | 2000 | Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm] |
| nav_land_slowdown_minalt | 500 | Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm] | | nav_land_slowdown_minalt | 500 | Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm] |
| nav_landing_speed | 200 | Vertical descent velocity during the RTH landing phase. [cm/s] | | nav_landing_speed | 200 | Vertical descent velocity during the RTH landing phase. [cm/s] |
| nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] | | nav_manual_climb_rate | 200 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] |
| nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | | nav_manual_speed | 500 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] |
| nav_max_terrain_follow_alt | | | | nav_max_terrain_follow_alt | 100 | Max allowed above the ground altitude for terrain following mode |
| nav_mc_auto_disarm_delay | 2000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) | | nav_mc_auto_disarm_delay | 2000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) |
| nav_mc_bank_angle | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude | | nav_mc_bank_angle | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude |
| nav_mc_braking_bank_angle | 40 | max angle that MR is allowed to bank in BOOST mode | | nav_mc_braking_bank_angle | 40 | max angle that MR is allowed to bank in BOOST mode |
@ -320,14 +341,15 @@
| nav_mc_vel_xy_d | 100 | D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. | | nav_mc_vel_xy_d | 100 | D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. |
| nav_mc_vel_xy_dterm_attenuation | 90 | Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating. | | nav_mc_vel_xy_dterm_attenuation | 90 | Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating. |
| nav_mc_vel_xy_dterm_attenuation_end | 60 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum | | nav_mc_vel_xy_dterm_attenuation_end | 60 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum |
| nav_mc_vel_xy_dterm_attenuation_start | 10" | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins | | nav_mc_vel_xy_dterm_attenuation_start | 10 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins |
| nav_mc_vel_xy_dterm_lpf_hz | | | | nav_mc_vel_xy_dterm_lpf_hz | 2 | |
| nav_mc_vel_xy_ff | | | | nav_mc_vel_xy_ff | 40 | |
| nav_mc_vel_xy_i | 15 | I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot | | nav_mc_vel_xy_i | 15 | I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot |
| nav_mc_vel_xy_p | 40 | P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause "nervous" behavior and oscillations | | nav_mc_vel_xy_p | 40 | P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause "nervous" behavior and oscillations |
| nav_mc_vel_z_d | 10 | D gain of velocity PID controller | | nav_mc_vel_z_d | 10 | D gain of velocity PID controller |
| nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_i | 50 | I gain of velocity PID controller |
| nav_mc_vel_z_p | 100 | P gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller |
| nav_mc_wp_slowdown | ON | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. |
| nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. |
| nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV |
| nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) |
@ -345,22 +367,22 @@
| nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. | | nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. |
| nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | | nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius |
| nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. | | nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. |
| opflow_hardware | | Selection of OPFLOW hardware. | | opflow_hardware | NONE | Selection of OPFLOW hardware. |
| opflow_scale | | | | opflow_scale | 10.5 | |
| osd_ahi_bordered | OFF | Shows a border/corners around the AHI region (pixel OSD only) | | osd_ahi_bordered | OFF | Shows a border/corners around the AHI region (pixel OSD only) |
| osd_ahi_height | 162 | AHI height in pixels (pixel OSD only) | | osd_ahi_height | 162 | AHI height in pixels (pixel OSD only) |
| osd_ahi_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon |
| osd_ahi_reverse_roll | OFF | |
| osd_ahi_style | DEFAULT | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. | | osd_ahi_style | DEFAULT | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. |
| osd_ahi_vertical_offset | 0 | AHI vertical offset from center (pixel OSD only) | | osd_ahi_vertical_offset | -18 | AHI vertical offset from center (pixel OSD only) |
| osd_ahi_width | 132 | AHI width in pixels (pixel OSD only) | | osd_ahi_width | 132 | AHI width in pixels (pixel OSD only) |
| osd_alt_alarm | 100 | Value above which to make the OSD relative altitude indicator blink (meters) | | osd_alt_alarm | 100 | Value above which to make the OSD relative altitude indicator blink (meters) |
| osd_artificial_horizon_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon |
| osd_artificial_horizon_reverse_roll | | |
| osd_baro_temp_alarm_max | 600 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) | | osd_baro_temp_alarm_max | 600 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) |
| osd_baro_temp_alarm_min | -200 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) | | osd_baro_temp_alarm_min | -200 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) |
| osd_camera_fov_h | 135 | Horizontal field of view for the camera in degres | | osd_camera_fov_h | 135 | Horizontal field of view for the camera in degres |
| osd_camera_fov_v | 85 | Vertical field of view for the camera in degres | | osd_camera_fov_v | 85 | Vertical field of view for the camera in degres |
| osd_camera_uptilt | 0 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal | | osd_camera_uptilt | 0 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal |
| osd_coordinate_digits | | | | osd_coordinate_digits | 9 | |
| osd_crosshairs_style | DEFAULT | To set the visual type for the crosshair | | osd_crosshairs_style | DEFAULT | To set the visual type for the crosshair |
| osd_crsf_lq_format | TYPE1 | To select LQ format | | osd_crsf_lq_format | TYPE1 | To select LQ format |
| osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. | | osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. |
@ -375,49 +397,52 @@
| osd_gforce_axis_alarm_min | -5 | Value under which the OSD axis g force indicators will blink (g) | | osd_gforce_axis_alarm_min | -5 | Value under which the OSD axis g force indicators will blink (g) |
| osd_home_position_arm_screen | ON | Should home position coordinates be displayed on the arming screen. | | osd_home_position_arm_screen | ON | Should home position coordinates be displayed on the arming screen. |
| osd_horizon_offset | 0 | To vertically adjust the whole OSD and AHI and scrolling bars | | osd_horizon_offset | 0 | To vertically adjust the whole OSD and AHI and scrolling bars |
| osd_hud_homepoint | 0 | To 3D-display the home point location in the hud | | osd_hud_homepoint | OFF | To 3D-display the home point location in the hud |
| osd_hud_homing | 0 | To display little arrows around the crossair showing where the home point is in the hud | | osd_hud_homing | OFF | To display little arrows around the crossair showing where the home point is in the hud |
| osd_hud_margin_h | 3 | Left and right margins for the hud area | | osd_hud_margin_h | 3 | Left and right margins for the hud area |
| osd_hud_margin_v | 3 | Top and bottom margins for the hud area | | osd_hud_margin_v | 3 | Top and bottom margins for the hud area |
| osd_hud_radar_disp | 0 | Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc | | osd_hud_radar_disp | 0 | Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc |
| osd_hud_radar_nearest | 0 | To display an extra bar of informations at the bottom of the hud area for the closest radar aircraft found, if closest than the set value, in meters. Shows relative altitude (meters or feet, with an up or down arrow to indicate if above or below), speed (in m/s or f/s), and absolute heading (in °, 0 is north, 90 is east, 180 is south, 270 is west). Set to 0 (zero) to disable. | | osd_hud_radar_nearest | 0 | To display an extra bar of informations at the bottom of the hud area for the closest radar aircraft found, if closest than the set value, in meters. Shows relative altitude (meters or feet, with an up or down arrow to indicate if above or below), speed (in m/s or f/s), and absolute heading (in °, 0 is north, 90 is east, 180 is south, 270 is west). Set to 0 (zero) to disable. |
| osd_hud_radar_range_max | 4000" | In meters, radar aircrafts further away than this will not be displayed in the hud | | osd_hud_radar_range_max | 4000 | In meters, radar aircrafts further away than this will not be displayed in the hud |
| osd_hud_radar_range_min | 3 | In meters, radar aircrafts closer than this will not be displayed in the hud | | osd_hud_radar_range_min | 3 | In meters, radar aircrafts closer than this will not be displayed in the hud |
| osd_hud_wp_disp | 0 | How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2) | | osd_hud_wp_disp | 0 | How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2) |
| osd_imu_temp_alarm_max | 600 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_imu_temp_alarm_max | 600 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
| osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) |
| osd_left_sidebar_scroll | | | | osd_left_sidebar_scroll | NONE | |
| osd_left_sidebar_scroll_step | 0 | How many units each sidebar step represents. 0 means the default value for the scroll type. | | osd_left_sidebar_scroll_step | 0 | How many units each sidebar step represents. 0 means the default value for the scroll type. |
| osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | | osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% |
| osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. |
| osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | | osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) |
| osd_plus_code_digits | 10 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. | | osd_pan_servo_index | 0 | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. |
| osd_pan_servo_pwm2centideg | 0 | Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. |
| osd_plus_code_digits | 11 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. |
| osd_plus_code_short | 0 | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. | | osd_plus_code_short | 0 | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. |
| osd_right_sidebar_scroll | | | | osd_right_sidebar_scroll | NONE | |
| osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar | | osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar |
| osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | | osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) |
| osd_rssi_alarm | 20 | Value below which to make the OSD RSSI indicator blink | | osd_rssi_alarm | 20 | Value below which to make the OSD RSSI indicator blink |
| osd_sidebar_horizontal_offset | 0 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. | | osd_sidebar_horizontal_offset | 0 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. |
| osd_sidebar_scroll_arrows | | | | osd_sidebar_scroll_arrows | OFF | |
| osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) | | osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) |
| osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | | osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) |
| osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | | osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` |
| osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | | osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) |
| osd_units | METRIC | IMPERIAL, METRIC, UK | | osd_units | METRIC | IMPERIAL, METRIC, UK |
| osd_video_system | AUTO | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` | | osd_video_system | AUTO | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` |
| 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` | | pid_type | AUTO | 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` |
| pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
| pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | pidsum_limit_yaw | 350 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help |
| pinio_box1 | | | | pinio_box1 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 |
| pinio_box2 | | | | pinio_box2 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 |
| pinio_box3 | | | | pinio_box3 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 |
| pinio_box4 | | | | pinio_box4 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 |
| pitch_rate | 20 | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | | pitch_rate | 20 | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
| pitot_hardware | NONE | Selection of pitot hardware. | | pitot_hardware | NONE | Selection of pitot hardware. |
| pitot_lpf_milli_hz | | | | pitot_lpf_milli_hz | 350 | |
| pitot_scale | | | | pitot_scale | 1.0 | |
| platform_type | "MULTIROTOR" | Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented | | platform_type | MULTIROTOR | Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented |
| pos_hold_deadband | 20 | Stick deadband in [r/c points], applied after r/c deadband and expo | | pos_hold_deadband | 10 | Stick deadband in [r/c points], applied after r/c deadband and expo |
| prearm_timeout | 10000 | Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. |
| rangefinder_hardware | NONE | Selection of rangefinder hardware. | | rangefinder_hardware | NONE | Selection of rangefinder hardware. |
| rangefinder_median_filter | OFF | 3-point median filtering for rangefinder readouts | | rangefinder_median_filter | OFF | 3-point median filtering for rangefinder readouts |
| rate_accel_limit_roll_pitch | 0 | 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. | | rate_accel_limit_roll_pitch | 0 | 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. |
@ -426,30 +451,30 @@
| rc_filter_frequency | 50 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values | | rc_filter_frequency | 50 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values |
| rc_yaw_expo | 20 | Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) | | rc_yaw_expo | 20 | Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) |
| reboot_character | 82 | Special character used to trigger reboot | | reboot_character | 82 | Special character used to trigger reboot |
| receiver_type | `TARGET dependent` | Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` | | receiver_type | _target default_ | Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` |
| report_cell_voltage | OFF | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | | report_cell_voltage | OFF | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON |
| roll_rate | 20 | Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | | roll_rate | 20 | Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
| rpm_gyro_filter_enabled | `OFF` | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV | | rpm_gyro_filter_enabled | OFF | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV |
| rpm_gyro_harmonics | 1 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine | | rpm_gyro_harmonics | 1 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine |
| rpm_gyro_min_hz | 150 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` | | rpm_gyro_min_hz | 100 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` |
| rpm_gyro_q | 500 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting | | rpm_gyro_q | 500 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting |
| rssi_adc_channel | - | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled | | rssi_adc_channel | _target default_ | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled |
| rssi_channel | 0 | RX channel containing the RSSI signal | | rssi_channel | 0 | RX channel containing the RSSI signal |
| rssi_max | 100 | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. | | rssi_max | 100 | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. |
| rssi_min | 0 | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). | | rssi_min | 0 | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). |
| rssi_source | `AUTO` | Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP` | | rssi_source | AUTO | Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP` |
| rth_energy_margin | 5 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation | | rth_energy_margin | 5 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation |
| rx_max_usec | 2115 | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. | | rx_max_usec | 2115 | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. |
| rx_min_usec | 885 | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. | | rx_min_usec | 885 | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. |
| rx_spi_id | | | | rx_spi_id | 0 | |
| rx_spi_protocol | | | | rx_spi_protocol | _target default_ | |
| rx_spi_rf_channel_count | | | | rx_spi_rf_channel_count | 0 | |
| safehome_max_distance | 20000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. | | safehome_max_distance | 20000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. |
| sbus_sync_interval | | | | sbus_sync_interval | 3000 | |
| sdcard_detect_inverted | `TARGET dependent` | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | | sdcard_detect_inverted | _target default_ | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. |
| serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. | | serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. |
| serialrx_inverted | OFF | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | | serialrx_inverted | OFF | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). |
| serialrx_provider | SPEK1024 | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | | serialrx_provider | _target default_ | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. |
| servo_center_pulse | 1500 | Servo midpoint | | servo_center_pulse | 1500 | Servo midpoint |
| servo_lpf_hz | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] | | servo_lpf_hz | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] |
| servo_protocol | PWM | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) | | servo_protocol | PWM | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) |
@ -458,54 +483,54 @@
| setpoint_kalman_q | 100 | Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds | | setpoint_kalman_q | 100 | Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds |
| setpoint_kalman_sharpness | 100 | Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets | | setpoint_kalman_sharpness | 100 | Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets |
| setpoint_kalman_w | 4 | Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response | | setpoint_kalman_w | 4 | Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response |
| sim_ground_station_number | Empty string | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. | | sim_ground_station_number | _empty_ | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. |
| sim_low_altitude | 0 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. | | sim_low_altitude | -32767 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. |
| sim_pin | Empty string | PIN code for the SIM module | | sim_pin | 0000 | PIN code for the SIM module |
| sim_transmit_flags | F | String specifying text message transmit condition flags for the SIM module. Flags can be given in any order. Empty string means the module only sends response messages. `A`: acceleration events, `T`: continuous transmission, `F`: continuous transmission in failsafe mode, `L`: continuous transmission when altitude is below `sim_low_altitude`, `G`: continuous transmission when GPS signal quality is low | | sim_transmit_flags | `SIM_TX_FLAG_FAILSAFE` | Bitmask specifying text message transmit condition flags for the SIM module. 1: continuous transmission, 2: continuous transmission in failsafe mode, 4: continuous transmission when GPS signal quality is low, 8: acceleration events, 16: continuous transmission when altitude is below `sim_low_altitude` |
| sim_transmit_interval | 60 | Text message transmission interval in seconds for SIM module. Minimum value: 10 | | sim_transmit_interval | 60 | Text message transmission interval in seconds for SIM module. Minimum value: 10 |
| small_angle | 25 | If the aircraft tilt angle exceed this value the copter will refuse to arm. | | small_angle | 25 | If the aircraft tilt angle exceed this value the copter will refuse to arm. |
| smartport_fuel_unit | MAH | S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends percent). [PERCENT/MAH/MWH] | | smartport_fuel_unit | MAH | S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends percent). [PERCENT/MAH/MWH] |
| smartport_master_halfduplex | | | | smartport_master_halfduplex | ON | |
| smartport_master_inverted | | | | smartport_master_inverted | OFF | |
| spektrum_sat_bind | 0 | 0 = disabled. Used to bind the spektrum satellite to RX | | spektrum_sat_bind | `SPEKTRUM_SAT_BIND_DISABLED` | 0 = disabled. Used to bind the spektrum satellite to RX |
| srxl2_baud_fast | | | | srxl2_baud_fast | ON | |
| srxl2_unit_id | | | | srxl2_unit_id | 1 | |
| stats | OFF | General switch of the statistics recording feature (a.k.a. odometer) | | stats | OFF | General switch of the statistics recording feature (a.k.a. odometer) |
| stats_total_dist | 0 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. | | stats_total_dist | 0 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. |
| stats_total_energy | | | | stats_total_energy | 0 | |
| stats_total_time | 0 | Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. | | stats_total_time | 0 | Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. |
| switch_disarm_delay | 250 | Delay before disarming when requested by switch (ms) [0-1000] | | switch_disarm_delay | 250 | Delay before disarming when requested by switch (ms) [0-1000] |
| telemetry_halfduplex | OFF | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details | | telemetry_halfduplex | ON | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details |
| telemetry_inverted | OFF | Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. | | telemetry_inverted | OFF | Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. |
| telemetry_switch | OFF | Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. | | telemetry_switch | OFF | Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. |
| thr_comp_weight | 0.692 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) | | thr_comp_weight | 1 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) |
| thr_expo | 0 | Throttle exposition value | | thr_expo | 0 | Throttle exposition value |
| thr_mid | 50 | Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. | | thr_mid | 50 | Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. |
| throttle_idle | 15 | The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. | | throttle_idle | 15 | The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. |
| throttle_scale | 1.000 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% | | throttle_scale | 1.0 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% |
| throttle_tilt_comp_str | 0 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. | | throttle_tilt_comp_str | 0 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. |
| tpa_breakpoint | 1500 | See tpa_rate. | | tpa_breakpoint | 1500 | See tpa_rate. |
| tpa_rate | 0 | 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. | | tpa_rate | 0 | 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. |
| tri_unarmed_servo | ON | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. | | tri_unarmed_servo | ON | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. |
| tz_automatic_dst | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. | | tz_automatic_dst | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. |
| tz_offset | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs | | tz_offset | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs |
| vbat_adc_channel | - | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | | vbat_adc_channel | _target default_ | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled |
| vbat_cell_detect_voltage | 430 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V | | vbat_cell_detect_voltage | 425 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units. |
| vbat_max_cell_voltage | 420 | Maximum voltage per cell in 0.01V units, default is 4.20V | | vbat_max_cell_voltage | 420 | Maximum voltage per cell in 0.01V units, default is 4.20V |
| vbat_meter_type | `ADC` | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running | | vbat_meter_type | ADC | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running |
| vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | | vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) |
| vbat_scale | 1100 | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | | vbat_scale | _target default_ | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. |
| vbat_warning_cell_voltage | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | | vbat_warning_cell_voltage | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) |
| vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | | vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. |
| vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | | vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. |
| vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | | vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. |
| vtx_low_power_disarm | OFF | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. | | vtx_low_power_disarm | OFF | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. |
| vtx_max_power_override | 0 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities | | vtx_max_power_override | 0 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities |
| vtx_pit_mode_chan | | | | vtx_pit_mode_chan | 1 | |
| vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | | vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. |
| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. | | vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. |
| yaw_deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | | yaw_deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. |
| yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | yaw_lpf_hz | 0 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) |
| yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | | yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. |
> Note: this table is autogenerated. Do not edit it manually. > Note: this table is autogenerated. Do not edit it manually.

2
docs/Telemetry.md
View file

@ -123,7 +123,7 @@ The following sensors are transmitted
* **0450** : 'Flight Path Vector' or 'Course over ground' in degrees*10 * **0450** : 'Flight Path Vector' or 'Course over ground' in degrees*10
### Compatible SmartPort/INAV telemetry flight status ### Compatible SmartPort/INAV telemetry flight status
To quickly and easily monitor these SmartPort sensors and flight modes, install [iNav LuaTelemetry](https://github.com/iNavFlight/LuaTelemetry) to your Taranis Q X7, X9D, X9D+ or X9E transmitter. To quickly and easily monitor these SmartPort sensors and flight modes, install [OpenTX Telemetry Widget](https://github.com/iNavFlight/OpenTX-Telemetry-Widget) to your Taranis Q X7, X9D, X9D+ or X9E transmitter.
## FrSky telemetry ## FrSky telemetry

16
src/main/CMakeLists.txt
View file

@ -48,6 +48,8 @@ main_sources(COMMON_SRC
common/typeconversion.h common/typeconversion.h
common/uvarint.c common/uvarint.c
common/uvarint.h common/uvarint.h
common/circular_queue.c
common/circular_queue.h
config/config_eeprom.c config/config_eeprom.c
config/config_eeprom.h config/config_eeprom.h
@ -73,6 +75,8 @@ main_sources(COMMON_SRC
drivers/accgyro/accgyro_adxl345.h drivers/accgyro/accgyro_adxl345.h
drivers/accgyro/accgyro_bma280.c drivers/accgyro/accgyro_bma280.c
drivers/accgyro/accgyro_bma280.h drivers/accgyro/accgyro_bma280.h
drivers/accgyro/accgyro_bmi088.c
drivers/accgyro/accgyro_bmi088.h
drivers/accgyro/accgyro_bmi160.c drivers/accgyro/accgyro_bmi160.c
drivers/accgyro/accgyro_bmi160.h drivers/accgyro/accgyro_bmi160.h
drivers/accgyro/accgyro_fake.c drivers/accgyro/accgyro_fake.c
@ -99,6 +103,8 @@ main_sources(COMMON_SRC
drivers/accgyro/accgyro_mpu6500.h drivers/accgyro/accgyro_mpu6500.h
drivers/accgyro/accgyro_mpu9250.c drivers/accgyro/accgyro_mpu9250.c
drivers/accgyro/accgyro_mpu9250.h drivers/accgyro/accgyro_mpu9250.h
drivers/accgyro/accgyro_bno055.c
drivers/accgyro/accgyro_bno055.h
drivers/adc.c drivers/adc.c
drivers/adc.h drivers/adc.h
@ -152,6 +158,8 @@ main_sources(COMMON_SRC
drivers/compass/compass_mpu9250.h drivers/compass/compass_mpu9250.h
drivers/compass/compass_qmc5883l.c drivers/compass/compass_qmc5883l.c
drivers/compass/compass_qmc5883l.h drivers/compass/compass_qmc5883l.h
drivers/compass/compass_rm3100.c
drivers/compass/compass_rm3100.h
drivers/compass/compass_msp.c drivers/compass/compass_msp.c
drivers/compass/compass_msp.h drivers/compass/compass_msp.h
@ -226,9 +234,13 @@ main_sources(COMMON_SRC
drivers/rangefinder/rangefinder_srf10.h drivers/rangefinder/rangefinder_srf10.h
drivers/rangefinder/rangefinder_vl53l0x.c drivers/rangefinder/rangefinder_vl53l0x.c
drivers/rangefinder/rangefinder_vl53l0x.h drivers/rangefinder/rangefinder_vl53l0x.h
drivers/rangefinder/rangefinder_vl53l1x.c
drivers/rangefinder/rangefinder_vl53l1x.h
drivers/rangefinder/rangefinder_virtual.c drivers/rangefinder/rangefinder_virtual.c
drivers/rangefinder/rangefinder_virtual.h drivers/rangefinder/rangefinder_virtual.h
drivers/rangefinder/rangefinder_us42.c
drivers/rangefinder/rangefinder_us42.h
drivers/resource.c drivers/resource.c
drivers/resource.h drivers/resource.h
drivers/rcc.c drivers/rcc.c
@ -325,6 +337,8 @@ main_sources(COMMON_SRC
flight/dynamic_gyro_notch.h flight/dynamic_gyro_notch.h
flight/dynamic_lpf.c flight/dynamic_lpf.c
flight/dynamic_lpf.h flight/dynamic_lpf.h
flight/secondary_imu.c
flight/secondary_imu.h
io/beeper.c io/beeper.c
io/beeper.h io/beeper.h

21
src/main/blackbox/blackbox.c
View file

@ -52,6 +52,7 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/settings.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
@ -91,18 +92,18 @@
#endif #endif
#ifdef SDCARD_DETECT_INVERTED #ifdef SDCARD_DETECT_INVERTED
#define BLACKBOX_INTERVED_CARD_DETECTION 1 #define BLACKBOX_INVERTED_CARD_DETECTION 1
#else #else
#define BLACKBOX_INTERVED_CARD_DETECTION 0 #define BLACKBOX_INVERTED_CARD_DETECTION 0
#endif #endif
PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 1);
PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig,
.device = DEFAULT_BLACKBOX_DEVICE, .device = DEFAULT_BLACKBOX_DEVICE,
.rate_num = 1, .rate_num = SETTING_BLACKBOX_RATE_NUM_DEFAULT,
.rate_denom = 1, .rate_denom = SETTING_BLACKBOX_RATE_DENOM_DEFAULT,
.invertedCardDetection = BLACKBOX_INTERVED_CARD_DETECTION, .invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION,
); );
#define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200 #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200
@ -1656,6 +1657,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", getThrottleIdleValue(),motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", getThrottleIdleValue(),motorConfig()->maxthrottle);
BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G);
#ifdef USE_ADC
BLACKBOX_PRINT_HEADER_LINE_CUSTOM( BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
blackboxPrintfHeaderLine("vbat_scale", "%u", batteryMetersConfig()->voltage.scale / 10); blackboxPrintfHeaderLine("vbat_scale", "%u", batteryMetersConfig()->voltage.scale / 10);
@ -1667,6 +1669,7 @@ static bool blackboxWriteSysinfo(void)
currentBatteryProfile->voltage.cellWarning / 10, currentBatteryProfile->voltage.cellWarning / 10,
currentBatteryProfile->voltage.cellMax / 10); currentBatteryProfile->voltage.cellMax / 10);
BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference); BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference);
#endif
BLACKBOX_PRINT_HEADER_LINE_CUSTOM( BLACKBOX_PRINT_HEADER_LINE_CUSTOM(
//Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too: //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too:
@ -1723,17 +1726,25 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_soft_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_soft_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_soft_lpf_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_soft_lpf_type);
BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz);
#ifdef USE_DYNAMIC_FILTERS
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ);
BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz);
#endif
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_notch_hz, BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_notch_hz,
0); 0);
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_notch_cutoff, BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_notch_cutoff,
1); 1);
BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz);
BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware);
#ifdef USE_BARO
BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware); BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware);
#endif
#ifdef USE_MAG
BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware); BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware);
#else
BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", MAG_NONE);
#endif
BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider); BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->motorPwmProtocol); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->motorPwmProtocol);
BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->motorPwmRate); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->motorPwmRate);

3
src/main/build/debug.h
View file

@ -80,5 +80,8 @@ typedef enum {
DEBUG_SPM_VARIO, // Smartport master variometer DEBUG_SPM_VARIO, // Smartport master variometer
DEBUG_PCF8574, DEBUG_PCF8574,
DEBUG_DYNAMIC_GYRO_LPF, DEBUG_DYNAMIC_GYRO_LPF,
DEBUG_FW_D,
DEBUG_IMU2,
DEBUG_ALTITUDE,
DEBUG_COUNT DEBUG_COUNT
} debugType_e; } debugType_e;

2
src/main/cms/cms_menu_imu.c
View file

@ -55,7 +55,7 @@
#define RPY_PIDFF_MAX 200 #define RPY_PIDFF_MAX 200
#define OTHER_PIDDF_MAX 255 #define OTHER_PIDDF_MAX 255
#define PIDFF_ENTRY(label, ptr, max) OSD_UINT8_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP })) #define PIDFF_ENTRY(label, ptr, max) OSD_UINT16_ENTRY(label, (&(const OSD_UINT16_t){ ptr, PIDFF_MIN, max, PIDFF_STEP }))
#define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX) #define RPY_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, RPY_PIDFF_MAX)
#define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX) #define OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX)

1
src/main/cms/cms_menu_misc.c
View file

@ -47,6 +47,7 @@ static const OSD_Entry menuMiscEntries[]=
OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS), OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS),
OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT), OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT),
#endif #endif
OSD_SETTING_ENTRY("FS PROCEDURE", SETTING_FAILSAFE_PROCEDURE),
OSD_BACK_AND_END_ENTRY, OSD_BACK_AND_END_ENTRY,
}; };

4
src/main/cms/cms_menu_osd.c
View file

@ -217,8 +217,8 @@ static const OSD_Entry menuOsdElemsEntries[] =
#endif // GPS #endif // GPS
OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING), OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING),
OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH), OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH),
OSD_ELEMENT_ENTRY("CRS HEAD. ERR", OSD_CRUISE_HEADING_ERROR), OSD_ELEMENT_ENTRY("CRS HLD ERR", OSD_COURSE_HOLD_ERROR),
OSD_ELEMENT_ENTRY("CRS HEAD. ADJ", OSD_CRUISE_HEADING_ADJUSTMENT), OSD_ELEMENT_ENTRY("CRS HLD ADJ", OSD_COURSE_HOLD_ADJUSTMENT),
#if defined(USE_BARO) || defined(USE_GPS) #if defined(USE_BARO) || defined(USE_GPS)
OSD_ELEMENT_ENTRY("VARIO", OSD_VARIO), OSD_ELEMENT_ENTRY("VARIO", OSD_VARIO),
OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM), OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM),

66
src/main/common/circular_queue.c Normal file
View file

@ -0,0 +1,66 @@
/*
* 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 "circular_queue.h"
void circularBufferInit(circularBuffer_t *circular_buffer, uint8_t *buffer, size_t buffer_size,
size_t buffer_element_size) {
circular_buffer->buffer = buffer;
circular_buffer->bufferSize = buffer_size;
circular_buffer->elementSize = buffer_element_size;
circular_buffer->head = 0;
circular_buffer->tail = 0;
circular_buffer->size = 0;
}
void circularBufferPushElement(circularBuffer_t *circularBuffer, uint8_t *element) {
if (circularBufferIsFull(circularBuffer))
return;
memcpy(circularBuffer->buffer + circularBuffer->tail, element, circularBuffer->elementSize);
circularBuffer->tail += circularBuffer->elementSize;
circularBuffer->tail %= circularBuffer->bufferSize;
circularBuffer->size += 1;
}
void circularBufferPopHead(circularBuffer_t *circularBuffer, uint8_t *element) {
memcpy(element, circularBuffer->buffer + circularBuffer->head, circularBuffer->elementSize);
circularBuffer->head += circularBuffer->elementSize;
circularBuffer->head %= circularBuffer->bufferSize;
circularBuffer->size -= 1;
}
int circularBufferIsFull(circularBuffer_t *circularBuffer) {
return circularBufferCountElements(circularBuffer) * circularBuffer->elementSize == circularBuffer->bufferSize;
}
int circularBufferIsEmpty(circularBuffer_t *circularBuffer) {
return circularBuffer==NULL || circularBufferCountElements(circularBuffer) == 0;
}
size_t circularBufferCountElements(circularBuffer_t *circularBuffer) {
return circularBuffer->size;
}

47
src/main/common/circular_queue.h Normal file
View file

@ -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/.
*/
#ifndef INAV_CIRCULAR_QUEUE_H
#define INAV_CIRCULAR_QUEUE_H
#include "stdint.h"
#include "string.h"
typedef struct circularBuffer_s{
size_t head;
size_t tail;
size_t bufferSize;
uint8_t * buffer;
size_t elementSize;
size_t size;
}circularBuffer_t;
void circularBufferInit(circularBuffer_t * circularBuffer, uint8_t * buffer, size_t bufferSize, size_t bufferElementSize);
void circularBufferPushElement(circularBuffer_t * circularBuffer, uint8_t * element);
void circularBufferPopHead(circularBuffer_t * circularBuffer, uint8_t * element);
int circularBufferIsFull(circularBuffer_t * circularBuffer);
int circularBufferIsEmpty(circularBuffer_t *circularBuffer);
size_t circularBufferCountElements(circularBuffer_t * circularBuffer);
#endif //INAV_CIRCULAR_QUEUE_H

6
src/main/common/log.c
View file

@ -39,6 +39,7 @@
#include "io/serial.h" #include "io/serial.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/settings.h"
#include "msp/msp.h" #include "msp/msp.h"
#include "msp/msp_serial.h" #include "msp/msp_serial.h"
@ -54,6 +55,11 @@ static mspPort_t * mspLogPort = NULL;
PG_REGISTER(logConfig_t, logConfig, PG_LOG_CONFIG, 0); PG_REGISTER(logConfig_t, logConfig, PG_LOG_CONFIG, 0);
PG_RESET_TEMPLATE(logConfig_t, logConfig,
.level = SETTING_LOG_LEVEL_DEFAULT,
.topics = SETTING_LOG_TOPICS_DEFAULT
);
void logInit(void) void logInit(void)
{ {
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LOG); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LOG);

2
src/main/common/maths.c
View file

@ -142,7 +142,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband)
return value; return value;
} }
int constrain(int amt, int low, int high) int32_t constrain(int32_t amt, int32_t low, int32_t high)
{ {
if (amt < low) if (amt < low)
return low; return low;

4
src/main/common/maths.h
View file

@ -88,6 +88,8 @@
#define _ABS_I(x, var) _ABS_II(x, var) #define _ABS_I(x, var) _ABS_II(x, var)
#define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__)) #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__))
#define power3(x) ((x)*(x)*(x))
// Floating point Euler angles. // Floating point Euler angles.
typedef struct fp_angles { typedef struct fp_angles {
float roll; float roll;
@ -131,7 +133,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu
int gcd(int num, int denom); int gcd(int num, int denom);
int32_t applyDeadband(int32_t value, int32_t deadband); int32_t applyDeadband(int32_t value, int32_t deadband);
int constrain(int amt, int low, int high); int32_t constrain(int32_t amt, int32_t low, int32_t high);
float constrainf(float amt, float low, float high); float constrainf(float amt, float low, float high);
void devClear(stdev_t *dev); void devClear(stdev_t *dev);

6
src/main/common/time.c
View file

@ -33,6 +33,8 @@
#include "drivers/time.h" #include "drivers/time.h"
#include "fc/settings.h"
// For the "modulo 4" arithmetic to work, we need a leap base year // For the "modulo 4" arithmetic to work, we need a leap base year
#define REFERENCE_YEAR 2000 #define REFERENCE_YEAR 2000
// Offset (seconds) from the UNIX epoch (1970-01-01) to 2000-01-01 // Offset (seconds) from the UNIX epoch (1970-01-01) to 2000-01-01
@ -55,8 +57,8 @@ static const uint16_t days[4][12] =
PG_REGISTER_WITH_RESET_TEMPLATE(timeConfig_t, timeConfig, PG_TIME_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(timeConfig_t, timeConfig, PG_TIME_CONFIG, 1);
PG_RESET_TEMPLATE(timeConfig_t, timeConfig, PG_RESET_TEMPLATE(timeConfig_t, timeConfig,
.tz_offset = 0, .tz_offset = SETTING_TZ_OFFSET_DEFAULT,
.tz_automatic_dst = TZ_AUTO_DST_OFF, .tz_automatic_dst = SETTING_TZ_AUTOMATIC_DST_DEFAULT,
); );
static rtcTime_t dateTimeToRtcTime(const dateTime_t *dt) static rtcTime_t dateTimeToRtcTime(const dateTime_t *dt)

6
src/main/config/general_settings.c
View file

@ -28,8 +28,10 @@
#include "config/general_settings.h" #include "config/general_settings.h"
#include "fc/settings.h"
PG_REGISTER_WITH_RESET_TEMPLATE(generalSettings_t, generalSettings, PG_GENERAL_SETTINGS, 0); PG_REGISTER_WITH_RESET_TEMPLATE(generalSettings_t, generalSettings, PG_GENERAL_SETTINGS, 0);
PG_RESET_TEMPLATE(generalSettings_t, generalSettings, PG_RESET_TEMPLATE(generalSettings_t, generalSettings,
.appliedDefaults = APPLIED_DEFAULTS_NONE, .appliedDefaults = SETTING_APPLIED_DEFAULTS_DEFAULT,
); );

3
src/main/config/parameter_group_ids.h
View file

@ -116,7 +116,8 @@
#define PG_SAFE_HOME_CONFIG 1026 #define PG_SAFE_HOME_CONFIG 1026
#define PG_DJI_OSD_CONFIG 1027 #define PG_DJI_OSD_CONFIG 1027
#define PG_PROGRAMMING_PID 1028 #define PG_PROGRAMMING_PID 1028
#define PG_INAV_END 1028 #define PG_SECONDARY_IMU 1029
#define PG_INAV_END 1029
// OSD configuration (subject to change) // OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047 //#define PG_OSD_FONT_CONFIG 2047

245
src/main/drivers/accgyro/accgyro_bmi088.c Normal file
View file

@ -0,0 +1,245 @@
/*
* This file is part of INAV.
*
* 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 <stdbool.h>
#include <stdint.h>
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "drivers/io.h"
#include "drivers/exti.h"
#include "drivers/bus.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro.h"
#include "drivers/accgyro/accgyro_bmi088.h"
#if defined(USE_IMU_BMI088)
/*
device registers, names follow datasheet conventions, with REGA_
prefix for accel, and REGG_ prefix for gyro
*/
#define REGA_CHIPID 0x00
#define REGA_ERR_REG 0x02
#define REGA_STATUS 0x03
#define REGA_X_LSB 0x12
#define REGA_INT_STATUS_1 0x1D
#define REGA_TEMP_LSB 0x22
#define REGA_TEMP_MSB 0x23
#define REGA_CONF 0x40
#define REGA_RANGE 0x41
#define REGA_PWR_CONF 0x7C
#define REGA_PWR_CTRL 0x7D
#define REGA_SOFTRESET 0x7E
#define REGA_FIFO_CONFIG0 0x48
#define REGA_FIFO_CONFIG1 0x49
#define REGA_FIFO_DOWNS 0x45
#define REGA_FIFO_DATA 0x26
#define REGA_FIFO_LEN0 0x24
#define REGA_FIFO_LEN1 0x25
#define REGG_CHIPID 0x00
#define REGG_RATE_X_LSB 0x02
#define REGG_INT_CTRL 0x15
#define REGG_INT_STATUS_1 0x0A
#define REGG_INT_STATUS_2 0x0B
#define REGG_INT_STATUS_3 0x0C
#define REGG_FIFO_STATUS 0x0E
#define REGG_RANGE 0x0F
#define REGG_BW 0x10
#define REGG_LPM1 0x11
#define REGG_RATE_HBW 0x13
#define REGG_BGW_SOFTRESET 0x14
#define REGG_FIFO_CONFIG_1 0x3E
#define REGG_FIFO_DATA 0x3F
static void bmi088GyroInit(gyroDev_t *gyro)
{
gyroIntExtiInit(gyro);
busSetSpeed(gyro->busDev, BUS_SPEED_INITIALIZATION);
// Soft reset
busWrite(gyro->busDev, REGG_BGW_SOFTRESET, 0xB6);
delay(100);
// ODR 2kHz, BW 532Hz
busWrite(gyro->busDev, REGG_BW, 0x81);
delay(1);
// Enable sampling
busWrite(gyro->busDev, REGG_INT_CTRL, 0x80);
delay(1);
busSetSpeed(gyro->busDev, BUS_SPEED_FAST);
}
static void bmi088AccInit(accDev_t *acc)
{
busSetSpeed(acc->busDev, BUS_SPEED_INITIALIZATION);
// Soft reset
busWrite(acc->busDev, REGA_SOFTRESET, 0xB6);
delay(100);
// Active mode
busWrite(acc->busDev, REGA_PWR_CONF, 0);
delay(100);
// ACC ON
busWrite(acc->busDev, REGA_PWR_CTRL, 0x04);
delay(100);
// OSR4, ODR 1600Hz
busWrite(acc->busDev, REGA_CONF, 0x8C);
delay(1);
// Range 12g
busWrite(acc->busDev, REGA_RANGE, 0x02);
delay(1);
busSetSpeed(acc->busDev, BUS_SPEED_STANDARD);
acc->acc_1G = 2048;
}
static bool bmi088GyroRead(gyroDev_t *gyro)
{
uint8_t gyroRaw[6];
if (busReadBuf(gyro->busDev, REGG_RATE_X_LSB, gyroRaw, 6)) {
gyro->gyroADCRaw[X] = (int16_t)((gyroRaw[1] << 8) | gyroRaw[0]);
gyro->gyroADCRaw[Y] = (int16_t)((gyroRaw[3] << 8) | gyroRaw[2]);
gyro->gyroADCRaw[Z] = (int16_t)((gyroRaw[5] << 8) | gyroRaw[4]);
return true;
}
return false;
}
static bool bmi088AccRead(accDev_t *acc)
{
uint8_t buffer[7];
if (busReadBuf(acc->busDev, REGA_STATUS, buffer, 2) && (buffer[1] & 0x80) && busReadBuf(acc->busDev, REGA_X_LSB, buffer, 7)) {
// first byte is discarded, see datasheet
acc->ADCRaw[X] = (int32_t)(((int16_t)(buffer[2] << 8) | buffer[1]) * 3 / 4);
acc->ADCRaw[Y] = (int32_t)(((int16_t)(buffer[4] << 8) | buffer[3]) * 3 / 4);
acc->ADCRaw[Z] = (int32_t)(((int16_t)(buffer[6] << 8) | buffer[5]) * 3 / 4);
return true;
}
return false;
}
static bool gyroDeviceDetect(busDevice_t * busDev)
{
uint8_t attempts;
busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
for (attempts = 0; attempts < 5; attempts++) {
uint8_t chipId;
delay(100);
busRead(busDev, REGG_CHIPID, &chipId);
if (chipId == 0x0F) {
return true;
}
}
return false;
}
static bool accDeviceDetect(busDevice_t * busDev)
{
uint8_t attempts;
busSetSpeed(busDev, BUS_SPEED_INITIALIZATION);
for (attempts = 0; attempts < 5; attempts++) {
uint8_t chipId;
delay(100);
busRead(busDev, REGA_CHIPID, &chipId);
if (chipId == 0x1E) {
return true;
}
}
return false;
}
bool bmi088AccDetect(accDev_t *acc)
{
acc->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI088_ACC, acc->imuSensorToUse, OWNER_MPU);
if (acc->busDev == NULL) {
return false;
}
if (!accDeviceDetect(acc->busDev)) {
busDeviceDeInit(acc->busDev);
return false;
}
acc->initFn = bmi088AccInit;
acc->readFn = bmi088AccRead;
return true;
}
bool bmi088GyroDetect(gyroDev_t *gyro)
{
gyro->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_BMI088_GYRO, gyro->imuSensorToUse, OWNER_MPU);
if (gyro->busDev == NULL) {
return false;
}
if (!gyroDeviceDetect(gyro->busDev)) {
busDeviceDeInit(gyro->busDev);
return false;
}
gyro->initFn = bmi088GyroInit;
gyro->readFn = bmi088GyroRead;
gyro->scale = 1.0f / 16.4f; // 16.4 dps/lsb
gyro->intStatusFn = gyroCheckDataReady;
gyro->gyroAlign = gyro->busDev->param;
return true;
}
#endif /* USE_IMU_BMI088 */

30
src/main/drivers/accgyro/accgyro_bmi088.h Normal file
View file

@ -0,0 +1,30 @@
/*
* This file is part of INAV.
*
* 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 "drivers/sensor.h"
bool bmi088AccDetect(accDev_t *acc);
bool bmi088GyroDetect(gyroDev_t *gyro);

224
src/main/drivers/accgyro/accgyro_bno055.c Normal file
View file

@ -0,0 +1,224 @@
/*
* This file is part of INAV.
*
* 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 <stdbool.h>
#include <stdint.h>
#include "drivers/bus.h"
#include "drivers/time.h"
#include "build/debug.h"
#include "common/vector.h"
#include "drivers/accgyro/accgyro_bno055.h"
#ifdef USE_IMU_BNO055
#define BNO055_ADDR_PWR_MODE 0x3E
#define BNO055_ADDR_OPR_MODE 0x3D
#define BNO055_ADDR_CALIB_STAT 0x35
#define BNO055_PWR_MODE_NORMAL 0x00
#define BNO055_OPR_MODE_CONFIG 0x00
#define BNO055_OPR_MODE_NDOF 0x0C
#define BNO055_ADDR_EUL_YAW_LSB 0x1A
#define BNO055_ADDR_EUL_YAW_MSB 0x1B
#define BNO055_ADDR_EUL_ROLL_LSB 0x1C
#define BNO055_ADDR_EUL_ROLL_MSB 0x1D
#define BNO055_ADDR_EUL_PITCH_LSB 0x1E
#define BNO055_ADDR_EUL_PITCH_MSB 0x1F
#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A
#define BNO055_ADDR_MAG_RADIUS_LSB 0x69
#define BNO055_ADDR_ACC_RADIUS_MSB 0x68
#define BNO055_ADDR_ACC_RADIUS_LSB 0x67
#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66
#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65
#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64
#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63
#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62
#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61
#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60
#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F
#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E
#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D
#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C
#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B
#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A
#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59
#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58
#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57
#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56
#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55
static busDevice_t *busDev;
static bool deviceDetect(busDevice_t *busDev)
{
for (int retry = 0; retry < 5; retry++)
{
uint8_t sig;
delay(150);
bool ack = busRead(busDev, 0x00, &sig);
if (ack)
{
return true;
}
};
return false;
}
static void bno055SetMode(uint8_t mode)
{
busWrite(busDev, BNO055_ADDR_OPR_MODE, mode);
delay(25);
}
bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration)
{
busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0);
if (busDev == NULL)
{
DEBUG_SET(DEBUG_IMU2, 2, 1);
return false;
}
if (!deviceDetect(busDev))
{
DEBUG_SET(DEBUG_IMU2, 2, 2);
busDeviceDeInit(busDev);
return false;
}
busWrite(busDev, BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL
delay(25);
if (setCalibration) {
bno055SetMode(BNO055_OPR_MODE_CONFIG);
bno055SetCalibrationData(calibrationData);
}
bno055SetMode(BNO055_OPR_MODE_NDOF);
return true;
}
void bno055FetchEulerAngles(int16_t *buffer)
{
uint8_t buf[6];
busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6);
buffer[0] = ((int16_t)((buf[3] << 8) | buf[2])) / 1.6f;
buffer[1] = ((int16_t)((buf[5] << 8) | buf[4])) / -1.6f; //Pitch has to be reversed to match INAV notation
buffer[2] = ((int16_t)((buf[1] << 8) | buf[0])) / 1.6f;
}
fpVector3_t bno055GetEurlerAngles(void)
{
fpVector3_t eurlerAngles;
uint8_t buf[6];
busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6);
eurlerAngles.x = ((int16_t)((buf[3] << 8) | buf[2])) / 16;
eurlerAngles.y = ((int16_t)((buf[5] << 8) | buf[4])) / 16;
eurlerAngles.z = ((int16_t)((buf[1] << 8) | buf[0])) / 16;
return eurlerAngles;
}
bno055CalibStat_t bno055GetCalibStat(void)
{
bno055CalibStat_t stats;
uint8_t buf;
busRead(busDev, BNO055_ADDR_CALIB_STAT, &buf);
stats.mag = buf & 0b00000011;
stats.acc = (buf >> 2) & 0b00000011;
stats.gyr = (buf >> 4) & 0b00000011;
stats.sys = (buf >> 6) & 0b00000011;
return stats;
}
bno055CalibrationData_t bno055GetCalibrationData(void)
{
bno055CalibrationData_t data;
uint8_t buf[22];
bno055SetMode(BNO055_OPR_MODE_CONFIG);
busReadBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 22);
bno055SetMode(BNO055_OPR_MODE_NDOF);
uint8_t bufferBit = 0;
for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++)
{
for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
{
data.offset[sensorIndex][axisIndex] = (int16_t)((buf[bufferBit + 1] << 8) | buf[bufferBit]);
bufferBit += 2;
}
}
data.radius[ACC] = (int16_t)((buf[19] << 8) | buf[18]);
data.radius[MAG] = (int16_t)((buf[21] << 8) | buf[20]);
return data;
}
void bno055SetCalibrationData(bno055CalibrationData_t data)
{
uint8_t buf[12];
//Prepare gains
//We do not restore gyro offsets, they are quickly calibrated at startup
uint8_t bufferBit = 0;
for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++)
{
for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++)
{
buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff);
buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff);
bufferBit += 2;
}
}
busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12);
//Prepare radius
buf[0] = (uint8_t)(data.radius[ACC] & 0xff);
buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff);
buf[2] = (uint8_t)(data.radius[MAG] & 0xff);
buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff);
//Write to the device
busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4);
}
#endif

51
src/main/drivers/accgyro/accgyro_bno055.h Normal file
View file

@ -0,0 +1,51 @@
/*
* This file is part of INAV.
*
* 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 "common/vector.h"
typedef struct {
uint8_t sys;
uint8_t gyr;
uint8_t acc;
uint8_t mag;
} bno055CalibStat_t;
typedef enum {
ACC = 0,
MAG = 1,
GYRO = 2
} bno055Sensor_e;
typedef struct {
int16_t radius[2];
int16_t offset[3][3];
} bno055CalibrationData_t;
bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration);
fpVector3_t bno055GetEurlerAngles(void);
void bno055FetchEulerAngles(int16_t * buffer);
bno055CalibStat_t bno055GetCalibStat(void);
bno055CalibrationData_t bno055GetCalibrationData(void);
void bno055SetCalibrationData(bno055CalibrationData_t data);

6
src/main/drivers/bus.h
View file

@ -91,6 +91,8 @@ typedef enum {
DEVHW_MPU6050, DEVHW_MPU6050,
DEVHW_MPU6500, DEVHW_MPU6500,
DEVHW_BMI160, DEVHW_BMI160,
DEVHW_BMI088_GYRO,
DEVHW_BMI088_ACC,
DEVHW_ICM20689, DEVHW_ICM20689,
/* Combined ACC/GYRO/MAG chips */ /* Combined ACC/GYRO/MAG chips */
@ -116,6 +118,7 @@ typedef enum {
DEVHW_QMC5883, DEVHW_QMC5883,
DEVHW_MAG3110, DEVHW_MAG3110,
DEVHW_LIS3MDL, DEVHW_LIS3MDL,
DEVHW_RM3100,
/* Temp sensor chips */ /* Temp sensor chips */
DEVHW_LM75_0, DEVHW_LM75_0,
@ -137,6 +140,8 @@ typedef enum {
DEVHW_SRF10, DEVHW_SRF10,
DEVHW_HCSR04_I2C, // DIY-style adapter DEVHW_HCSR04_I2C, // DIY-style adapter
DEVHW_VL53L0X, DEVHW_VL53L0X,
DEVHW_VL53L1X,
DEVHW_US42,
/* Other hardware */ /* Other hardware */
DEVHW_MS4525, // Pitot meter DEVHW_MS4525, // Pitot meter
@ -146,6 +151,7 @@ typedef enum {
DEVHW_SDCARD, // Generic SD-Card DEVHW_SDCARD, // Generic SD-Card
DEVHW_IRLOCK, // IR-Lock visual positioning hardware DEVHW_IRLOCK, // IR-Lock visual positioning hardware
DEVHW_PCF8574, // 8-bit I/O expander DEVHW_PCF8574, // 8-bit I/O expander
DEVHW_BNO055, // BNO055 IMU
} devHardwareType_e; } devHardwareType_e;
typedef enum { typedef enum {

2
src/main/drivers/bus_i2c_hal.c
View file

@ -174,7 +174,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_,
HAL_StatusTypeDef status; HAL_StatusTypeDef status;
if (reg_ == 0xFF && allowRawAccess) { if ((reg_ == 0xFF || len_ == 0) && allowRawAccess) {
status = HAL_I2C_Master_Transmit(&state->handle, addr_ << 1, (uint8_t *)data, len_, I2C_DEFAULT_TIMEOUT); status = HAL_I2C_Master_Transmit(&state->handle, addr_ << 1, (uint8_t *)data, len_, I2C_DEFAULT_TIMEOUT);
} }
else { else {

179
src/main/drivers/compass/compass_rm3100.c Normal file
View file

@ -0,0 +1,179 @@
/*
* 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 <stdbool.h>
#include <stdint.h>
#include <math.h>
#include "platform.h"
#ifdef USE_MAG_RM3100
#include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/utils.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "sensors/boardalignment.h"
#include "sensors/sensors.h"
#include "drivers/sensor.h"
#include "drivers/compass/compass.h"
#include "drivers/compass/compass_rm3100.h"
#define RM3100_REG_POLL 0x00
#define RM3100_REG_CMM 0x01
#define RM3100_REG_CCX1 0x04
#define RM3100_REG_CCX0 0x05
#define RM3100_REG_CCY1 0x06
#define RM3100_REG_CCY0 0x07
#define RM3100_REG_CCZ1 0x08
#define RM3100_REG_CCZ0 0x09
#define RM3100_REG_TMRC 0x0B
#define RM3100_REG_MX 0x24
#define RM3100_REG_MY 0x27
#define RM3100_REG_MZ 0x2A
#define RM3100_REG_BIST 0x33
#define RM3100_REG_STATUS 0x34
#define RM3100_REG_HSHAKE 0x35
#define RM3100_REG_REVID 0x36
#define RM3100_REVID 0x22
#define CCX_DEFAULT_MSB 0x00
#define CCX_DEFAULT_LSB 0xC8
#define CCY_DEFAULT_MSB CCX_DEFAULT_MSB
#define CCY_DEFAULT_LSB CCX_DEFAULT_LSB
#define CCZ_DEFAULT_MSB CCX_DEFAULT_MSB
#define CCZ_DEFAULT_LSB CCX_DEFAULT_LSB
#define CMM_DEFAULT 0x71 // Continuous mode
#define TMRC_DEFAULT 0x94
static bool deviceInit(magDev_t * mag)
{
busWrite(mag->busDev, RM3100_REG_TMRC, TMRC_DEFAULT);
busWrite(mag->busDev, RM3100_REG_CMM, CMM_DEFAULT);
busWrite(mag->busDev, RM3100_REG_CCX1, CCX_DEFAULT_MSB);
busWrite(mag->busDev, RM3100_REG_CCX0, CCX_DEFAULT_LSB);
busWrite(mag->busDev, RM3100_REG_CCY1, CCY_DEFAULT_MSB);
busWrite(mag->busDev, RM3100_REG_CCY0, CCY_DEFAULT_LSB);
busWrite(mag->busDev, RM3100_REG_CCZ1, CCZ_DEFAULT_MSB);
busWrite(mag->busDev, RM3100_REG_CCZ0, CCZ_DEFAULT_LSB);
return true;
}
static bool deviceRead(magDev_t * mag)
{
uint8_t status;
#pragma pack(push, 1)
struct {
uint8_t x[3];
uint8_t y[3];
uint8_t z[3];
} rm_report;
#pragma pack(pop)
mag->magADCRaw[X] = 0;
mag->magADCRaw[Y] = 0;
mag->magADCRaw[Z] = 0;
/* Check if new measurement is ready */
bool ack = busRead(mag->busDev, RM3100_REG_STATUS, &status);
if (!ack || (status & 0x80) == 0) {
return false;
}
ack = busReadBuf(mag->busDev, RM3100_REG_MX, (uint8_t *)&rm_report, sizeof(rm_report));
if (!ack) {
return false;
}
int32_t xraw;
int32_t yraw;
int32_t zraw;
/* Rearrange mag data */
xraw = ((rm_report.x[0] << 24) | (rm_report.x[1] << 16) | (rm_report.x[2]) << 8);
yraw = ((rm_report.y[0] << 24) | (rm_report.y[1] << 16) | (rm_report.y[2]) << 8);
zraw = ((rm_report.z[0] << 24) | (rm_report.z[1] << 16) | (rm_report.z[2]) << 8);
/* Truncate to 16-bit integers and pass along */
mag->magADCRaw[X] = (int16_t)(xraw >> 16);
mag->magADCRaw[Y] = (int16_t)(yraw >> 16);
mag->magADCRaw[Z] = (int16_t)(zraw >> 16);
return true;
}
#define DETECTION_MAX_RETRY_COUNT 5
static bool deviceDetect(magDev_t * mag)
{
for (int retryCount = 0; retryCount < DETECTION_MAX_RETRY_COUNT; retryCount++) {
uint8_t revid = 0;
bool ack = busRead(mag->busDev, RM3100_REG_REVID, &revid);
if (ack && revid == RM3100_REVID) {
return true;
}
}
return false;
}
bool rm3100MagDetect(magDev_t * mag)
{
busSetSpeed(mag->busDev, BUS_SPEED_STANDARD);
mag->busDev = busDeviceInit(BUSTYPE_ANY, DEVHW_RM3100, mag->magSensorToUse, OWNER_COMPASS);
if (mag->busDev == NULL) {
return false;
}
if (!deviceDetect(mag)) {
busDeviceDeInit(mag->busDev);
return false;
}
mag->init = deviceInit;
mag->read = deviceRead;
return true;
}
#endif

27
src/main/drivers/compass/compass_rm3100.h Normal file
View file

@ -0,0 +1,27 @@
/*
* 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
bool rm3100MagDetect(magDev_t *mag);

3
src/main/drivers/display.c
View file

@ -30,6 +30,7 @@
#include "drivers/display_font_metadata.h" #include "drivers/display_font_metadata.h"
#include "drivers/time.h" #include "drivers/time.h"
#include "fc/settings.h"
#define SW_BLINK_CYCLE_MS 200 // 200ms on / 200ms off #define SW_BLINK_CYCLE_MS 200 // 200ms on / 200ms off
@ -43,7 +44,7 @@
PG_REGISTER_WITH_RESET_TEMPLATE(displayConfig_t, displayConfig, PG_DISPLAY_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(displayConfig_t, displayConfig, PG_DISPLAY_CONFIG, 0);
PG_RESET_TEMPLATE(displayConfig_t, displayConfig, PG_RESET_TEMPLATE(displayConfig_t, displayConfig,
.force_sw_blink = false, .force_sw_blink = SETTING_DISPLAY_FORCE_SW_BLINK_DEFAULT
); );
static bool displayAttributesRequireEmulation(displayPort_t *instance, textAttributes_t attr) static bool displayAttributesRequireEmulation(displayPort_t *instance, textAttributes_t attr)

2
src/main/drivers/lights_io.c
View file

@ -10,7 +10,7 @@
static IO_t lightsIO = DEFIO_IO(NONE); static IO_t lightsIO = DEFIO_IO(NONE);
bool lightsHardwareInit() bool lightsHardwareInit(void)
{ {
lightsIO = IOGetByTag(IO_TAG(LIGHTS_PIN)); lightsIO = IOGetByTag(IO_TAG(LIGHTS_PIN));

2
src/main/drivers/lights_io.h
View file

@ -4,7 +4,7 @@
#ifdef USE_LIGHTS #ifdef USE_LIGHTS
bool lightsHardwareInit(); bool lightsHardwareInit(void);
void lightsHardwareSetStatus(bool status); void lightsHardwareSetStatus(bool status);
#endif /* USE_LIGHTS */ #endif /* USE_LIGHTS */

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

@ -243,7 +243,7 @@
#define SYM_2RSS 0xEA // RSSI 2 #define SYM_2RSS 0xEA // RSSI 2
#define SYM_DB 0xEB // dB #define SYM_DB 0xEB // dB
#define SYM_DBM 0xEC // dBm #define SYM_DBM 0xEC // dBm
#define SYM_SRN 0xEE // SNR #define SYM_SNR 0xEE // SNR
#define SYM_MW 0xED // mW #define SYM_MW 0xED // mW
#else #else

70
src/main/drivers/pwm_output.c
View file

@ -28,6 +28,7 @@ FILE_COMPILE_FOR_SPEED
#include "common/log.h" #include "common/log.h"
#include "common/maths.h" #include "common/maths.h"
#include "common/circular_queue.h"
#include "drivers/io.h" #include "drivers/io.h"
#include "drivers/timer.h" #include "drivers/timer.h"
@ -60,6 +61,10 @@ FILE_COMPILE_FOR_SPEED
#define DSHOT_MOTOR_BITLENGTH 20 #define DSHOT_MOTOR_BITLENGTH 20
#define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */ #define DSHOT_DMA_BUFFER_SIZE 18 /* resolution + frame reset (2us) */
#define DSHOT_COMMAND_INTERVAL_US 1000
#define DSHOT_COMMAND_QUEUE_LENGTH 8
#define DHSOT_COMMAND_QUEUE_SIZE DSHOT_COMMAND_QUEUE_LENGTH * sizeof(dshotCommands_e)
#endif #endif
typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors typedef void (*pwmWriteFuncPtr)(uint8_t index, uint16_t value); // function pointer used to write motors
@ -110,6 +115,12 @@ static uint8_t allocatedOutputPortCount = 0;
static bool pwmMotorsEnabled = true; static bool pwmMotorsEnabled = true;
#ifdef USE_DSHOT
static circularBuffer_t commandsCircularBuffer;
static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE];
static currentExecutingCommand_t currentExecutingCommand;
#endif
static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value)
{ {
p->tch = tch; p->tch = tch;
@ -340,8 +351,60 @@ void pwmRequestMotorTelemetry(int motorIndex)
} }
} }
void pwmCompleteMotorUpdate(void) #ifdef USE_DSHOT
{ void sendDShotCommand(dshotCommands_e cmd) {
circularBufferPushElement(&commandsCircularBuffer, (uint8_t *) &cmd);
}
void initDShotCommands(void) {
circularBufferInit(&commandsCircularBuffer, commandsBuff,DHSOT_COMMAND_QUEUE_SIZE, sizeof(dshotCommands_e));
currentExecutingCommand.remainingRepeats = 0;
}
static int getDShotCommandRepeats(dshotCommands_e cmd) {
int repeats = 1;
switch (cmd) {
case DSHOT_CMD_SPIN_DIRECTION_NORMAL:
case DSHOT_CMD_SPIN_DIRECTION_REVERSED:
repeats = 6;
break;
default:
break;
}
return repeats;
}
static void executeDShotCommands(void){
if(currentExecutingCommand.remainingRepeats == 0) {
const int isTherePendingCommands = !circularBufferIsEmpty(&commandsCircularBuffer);
if (isTherePendingCommands) {
//Load the command
dshotCommands_e cmd;
circularBufferPopHead(&commandsCircularBuffer, (uint8_t *) &cmd);
currentExecutingCommand.cmd = cmd;
currentExecutingCommand.remainingRepeats = getDShotCommandRepeats(cmd);
}
else {
return;
}
}
for (uint8_t i = 0; i < getMotorCount(); i++) {
motors[i].requestTelemetry = true;
motors[i].value = currentExecutingCommand.cmd;
}
currentExecutingCommand.remainingRepeats--;
}
#endif
void pwmCompleteMotorUpdate(void) {
// This only makes sense for digital motor protocols // This only makes sense for digital motor protocols
if (!isMotorProtocolDigital()) { if (!isMotorProtocolDigital()) {
return; return;
@ -359,6 +422,9 @@ void pwmCompleteMotorUpdate(void)
#ifdef USE_DSHOT #ifdef USE_DSHOT
if (isMotorProtocolDshot()) { if (isMotorProtocolDshot()) {
executeDShotCommands();
// Generate DMA buffers // Generate DMA buffers
for (int index = 0; index < motorCount; index++) { for (int index = 0; index < motorCount; index++) {
if (motors[index].pwmPort && motors[index].pwmPort->configured) { if (motors[index].pwmPort && motors[index].pwmPort->configured) {

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

@ -20,6 +20,16 @@
#include "drivers/io_types.h" #include "drivers/io_types.h"
#include "drivers/time.h" #include "drivers/time.h"
typedef enum {
DSHOT_CMD_SPIN_DIRECTION_NORMAL = 20,
DSHOT_CMD_SPIN_DIRECTION_REVERSED = 21,
} dshotCommands_e;
typedef struct {
dshotCommands_e cmd;
int remainingRepeats;
} currentExecutingCommand_t;
void pwmRequestMotorTelemetry(int motorIndex); void pwmRequestMotorTelemetry(int motorIndex);
ioTag_t pwmGetMotorPinTag(int motorIndex); ioTag_t pwmGetMotorPinTag(int motorIndex);
@ -28,6 +38,7 @@ void pwmWriteMotor(uint8_t index, uint16_t value);
void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmShutdownPulsesForAllMotors(uint8_t motorCount);
void pwmCompleteMotorUpdate(void); void pwmCompleteMotorUpdate(void);
bool isMotorProtocolDigital(void); bool isMotorProtocolDigital(void);
bool isMotorProtocolDshot(void);
void pwmWriteServo(uint8_t index, uint16_t value); void pwmWriteServo(uint8_t index, uint16_t value);
@ -41,4 +52,7 @@ bool pwmMotorConfig(const struct timerHardware_s *timerHardware, uint8_t motorIn
void pwmServoPreconfigure(void); void pwmServoPreconfigure(void);
bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput); bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput);
void pwmWriteBeeper(bool onoffBeep); void pwmWriteBeeper(bool onoffBeep);
void beeperPwmInit(ioTag_t tag, uint16_t frequency); void beeperPwmInit(ioTag_t tag, uint16_t frequency);
void sendDShotCommand(dshotCommands_e directionSpin);
void initDShotCommands(void);

132
src/main/drivers/rangefinder/rangefinder_us42.c Normal file
View file

@ -0,0 +1,132 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include "platform.h"
#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_US42)
#include "build/build_config.h"
#include "drivers/time.h"
#include "drivers/bus_i2c.h"
#include "drivers/rangefinder/rangefinder.h"
#include "drivers/rangefinder/rangefinder_us42.h"
#include "build/debug.h"
// GY-US42(v2) Ultrasonic Range Sensor
#define US42_MAX_RANGE_CM 400 // vcc=3.3v -> 500cm; vcc=5v -> 700cm; Ardupilot recommends a maximum of 400cm
#define US42_DETECTION_CONE_DECIDEGREES 250
#define US42_DETECTION_CONE_EXTENDED_DECIDEGREES 300
#define US42_MIN_PROBE_INTERVAL 50
#define US42_I2C_ADDRESS 0x70
#define US42_I2C_REGISTRY_BASE 0x00
#define US42_I2C_REGISTRY_PROBE 0x51
#define US42_I2C_REGISTRY_STATUS_OK 0x00
#define US42_I2C_REGISTRY_DISTANCE_HIGH 0x00
#define US42_I2C_REGISTRY_DISTANCE_LOW 0x01
volatile int32_t us42MeasurementCm = RANGEFINDER_OUT_OF_RANGE;
static int16_t minimumReadingIntervalMs = US42_MIN_PROBE_INTERVAL;
static uint32_t timeOfLastMeasurementMs;
uint8_t nullProbeCommandValue[0];
static bool isUs42Responding = false;
static void us42Init(rangefinderDev_t *rangefinder)
{
busWriteBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0);
}
void us42Update(rangefinderDev_t *rangefinder)
{
uint8_t data[2];
isUs42Responding = busReadBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, data, 2);
if (isUs42Responding) {
us42MeasurementCm = (int32_t)data[0] << 8 | (int32_t)data[1];
if (us42MeasurementCm > US42_MAX_RANGE_CM) {
us42MeasurementCm = RANGEFINDER_OUT_OF_RANGE;
}
} else {
us42MeasurementCm = RANGEFINDER_HARDWARE_FAILURE;
}
const timeMs_t timeNowMs = millis();
if (timeNowMs > timeOfLastMeasurementMs + minimumReadingIntervalMs) {
// measurement repeat interval should be greater than minimumReadingIntervalMs
// to avoid interference between connective measurements.
timeOfLastMeasurementMs = timeNowMs;
busWriteBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0);
}
}
/**
* Get the distance that was measured by the last pulse, in centimeters.
*/
int32_t us42GetDistance(rangefinderDev_t *rangefinder)
{
UNUSED(rangefinder);
return us42MeasurementCm;
}
static bool deviceDetect(busDevice_t * busDev)
{
for (int retry = 0; retry < 5; retry++) {
uint8_t inquiryResult;
delay(150);
bool ack = busRead(busDev, US42_I2C_REGISTRY_BASE, &inquiryResult);
if (ack && inquiryResult == US42_I2C_REGISTRY_STATUS_OK) {
return true;
}
};
return false;
}
bool us42Detect(rangefinderDev_t *rangefinder)
{
rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_US42, 0, OWNER_RANGEFINDER);
if (rangefinder->busDev == NULL) {
return false;
}
if (!deviceDetect(rangefinder->busDev)) {
busDeviceDeInit(rangefinder->busDev);
return false;
}
rangefinder->delayMs = RANGEFINDER_US42_TASK_PERIOD_MS;
rangefinder->maxRangeCm = US42_MAX_RANGE_CM;
rangefinder->detectionConeDeciDegrees = US42_DETECTION_CONE_DECIDEGREES;
rangefinder->detectionConeExtendedDeciDegrees = US42_DETECTION_CONE_EXTENDED_DECIDEGREES;
rangefinder->init = &us42Init;
rangefinder->update = &us42Update;
rangefinder->read = &us42GetDistance;
return true;
}
#endif

22
src/main/drivers/rangefinder/rangefinder_us42.h Normal file
View file

@ -0,0 +1,22 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight 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.
*
* Cleanflight 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 Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#define RANGEFINDER_US42_TASK_PERIOD_MS 100
bool us42Detect(rangefinderDev_t *dev);

1696
src/main/drivers/rangefinder/rangefinder_vl53l1x.c Normal file
View file

File diff suppressed because it is too large Load diff

29
src/main/drivers/rangefinder/rangefinder_vl53l1x.h Normal file
View file

@ -0,0 +1,29 @@
/*
* This file is part of INAV.
*
* 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 RANGEFINDER_VL53L1X_TASK_PERIOD_MS (40)
bool vl53l1xDetect(rangefinderDev_t *dev);

2
src/main/drivers/sdcard/sdmmc_sdio_f4xx.c
View file

@ -1081,7 +1081,7 @@ SD_Error_t SD_GetStatus(void)
} }
} }
else { else {
ErrorState = SD_CARD_ERROR; ErrorState = SD_ERROR;
} }
return ErrorState; return ErrorState;

53
src/main/fc/cli.c
View file

@ -85,6 +85,7 @@ uint8_t cliMode = 0;
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "flight/secondary_imu.h"
#include "io/asyncfatfs/asyncfatfs.h" #include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h" #include "io/beeper.h"
@ -2918,6 +2919,55 @@ static void cliBatch(char *cmdline)
} }
#endif #endif
#ifdef USE_SECONDARY_IMU
static void printImu2Status(void)
{
cliPrintLinef("Secondary IMU active: %d", secondaryImuState.active);
cliPrintLine("Calibration status:");
cliPrintLinef("Sys: %d", secondaryImuState.calibrationStatus.sys);
cliPrintLinef("Gyro: %d", secondaryImuState.calibrationStatus.gyr);
cliPrintLinef("Acc: %d", secondaryImuState.calibrationStatus.acc);
cliPrintLinef("Mag: %d", secondaryImuState.calibrationStatus.mag);
cliPrintLine("Calibration gains:");
cliPrintLinef(
"Gyro: %d %d %d",
secondaryImuConfig()->calibrationOffsetGyro[X],
secondaryImuConfig()->calibrationOffsetGyro[Y],
secondaryImuConfig()->calibrationOffsetGyro[Z]
);
cliPrintLinef(
"Acc: %d %d %d",
secondaryImuConfig()->calibrationOffsetAcc[X],
secondaryImuConfig()->calibrationOffsetAcc[Y],
secondaryImuConfig()->calibrationOffsetAcc[Z]
);
cliPrintLinef(
"Mag: %d %d %d",
secondaryImuConfig()->calibrationOffsetMag[X],
secondaryImuConfig()->calibrationOffsetMag[Y],
secondaryImuConfig()->calibrationOffsetMag[Z]
);
cliPrintLinef(
"Radius: %d %d",
secondaryImuConfig()->calibrationRadiusAcc,
secondaryImuConfig()->calibrationRadiusMag
);
}
static void cliImu2(char *cmdline)
{
if (sl_strcasecmp(cmdline, "fetch") == 0) {
secondaryImuFetchCalibration();
printImu2Status();
} else {
printImu2Status();
}
}
#endif
static void cliSave(char *cmdline) static void cliSave(char *cmdline)
{ {
UNUSED(cmdline); UNUSED(cmdline);
@ -3666,6 +3716,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource),
#endif #endif
CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange),
#ifdef USE_SECONDARY_IMU
CLI_COMMAND_DEF("imu2", "Secondary IMU", NULL, cliImu2),
#endif
#if defined(USE_SAFE_HOME) #if defined(USE_SAFE_HOME)
CLI_COMMAND_DEF("safehome", "safe home list", NULL, cliSafeHomes), CLI_COMMAND_DEF("safehome", "safe home list", NULL, cliSafeHomes),
#endif #endif

14
src/main/fc/config.c
View file

@ -112,11 +112,15 @@ PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG,
PG_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.current_profile_index = 0, .current_profile_index = 0,
.current_battery_profile_index = 0, .current_battery_profile_index = 0,
.debug_mode = DEBUG_NONE, .debug_mode = SETTING_DEBUG_MODE_DEFAULT,
.i2c_speed = I2C_SPEED_400KHZ, #ifdef USE_I2C
.cpuUnderclock = 0, .i2c_speed = SETTING_I2C_SPEED_DEFAULT,
.throttle_tilt_compensation_strength = 0, // 0-100, 0 - disabled #endif
.name = { 0 } #ifdef USE_UNDERCLOCK
.cpuUnderclock = SETTING_CPU_UNDERCLOCK_DEFAULT,
#endif
.throttle_tilt_compensation_strength = SETTING_THROTTLE_TILT_COMP_STR_DEFAULT, // 0-100, 0 - disabled
.name = SETTING_NAME_DEFAULT
); );
PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0);

4
src/main/fc/config.h
View file

@ -73,8 +73,12 @@ typedef struct systemConfig_s {
uint8_t current_profile_index; uint8_t current_profile_index;
uint8_t current_battery_profile_index; uint8_t current_battery_profile_index;
uint8_t debug_mode; uint8_t debug_mode;
#ifdef USE_I2C
uint8_t i2c_speed; uint8_t i2c_speed;
#endif
#ifdef USE_UNDERCLOCK
uint8_t cpuUnderclock; uint8_t cpuUnderclock;
#endif
uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle.
char name[MAX_NAME_LENGTH + 1]; char name[MAX_NAME_LENGTH + 1];
} systemConfig_t; } systemConfig_t;

33
src/main/fc/controlrate_profile.c
View file

@ -29,6 +29,7 @@
#include "fc/config.h" #include "fc/config.h"
#include "fc/controlrate_profile.h" #include "fc/controlrate_profile.h"
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
#include "fc/settings.h"
const controlRateConfig_t *currentControlRateProfile; const controlRateConfig_t *currentControlRateProfile;
@ -40,31 +41,31 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance)
for (int i = 0; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) { for (int i = 0; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) {
RESET_CONFIG(controlRateConfig_t, &instance[i], RESET_CONFIG(controlRateConfig_t, &instance[i],
.throttle = { .throttle = {
.rcMid8 = 50, .rcMid8 = SETTING_THR_MID_DEFAULT,
.rcExpo8 = 0, .rcExpo8 = SETTING_THR_EXPO_DEFAULT,
.dynPID = 0, .dynPID = SETTING_TPA_RATE_DEFAULT,
.pa_breakpoint = 1500, .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT,
.fixedWingTauMs = 0 .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT
}, },
.stabilized = { .stabilized = {
.rcExpo8 = 70, .rcExpo8 = SETTING_RC_EXPO_DEFAULT,
.rcYawExpo8 = 20, .rcYawExpo8 = SETTING_RC_YAW_EXPO_DEFAULT,
.rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, .rates[FD_ROLL] = SETTING_ROLL_RATE_DEFAULT,
.rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, .rates[FD_PITCH] = SETTING_PITCH_RATE_DEFAULT,
.rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT, .rates[FD_YAW] = SETTING_YAW_RATE_DEFAULT,
}, },
.manual = { .manual = {
.rcExpo8 = 70, .rcExpo8 = SETTING_MANUAL_RC_EXPO_DEFAULT,
.rcYawExpo8 = 20, .rcYawExpo8 = SETTING_MANUAL_RC_YAW_EXPO_DEFAULT,
.rates[FD_ROLL] = 100, .rates[FD_ROLL] = SETTING_MANUAL_ROLL_RATE_DEFAULT,
.rates[FD_PITCH] = 100, .rates[FD_PITCH] = SETTING_MANUAL_PITCH_RATE_DEFAULT,
.rates[FD_YAW] = 100 .rates[FD_YAW] = SETTING_MANUAL_YAW_RATE_DEFAULT
}, },
.misc = { .misc = {
.fpvCamAngleDegrees = 0 .fpvCamAngleDegrees = SETTING_FPV_MIX_DEGREES_DEFAULT
} }
); );
} }

2
src/main/fc/controlrate_profile.h
View file

@ -33,10 +33,8 @@ and so on.
*/ */
#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 180 #define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 180
#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN 6 #define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN 6
#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT 20
#define CONTROL_RATE_CONFIG_YAW_RATE_MAX 180 #define CONTROL_RATE_CONFIG_YAW_RATE_MAX 180
#define CONTROL_RATE_CONFIG_YAW_RATE_MIN 2 #define CONTROL_RATE_CONFIG_YAW_RATE_MIN 2
#define CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT 20
#define CONTROL_RATE_CONFIG_TPA_MAX 100 #define CONTROL_RATE_CONFIG_TPA_MAX 100

54
src/main/fc/fc_core.c
View file

@ -38,6 +38,7 @@ FILE_COMPILE_FOR_SPEED
#include "drivers/time.h" #include "drivers/time.h"
#include "drivers/system.h" #include "drivers/system.h"
#include "drivers/pwm_output.h" #include "drivers/pwm_output.h"
#include "drivers/accgyro/accgyro_bno055.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/diagnostics.h" #include "sensors/diagnostics.h"
@ -89,6 +90,7 @@ FILE_COMPILE_FOR_SPEED
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "config/feature.h" #include "config/feature.h"
#include "common/vector.h"
#include "programming/pid.h" #include "programming/pid.h"
// June 2013 V2.2-dev // June 2013 V2.2-dev
@ -131,6 +133,9 @@ static uint32_t gyroSyncFailureCount;
static disarmReason_t lastDisarmReason = DISARM_NONE; static disarmReason_t lastDisarmReason = DISARM_NONE;
static emergencyArmingState_t emergencyArming; static emergencyArmingState_t emergencyArming;
static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible
static timeMs_t prearmActivationTime = 0;
bool isCalibrating(void) bool isCalibrating(void)
{ {
#ifdef USE_BARO #ifdef USE_BARO
@ -295,6 +300,22 @@ static void updateArmingStatus(void)
DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM);
} }
if (isModeActivationConditionPresent(BOXPREARM)) {
if (IS_RC_MODE_ACTIVE(BOXPREARM)) {
if (prearmWasReset && (armingConfig()->prearmTimeoutMs == 0 || millis() - prearmActivationTime < armingConfig()->prearmTimeoutMs)) {
DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
} else {
ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
}
} else {
prearmWasReset = true;
prearmActivationTime = millis();
ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
}
} else {
DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM);
}
/* CHECK: Arming switch */ /* CHECK: Arming switch */
// If arming is disabled and the ARM switch is on // If arming is disabled and the ARM switch is on
// Note that this should be last check so all other blockers could be cleared correctly // Note that this should be last check so all other blockers could be cleared correctly
@ -388,11 +409,22 @@ void disarm(disarmReason_t disarmReason)
blackboxFinish(); blackboxFinish();
} }
#endif #endif
#ifdef USE_DSHOT
if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL);
DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH);
}
#endif
statsOnDisarm(); statsOnDisarm();
logicConditionReset(); logicConditionReset();
#ifdef USE_PROGRAMMING_FRAMEWORK
programmingPidReset(); programmingPidReset();
#endif
beeper(BEEPER_DISARMING); // emit disarm tone beeper(BEEPER_DISARMING); // emit disarm tone
prearmWasReset = false;
} }
} }
@ -447,6 +479,21 @@ void releaseSharedTelemetryPorts(void) {
void tryArm(void) void tryArm(void)
{ {
updateArmingStatus(); updateArmingStatus();
#ifdef USE_DSHOT
if (
STATE(MULTIROTOR) &&
IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) &&
emergencyArmingCanOverrideArmingDisabled() &&
isMotorProtocolDshot() &&
!FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)
) {
sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED);
ENABLE_ARMING_FLAG(ARMED);
enableFlightMode(FLIP_OVER_AFTER_CRASH);
return;
}
#endif
#ifdef USE_PROGRAMMING_FRAMEWORK #ifdef USE_PROGRAMMING_FRAMEWORK
if ( if (
!isArmingDisabled() || !isArmingDisabled() ||
@ -482,7 +529,11 @@ void tryArm(void)
//It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction //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); ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD);
logicConditionReset(); logicConditionReset();
#ifdef USE_PROGRAMMING_FRAMEWORK
programmingPidReset(); programmingPidReset();
#endif
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
@ -506,6 +557,7 @@ void tryArm(void)
#else #else
beeper(BEEPER_ARMING); beeper(BEEPER_ARMING);
#endif #endif
statsOnArm(); statsOnArm();
return; return;

2
src/main/fc/fc_core.h
View file

@ -45,4 +45,4 @@ void emergencyArmingUpdate(bool armingSwitchIsOn);
bool isCalibrating(void); bool isCalibrating(void);
float getFlightTime(void); float getFlightTime(void);
void fcReboot(bool bootLoader); void fcReboot(bool bootLoader);

13
src/main/fc/fc_init.c
View file

@ -99,6 +99,7 @@
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/secondary_imu.h"
#include "io/asyncfatfs/asyncfatfs.h" #include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h" #include "io/beeper.h"
@ -219,8 +220,12 @@ void init(void)
ensureEEPROMContainsValidData(); ensureEEPROMContainsValidData();
readEEPROM(); readEEPROM();
#ifdef USE_UNDERCLOCK
// Re-initialize system clock to their final values (if necessary) // Re-initialize system clock to their final values (if necessary)
systemClockSetup(systemConfig()->cpuUnderclock); systemClockSetup(systemConfig()->cpuUnderclock);
#else
systemClockSetup(false);
#endif
#ifdef USE_I2C #ifdef USE_I2C
i2cSetSpeed(systemConfig()->i2c_speed); i2cSetSpeed(systemConfig()->i2c_speed);
@ -673,9 +678,17 @@ void init(void)
rcdeviceInit(); rcdeviceInit();
#endif // USE_RCDEVICE #endif // USE_RCDEVICE
#ifdef USE_DSHOT
initDShotCommands();
#endif
// Latch active features AGAIN since some may be modified by init(). // Latch active features AGAIN since some may be modified by init().
latchActiveFeatures(); latchActiveFeatures();
motorControlEnable = true; motorControlEnable = true;
#ifdef USE_SECONDARY_IMU
secondaryImuInit();
#endif
fcTasksInit(); fcTasksInit();
#ifdef USE_OSD #ifdef USE_OSD

109
src/main/fc/fc_msp.c
View file

@ -487,7 +487,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, gyroRateDps(i)); sbufWriteU16(dst, gyroRateDps(i));
} }
for (int i = 0; i < 3; i++) { for (int i = 0; i < 3; i++) {
#ifdef USE_MAG
sbufWriteU16(dst, mag.magADC[i]); sbufWriteU16(dst, mag.magADC[i]);
#else
sbufWriteU16(dst, 0);
#endif
} }
} }
break; break;
@ -567,6 +571,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, programmingPids(i)->gains.FF); sbufWriteU16(dst, programmingPids(i)->gains.FF);
} }
break; break;
case MSP2_INAV_PROGRAMMING_PID_STATUS:
for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) {
sbufWriteU32(dst, programmingPidGetOutput(i));
}
break;
#endif #endif
case MSP2_COMMON_MOTOR_MIXER: case MSP2_COMMON_MOTOR_MIXER:
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
@ -780,12 +789,23 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, rxConfig()->rssi_channel); sbufWriteU8(dst, rxConfig()->rssi_channel);
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
#ifdef USE_MAG
sbufWriteU16(dst, compassConfig()->mag_declination / 10); sbufWriteU16(dst, compassConfig()->mag_declination / 10);
#else
sbufWriteU16(dst, 0);
#endif
#ifdef USE_ADC
sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10); sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10);
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
#endif
break; break;
case MSP2_INAV_MISC: case MSP2_INAV_MISC:
@ -808,8 +828,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#endif #endif
sbufWriteU8(dst, rxConfig()->rssi_channel); sbufWriteU8(dst, rxConfig()->rssi_channel);
#ifdef USE_MAG
sbufWriteU16(dst, compassConfig()->mag_declination / 10); sbufWriteU16(dst, compassConfig()->mag_declination / 10);
#else
sbufWriteU16(dst, 0);
#endif
#ifdef USE_ADC
sbufWriteU16(dst, batteryMetersConfig()->voltage.scale); sbufWriteU16(dst, batteryMetersConfig()->voltage.scale);
sbufWriteU8(dst, batteryMetersConfig()->voltageSource); sbufWriteU8(dst, batteryMetersConfig()->voltageSource);
sbufWriteU8(dst, currentBatteryProfile->cells); sbufWriteU8(dst, currentBatteryProfile->cells);
@ -817,6 +842,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin); sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax); sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning); sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning);
#else
sbufWriteU16(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
#endif
sbufWriteU32(dst, currentBatteryProfile->capacity.value); sbufWriteU32(dst, currentBatteryProfile->capacity.value);
sbufWriteU32(dst, currentBatteryProfile->capacity.warning); sbufWriteU32(dst, currentBatteryProfile->capacity.warning);
@ -836,6 +870,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP2_INAV_BATTERY_CONFIG: case MSP2_INAV_BATTERY_CONFIG:
#ifdef USE_ADC
sbufWriteU16(dst, batteryMetersConfig()->voltage.scale); sbufWriteU16(dst, batteryMetersConfig()->voltage.scale);
sbufWriteU8(dst, batteryMetersConfig()->voltageSource); sbufWriteU8(dst, batteryMetersConfig()->voltageSource);
sbufWriteU8(dst, currentBatteryProfile->cells); sbufWriteU8(dst, currentBatteryProfile->cells);
@ -843,6 +878,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin); sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax); sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax);
sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning); sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning);
#else
sbufWriteU16(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 0);
#endif
sbufWriteU16(dst, batteryMetersConfig()->current.offset); sbufWriteU16(dst, batteryMetersConfig()->current.offset);
sbufWriteU16(dst, batteryMetersConfig()->current.scale); sbufWriteU16(dst, batteryMetersConfig()->current.scale);
@ -942,10 +986,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
break; break;
case MSP_VOLTAGE_METER_CONFIG: case MSP_VOLTAGE_METER_CONFIG:
#ifdef USE_ADC
sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10); sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10);
sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10);
#else
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, 0);
#endif
break; break;
case MSP_CURRENT_METER_CONFIG: case MSP_CURRENT_METER_CONFIG:
@ -964,15 +1015,25 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU16(dst, rxConfig()->maxcheck); sbufWriteU16(dst, rxConfig()->maxcheck);
sbufWriteU16(dst, PWM_RANGE_MIDDLE); sbufWriteU16(dst, PWM_RANGE_MIDDLE);
sbufWriteU16(dst, rxConfig()->mincheck); sbufWriteU16(dst, rxConfig()->mincheck);
#ifdef USE_SPEKTRUM_BIND
sbufWriteU8(dst, rxConfig()->spektrum_sat_bind); sbufWriteU8(dst, rxConfig()->spektrum_sat_bind);
#else
sbufWriteU8(dst, 0);
#endif
sbufWriteU16(dst, rxConfig()->rx_min_usec); sbufWriteU16(dst, rxConfig()->rx_min_usec);
sbufWriteU16(dst, rxConfig()->rx_max_usec); sbufWriteU16(dst, rxConfig()->rx_max_usec);
sbufWriteU8(dst, 0); // for compatibility with betaflight (rcInterpolation) sbufWriteU8(dst, 0); // for compatibility with betaflight (rcInterpolation)
sbufWriteU8(dst, 0); // for compatibility with betaflight (rcInterpolationInterval) sbufWriteU8(dst, 0); // for compatibility with betaflight (rcInterpolationInterval)
sbufWriteU16(dst, 0); // for compatibility with betaflight (airModeActivateThreshold) sbufWriteU16(dst, 0); // for compatibility with betaflight (airModeActivateThreshold)
#ifdef USE_RX_SPI
sbufWriteU8(dst, rxConfig()->rx_spi_protocol); sbufWriteU8(dst, rxConfig()->rx_spi_protocol);
sbufWriteU32(dst, rxConfig()->rx_spi_id); sbufWriteU32(dst, rxConfig()->rx_spi_id);
sbufWriteU8(dst, rxConfig()->rx_spi_rf_channel_count); sbufWriteU8(dst, rxConfig()->rx_spi_rf_channel_count);
#else
sbufWriteU8(dst, 0);
sbufWriteU32(dst, 0);
sbufWriteU8(dst, 0);
#endif
sbufWriteU8(dst, 0); // for compatibility with betaflight (fpvCamAngleDegrees) sbufWriteU8(dst, 0); // for compatibility with betaflight (fpvCamAngleDegrees)
sbufWriteU8(dst, rxConfig()->receiverType); sbufWriteU8(dst, rxConfig()->receiverType);
break; break;
@ -1149,7 +1210,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP_SENSOR_ALIGNMENT: case MSP_SENSOR_ALIGNMENT:
sbufWriteU8(dst, gyroConfig()->gyro_align); sbufWriteU8(dst, gyroConfig()->gyro_align);
sbufWriteU8(dst, accelerometerConfig()->acc_align); sbufWriteU8(dst, accelerometerConfig()->acc_align);
#ifdef USE_MAG
sbufWriteU8(dst, compassConfig()->mag_align); sbufWriteU8(dst, compassConfig()->mag_align);
#else
sbufWriteU8(dst, 0);
#endif
#ifdef USE_OPFLOW #ifdef USE_OPFLOW
sbufWriteU8(dst, opticalFlowConfig()->opflow_align); sbufWriteU8(dst, opticalFlowConfig()->opflow_align);
#else #else
@ -1826,10 +1891,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU16(src); sbufReadU16(src);
#endif #endif
#ifdef USE_ADC
batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10; batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI
currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI
currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert
#else
sbufReadU8(src);
sbufReadU8(src);
sbufReadU8(src);
sbufReadU8(src);
#endif
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
@ -1864,6 +1936,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU16(src); sbufReadU16(src);
#endif #endif
#ifdef USE_ADC
batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src); batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src);
batteryMetersConfigMutable()->voltageSource = sbufReadU8(src); batteryMetersConfigMutable()->voltageSource = sbufReadU8(src);
currentBatteryProfileMutable->cells = sbufReadU8(src); currentBatteryProfileMutable->cells = sbufReadU8(src);
@ -1871,6 +1944,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src); currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src); currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src); currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src);
#else
sbufReadU16(src);
sbufReadU8(src);
sbufReadU8(src);
sbufReadU16(src);
sbufReadU16(src);
sbufReadU16(src);
sbufReadU16(src);
#endif
currentBatteryProfileMutable->capacity.value = sbufReadU32(src); currentBatteryProfileMutable->capacity.value = sbufReadU32(src);
currentBatteryProfileMutable->capacity.warning = sbufReadU32(src); currentBatteryProfileMutable->capacity.warning = sbufReadU32(src);
@ -1890,6 +1972,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP2_INAV_SET_BATTERY_CONFIG: case MSP2_INAV_SET_BATTERY_CONFIG:
if (dataSize == 29) { if (dataSize == 29) {
#ifdef USE_ADC
batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src); batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src);
batteryMetersConfigMutable()->voltageSource = sbufReadU8(src); batteryMetersConfigMutable()->voltageSource = sbufReadU8(src);
currentBatteryProfileMutable->cells = sbufReadU8(src); currentBatteryProfileMutable->cells = sbufReadU8(src);
@ -1897,6 +1980,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src); currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src); currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src);
currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src); currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src);
#else
sbufReadU16(src);
sbufReadU8(src);
sbufReadU8(src);
sbufReadU16(src);
sbufReadU16(src);
sbufReadU16(src);
sbufReadU16(src);
#endif
batteryMetersConfigMutable()->current.offset = sbufReadU16(src); batteryMetersConfigMutable()->current.offset = sbufReadU16(src);
batteryMetersConfigMutable()->current.scale = sbufReadU16(src); batteryMetersConfigMutable()->current.scale = sbufReadU16(src);
@ -2533,10 +2625,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_VOLTAGE_METER_CONFIG: case MSP_SET_VOLTAGE_METER_CONFIG:
if (dataSize >= 4) { if (dataSize >= 4) {
#ifdef USE_ADC
batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10; batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10;
currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10;
#else
sbufReadU8(src);
sbufReadU8(src);
sbufReadU8(src);
sbufReadU8(src);
#endif
} else } else
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
break; break;
@ -2565,15 +2664,25 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
rxConfigMutable()->maxcheck = sbufReadU16(src); rxConfigMutable()->maxcheck = sbufReadU16(src);
sbufReadU16(src); // midrc sbufReadU16(src); // midrc
rxConfigMutable()->mincheck = sbufReadU16(src); rxConfigMutable()->mincheck = sbufReadU16(src);
#ifdef USE_SPEKTRUM_BIND
rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src); rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src);
#else
sbufReadU8(src);
#endif
rxConfigMutable()->rx_min_usec = sbufReadU16(src); rxConfigMutable()->rx_min_usec = sbufReadU16(src);
rxConfigMutable()->rx_max_usec = sbufReadU16(src); rxConfigMutable()->rx_max_usec = sbufReadU16(src);
sbufReadU8(src); // for compatibility with betaflight (rcInterpolation) sbufReadU8(src); // for compatibility with betaflight (rcInterpolation)
sbufReadU8(src); // for compatibility with betaflight (rcInterpolationInterval) sbufReadU8(src); // for compatibility with betaflight (rcInterpolationInterval)
sbufReadU16(src); // for compatibility with betaflight (airModeActivateThreshold) sbufReadU16(src); // for compatibility with betaflight (airModeActivateThreshold)
#ifdef USE_RX_SPI
rxConfigMutable()->rx_spi_protocol = sbufReadU8(src); rxConfigMutable()->rx_spi_protocol = sbufReadU8(src);
rxConfigMutable()->rx_spi_id = sbufReadU32(src); rxConfigMutable()->rx_spi_id = sbufReadU32(src);
rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src); rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src);
#else
sbufReadU8(src);
sbufReadU32(src);
sbufReadU8(src);
#endif
sbufReadU8(src); // for compatibility with betaflight (fpvCamAngleDegrees) sbufReadU8(src); // for compatibility with betaflight (fpvCamAngleDegrees)
rxConfigMutable()->receiverType = sbufReadU8(src); // Won't be modified if buffer is not large enough rxConfigMutable()->receiverType = sbufReadU8(src); // Won't be modified if buffer is not large enough
} else } else

16
src/main/fc/fc_msp_box.c
View file

@ -33,6 +33,8 @@
#include "io/osd.h" #include "io/osd.h"
#include "drivers/pwm_output.h"
#include "sensors/diagnostics.h" #include "sensors/diagnostics.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
@ -80,12 +82,14 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXOSDALT1, "OSD ALT 1", 42 }, { BOXOSDALT1, "OSD ALT 1", 42 },
{ BOXOSDALT2, "OSD ALT 2", 43 }, { BOXOSDALT2, "OSD ALT 2", 43 },
{ BOXOSDALT3, "OSD ALT 3", 44 }, { BOXOSDALT3, "OSD ALT 3", 44 },
{ BOXNAVCRUISE, "NAV CRUISE", 45 }, { BOXNAVCOURSEHOLD, "NAV COURSE HOLD", 45 },
{ BOXBRAKING, "MC BRAKING", 46 }, { BOXBRAKING, "MC BRAKING", 46 },
{ BOXUSER1, "USER1", BOX_PERMANENT_ID_USER1 }, { BOXUSER1, "USER1", BOX_PERMANENT_ID_USER1 },
{ BOXUSER2, "USER2", BOX_PERMANENT_ID_USER2 }, { BOXUSER2, "USER2", BOX_PERMANENT_ID_USER2 },
{ BOXLOITERDIRCHN, "LOITER CHANGE", 49 }, { BOXLOITERDIRCHN, "LOITER CHANGE", 49 },
{ BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 }, { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 },
{ BOXPREARM, "PREARM", 51 },
{ BOXFLIPOVERAFTERCRASH, "TURTLE", 52 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF } { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
}; };
@ -163,6 +167,7 @@ void initActiveBoxIds(void)
activeBoxIdCount = 0; activeBoxIdCount = 0;
activeBoxIds[activeBoxIdCount++] = BOXARM; activeBoxIds[activeBoxIdCount++] = BOXARM;
activeBoxIds[activeBoxIdCount++] = BOXPREARM;
if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) { if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) {
activeBoxIds[activeBoxIdCount++] = BOXANGLE; activeBoxIds[activeBoxIdCount++] = BOXANGLE;
@ -214,7 +219,7 @@ void initActiveBoxIds(void)
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
if (STATE(AIRPLANE)) { if (STATE(AIRPLANE)) {
activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD;
} }
} }
} }
@ -304,6 +309,11 @@ void initActiveBoxIds(void)
#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE)
activeBoxIds[activeBoxIdCount++] = BOXMSPRCOVERRIDE; activeBoxIds[activeBoxIdCount++] = BOXMSPRCOVERRIDE;
#endif #endif
#ifdef USE_DSHOT
if(STATE(MULTIROTOR) && isMotorProtocolDshot())
activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH;
#endif
} }
#define IS_ENABLED(mask) (mask == 0 ? 0 : 1) #define IS_ENABLED(mask) (mask == 0 ? 0 : 1)
@ -335,7 +345,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_CRUISE_MODE)), BOXNAVCRUISE); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)), BOXNAVCOURSEHOLD);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH);
CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE);

4
src/main/fc/fc_msp_box.h
View file

@ -19,9 +19,7 @@
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#define BOX_PERMANENT_ID_USER1 47 #include "io/piniobox.h"
#define BOX_PERMANENT_ID_USER2 48
#define BOX_PERMANENT_ID_NONE 255 // A permanent ID for no box mode
typedef struct box_s { typedef struct box_s {
const uint8_t boxId; // see boxId_e const uint8_t boxId; // see boxId_e

12
src/main/fc/fc_tasks.c
View file

@ -47,6 +47,7 @@
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/wind_estimator.h" #include "flight/wind_estimator.h"
#include "flight/secondary_imu.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/servos.h" #include "flight/servos.h"
#include "flight/dynamic_lpf.h" #include "flight/dynamic_lpf.h"
@ -375,6 +376,9 @@ void fcTasksInit(void)
#if defined(USE_SMARTPORT_MASTER) #if defined(USE_SMARTPORT_MASTER)
setTaskEnabled(TASK_SMARTPORT_MASTER, true); setTaskEnabled(TASK_SMARTPORT_MASTER, true);
#endif #endif
#ifdef USE_SECONDARY_IMU
setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->hardwareType != SECONDARY_IMU_NONE && secondaryImuState.active);
#endif
} }
cfTask_t cfTasks[TASK_COUNT] = { cfTask_t cfTasks[TASK_COUNT] = {
@ -597,6 +601,14 @@ cfTask_t cfTasks[TASK_COUNT] = {
.staticPriority = TASK_PRIORITY_IDLE, .staticPriority = TASK_PRIORITY_IDLE,
}, },
#endif #endif
#ifdef USE_SECONDARY_IMU
[TASK_SECONDARY_IMU] = {
.taskName = "IMU2",
.taskFunc = taskSecondaryImu,
.desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz @100msec
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
#ifdef USE_RPM_FILTER #ifdef USE_RPM_FILTER
[TASK_RPM_FILTER] = { [TASK_RPM_FILTER] = {
.taskName = "RPM", .taskName = "RPM",

102
src/main/fc/rc_adjustments.c
View file

@ -101,7 +101,43 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.mode = ADJUSTMENT_MODE_STEP, .mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }} .data = { .stepConfig = { .step = 1 }}
}, { }, {
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D_FF, .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_FF,
.mode = ADJUSTMENT_MODE_STEP, .mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }} .data = { .stepConfig = { .step = 1 }}
}, { }, {
@ -113,7 +149,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.mode = ADJUSTMENT_MODE_STEP, .mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }} .data = { .stepConfig = { .step = 1 }}
}, { }, {
.adjustmentFunction = ADJUSTMENT_YAW_D_FF, .adjustmentFunction = ADJUSTMENT_YAW_D,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_YAW_FF,
.mode = ADJUSTMENT_MODE_STEP, .mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }} .data = { .stepConfig = { .step = 1 }}
}, { }, {
@ -128,30 +168,6 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU
.adjustmentFunction = ADJUSTMENT_ROLL_RATE, .adjustmentFunction = ADJUSTMENT_ROLL_RATE,
.mode = ADJUSTMENT_MODE_STEP, .mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }} .data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_PITCH_D_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_P,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_I,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, {
.adjustmentFunction = ADJUSTMENT_ROLL_D_FF,
.mode = ADJUSTMENT_MODE_STEP,
.data = { .stepConfig = { .step = 1 }}
}, { }, {
.adjustmentFunction = ADJUSTMENT_RC_YAW_EXPO, .adjustmentFunction = ADJUSTMENT_RC_YAW_EXPO,
.mode = ADJUSTMENT_MODE_STEP, .mode = ADJUSTMENT_MODE_STEP,
@ -447,18 +463,32 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta); applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta);
schedulePidGainsUpdate(); schedulePidGainsUpdate();
break; break;
case ADJUSTMENT_PITCH_ROLL_D_FF: case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D_FF: case ADJUSTMENT_PITCH_D:
applyAdjustmentPID(ADJUSTMENT_PITCH_D_FF, getD_FFRefByBank(pidBankMutable(), PID_PITCH), delta); applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta);
if (adjustmentFunction == ADJUSTMENT_PITCH_D_FF) { if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
schedulePidGainsUpdate(); schedulePidGainsUpdate();
break; break;
} }
// follow though for combined ADJUSTMENT_PITCH_ROLL_D // follow though for combined ADJUSTMENT_PITCH_ROLL_D
FALLTHROUGH; FALLTHROUGH;
case ADJUSTMENT_ROLL_D_FF: case ADJUSTMENT_ROLL_D:
applyAdjustmentPID(ADJUSTMENT_ROLL_D_FF, getD_FFRefByBank(pidBankMutable(), PID_ROLL), delta); applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_PITCH_ROLL_FF:
case ADJUSTMENT_PITCH_FF:
applyAdjustmentPID(ADJUSTMENT_PITCH_FF, &pidBankMutable()->pid[PID_PITCH].FF, delta);
if (adjustmentFunction == ADJUSTMENT_PITCH_FF) {
schedulePidGainsUpdate();
break;
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_FF
FALLTHROUGH;
case ADJUSTMENT_ROLL_FF:
applyAdjustmentPID(ADJUSTMENT_ROLL_FF, &pidBankMutable()->pid[PID_ROLL].FF, delta);
schedulePidGainsUpdate(); schedulePidGainsUpdate();
break; break;
case ADJUSTMENT_YAW_P: case ADJUSTMENT_YAW_P:
@ -469,8 +499,12 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta); applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta);
schedulePidGainsUpdate(); schedulePidGainsUpdate();
break; break;
case ADJUSTMENT_YAW_D_FF: case ADJUSTMENT_YAW_D:
applyAdjustmentPID(ADJUSTMENT_YAW_D_FF, getD_FFRefByBank(pidBankMutable(), PID_YAW), delta); applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_FF:
applyAdjustmentPID(ADJUSTMENT_YAW_FF, &pidBankMutable()->pid[PID_YAW].FF, delta);
schedulePidGainsUpdate(); schedulePidGainsUpdate();
break; break;
case ADJUSTMENT_NAV_FW_CRUISE_THR: case ADJUSTMENT_NAV_FW_CRUISE_THR:

94
src/main/fc/rc_adjustments.h
View file

@ -33,51 +33,55 @@ typedef enum {
ADJUSTMENT_YAW_RATE = 5, ADJUSTMENT_YAW_RATE = 5,
ADJUSTMENT_PITCH_ROLL_P = 6, ADJUSTMENT_PITCH_ROLL_P = 6,
ADJUSTMENT_PITCH_ROLL_I = 7, ADJUSTMENT_PITCH_ROLL_I = 7,
ADJUSTMENT_PITCH_ROLL_D_FF = 8, ADJUSTMENT_PITCH_ROLL_D = 8,
ADJUSTMENT_YAW_P = 9, ADJUSTMENT_PITCH_ROLL_FF = 9,
ADJUSTMENT_YAW_I = 10, ADJUSTMENT_PITCH_P = 10,
ADJUSTMENT_YAW_D_FF = 11, ADJUSTMENT_PITCH_I = 11,
ADJUSTMENT_RATE_PROFILE = 12, // Unused, placeholder for compatibility ADJUSTMENT_PITCH_D = 12,
ADJUSTMENT_PITCH_RATE = 13, ADJUSTMENT_PITCH_FF = 13,
ADJUSTMENT_ROLL_RATE = 14, ADJUSTMENT_ROLL_P = 14,
ADJUSTMENT_PITCH_P = 15, ADJUSTMENT_ROLL_I = 15,
ADJUSTMENT_PITCH_I = 16, ADJUSTMENT_ROLL_D = 16,
ADJUSTMENT_PITCH_D_FF = 17, ADJUSTMENT_ROLL_FF = 17,
ADJUSTMENT_ROLL_P = 18, ADJUSTMENT_YAW_P = 18,
ADJUSTMENT_ROLL_I = 19, ADJUSTMENT_YAW_I = 19,
ADJUSTMENT_ROLL_D_FF = 20, ADJUSTMENT_YAW_D = 20,
ADJUSTMENT_RC_YAW_EXPO = 21, ADJUSTMENT_YAW_FF = 21,
ADJUSTMENT_MANUAL_RC_EXPO = 22, ADJUSTMENT_RATE_PROFILE = 22, // Unused, placeholder for compatibility
ADJUSTMENT_MANUAL_RC_YAW_EXPO = 23, ADJUSTMENT_PITCH_RATE = 23,
ADJUSTMENT_MANUAL_PITCH_ROLL_RATE = 24, ADJUSTMENT_ROLL_RATE = 24,
ADJUSTMENT_MANUAL_ROLL_RATE = 25, ADJUSTMENT_RC_YAW_EXPO = 25,
ADJUSTMENT_MANUAL_PITCH_RATE = 26, ADJUSTMENT_MANUAL_RC_EXPO = 26,
ADJUSTMENT_MANUAL_YAW_RATE = 27, ADJUSTMENT_MANUAL_RC_YAW_EXPO = 27,
ADJUSTMENT_NAV_FW_CRUISE_THR = 28, ADJUSTMENT_MANUAL_PITCH_ROLL_RATE = 28,
ADJUSTMENT_NAV_FW_PITCH2THR = 29, ADJUSTMENT_MANUAL_ROLL_RATE = 29,
ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 30, ADJUSTMENT_MANUAL_PITCH_RATE = 30,
ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 31, ADJUSTMENT_MANUAL_YAW_RATE = 31,
ADJUSTMENT_LEVEL_P = 32, ADJUSTMENT_NAV_FW_CRUISE_THR = 32,
ADJUSTMENT_LEVEL_I = 33, ADJUSTMENT_NAV_FW_PITCH2THR = 33,
ADJUSTMENT_LEVEL_D = 34, ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 34,
ADJUSTMENT_POS_XY_P = 35, ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 35,
ADJUSTMENT_POS_XY_I = 36, ADJUSTMENT_LEVEL_P = 36,
ADJUSTMENT_POS_XY_D = 37, ADJUSTMENT_LEVEL_I = 37,
ADJUSTMENT_POS_Z_P = 38, ADJUSTMENT_LEVEL_D = 38,
ADJUSTMENT_POS_Z_I = 39, ADJUSTMENT_POS_XY_P = 39,
ADJUSTMENT_POS_Z_D = 40, ADJUSTMENT_POS_XY_I = 40,
ADJUSTMENT_HEADING_P = 41, ADJUSTMENT_POS_XY_D = 41,
ADJUSTMENT_VEL_XY_P = 42, ADJUSTMENT_POS_Z_P = 42,
ADJUSTMENT_VEL_XY_I = 43, ADJUSTMENT_POS_Z_I = 43,
ADJUSTMENT_VEL_XY_D = 44, ADJUSTMENT_POS_Z_D = 44,
ADJUSTMENT_VEL_Z_P = 45, ADJUSTMENT_HEADING_P = 45,
ADJUSTMENT_VEL_Z_I = 46, ADJUSTMENT_VEL_XY_P = 46,
ADJUSTMENT_VEL_Z_D = 47, ADJUSTMENT_VEL_XY_I = 47,
ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48, ADJUSTMENT_VEL_XY_D = 48,
ADJUSTMENT_VTX_POWER_LEVEL = 49, ADJUSTMENT_VEL_Z_P = 49,
ADJUSTMENT_TPA = 50, ADJUSTMENT_VEL_Z_I = 50,
ADJUSTMENT_TPA_BREAKPOINT = 51, ADJUSTMENT_VEL_Z_D = 51,
ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 52, ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 52,
ADJUSTMENT_VTX_POWER_LEVEL = 53,
ADJUSTMENT_TPA = 54,
ADJUSTMENT_TPA_BREAKPOINT = 55,
ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 56,
ADJUSTMENT_FUNCTION_COUNT // must be last ADJUSTMENT_FUNCTION_COUNT // must be last
} adjustmentFunction_e; } adjustmentFunction_e;

25
src/main/fc/rc_controls.c
View file

@ -44,6 +44,7 @@
#include "fc/rc_curves.h" #include "fc/rc_curves.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/settings.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
@ -64,29 +65,31 @@
#define AIRMODE_DEADBAND 25 #define AIRMODE_DEADBAND 25
#define MIN_RC_TICK_INTERVAL_MS 20 #define MIN_RC_TICK_INTERVAL_MS 20
#define DEFAULT_RC_SWITCH_DISARM_DELAY_MS 250 // Wait at least 250ms before disarming via switch #define DEFAULT_RC_SWITCH_DISARM_DELAY_MS 250 // Wait at least 250ms before disarming via switch
#define DEFAULT_PREARM_TIMEOUT 10000 // Prearm is invalidated after 10 seconds
stickPositions_e rcStickPositions; stickPositions_e rcStickPositions;
FASTRAM int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW FASTRAM int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 2);
PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
.deadband = 5, .deadband = SETTING_DEADBAND_DEFAULT,
.yaw_deadband = 5, .yaw_deadband = SETTING_YAW_DEADBAND_DEFAULT,
.pos_hold_deadband = 10, .pos_hold_deadband = SETTING_POS_HOLD_DEADBAND_DEFAULT,
.alt_hold_deadband = 50, .alt_hold_deadband = SETTING_ALT_HOLD_DEADBAND_DEFAULT,
.mid_throttle_deadband = 50, .mid_throttle_deadband = SETTING_3D_DEADBAND_THROTTLE_DEFAULT,
.airmodeHandlingType = STICK_CENTER, .airmodeHandlingType = SETTING_AIRMODE_TYPE_DEFAULT,
.airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD, .airmodeThrottleThreshold = SETTING_AIRMODE_THROTTLE_THRESHOLD_DEFAULT,
); );
PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 2); PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 2);
PG_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
.fixed_wing_auto_arm = 0, .fixed_wing_auto_arm = SETTING_FIXED_WING_AUTO_ARM_DEFAULT,
.disarm_kill_switch = 1, .disarm_kill_switch = SETTING_DISARM_KILL_SWITCH_DEFAULT,
.switchDisarmDelayMs = DEFAULT_RC_SWITCH_DISARM_DELAY_MS, .switchDisarmDelayMs = SETTING_SWITCH_DISARM_DELAY_DEFAULT,
.prearmTimeoutMs = SETTING_PREARM_TIMEOUT_DEFAULT,
); );
bool areSticksInApModePosition(uint16_t ap_mode) bool areSticksInApModePosition(uint16_t ap_mode)

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

@ -97,6 +97,7 @@ typedef struct armingConfig_s {
uint8_t fixed_wing_auto_arm; // Auto-arm fixed wing aircraft on throttle up and never disarm uint8_t fixed_wing_auto_arm; // Auto-arm fixed wing aircraft on throttle up and never disarm
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
uint16_t switchDisarmDelayMs; // additional delay between ARM box going off and actual disarm uint16_t switchDisarmDelayMs; // additional delay between ARM box going off and actual disarm
uint16_t prearmTimeoutMs; // duration for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout.
} armingConfig_t; } armingConfig_t;
PG_DECLARE(armingConfig_t, armingConfig); PG_DECLARE(armingConfig_t, armingConfig);

15
src/main/fc/rc_modes.c
View file

@ -34,6 +34,7 @@
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/settings.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -56,6 +57,10 @@ boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e
PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0);
PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0); PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0);
PG_RESET_TEMPLATE(modeActivationOperatorConfig_t, modeActivationOperatorConfig,
.modeActivationOperator = SETTING_MODE_RANGE_LOGIC_OPERATOR_DEFAULT
);
static void processAirmodeAirplane(void) { static void processAirmodeAirplane(void) {
if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) { if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
ENABLE_STATE(AIRMODE_ACTIVE); ENABLE_STATE(AIRMODE_ACTIVE);
@ -134,13 +139,7 @@ void rcModeUpdate(boxBitmask_t *newState)
bool isModeActivationConditionPresent(boxId_e modeId) bool isModeActivationConditionPresent(boxId_e modeId)
{ {
for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { return specifiedConditionCountPerMode[modeId] > 0;
if (modeActivationConditions(index)->modeId == modeId && IS_RANGE_USABLE(&modeActivationConditions(index)->range)) {
return true;
}
}
return false;
} }
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range)
@ -212,7 +211,7 @@ void updateUsedModeActivationConditionFlags(void)
#ifdef USE_NAV #ifdef USE_NAV
isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) || isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) ||
isModeActivationConditionPresent(BOXNAVRTH) || isModeActivationConditionPresent(BOXNAVRTH) ||
isModeActivationConditionPresent(BOXNAVCRUISE) || isModeActivationConditionPresent(BOXNAVCOURSEHOLD) ||
isModeActivationConditionPresent(BOXNAVWP); isModeActivationConditionPresent(BOXNAVWP);
#endif #endif
} }

8
src/main/fc/rc_modes.h
View file

@ -61,20 +61,22 @@ typedef enum {
BOXOSDALT1 = 32, BOXOSDALT1 = 32,
BOXOSDALT2 = 33, BOXOSDALT2 = 33,
BOXOSDALT3 = 34, BOXOSDALT3 = 34,
BOXNAVCRUISE = 35, BOXNAVCOURSEHOLD = 35,
BOXBRAKING = 36, BOXBRAKING = 36,
BOXUSER1 = 37, BOXUSER1 = 37,
BOXUSER2 = 38, BOXUSER2 = 38,
BOXFPVANGLEMIX = 39, BOXFPVANGLEMIX = 39,
BOXLOITERDIRCHN = 40, BOXLOITERDIRCHN = 40,
BOXMSPRCOVERRIDE = 41, BOXMSPRCOVERRIDE = 41,
BOXPREARM = 42,
BOXFLIPOVERAFTERCRASH = 43,
CHECKBOX_ITEM_COUNT CHECKBOX_ITEM_COUNT
} boxId_e; } boxId_e;
// type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior // type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior
typedef struct boxBitmask_s { BITARRAY_DECLARE(bits, CHECKBOX_ITEM_COUNT); } boxBitmask_t; typedef struct boxBitmask_s { BITARRAY_DECLARE(bits, CHECKBOX_ITEM_COUNT); } boxBitmask_t;
#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20 #define MAX_MODE_ACTIVATION_CONDITION_COUNT 40
#define CHANNEL_RANGE_MIN 900 #define CHANNEL_RANGE_MIN 900
#define CHANNEL_RANGE_MAX 2100 #define CHANNEL_RANGE_MAX 2100
@ -127,4 +129,4 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
void updateActivatedModes(void); void updateActivatedModes(void);
void updateUsedModeActivationConditionFlags(void); void updateUsedModeActivationConditionFlags(void);
void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm); void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm);

7
src/main/fc/runtime_config.c
View file

@ -36,7 +36,7 @@ const char *armingDisableFlagNames[]= {
"FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS", "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS",
"ACC", "ARMSW", "HWFAIL", "BOXFS", "KILLSW", "RX", "ACC", "ARMSW", "HWFAIL", "BOXFS", "KILLSW", "RX",
"THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM", "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM",
"SETTINGFAIL", "PWMOUT" "SETTINGFAIL", "PWMOUT", "NOPREARM"
}; };
#endif #endif
@ -57,7 +57,8 @@ const armingFlag_e armDisableReasonsChecklist[] = {
ARMING_DISABLED_OSD_MENU, ARMING_DISABLED_OSD_MENU,
ARMING_DISABLED_ROLLPITCH_NOT_CENTERED, ARMING_DISABLED_ROLLPITCH_NOT_CENTERED,
ARMING_DISABLED_SERVO_AUTOTRIM, ARMING_DISABLED_SERVO_AUTOTRIM,
ARMING_DISABLED_OOM ARMING_DISABLED_OOM,
ARMING_DISABLED_NO_PREARM
}; };
armingFlag_e isArmingDisabledReason(void) armingFlag_e isArmingDisabledReason(void)
@ -153,7 +154,7 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void)
if (FLIGHT_MODE(NAV_POSHOLD_MODE)) if (FLIGHT_MODE(NAV_POSHOLD_MODE))
return FLM_POSITION_HOLD; return FLM_POSITION_HOLD;
if (FLIGHT_MODE(NAV_CRUISE_MODE)) if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
return FLM_CRUISE; return FLM_CRUISE;
if (FLIGHT_MODE(NAV_WP_MODE)) if (FLIGHT_MODE(NAV_WP_MODE))

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

@ -23,7 +23,6 @@ typedef enum {
WAS_EVER_ARMED = (1 << 3), WAS_EVER_ARMED = (1 << 3),
ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7), ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7),
ARMING_DISABLED_NOT_LEVEL = (1 << 8), ARMING_DISABLED_NOT_LEVEL = (1 << 8),
ARMING_DISABLED_SENSORS_CALIBRATING = (1 << 9), ARMING_DISABLED_SENSORS_CALIBRATING = (1 << 9),
ARMING_DISABLED_SYSTEM_OVERLOADED = (1 << 10), ARMING_DISABLED_SYSTEM_OVERLOADED = (1 << 10),
@ -44,6 +43,7 @@ typedef enum {
ARMING_DISABLED_OOM = (1 << 25), ARMING_DISABLED_OOM = (1 << 25),
ARMING_DISABLED_INVALID_SETTING = (1 << 26), ARMING_DISABLED_INVALID_SETTING = (1 << 26),
ARMING_DISABLED_PWM_OUTPUT_ERROR = (1 << 27), ARMING_DISABLED_PWM_OUTPUT_ERROR = (1 << 27),
ARMING_DISABLED_NO_PREARM = (1 << 28),
ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING |
ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE |
@ -52,7 +52,7 @@ typedef enum {
ARMING_DISABLED_BOXKILLSWITCH | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | ARMING_DISABLED_BOXKILLSWITCH | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI |
ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED |
ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING | ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING |
ARMING_DISABLED_PWM_OUTPUT_ERROR), ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM),
} armingFlag_e; } armingFlag_e;
// Arming blockers that can be overriden by emergency arming. // Arming blockers that can be overriden by emergency arming.
@ -78,25 +78,26 @@ extern const char *armingDisableFlagNames[];
#define ARMING_FLAG(mask) (armingFlags & (mask)) #define ARMING_FLAG(mask) (armingFlags & (mask))
// Returns the 1st flag from ARMING_DISABLED_ALL_FLAGS which is // Returns the 1st flag from ARMING_DISABLED_ALL_FLAGS which is
// preventing arming, or zero is arming is not disabled. // preventing arming, or zero if arming is not disabled.
armingFlag_e isArmingDisabledReason(void); armingFlag_e isArmingDisabledReason(void);
typedef enum { typedef enum {
ANGLE_MODE = (1 << 0), ANGLE_MODE = (1 << 0),
HORIZON_MODE = (1 << 1), HORIZON_MODE = (1 << 1),
HEADING_MODE = (1 << 2), HEADING_MODE = (1 << 2),
NAV_ALTHOLD_MODE= (1 << 3), // old BARO NAV_ALTHOLD_MODE = (1 << 3), // old BARO
NAV_RTH_MODE = (1 << 4), // old GPS_HOME NAV_RTH_MODE = (1 << 4), // old GPS_HOME
NAV_POSHOLD_MODE= (1 << 5), // old GPS_HOLD NAV_POSHOLD_MODE = (1 << 5), // old GPS_HOLD
HEADFREE_MODE = (1 << 6), HEADFREE_MODE = (1 << 6),
NAV_LAUNCH_MODE = (1 << 7), NAV_LAUNCH_MODE = (1 << 7),
MANUAL_MODE = (1 << 8), MANUAL_MODE = (1 << 8),
FAILSAFE_MODE = (1 << 9), FAILSAFE_MODE = (1 << 9),
AUTO_TUNE = (1 << 10), // old G-Tune AUTO_TUNE = (1 << 10), // old G-Tune
NAV_WP_MODE = (1 << 11), NAV_WP_MODE = (1 << 11),
NAV_CRUISE_MODE = (1 << 12), NAV_COURSE_HOLD_MODE = (1 << 12),
FLAPERON = (1 << 13), FLAPERON = (1 << 13),
TURN_ASSISTANT = (1 << 14), TURN_ASSISTANT = (1 << 14),
FLIP_OVER_AFTER_CRASH = (1 << 15),
} flightModeFlags_e; } flightModeFlags_e;
extern uint32_t flightModeFlags; extern uint32_t flightModeFlags;

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

File diff suppressed because it is too large Load diff

9
src/main/fc/stats.c
View file

@ -3,6 +3,7 @@
#ifdef USE_STATS #ifdef USE_STATS
#include "fc/settings.h"
#include "fc/stats.h" #include "fc/stats.h"
#include "sensors/battery.h" #include "sensors/battery.h"
@ -20,11 +21,11 @@
PG_REGISTER_WITH_RESET_TEMPLATE(statsConfig_t, statsConfig, PG_STATS_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(statsConfig_t, statsConfig, PG_STATS_CONFIG, 1);
PG_RESET_TEMPLATE(statsConfig_t, statsConfig, PG_RESET_TEMPLATE(statsConfig_t, statsConfig,
.stats_enabled = 0, .stats_enabled = SETTING_STATS_DEFAULT,
.stats_total_time = 0, .stats_total_time = SETTING_STATS_TOTAL_TIME_DEFAULT,
.stats_total_dist = 0, .stats_total_dist = SETTING_STATS_TOTAL_DIST_DEFAULT,
#ifdef USE_ADC #ifdef USE_ADC
.stats_total_energy = 0 .stats_total_energy = SETTING_STATS_TOTAL_ENERGY_DEFAULT
#endif #endif
); );

27
src/main/flight/failsafe.c
View file

@ -40,6 +40,7 @@
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/controlrate_profile.h" #include "fc/controlrate_profile.h"
#include "fc/settings.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/mixer.h" #include "flight/mixer.h"
@ -67,19 +68,19 @@ static failsafeState_t failsafeState;
PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 1);
PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_delay = 5, // 0.5 sec .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT, // 0.5 sec
.failsafe_recovery_delay = 5, // 0.5 seconds (plus 200ms explicit delay) .failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT, // 0.5 seconds (plus 200ms explicit delay)
.failsafe_off_delay = 200, // 20sec .failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT, // 20sec
.failsafe_throttle = 1000, // default throttle off. .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off.
.failsafe_throttle_low_delay = 0, // default throttle low delay for "just disarm" on failsafe condition .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT, // default throttle low delay for "just disarm" on failsafe condition
.failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING, // default full failsafe procedure .failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT, // default full failsafe procedure
.failsafe_fw_roll_angle = -200, // 20 deg left .failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT, // 20 deg left
.failsafe_fw_pitch_angle = 100, // 10 deg dive (yes, positive means dive) .failsafe_fw_pitch_angle = SETTING_FAILSAFE_FW_PITCH_ANGLE_DEFAULT, // 10 deg dive (yes, positive means dive)
.failsafe_fw_yaw_rate = -45, // 45 deg/s left yaw (left is negative, 8s for full turn) .failsafe_fw_yaw_rate = SETTING_FAILSAFE_FW_YAW_RATE_DEFAULT, // 45 deg/s left yaw (left is negative, 8s for full turn)
.failsafe_stick_motion_threshold = 50, .failsafe_stick_motion_threshold = SETTING_FAILSAFE_STICK_THRESHOLD_DEFAULT,
.failsafe_min_distance = 0, // No minimum distance for failsafe by default .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default
.failsafe_min_distance_procedure = FAILSAFE_PROCEDURE_DROP_IT, // default minimum distance failsafe procedure .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure
.failsafe_mission = true, // Enable failsafe in WP mode or not .failsafe_mission = SETTING_FAILSAFE_MISSION_DEFAULT, // Enable failsafe in WP mode or not
); );
typedef enum { typedef enum {

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

@ -45,6 +45,7 @@ FILE_COMPILE_FOR_SPEED
#include "fc/config.h" #include "fc/config.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/settings.h"
#include "flight/hil.h" #include "flight/hil.h"
#include "flight/imu.h" #include "flight/imu.h"
@ -99,13 +100,13 @@ STATIC_FASTRAM bool gpsHeadingInitialized;
PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2); PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2);
PG_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_RESET_TEMPLATE(imuConfig_t, imuConfig,
.dcm_kp_acc = 2500, // 0.25 * 10000 .dcm_kp_acc = SETTING_IMU_DCM_KP_DEFAULT, // 0.25 * 10000
.dcm_ki_acc = 50, // 0.005 * 10000 .dcm_ki_acc = SETTING_IMU_DCM_KI_DEFAULT, // 0.005 * 10000
.dcm_kp_mag = 10000, // 1.00 * 10000 .dcm_kp_mag = SETTING_IMU_DCM_KP_MAG_DEFAULT, // 1.00 * 10000
.dcm_ki_mag = 0, // 0.00 * 10000 .dcm_ki_mag = SETTING_IMU_DCM_KI_MAG_DEFAULT, // 0.00 * 10000
.small_angle = 25, .small_angle = SETTING_SMALL_ANGLE_DEFAULT,
.acc_ignore_rate = 0, .acc_ignore_rate = SETTING_IMU_ACC_IGNORE_RATE_DEFAULT,
.acc_ignore_slope = 0 .acc_ignore_slope = SETTING_IMU_ACC_IGNORE_SLOPE_DEFAULT
); );
STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) STATIC_UNIT_TESTED void imuComputeRotationMatrix(void)
@ -156,8 +157,13 @@ void imuInit(void)
gpsHeadingInitialized = false; gpsHeadingInitialized = false;
// Create magnetic declination matrix // Create magnetic declination matrix
#ifdef USE_MAG
const int deg = compassConfig()->mag_declination / 100; const int deg = compassConfig()->mag_declination / 100;
const int min = compassConfig()->mag_declination % 100; const int min = compassConfig()->mag_declination % 100;
#else
const int deg = 0;
const int min = 0;
#endif
imuSetMagneticDeclination(deg + min / 60.0f); imuSetMagneticDeclination(deg + min / 60.0f);
quaternionInitUnit(&orientation); quaternionInitUnit(&orientation);

138
src/main/flight/mixer.c
View file

@ -42,6 +42,8 @@ FILE_COMPILE_FOR_SPEED
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/controlrate_profile.h"
#include "fc/settings.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
#include "flight/imu.h" #include "flight/imu.h"
@ -74,19 +76,19 @@ static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1;
PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0);
PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig,
.deadband_low = 1406, .deadband_low = SETTING_3D_DEADBAND_LOW_DEFAULT,
.deadband_high = 1514, .deadband_high = SETTING_3D_DEADBAND_HIGH_DEFAULT,
.neutral = 1460 .neutral = SETTING_3D_NEUTRAL_DEFAULT
); );
PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3); PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3);
PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
.motorDirectionInverted = 0, .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT,
.platformType = PLATFORM_MULTIROTOR, .platformType = SETTING_PLATFORM_TYPE_DEFAULT,
.hasFlaps = false, .hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
.appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only
.fwMinThrottleDownPitchAngle = 0 .fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT
); );
#ifdef BRUSHED_MOTORS #ifdef BRUSHED_MOTORS
@ -99,22 +101,28 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
#define DEFAULT_MAX_THROTTLE 1850 #define DEFAULT_MAX_THROTTLE 1850
PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 6); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 7);
PG_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.motorPwmProtocol = DEFAULT_PWM_PROTOCOL, .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT,
.motorPwmRate = DEFAULT_PWM_RATE, .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT,
.maxthrottle = DEFAULT_MAX_THROTTLE, .maxthrottle = SETTING_MAX_THROTTLE_DEFAULT,
.mincommand = 1000, .mincommand = SETTING_MIN_COMMAND_DEFAULT,
.motorAccelTimeMs = 0, .motorAccelTimeMs = SETTING_MOTOR_ACCEL_TIME_DEFAULT,
.motorDecelTimeMs = 0, .motorDecelTimeMs = SETTING_MOTOR_DECEL_TIME_DEFAULT,
.throttleIdle = 15.0f, .throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT,
.throttleScale = 1.0f, .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT,
.motorPoleCount = 14 // Most brushless motors that we use are 14 poles .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles
#ifdef USE_DSHOT
.flipOverAfterPowerFactor = SETTING_FLIP_OVER_AFTER_CRASH_POWER_FACTOR_DEFAULT,
#endif
); );
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
#define CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND 20.0f
#define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f
typedef void (*motorRateLimitingApplyFnPtr)(const float dT); typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn; static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
@ -184,7 +192,7 @@ void mixerUpdateStateFlags(void)
} else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) { } else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) {
ENABLE_STATE(MULTIROTOR); ENABLE_STATE(MULTIROTOR);
ENABLE_STATE(ALTITUDE_CONTROL); ENABLE_STATE(ALTITUDE_CONTROL);
} }
if (mixerConfig()->hasFlaps) { if (mixerConfig()->hasFlaps) {
ENABLE_STATE(FLAPERON_AVAILABLE); ENABLE_STATE(FLAPERON_AVAILABLE);
@ -266,7 +274,7 @@ void mixerInit(void)
void mixerResetDisarmedMotors(void) void mixerResetDisarmedMotors(void)
{ {
int motorZeroCommand; int motorZeroCommand;
if (feature(FEATURE_REVERSIBLE_MOTORS)) { if (feature(FEATURE_REVERSIBLE_MOTORS)) {
motorZeroCommand = reversibleMotorsConfig()->neutral; motorZeroCommand = reversibleMotorsConfig()->neutral;
throttleRangeMin = throttleDeadbandHigh; throttleRangeMin = throttleDeadbandHigh;
@ -319,6 +327,85 @@ static uint16_t handleOutputScaling(
} }
return value; return value;
} }
static void applyFlipOverAfterCrashModeToMotors(void) {
if (ARMING_FLAG(ARMED)) {
const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f;
const float stickDeflectionPitchAbs = ABS(((float) rcCommand[PITCH]) / 500.0f);
const float stickDeflectionRollAbs = ABS(((float) rcCommand[ROLL]) / 500.0f);
const float stickDeflectionYawAbs = ABS(((float) rcCommand[YAW]) / 500.0f);
//deflection stick position
const float stickDeflectionPitchExpo =
flipPowerFactor * stickDeflectionPitchAbs + power3(stickDeflectionPitchAbs) * (1 - flipPowerFactor);
const float stickDeflectionRollExpo =
flipPowerFactor * stickDeflectionRollAbs + power3(stickDeflectionRollAbs) * (1 - flipPowerFactor);
const float stickDeflectionYawExpo =
flipPowerFactor * stickDeflectionYawAbs + power3(stickDeflectionYawAbs) * (1 - flipPowerFactor);
float signPitch = rcCommand[PITCH] < 0 ? 1 : -1;
float signRoll = rcCommand[ROLL] < 0 ? 1 : -1;
float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 1 : -1));
float stickDeflectionLength = sqrtf(sq(stickDeflectionPitchAbs) + sq(stickDeflectionRollAbs));
float stickDeflectionExpoLength = sqrtf(sq(stickDeflectionPitchExpo) + sq(stickDeflectionRollExpo));
if (stickDeflectionYawAbs > MAX(stickDeflectionPitchAbs, stickDeflectionRollAbs)) {
// If yaw is the dominant, disable pitch and roll
stickDeflectionLength = stickDeflectionYawAbs;
stickDeflectionExpoLength = stickDeflectionYawExpo;
signRoll = 0;
signPitch = 0;
} else {
// If pitch/roll dominant, disable yaw
signYaw = 0;
}
const float cosPhi = (stickDeflectionLength > 0) ? (stickDeflectionPitchAbs + stickDeflectionRollAbs) /
(sqrtf(2.0f) * stickDeflectionLength) : 0;
const float cosThreshold = sqrtf(3.0f) / 2.0f; // cos(PI/6.0f)
if (cosPhi < cosThreshold) {
// Enforce either roll or pitch exclusively, if not on diagonal
if (stickDeflectionRollAbs > stickDeflectionPitchAbs) {
signPitch = 0;
} else {
signRoll = 0;
}
}
// Apply a reasonable amount of stick deadband
const float crashFlipStickMinExpo =
flipPowerFactor * CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN + power3(CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN) * (1 - flipPowerFactor);
const float flipStickRange = 1.0f - crashFlipStickMinExpo;
const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange;
for (int i = 0; i < motorCount; ++i) {
float motorOutputNormalised =
signPitch * currentMixer[i].pitch +
signRoll * currentMixer[i].roll +
signYaw * currentMixer[i].yaw;
if (motorOutputNormalised < 0) {
motorOutputNormalised = 0;
}
motorOutputNormalised = MIN(1.0f, flipPower * motorOutputNormalised);
float motorOutput = (float)motorConfig()->mincommand + motorOutputNormalised * (float)motorConfig()->maxthrottle;
// Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered
motorOutput = (motorOutput < (float)motorConfig()->mincommand + CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND) ? DSHOT_DISARM_COMMAND : (
motorOutput - CRASH_OVER_AFTER_CRASH_FLIP_DEADBAND);
motor[i] = motorOutput;
}
} else {
// Disarmed mode
stopMotors();
}
}
#endif #endif
void FAST_CODE writeMotors(void) void FAST_CODE writeMotors(void)
@ -443,6 +530,13 @@ static int getReversibleMotorsThrottleDeadband(void)
void FAST_CODE mixTable(const float dT) void FAST_CODE mixTable(const float dT)
{ {
#ifdef USE_DSHOT
if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) {
applyFlipOverAfterCrashModeToMotors();
return;
}
#endif
int16_t input[3]; // RPY, range [-500:+500] int16_t input[3]; // RPY, range [-500:+500]
// Allow direct stick input to motors in passthrough mode on airplanes // Allow direct stick input to motors in passthrough mode on airplanes
if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) { if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {
@ -482,7 +576,7 @@ void FAST_CODE mixTable(const float dT)
if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)) { if (LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_THROTTLE)) {
throttleRangeMin = throttleIdleValue; throttleRangeMin = throttleIdleValue;
throttleRangeMax = motorConfig()->maxthrottle; throttleRangeMax = motorConfig()->maxthrottle;
mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax);
} else } else
#endif #endif
if (feature(FEATURE_REVERSIBLE_MOTORS)) { if (feature(FEATURE_REVERSIBLE_MOTORS)) {
@ -519,7 +613,7 @@ void FAST_CODE mixTable(const float dT)
mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin; mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin;
#endif #endif
// Throttle compensation based on battery voltage // Throttle compensation based on battery voltage
if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) { if (feature(FEATURE_THR_VBAT_COMP) && isAmperageConfigured() && feature(FEATURE_VBAT)) {
mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax); mixerThrottleCommand = MIN(throttleRangeMin + (mixerThrottleCommand - throttleRangeMin) * calculateThrottleCompensationFactor(), throttleRangeMax);
} }
} }

1
src/main/flight/mixer.h
View file

@ -92,6 +92,7 @@ typedef struct motorConfig_s {
float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent
float throttleScale; // Scaling factor for throttle. float throttleScale; // Scaling factor for throttle.
uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry uint8_t motorPoleCount; // Magnetic poles in the motors for calculating actual RPM from eRPM provided by ESC telemetry
uint8_t flipOverAfterPowerFactor; // Power factor from 0 to 100% of flip over after crash
} motorConfig_t; } motorConfig_t;
PG_DECLARE(motorConfig_t, motorConfig); PG_DECLARE(motorConfig_t, motorConfig);

390
src/main/flight/pid.c
View file

@ -40,11 +40,13 @@ FILE_COMPILE_FOR_SPEED
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/settings.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
#include "flight/rpm_filter.h" #include "flight/rpm_filter.h"
#include "flight/secondary_imu.h"
#include "flight/kalman.h" #include "flight/kalman.h"
#include "io/gps.h" #include "io/gps.h"
@ -104,6 +106,7 @@ typedef struct {
uint16_t pidSumLimit; uint16_t pidSumLimit;
filterApply4FnPtr ptermFilterApplyFn; filterApply4FnPtr ptermFilterApplyFn;
bool itermLimitActive; bool itermLimitActive;
bool itermFreezeActive;
biquadFilter_t rateTargetFilter; biquadFilter_t rateTargetFilter;
} pidState_t; } pidState_t;
@ -161,38 +164,38 @@ PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE
PG_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = { .bank_mc = {
.pid = { .pid = {
[PID_ROLL] = { 40, 30, 23, 60 }, [PID_ROLL] = { SETTING_MC_P_ROLL_DEFAULT, SETTING_MC_I_ROLL_DEFAULT, SETTING_MC_D_ROLL_DEFAULT, SETTING_MC_CD_ROLL_DEFAULT },
[PID_PITCH] = { 40, 30, 23, 60 }, [PID_PITCH] = { SETTING_MC_P_PITCH_DEFAULT, SETTING_MC_I_PITCH_DEFAULT, SETTING_MC_D_PITCH_DEFAULT, SETTING_MC_CD_PITCH_DEFAULT },
[PID_YAW] = { 85, 45, 0, 60 }, [PID_YAW] = { SETTING_MC_P_YAW_DEFAULT, SETTING_MC_I_YAW_DEFAULT, SETTING_MC_D_YAW_DEFAULT, SETTING_MC_CD_YAW_DEFAULT },
[PID_LEVEL] = { [PID_LEVEL] = {
.P = 20, // Self-level strength .P = SETTING_MC_P_LEVEL_DEFAULT, // Self-level strength
.I = 15, // Self-leveing low-pass frequency (0 - disabled) .I = SETTING_MC_I_LEVEL_DEFAULT, // Self-leveing low-pass frequency (0 - disabled)
.D = 75, // 75% horizon strength .D = SETTING_MC_D_LEVEL_DEFAULT, // 75% horizon strength
.FF = 0, .FF = 0,
}, },
[PID_HEADING] = { 60, 0, 0, 0 }, [PID_HEADING] = { SETTING_NAV_MC_HEADING_P_DEFAULT, 0, 0, 0 },
[PID_POS_XY] = { [PID_POS_XY] = {
.P = 65, // NAV_POS_XY_P * 100 .P = SETTING_NAV_MC_POS_XY_P_DEFAULT, // NAV_POS_XY_P * 100
.I = 0, .I = 0,
.D = 0, .D = 0,
.FF = 0, .FF = 0,
}, },
[PID_VEL_XY] = { [PID_VEL_XY] = {
.P = 40, // NAV_VEL_XY_P * 20 .P = SETTING_NAV_MC_VEL_XY_P_DEFAULT, // NAV_VEL_XY_P * 20
.I = 15, // NAV_VEL_XY_I * 100 .I = SETTING_NAV_MC_VEL_XY_I_DEFAULT, // NAV_VEL_XY_I * 100
.D = 100, // NAV_VEL_XY_D * 100 .D = SETTING_NAV_MC_VEL_XY_D_DEFAULT, // NAV_VEL_XY_D * 100
.FF = 40, // NAV_VEL_XY_D * 100 .FF = SETTING_NAV_MC_VEL_XY_FF_DEFAULT, // NAV_VEL_XY_D * 100
}, },
[PID_POS_Z] = { [PID_POS_Z] = {
.P = 50, // NAV_POS_Z_P * 100 .P = SETTING_NAV_MC_POS_Z_P_DEFAULT, // NAV_POS_Z_P * 100
.I = 0, // not used .I = 0, // not used
.D = 0, // not used .D = 0, // not used
.FF = 0, .FF = 0,
}, },
[PID_VEL_Z] = { [PID_VEL_Z] = {
.P = 100, // NAV_VEL_Z_P * 66.7 .P = SETTING_NAV_MC_VEL_Z_P_DEFAULT, // NAV_VEL_Z_P * 66.7
.I = 50, // NAV_VEL_Z_I * 20 .I = SETTING_NAV_MC_VEL_Z_I_DEFAULT, // NAV_VEL_Z_I * 20
.D = 10, // NAV_VEL_Z_D * 100 .D = SETTING_NAV_MC_VEL_Z_D_DEFAULT, // NAV_VEL_Z_D * 100
.FF = 0, .FF = 0,
}, },
[PID_POS_HEADING] = { [PID_POS_HEADING] = {
@ -206,82 +209,94 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_fw = { .bank_fw = {
.pid = { .pid = {
[PID_ROLL] = { 5, 7, 0, 50 }, [PID_ROLL] = { SETTING_FW_P_ROLL_DEFAULT, SETTING_FW_I_ROLL_DEFAULT, 0, SETTING_FW_FF_ROLL_DEFAULT },
[PID_PITCH] = { 5, 7, 0, 50 }, [PID_PITCH] = { SETTING_FW_P_PITCH_DEFAULT, SETTING_FW_I_PITCH_DEFAULT, 0, SETTING_FW_FF_PITCH_DEFAULT },
[PID_YAW] = { 6, 10, 0, 60 }, [PID_YAW] = { SETTING_FW_P_YAW_DEFAULT, SETTING_FW_I_YAW_DEFAULT, 0, SETTING_FW_FF_YAW_DEFAULT },
[PID_LEVEL] = { [PID_LEVEL] = {
.P = 20, // Self-level strength .P = SETTING_FW_P_LEVEL_DEFAULT, // Self-level strength
.I = 5, // Self-leveing low-pass frequency (0 - disabled) .I = SETTING_FW_I_LEVEL_DEFAULT, // Self-leveing low-pass frequency (0 - disabled)
.D = 75, // 75% horizon strength .D = SETTING_FW_D_LEVEL_DEFAULT, // 75% horizon strength
.FF = 0, .FF = 0,
}, },
[PID_HEADING] = { 60, 0, 0, 0 }, [PID_HEADING] = { SETTING_NAV_FW_HEADING_P_DEFAULT, 0, 0, 0 },
[PID_POS_Z] = { [PID_POS_Z] = {
.P = 40, // FW_POS_Z_P * 10 .P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 10
.I = 5, // FW_POS_Z_I * 10 .I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 10
.D = 10, // FW_POS_Z_D * 10 .D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 10
.FF = 0, .FF = 0,
}, },
[PID_POS_XY] = { [PID_POS_XY] = {
.P = 75, // FW_POS_XY_P * 100 .P = SETTING_NAV_FW_POS_XY_P_DEFAULT, // FW_POS_XY_P * 100
.I = 5, // FW_POS_XY_I * 100 .I = SETTING_NAV_FW_POS_XY_I_DEFAULT, // FW_POS_XY_I * 100
.D = 8, // FW_POS_XY_D * 100 .D = SETTING_NAV_FW_POS_XY_D_DEFAULT, // FW_POS_XY_D * 100
.FF = 0, .FF = 0,
}, },
[PID_POS_HEADING] = { [PID_POS_HEADING] = {
.P = 30, .P = SETTING_NAV_FW_POS_HDG_P_DEFAULT,
.I = 2, .I = SETTING_NAV_FW_POS_HDG_I_DEFAULT,
.D = 0, .D = SETTING_NAV_FW_POS_HDG_D_DEFAULT,
.FF = 0 .FF = 0
} }
} }
}, },
.dterm_lpf_type = 1, //Default to BIQUAD .dterm_lpf_type = SETTING_DTERM_LPF_TYPE_DEFAULT,
.dterm_lpf_hz = 40, .dterm_lpf_hz = SETTING_DTERM_LPF_HZ_DEFAULT,
.dterm_lpf2_type = 1, //Default to BIQUAD .dterm_lpf2_type = SETTING_DTERM_LPF2_TYPE_DEFAULT,
.dterm_lpf2_hz = 0, // Off by default .dterm_lpf2_hz = SETTING_DTERM_LPF2_HZ_DEFAULT,
.yaw_lpf_hz = 0, .yaw_lpf_hz = SETTING_YAW_LPF_HZ_DEFAULT,
.itermWindupPointPercent = 50, // Percent .itermWindupPointPercent = SETTING_ITERM_WINDUP_DEFAULT,
.axisAccelerationLimitYaw = 10000, // dps/s .axisAccelerationLimitYaw = SETTING_RATE_ACCEL_LIMIT_YAW_DEFAULT,
.axisAccelerationLimitRollPitch = 0, // dps/s .axisAccelerationLimitRollPitch = SETTING_RATE_ACCEL_LIMIT_ROLL_PITCH_DEFAULT,
.heading_hold_rate_limit = HEADING_HOLD_RATE_LIMIT_DEFAULT, .heading_hold_rate_limit = SETTING_HEADING_HOLD_RATE_LIMIT_DEFAULT,
.max_angle_inclination[FD_ROLL] = 300, // 30 degrees .max_angle_inclination[FD_ROLL] = SETTING_MAX_ANGLE_INCLINATION_RLL_DEFAULT,
.max_angle_inclination[FD_PITCH] = 300, // 30 degrees .max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT,
.pidSumLimit = PID_SUM_LIMIT_DEFAULT, .pidSumLimit = SETTING_PIDSUM_LIMIT_DEFAULT,
.pidSumLimitYaw = PID_SUM_LIMIT_YAW_DEFAULT, .pidSumLimitYaw = SETTING_PIDSUM_LIMIT_YAW_DEFAULT,
.fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingItermThrowLimit = SETTING_FW_ITERM_THROW_LIMIT_DEFAULT,
.fixedWingReferenceAirspeed = 1000, .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT,
.fixedWingCoordinatedYawGain = 1.0f, .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT,
.fixedWingCoordinatedPitchGain = 1.0f, .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT,
.fixedWingItermLimitOnStickPosition = 0.5f, .fixedWingItermLimitOnStickPosition = SETTING_FW_ITERM_LIMIT_STICK_POSITION_DEFAULT,
.fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT,
.loiter_direction = NAV_LOITER_RIGHT, .loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
.navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, .navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT,
.navVelXyDtermAttenuation = 90, .navVelXyDtermAttenuation = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_DEFAULT,
.navVelXyDtermAttenuationStart = 10, .navVelXyDtermAttenuationStart = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_DEFAULT,
.navVelXyDtermAttenuationEnd = 60, .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_DEFAULT,
.iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT, .iterm_relax_cutoff = SETTING_MC_ITERM_RELAX_CUTOFF_DEFAULT,
.iterm_relax = ITERM_RELAX_RP, .iterm_relax = SETTING_MC_ITERM_RELAX_DEFAULT,
.dBoostFactor = 1.25f,
.dBoostMaxAtAlleceleration = 7500.0f, #ifdef USE_D_BOOST
.dBoostGyroDeltaLpfHz = D_BOOST_GYRO_LPF_HZ, .dBoostFactor = SETTING_D_BOOST_FACTOR_DEFAULT,
.antigravityGain = 1.0f, .dBoostMaxAtAlleceleration = SETTING_D_BOOST_MAX_AT_ACCELERATION_DEFAULT,
.antigravityAccelerator = 1.0f, .dBoostGyroDeltaLpfHz = SETTING_D_BOOST_GYRO_DELTA_LPF_HZ_DEFAULT,
.antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, #endif
.pidControllerType = PID_TYPE_AUTO,
.navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT, #ifdef USE_ANTIGRAVITY
.controlDerivativeLpfHz = 30, .antigravityGain = SETTING_ANTIGRAVITY_GAIN_DEFAULT,
.kalman_q = 100, .antigravityAccelerator = SETTING_ANTIGRAVITY_ACCELERATOR_DEFAULT,
.kalman_w = 4, .antigravityCutoff = SETTING_ANTIGRAVITY_CUTOFF_LPF_HZ_DEFAULT,
.kalman_sharpness = 100, #endif
.kalmanEnabled = 0,
.fixedWingLevelTrim = 0, .pidControllerType = SETTING_PID_TYPE_DEFAULT,
.navFwPosHdgPidsumLimit = SETTING_NAV_FW_POS_HDG_PIDSUM_LIMIT_DEFAULT,
.controlDerivativeLpfHz = SETTING_MC_CD_LPF_HZ_DEFAULT,
#ifdef USE_GYRO_KALMAN
.kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT,
.kalman_w = SETTING_SETPOINT_KALMAN_W_DEFAULT,
.kalman_sharpness = SETTING_SETPOINT_KALMAN_SHARPNESS_DEFAULT,
.kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT,
#endif
.fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
); );
bool pidInitFilters(void) bool pidInitFilters(void)
@ -481,7 +496,7 @@ void updatePIDCoefficients()
// Airplanes - scale all PIDs according to TPA // Airplanes - scale all PIDs according to TPA
pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor; pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor;
pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor;
pidState[axis].kD = 0.0f; pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor;
pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor; pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor;
pidState[axis].kCD = 0.0f; pidState[axis].kCD = 0.0f;
pidState[axis].kT = 0.0f; pidState[axis].kT = 0.0f;
@ -495,7 +510,7 @@ void updatePIDCoefficients()
pidState[axis].kFF = 0.0f; pidState[axis].kFF = 0.0f;
// Tracking anti-windup requires P/I/D to be all defined which is only true for MC // Tracking anti-windup requires P/I/D to be all defined which is only true for MC
if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0)) { if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0) && (usedPidControllerType == PID_TYPE_PID)) {
pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP)); pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP));
} else { } else {
pidState[axis].kT = 0; pidState[axis].kT = 0;
@ -536,8 +551,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle())
angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle); angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle);
//PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
//PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
if (axis == FD_PITCH && STATE(AIRPLANE)) { if (axis == FD_PITCH && STATE(AIRPLANE)) {
/* /*
* fixedWingLevelTrim has opposite sign to rcCommand. * fixedWingLevelTrim has opposite sign to rcCommand.
@ -551,7 +565,22 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
} }
#ifdef USE_SECONDARY_IMU
float actual;
if (secondaryImuState.active && secondaryImuConfig()->useForStabilized) {
if (axis == FD_ROLL) {
actual = secondaryImuState.eulerAngles.values.roll;
} else {
actual = secondaryImuState.eulerAngles.values.pitch;
}
} else {
actual = attitude.raw[axis];
}
const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - actual);
#else
const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
#endif
float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
@ -608,66 +637,6 @@ static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT); return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
} }
static void applyItermLimiting(pidState_t *pidState) {
if (pidState->itermLimitActive) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newFFTerm = pidState->rateTarget * pidState->kFF;
// Calculate integral
pidState->errorGyroIf += rateError * pidState->kI * dT;
applyItermLimiting(pidState);
if (pidProfile()->fixedWingItermThrowLimit != 0) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
}
#ifdef USE_AUTOTUNE_FIXED_WING
if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm);
}
#endif
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit);
#ifdef USE_BLACKBOX
axisPID_P[axis] = newPTerm;
axisPID_I[axis] = pidState->errorGyroIf;
axisPID_D[axis] = newFFTerm;
axisPID_Setpoint[axis] = pidState->rateTarget;
#endif
}
static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
{
if (itermRelax) {
if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
return itermErrorRate * itermRelaxFactor;
}
}
return itermErrorRate;
}
#ifdef USE_D_BOOST #ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
@ -694,10 +663,100 @@ static float applyDBoost(pidState_t *pidState, float dT) {
} }
#endif #endif
static float dTermProcess(pidState_t *pidState, float dT) {
// Calculate new D-term
float newDTerm = 0;
if (pidState->kD == 0) {
// optimisation for when D is zero, often used by YAW axis
newDTerm = 0;
} else {
float delta = pidState->previousRateGyro - pidState->gyroRate;
delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
// Calculate derivative
newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
}
return(newDTerm);
}
static void applyItermLimiting(pidState_t *pidState) {
if (pidState->itermLimitActive) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else
{
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, dT);
const float newFFTerm = pidState->rateTarget * pidState->kFF;
DEBUG_SET(DEBUG_FW_D, axis, newDTerm);
/*
* Integral should be updated only if axis Iterm is not frozen
*/
if (!pidState->itermFreezeActive) {
pidState->errorGyroIf += rateError * pidState->kI * dT;
}
applyItermLimiting(pidState);
if (pidProfile()->fixedWingItermThrowLimit != 0) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
}
#ifdef USE_AUTOTUNE_FIXED_WING
if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm);
}
#endif
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit);
#ifdef USE_BLACKBOX
axisPID_P[axis] = newPTerm;
axisPID_I[axis] = pidState->errorGyroIf;
axisPID_D[axis] = newDTerm;
axisPID_Setpoint[axis] = pidState->rateTarget;
#endif
pidState->previousRateGyro = pidState->gyroRate;
}
static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
{
if (itermRelax) {
if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
return itermErrorRate * itermRelaxFactor;
}
}
return itermErrorRate;
}
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{ {
const float rateError = pidState->rateTarget - pidState->gyroRate; const float rateError = pidState->rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT); const float newPTerm = pTermProcess(pidState, rateError, dT);
const float newDTerm = dTermProcess(pidState, dT);
const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget; const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget;
const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta); const float rateTargetDeltaFiltered = biquadFilterApply(&pidState->rateTargetFilter, rateTargetDelta);
@ -714,21 +773,6 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid
} }
DEBUG_SET(DEBUG_CD, axis, newCDTerm); DEBUG_SET(DEBUG_CD, axis, newCDTerm);
// Calculate new D-term
float newDTerm;
if (pidState->kD == 0) {
// optimisation for when D8 is zero, often used by YAW axis
newDTerm = 0;
} else {
float delta = pidState->previousRateGyro - pidState->gyroRate;
delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
delta = dTermLpf2FilterApplyFn((filter_t *) &pidState->dtermLpf2State, delta);
// Calculate derivative
newDTerm = delta * (pidState->kD / dT) * applyDBoost(pidState, dT);
}
// TODO: Get feedback from mixer on available correction range for each axis // TODO: Get feedback from mixer on available correction range for each axis
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit); const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
@ -861,7 +905,7 @@ float pidHeadingHold(float dT)
* TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
* and keeping ROLL and PITCH attitude though the turn. * and keeping ROLL and PITCH attitude though the turn.
*/ */
static void NOINLINE pidTurnAssistant(pidState_t *pidState) static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget, float pitchAngleTarget)
{ {
fpVector3_t targetRates; fpVector3_t targetRates;
targetRates.x = 0.0f; targetRates.x = 0.0f;
@ -889,8 +933,9 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000);
// Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
float bankAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60));
float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn; float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget));
float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor;
targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
} }
@ -951,6 +996,24 @@ void checkItermLimitingActive(pidState_t *pidState)
pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
} }
void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
{
if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) {
// Do not allow yaw I-term to grow when bank angle is too large
float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
pidState->itermFreezeActive = true;
} else
{
pidState->itermFreezeActive = false;
}
} else
{
pidState->itermFreezeActive = false;
}
}
void FAST_CODE pidController(float dT) void FAST_CODE pidController(float dT)
{ {
if (!pidFiltersConfigured) { if (!pidFiltersConfigured) {
@ -1003,8 +1066,10 @@ void FAST_CODE pidController(float dT)
levelingEnabled = false; levelingEnabled = false;
} }
if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
pidTurnAssistant(pidState); float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT
} }
@ -1021,6 +1086,8 @@ void FAST_CODE pidController(float dT)
// Step 4: Run gyro-driven control // Step 4: Run gyro-driven control
checkItermLimitingActive(&pidState[axis]); checkItermLimitingActive(&pidState[axis]);
checkItermFreezingActive(&pidState[axis], axis);
pidControllerApplyFn(&pidState[axis], axis, dT); pidControllerApplyFn(&pidState[axis], axis, dT);
} }
} }
@ -1135,12 +1202,3 @@ const pidBank_t * pidBank(void) {
pidBank_t * pidBankMutable(void) { pidBank_t * pidBankMutable(void) {
return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
} }
uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex)
{
if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) {
return &pidBank->pid[pidIndex].FF;
} else {
return &pidBank->pid[pidIndex].D;
}
}

12
src/main/flight/pid.h
View file

@ -76,7 +76,7 @@ typedef enum {
typedef enum { typedef enum {
PID_TYPE_NONE = 0, // Not used in the current platform/mixer/configuration PID_TYPE_NONE = 0, // Not used in the current platform/mixer/configuration
PID_TYPE_PID, // Uses P, I and D terms PID_TYPE_PID, // Uses P, I and D terms
PID_TYPE_PIFF, // Uses P, I and FF, ignoring D PID_TYPE_PIFF, // Uses P, I, D and FF
PID_TYPE_AUTO, // Autodetect PID_TYPE_AUTO, // Autodetect
} pidType_e; } pidType_e;
@ -128,6 +128,7 @@ typedef struct pidProfile_s {
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 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 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 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
uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
float navVelXyDTermLpfHz; float navVelXyDTermLpfHz;
@ -137,19 +138,27 @@ typedef struct pidProfile_s {
uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
uint8_t iterm_relax; // Enable iterm suppression during stick input uint8_t iterm_relax; // Enable iterm suppression during stick input
#ifdef USE_D_BOOST
float dBoostFactor; float dBoostFactor;
float dBoostMaxAtAlleceleration; float dBoostMaxAtAlleceleration;
uint8_t dBoostGyroDeltaLpfHz; uint8_t dBoostGyroDeltaLpfHz;
#endif
#ifdef USE_ANTIGRAVITY
float antigravityGain; float antigravityGain;
float antigravityAccelerator; float antigravityAccelerator;
uint8_t antigravityCutoff; uint8_t antigravityCutoff;
#endif
uint16_t navFwPosHdgPidsumLimit; uint16_t navFwPosHdgPidsumLimit;
uint8_t controlDerivativeLpfHz; uint8_t controlDerivativeLpfHz;
#ifdef USE_GYRO_KALMAN
uint16_t kalman_q; uint16_t kalman_q;
uint16_t kalman_w; uint16_t kalman_w;
uint16_t kalman_sharpness; uint16_t kalman_sharpness;
uint8_t kalmanEnabled; uint8_t kalmanEnabled;
#endif
float fixedWingLevelTrim; float fixedWingLevelTrim;
} pidProfile_t; } pidProfile_t;
@ -201,4 +210,3 @@ void autotuneUpdateState(void);
void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput); void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);
pidType_e pidIndexGetType(pidIndex_e pidIndex); pidType_e pidIndexGetType(pidIndex_e pidIndex);
uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex);

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

@ -40,11 +40,10 @@
#include "fc/rc_controls.h" #include "fc/rc_controls.h"
#include "fc/rc_adjustments.h" #include "fc/rc_adjustments.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/settings.h"
#include "flight/pid.h" #include "flight/pid.h"
#define AUTOTUNE_FIXED_WING_OVERSHOOT_TIME 100
#define AUTOTUNE_FIXED_WING_UNDERSHOOT_TIME 200
#define AUTOTUNE_FIXED_WING_INTEGRATOR_TC 600 #define AUTOTUNE_FIXED_WING_INTEGRATOR_TC 600
#define AUTOTUNE_FIXED_WING_DECREASE_STEP 8 // 8% #define AUTOTUNE_FIXED_WING_DECREASE_STEP 8 // 8%
#define AUTOTUNE_FIXED_WING_INCREASE_STEP 5 // 5% #define AUTOTUNE_FIXED_WING_INCREASE_STEP 5 // 5%
@ -54,11 +53,11 @@
PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 0);
PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig,
.fw_overshoot_time = AUTOTUNE_FIXED_WING_OVERSHOOT_TIME, .fw_overshoot_time = SETTING_FW_AUTOTUNE_OVERSHOOT_TIME_DEFAULT,
.fw_undershoot_time = AUTOTUNE_FIXED_WING_UNDERSHOOT_TIME, .fw_undershoot_time = SETTING_FW_AUTOTUNE_UNDERSHOOT_TIME_DEFAULT,
.fw_max_rate_threshold = 50, .fw_max_rate_threshold = SETTING_FW_AUTOTUNE_THRESHOLD_DEFAULT,
.fw_ff_to_p_gain = 10, .fw_ff_to_p_gain = SETTING_FW_AUTOTUNE_FF_TO_P_GAIN_DEFAULT,
.fw_ff_to_i_time_constant = AUTOTUNE_FIXED_WING_INTEGRATOR_TC, .fw_ff_to_i_time_constant = SETTING_FW_AUTOTUNE_FF_TO_I_TC_DEFAULT,
); );
typedef enum { typedef enum {
@ -230,15 +229,15 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa
switch (axis) { switch (axis) {
case FD_ROLL: case FD_ROLL:
blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D_FF, tuneCurrent[axis].gainFF); blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF);
break; break;
case FD_PITCH: case FD_PITCH:
blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D_FF, tuneCurrent[axis].gainFF); blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF);
break; break;
case FD_YAW: case FD_YAW:
blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D_FF, tuneCurrent[axis].gainFF); blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF);
break; break;
} }
} }

11
src/main/flight/rpm_filter.c
View file

@ -38,6 +38,7 @@
#include "flight/mixer.h" #include "flight/mixer.h"
#include "sensors/esc_sensor.h" #include "sensors/esc_sensor.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/settings.h"
#ifdef USE_RPM_FILTER #ifdef USE_RPM_FILTER
@ -48,10 +49,10 @@
PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1);
PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig,
.gyro_filter_enabled = 0, .gyro_filter_enabled = SETTING_RPM_GYRO_FILTER_ENABLED_DEFAULT,
.gyro_harmonics = 1, .gyro_harmonics = SETTING_RPM_GYRO_HARMONICS_DEFAULT,
.gyro_min_hz = 100, .gyro_min_hz = SETTING_RPM_GYRO_MIN_HZ_DEFAULT,
.gyro_q = 500, ); .gyro_q = SETTING_RPM_GYRO_Q_DEFAULT, );
typedef struct typedef struct
{ {
@ -203,4 +204,4 @@ float rpmFilterGyroApply(uint8_t axis, float input)
return rpmGyroApplyFn(&gyroRpmFilters, axis, input); return rpmGyroApplyFn(&gyroRpmFilters, axis, input);
} }
#endif #endif

186
src/main/flight/secondary_imu.c Normal file
View file

@ -0,0 +1,186 @@
/*
* This file is part of INAV.
*
* 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 "stdint.h"
#include "build/debug.h"
#include "common/utils.h"
#include "common/axis.h"
#include "config/parameter_group_ids.h"
#include "drivers/sensor.h"
#include "drivers/accgyro/accgyro_bno055.h"
#include "fc/settings.h"
#include "flight/secondary_imu.h"
#include "sensors/boardalignment.h"
#include "sensors/compass.h"
PG_REGISTER_WITH_RESET_FN(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 1);
EXTENDED_FASTRAM secondaryImuState_t secondaryImuState;
void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance)
{
instance->hardwareType = SECONDARY_IMU_NONE,
instance->rollDeciDegrees = 0;
instance->pitchDeciDegrees = 0;
instance->yawDeciDegrees = 0;
instance->useForOsdHeading = 0;
instance->useForOsdAHI = 0;
instance->useForStabilized = 0;
for (uint8_t i = 0; i < 3; i++)
{
instance->calibrationOffsetGyro[i] = 0;
instance->calibrationOffsetMag[i] = 0;
instance->calibrationOffsetAcc[i] = 0;
}
instance->calibrationRadiusAcc = 0;
instance->calibrationRadiusMag = 0;
}
void secondaryImuInit(void)
{
secondaryImuState.active = false;
// Create magnetic declination matrix
#ifdef USE_MAG
const int deg = compassConfig()->mag_declination / 100;
const int min = compassConfig()->mag_declination % 100;
#else
const int deg = 0;
const int min = 0;
#endif
secondaryImuSetMagneticDeclination(deg + min / 60.0f);
bno055CalibrationData_t calibrationData;
calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X];
calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y];
calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z];
calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X];
calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y];
calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z];
calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X];
calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y];
calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z];
calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc;
calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag;
if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) {
secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag));
}
if (!secondaryImuState.active) {
secondaryImuConfigMutable()->hardwareType = SECONDARY_IMU_NONE;
}
}
void taskSecondaryImu(timeUs_t currentTimeUs)
{
if (!secondaryImuState.active)
{
return;
}
/*
* Secondary IMU works in decidegrees
*/
UNUSED(currentTimeUs);
bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw);
//Apply mag declination
secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination;
//TODO this way of rotating a vector makes no sense, something simpler have to be developed
const fpVector3_t v = {
.x = secondaryImuState.eulerAngles.raw[0],
.y = secondaryImuState.eulerAngles.raw[1],
.z = secondaryImuState.eulerAngles.raw[2],
};
fpVector3_t rotated;
fp_angles_t imuAngles = {
.angles.roll = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->rollDeciDegrees),
.angles.pitch = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->pitchDeciDegrees),
.angles.yaw = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->yawDeciDegrees),
};
fpMat3_t rotationMatrix;
rotationMatrixFromAngles(&rotationMatrix, &imuAngles);
rotationMatrixRotateVector(&rotated, &v, &rotationMatrix);
rotated.z = ((int32_t)(rotated.z + secondaryImuConfig()->yawDeciDegrees)) % 3600;
secondaryImuState.eulerAngles.values.roll = rotated.x;
secondaryImuState.eulerAngles.values.pitch = rotated.y;
secondaryImuState.eulerAngles.values.yaw = rotated.z;
/*
* Every 10 cycles fetch current calibration state
*/
static uint8_t tick = 0;
tick++;
if (tick == 10)
{
secondaryImuState.calibrationStatus = bno055GetCalibStat();
tick = 0;
}
DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll);
DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch);
DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw);
DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag);
DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr);
DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc);
DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination);
}
void secondaryImuFetchCalibration(void) {
bno055CalibrationData_t calibrationData = bno055GetCalibrationData();
secondaryImuConfigMutable()->calibrationOffsetAcc[X] = calibrationData.offset[ACC][X];
secondaryImuConfigMutable()->calibrationOffsetAcc[Y] = calibrationData.offset[ACC][Y];
secondaryImuConfigMutable()->calibrationOffsetAcc[Z] = calibrationData.offset[ACC][Z];
secondaryImuConfigMutable()->calibrationOffsetMag[X] = calibrationData.offset[MAG][X];
secondaryImuConfigMutable()->calibrationOffsetMag[Y] = calibrationData.offset[MAG][Y];
secondaryImuConfigMutable()->calibrationOffsetMag[Z] = calibrationData.offset[MAG][Z];
secondaryImuConfigMutable()->calibrationOffsetGyro[X] = calibrationData.offset[GYRO][X];
secondaryImuConfigMutable()->calibrationOffsetGyro[Y] = calibrationData.offset[GYRO][Y];
secondaryImuConfigMutable()->calibrationOffsetGyro[Z] = calibrationData.offset[GYRO][Z];
secondaryImuConfigMutable()->calibrationRadiusAcc = calibrationData.radius[ACC];
secondaryImuConfigMutable()->calibrationRadiusMag = calibrationData.radius[MAG];
}
void secondaryImuSetMagneticDeclination(float declination) { //Incoming units are degrees
secondaryImuState.magDeclination = declination * 10.0f; //Internally declination is stored in decidegrees
}

66
src/main/flight/secondary_imu.h Normal file
View file

@ -0,0 +1,66 @@
/*
* This file is part of INAV.
*
* 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"
#include "common/time.h"
#include "sensors/sensors.h"
#include "drivers/accgyro/accgyro_bno055.h"
typedef enum {
SECONDARY_IMU_NONE = 0,
SECONDARY_IMU_BNO055 = 1,
} secondaryImuType_e;
typedef struct secondaryImuConfig_s {
uint8_t hardwareType;
int16_t rollDeciDegrees;
int16_t pitchDeciDegrees;
int16_t yawDeciDegrees;
uint8_t useForOsdHeading;
uint8_t useForOsdAHI;
uint8_t useForStabilized;
int16_t calibrationOffsetGyro[3];
int16_t calibrationOffsetMag[3];
int16_t calibrationOffsetAcc[3];
int16_t calibrationRadiusAcc;
int16_t calibrationRadiusMag;
} secondaryImuConfig_t;
typedef struct secondaryImuState_s {
flightDynamicsTrims_t eulerAngles;
bno055CalibStat_t calibrationStatus;
uint8_t active;
float magDeclination;
} secondaryImuState_t;
extern secondaryImuState_t secondaryImuState;
PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig);
void secondaryImuInit(void);
void taskSecondaryImu(timeUs_t currentTimeUs);
void secondaryImuFetchCalibration(void);
void secondaryImuSetMagneticDeclination(float declination);

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

@ -44,6 +44,7 @@
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/controlrate_profile.h" #include "fc/controlrate_profile.h"
#include "fc/settings.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/mixer.h" #include "flight/mixer.h"
@ -57,12 +58,12 @@
PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1);
PG_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
.servoCenterPulse = 1500, .servoCenterPulse = SETTING_SERVO_CENTER_PULSE_DEFAULT,
.servoPwmRate = 50, // Default for analog servos .servoPwmRate = SETTING_SERVO_PWM_RATE_DEFAULT, // Default for analog servos
.servo_lowpass_freq = 20, // Default servo update rate is 50Hz, everything above Nyquist frequency (25Hz) is going to fold and cause distortions .servo_lowpass_freq = SETTING_SERVO_LPF_HZ_DEFAULT, // Default servo update rate is 50Hz, everything above Nyquist frequency (25Hz) is going to fold and cause distortions
.servo_protocol = SERVO_TYPE_PWM, .servo_protocol = SETTING_SERVO_PROTOCOL_DEFAULT,
.flaperon_throw_offset = FLAPERON_THROW_DEFAULT, .flaperon_throw_offset = SETTING_FLAPERON_THROW_OFFSET_DEFAULT,
.tri_unarmed_servo = 1 .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT
); );
PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1);

17
src/main/io/gps.c
View file

@ -58,6 +58,7 @@
#include "fc/config.h" #include "fc/config.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/settings.h"
typedef struct { typedef struct {
bool isDriverBased; bool isDriverBased;
@ -125,13 +126,13 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = {
PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig,
.provider = GPS_UBLOX, .provider = SETTING_GPS_PROVIDER_DEFAULT,
.sbasMode = SBAS_NONE, .sbasMode = SETTING_GPS_SBAS_MODE_DEFAULT,
.autoConfig = GPS_AUTOCONFIG_ON, .autoConfig = SETTING_GPS_AUTO_CONFIG_DEFAULT,
.autoBaud = GPS_AUTOBAUD_ON, .autoBaud = SETTING_GPS_AUTO_BAUD_DEFAULT,
.dynModel = GPS_DYNMODEL_AIR_1G, .dynModel = SETTING_GPS_DYN_MODEL_DEFAULT,
.gpsMinSats = 6, .gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT,
.ubloxUseGalileo = false .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT
); );
void gpsSetState(gpsState_e state) void gpsSetState(gpsState_e state)
@ -192,6 +193,8 @@ static void gpsResetSolution(void)
{ {
gpsSol.eph = 9999; gpsSol.eph = 9999;
gpsSol.epv = 9999; gpsSol.epv = 9999;
gpsSol.numSat = 0;
gpsSol.hdop = 9999;
gpsSol.fixType = GPS_NO_FIX; gpsSol.fixType = GPS_NO_FIX;

8
src/main/io/lights.c
View file

@ -31,9 +31,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(lightsConfig_t, lightsConfig, PG_LIGHTS_CONFIG,
PG_RESET_TEMPLATE(lightsConfig_t, lightsConfig, PG_RESET_TEMPLATE(lightsConfig_t, lightsConfig,
.failsafe = { .failsafe = {
.enabled = true, .enabled = SETTING_FAILSAFE_LIGHTS_DEFAULT,
.flash_period = 1000, .flash_period = SETTING_FAILSAFE_LIGHTS_FLASH_PERIOD_DEFAULT,
.flash_on_time = 100 .flash_on_time = SETTING_FAILSAFE_LIGHTS_FLASH_ON_TIME_DEFAULT
} }
); );
@ -72,7 +72,7 @@ void lightsUpdate(timeUs_t currentTimeUs)
lightsSetStatus(IS_RC_MODE_ACTIVE(BOXLIGHTS), currentTimeUs); lightsSetStatus(IS_RC_MODE_ACTIVE(BOXLIGHTS), currentTimeUs);
} }
void lightsInit() void lightsInit(void)
{ {
lightsHardwareInit(); lightsHardwareInit();
} }

2
src/main/io/lights.h
View file

@ -33,6 +33,6 @@ typedef struct lightsConfig_s {
PG_DECLARE(lightsConfig_t, lightsConfig); PG_DECLARE(lightsConfig_t, lightsConfig);
void lightsUpdate(timeUs_t currentTimeUs); void lightsUpdate(timeUs_t currentTimeUs);
void lightsInit(); void lightsInit(void);
#endif /* USE_LIGHTS */ #endif /* USE_LIGHTS */

495
src/main/io/osd.c Executable file → Normal file
View file

@ -87,6 +87,8 @@ FILE_COMPILE_FOR_SPEED
#include "flight/pid.h" #include "flight/pid.h"
#include "flight/rth_estimator.h" #include "flight/rth_estimator.h"
#include "flight/wind_estimator.h" #include "flight/wind_estimator.h"
#include "flight/secondary_imu.h"
#include "flight/servos.h"
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
@ -115,6 +117,8 @@ FILE_COMPILE_FOR_SPEED
#define GFORCE_FILTER_TC 0.2 #define GFORCE_FILTER_TC 0.2
#define DELAYED_REFRESH_RESUME_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI)) #define DELAYED_REFRESH_RESUME_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI))
#define STATS_PAGE2 (checkStickPosition(ROL_HI))
#define STATS_PAGE1 (checkStickPosition(ROL_LO))
#define SPLASH_SCREEN_DISPLAY_TIME 4000 // ms #define SPLASH_SCREEN_DISPLAY_TIME 4000 // ms
#define ARMED_SCREEN_DISPLAY_TIME 1500 // ms #define ARMED_SCREEN_DISPLAY_TIME 1500 // ms
@ -154,6 +158,8 @@ typedef struct statistic_s {
int16_t max_current; // /100 int16_t max_current; // /100
int16_t max_power; // /100 int16_t max_power; // /100
int16_t min_rssi; int16_t min_rssi;
int16_t min_lq; // for CRSF
int16_t min_rssi_dbm; // for CRSF
int32_t max_altitude; int32_t max_altitude;
uint32_t max_distance; uint32_t max_distance;
} statistic_t; } statistic_t;
@ -166,6 +172,7 @@ static bool refreshWaitForResumeCmdRelease;
static bool fullRedraw = false; static bool fullRedraw = false;
static uint8_t armState; static uint8_t armState;
static uint8_t statsPagesCheck = 0;
typedef struct osdMapData_s { typedef struct osdMapData_s {
uint32_t scale; uint32_t scale;
@ -185,7 +192,7 @@ static bool osdDisplayHasCanvas;
#define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) #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, 14); PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 15);
PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0);
static int digitCount(int32_t value) static int digitCount(int32_t value)
@ -526,6 +533,21 @@ static uint16_t osdConvertRSSI(void)
return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
} }
static uint16_t osdGetLQ(void)
{
int16_t statsLQ = rxLinkStatistics.uplinkLQ;
int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300);
if (rxLinkStatistics.rfMode == 2) {
return scaledLQ;
} else {
return statsLQ;
}
}
static uint16_t osdGetdBm(void)
{
return rxLinkStatistics.uplinkRSSI;
}
/** /**
* Displays a temperature postfixed with a symbol depending on the current unit system * Displays a temperature postfixed with a symbol depending on the current unit system
* @param label to display * @param label to display
@ -712,6 +734,8 @@ static const char * osdArmingDisabledReasonMessage(void)
return OSD_MESSAGE_STR(OSD_MSG_CLI_ACTIVE); return OSD_MESSAGE_STR(OSD_MSG_CLI_ACTIVE);
case ARMING_DISABLED_PWM_OUTPUT_ERROR: case ARMING_DISABLED_PWM_OUTPUT_ERROR:
return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR); return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR);
case ARMING_DISABLED_NO_PREARM:
return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM);
// Cases without message // Cases without message
case ARMING_DISABLED_CMS_MENU: case ARMING_DISABLED_CMS_MENU:
FALLTHROUGH; FALLTHROUGH;
@ -785,7 +809,7 @@ static const char * navigationStateMessage(void)
break; break;
case MW_NAV_STATE_RTH_START: case MW_NAV_STATE_RTH_START:
return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH); return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH);
case MW_NAV_STATE_RTH_CLIMB: case MW_NAV_STATE_RTH_CLIMB:
return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB); return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB);
case MW_NAV_STATE_RTH_ENROUTE: case MW_NAV_STATE_RTH_ENROUTE:
return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME);
@ -937,12 +961,36 @@ static inline int32_t osdGetAltitudeMsl(void)
static bool osdIsHeadingValid(void) static bool osdIsHeadingValid(void)
{ {
#ifdef USE_SECONDARY_IMU
if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) {
return true;
} else {
return isImuHeadingValid();
}
#else
return isImuHeadingValid(); return isImuHeadingValid();
#endif
} }
int16_t osdGetHeading(void) int16_t osdGetHeading(void)
{ {
#ifdef USE_SECONDARY_IMU
if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) {
return secondaryImuState.eulerAngles.values.yaw;
} else {
return attitude.values.yaw;
}
#else
return attitude.values.yaw; return attitude.values.yaw;
#endif
}
int16_t osdPanServoHomeDirectionOffset(void)
{
int8_t servoIndex = osdConfig()->pan_servo_index;
int16_t servoPosition = servo[servoIndex];
int16_t servoMiddle = servoParams(servoIndex)->middle;
return (int16_t)CENTIDEGREES_TO_DEGREES((servoPosition - servoMiddle) * osdConfig()->pan_servo_pwm2centideg);
} }
// Returns a heading angle in degrees normalized to [0, 360). // Returns a heading angle in degrees normalized to [0, 360).
@ -1118,13 +1166,6 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale)
osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale);
} }
static int16_t osdGet3DSpeed(void)
{
int16_t vert_speed = getEstimatedActualVelocity(Z);
int16_t hor_speed = gpsSol.groundSpeed;
return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
}
#endif #endif
static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) {
@ -1161,7 +1202,49 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_
displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
} }
static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD) static void osdDisplayFlightPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD, adjustmentFunction_e adjFuncFF)
{
textAttributes_t elemAttr;
char buff[4];
const pid8_t *pid = &pidBank()->pid[pidIndex];
pidType_e pidType = pidIndexGetType(pidIndex);
displayWrite(osdDisplayPort, elemPosX, elemPosY, str);
if (pidType == PID_TYPE_NONE) {
// PID is not used in this configuration. Draw dashes.
// XXX: Keep this in sync with the %3d format and spacing used below
displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - - -");
return;
}
elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pid->P);
if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 4, elemPosY, buff, elemAttr);
elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pid->I);
if ((isAdjustmentFunctionSelected(adjFuncI)) || (((adjFuncI == ADJUSTMENT_ROLL_I) || (adjFuncI == ADJUSTMENT_PITCH_I)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_I))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr);
elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pid->D);
if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pid->FF);
if ((isAdjustmentFunctionSelected(adjFuncFF)) || (((adjFuncFF == ADJUSTMENT_ROLL_FF) || (adjFuncFF == ADJUSTMENT_PITCH_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_FF))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 16, elemPosY, buff, elemAttr);
}
static void osdDisplayNavPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD)
{ {
textAttributes_t elemAttr; textAttributes_t elemAttr;
char buff[4]; char buff[4];
@ -1192,7 +1275,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *
elemAttr = TEXT_ATTRIBUTES_NONE; elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D); tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D);
if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D_FF) || (adjFuncD == ADJUSTMENT_PITCH_D_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D_FF)))) if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
} }
@ -1304,6 +1387,9 @@ static bool osdDrawSingleElement(uint8_t item)
buff[1] = SYM_SAT_R; buff[1] = SYM_SAT_R;
tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); tfp_sprintf(buff + 2, "%2d", gpsSol.numSat);
if (!STATE(GPS_FIX)) { if (!STATE(GPS_FIX)) {
if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
strcpy(buff + 2, "X!");
}
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
} }
break; break;
@ -1334,7 +1420,11 @@ static bool osdDrawSingleElement(uint8_t item)
} }
else else
{ {
int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()); int16_t panHomeDirOffset = 0;
if (!(osdConfig()->pan_servo_pwm2centideg == 0)){
panHomeDirOffset = osdPanServoHomeDirectionOffset();
}
int homeDirection = GPS_directionToHome - DECIDEGREES_TO_DEGREES(osdGetHeading()) + panHomeDirOffset;
osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection); osdDrawDirArrow(osdDisplayPort, osdGetDisplayPortCanvas(), OSD_DRAW_POINT_GRID(elemPosX, elemPosY), homeDirection);
} }
} else { } else {
@ -1397,18 +1487,18 @@ static bool osdDrawSingleElement(uint8_t item)
break; break;
} }
case OSD_CRUISE_HEADING_ERROR: case OSD_COURSE_HOLD_ERROR:
{ {
if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_CRUISE_MODE)) { if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
return true; return true;
} }
buff[0] = SYM_HEADING; buff[0] = SYM_HEADING;
if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_CRUISE_MODE) && isAdjustingPosition())) { if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && isAdjustingPosition())) {
buff[1] = buff[2] = buff[3] = '-'; buff[1] = buff[2] = buff[3] = '-';
} else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError())); int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError()));
if (ABS(herr) > 99) if (ABS(herr) > 99)
strcpy(buff + 1, ">99"); strcpy(buff + 1, ">99");
@ -1421,11 +1511,11 @@ static bool osdDrawSingleElement(uint8_t item)
break; break;
} }
case OSD_CRUISE_HEADING_ADJUSTMENT: case OSD_COURSE_HOLD_ADJUSTMENT:
{ {
int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment())); int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment()));
if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_CRUISE_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) { if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) {
displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); displayWrite(osdDisplayPort, elemPosX, elemPosY, " ");
return true; return true;
} }
@ -1434,7 +1524,7 @@ static bool osdDrawSingleElement(uint8_t item)
if (!ARMING_FLAG(ARMED)) { if (!ARMING_FLAG(ARMED)) {
buff[1] = buff[2] = buff[3] = buff[4] = '-'; buff[1] = buff[2] = buff[3] = buff[4] = '-';
} else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) {
tfp_sprintf(buff + 1, "%4d", heading_adjust); tfp_sprintf(buff + 1, "%4d", heading_adjust);
} }
@ -1525,7 +1615,11 @@ static bool osdDrawSingleElement(uint8_t item)
timeUs_t currentTimeUs = micros(); timeUs_t currentTimeUs = micros();
static int32_t timeSeconds = -1; static int32_t timeSeconds = -1;
if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
#ifdef USE_WIND_ESTIMATOR
timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation);
#else
timeSeconds = calculateRemainingFlightTimeBeforeRTH(false);
#endif
updatedTimestamp = currentTimeUs; updatedTimestamp = currentTimeUs;
} }
if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) {
@ -1552,7 +1646,11 @@ static bool osdDrawSingleElement(uint8_t item)
timeUs_t currentTimeUs = micros(); timeUs_t currentTimeUs = micros();
static int32_t distanceMeters = -1; static int32_t distanceMeters = -1;
if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) {
#ifdef USE_WIND_ESTIMATOR
distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation);
#else
distanceMeters = calculateRemainingDistanceBeforeRTH(false);
#endif
updatedTimestamp = currentTimeUs; updatedTimestamp = currentTimeUs;
} }
buff[0] = SYM_TRIP_DIST; buff[0] = SYM_TRIP_DIST;
@ -1585,10 +1683,10 @@ static bool osdDrawSingleElement(uint8_t item)
p = "RTH "; p = "RTH ";
else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) else if (FLIGHT_MODE(NAV_POSHOLD_MODE))
p = "HOLD"; p = "HOLD";
else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE))
p = "3CRS"; p = "CRUZ";
else if (FLIGHT_MODE(NAV_CRUISE_MODE)) else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE))
p = "CRS "; p = "CRSH";
else if (FLIGHT_MODE(NAV_WP_MODE)) else if (FLIGHT_MODE(NAV_WP_MODE))
p = " WP "; p = " WP ";
else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) { else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) {
@ -1601,6 +1699,8 @@ static bool osdDrawSingleElement(uint8_t item)
p = "ANGL"; p = "ANGL";
else if (FLIGHT_MODE(HORIZON_MODE)) else if (FLIGHT_MODE(HORIZON_MODE))
p = "HOR "; p = "HOR ";
else if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH))
p = "TURT";
displayWrite(osdDisplayPort, elemPosX, elemPosY, p); displayWrite(osdDisplayPort, elemPosX, elemPosY, p);
return true; return true;
@ -1642,7 +1742,9 @@ static bool osdDrawSingleElement(uint8_t item)
return true; return true;
} }
#if defined(USE_SERIALRX_CRSF)
case OSD_CRSF_RSSI_DBM: case OSD_CRSF_RSSI_DBM:
{
if (rxLinkStatistics.activeAnt == 0) { if (rxLinkStatistics.activeAnt == 0) {
buff[0] = SYM_RSSI; buff[0] = SYM_RSSI;
tfp_sprintf(buff + 1, "%4d%c", rxLinkStatistics.uplinkRSSI, SYM_DBM); tfp_sprintf(buff + 1, "%4d%c", rxLinkStatistics.uplinkRSSI, SYM_DBM);
@ -1657,12 +1759,12 @@ static bool osdDrawSingleElement(uint8_t item)
} }
} }
break; break;
}
#if defined(USE_SERIALRX_CRSF) case OSD_CRSF_LQ:
case OSD_CRSF_LQ: { {
buff[0] = SYM_BLANK; buff[0] = SYM_BLANK;
int16_t statsLQ = rxLinkStatistics.uplinkLQ; int16_t statsLQ = rxLinkStatistics.uplinkLQ;
int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300);
if (rxLinkStatistics.rfMode == 2) { if (rxLinkStatistics.rfMode == 2) {
if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) { if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) {
tfp_sprintf(buff, "%5d%s", scaledLQ, "%"); tfp_sprintf(buff, "%5d%s", scaledLQ, "%");
@ -1672,9 +1774,9 @@ static bool osdDrawSingleElement(uint8_t item)
} else { } else {
if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) { if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) {
tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%"); tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%");
} else { } else {
tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%");
} }
} }
if (!failsafeIsReceivingRxData()){ if (!failsafeIsReceivingRxData()){
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
@ -1684,31 +1786,32 @@ static bool osdDrawSingleElement(uint8_t item)
break; break;
} }
case OSD_CRSF_SNR_DB: { case OSD_CRSF_SNR_DB:
const char* showsnr = "-12"; {
const char* hidesnr = " "; const char* showsnr = "-20";
int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR; const char* hidesnr = " ";
if (osdSNR_Alarm <= osdConfig()->snr_alarm) { int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR;
buff[0] = SYM_SRN; if (osdSNR_Alarm > osdConfig()->snr_alarm) {
tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB); if (cmsInMenu) {
} buff[0] = SYM_SNR;
else if (osdSNR_Alarm > osdConfig()->snr_alarm) { tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB);
if (cmsInMenu) { } else {
buff[0] = SYM_SRN; buff[0] = SYM_BLANK;
tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK);
} else { }
buff[0] = SYM_BLANK; } else if (osdSNR_Alarm <= osdConfig()->snr_alarm) {
tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); buff[0] = SYM_SNR;
tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB);
} }
break;
} }
break;
}
#endif
case OSD_CRSF_TX_POWER: { case OSD_CRSF_TX_POWER:
tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); {
break; tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW);
} break;
}
#endif
case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair
@ -1762,9 +1865,6 @@ static bool osdDrawSingleElement(uint8_t item)
gpsLocation_t wp2; gpsLocation_t wp2;
int j; int j;
tfp_sprintf(buff, "W%u/%u", posControl.activeWaypointIndex, posControl.waypointCount);
displayWrite(osdGetDisplayPort(), 13, osdConfig()->hud_margin_v - 1, buff);
for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top
j = posControl.activeWaypointIndex + i; j = posControl.activeWaypointIndex + i;
if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0 && j <= posControl.waypointCount) { if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0 && j <= posControl.waypointCount) {
@ -1802,9 +1902,22 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_ARTIFICIAL_HORIZON: case OSD_ARTIFICIAL_HORIZON:
{ {
float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); float rollAngle;
float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); float pitchAngle;
#ifdef USE_SECONDARY_IMU
if (secondaryImuState.active && secondaryImuConfig()->useForOsdAHI) {
rollAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.roll);
pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch);
} else {
rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
}
#else
rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll);
pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch);
#endif
pitchAngle -= DEGREES_TO_RADIANS(osdConfig()->camera_uptilt);
if (osdConfig()->ahi_reverse_roll) { if (osdConfig()->ahi_reverse_roll) {
rollAngle = -rollAngle; rollAngle = -rollAngle;
} }
@ -1857,35 +1970,35 @@ static bool osdDrawSingleElement(uint8_t item)
#endif #endif
case OSD_ROLL_PIDS: case OSD_ROLL_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D_FF); osdDisplayFlightPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D, ADJUSTMENT_ROLL_FF);
return true; return true;
case OSD_PITCH_PIDS: case OSD_PITCH_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D_FF); osdDisplayFlightPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D, ADJUSTMENT_PITCH_FF);
return true; return true;
case OSD_YAW_PIDS: case OSD_YAW_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D_FF); osdDisplayFlightPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D, ADJUSTMENT_YAW_FF);
return true; return true;
case OSD_LEVEL_PIDS: case OSD_LEVEL_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D); osdDisplayNavPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D);
return true; return true;
case OSD_POS_XY_PIDS: case OSD_POS_XY_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D); osdDisplayNavPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D);
return true; return true;
case OSD_POS_Z_PIDS: case OSD_POS_Z_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D); osdDisplayNavPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D);
return true; return true;
case OSD_VEL_XY_PIDS: case OSD_VEL_XY_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D); osdDisplayNavPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D);
return true; return true;
case OSD_VEL_Z_PIDS: case OSD_VEL_Z_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D); osdDisplayNavPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
return true; return true;
case OSD_HEADING_P: case OSD_HEADING_P:
@ -1969,7 +2082,7 @@ static bool osdDrawSingleElement(uint8_t item)
return true; return true;
case OSD_NAV_FW_CRUISE_THR: case OSD_NAV_FW_CRUISE_THR:
osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRS", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR);
return true; return true;
case OSD_NAV_FW_PITCH2THR: case OSD_NAV_FW_PITCH2THR:
@ -2189,12 +2302,21 @@ static bool osdDrawSingleElement(uint8_t item)
} }
break; break;
} }
case OSD_DEBUG: case OSD_DEBUG:
{ {
// Longest representable string is -2147483648, hence 11 characters /*
* Longest representable string is -2147483648 does not fit in the screen.
* Only 7 digits for negative and 8 digits for positive values allowed
*/
for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) { for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) {
tfp_sprintf(buff, "[%u]=%11ld [%u]=%11ld", bufferIndex, debug[bufferIndex], bufferIndex+1, debug[bufferIndex+1]); tfp_sprintf(
buff,
"[%u]=%8ld [%u]=%8ld",
bufferIndex,
constrain(debug[bufferIndex], -9999999, 99999999),
bufferIndex+1,
constrain(debug[bufferIndex+1], -9999999, 99999999)
);
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
} }
break; break;
@ -2545,69 +2667,84 @@ void osdDrawNextElement(void)
} }
PG_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
.rssi_alarm = 20, .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT,
.time_alarm = 10, .time_alarm = SETTING_OSD_TIME_ALARM_DEFAULT,
.alt_alarm = 100, .alt_alarm = SETTING_OSD_ALT_ALARM_DEFAULT,
.dist_alarm = 1000, .dist_alarm = SETTING_OSD_DIST_ALARM_DEFAULT,
.neg_alt_alarm = 5, .neg_alt_alarm = SETTING_OSD_NEG_ALT_ALARM_DEFAULT,
.current_alarm = 0, .current_alarm = SETTING_OSD_CURRENT_ALARM_DEFAULT,
.imu_temp_alarm_min = -200, .imu_temp_alarm_min = SETTING_OSD_IMU_TEMP_ALARM_MIN_DEFAULT,
.imu_temp_alarm_max = 600, .imu_temp_alarm_max = SETTING_OSD_IMU_TEMP_ALARM_MAX_DEFAULT,
.esc_temp_alarm_min = -200, .esc_temp_alarm_min = SETTING_OSD_ESC_TEMP_ALARM_MIN_DEFAULT,
.esc_temp_alarm_max = 900, .esc_temp_alarm_max = SETTING_OSD_ESC_TEMP_ALARM_MAX_DEFAULT,
.gforce_alarm = 5, .gforce_alarm = SETTING_OSD_GFORCE_ALARM_DEFAULT,
.gforce_axis_alarm_min = -5, .gforce_axis_alarm_min = SETTING_OSD_GFORCE_AXIS_ALARM_MIN_DEFAULT,
.gforce_axis_alarm_max = 5, .gforce_axis_alarm_max = SETTING_OSD_GFORCE_AXIS_ALARM_MAX_DEFAULT,
#ifdef USE_BARO #ifdef USE_BARO
.baro_temp_alarm_min = -200, .baro_temp_alarm_min = SETTING_OSD_BARO_TEMP_ALARM_MIN_DEFAULT,
.baro_temp_alarm_max = 600, .baro_temp_alarm_max = SETTING_OSD_BARO_TEMP_ALARM_MAX_DEFAULT,
#endif #endif
#ifdef USE_SERIALRX_CRSF #ifdef USE_SERIALRX_CRSF
.snr_alarm = 4, .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT,
.crsf_lq_format = OSD_CRSF_LQ_TYPE1, .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT,
.link_quality_alarm = 70, .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT,
#endif #endif
#ifdef USE_TEMPERATURE_SENSOR #ifdef USE_TEMPERATURE_SENSOR
.temp_label_align = OSD_ALIGN_LEFT, .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT,
#endif #endif
.video_system = VIDEO_SYSTEM_AUTO, .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT,
.row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT,
.ahi_reverse_roll = 0, .ahi_reverse_roll = SETTING_OSD_AHI_REVERSE_ROLL_DEFAULT,
.ahi_max_pitch = AH_MAX_PITCH_DEFAULT, .ahi_max_pitch = SETTING_OSD_AHI_MAX_PITCH_DEFAULT,
.crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT, .crosshairs_style = SETTING_OSD_CROSSHAIRS_STYLE_DEFAULT,
.horizon_offset = 0, .horizon_offset = SETTING_OSD_HORIZON_OFFSET_DEFAULT,
.camera_uptilt = 0, .camera_uptilt = SETTING_OSD_CAMERA_UPTILT_DEFAULT,
.camera_fov_h = 135, .camera_fov_h = SETTING_OSD_CAMERA_FOV_H_DEFAULT,
.camera_fov_v = 85, .camera_fov_v = SETTING_OSD_CAMERA_FOV_V_DEFAULT,
.hud_margin_h = 3, .hud_margin_h = SETTING_OSD_HUD_MARGIN_H_DEFAULT,
.hud_margin_v = 3, .hud_margin_v = SETTING_OSD_HUD_MARGIN_V_DEFAULT,
.hud_homing = 0, .hud_homing = SETTING_OSD_HUD_HOMING_DEFAULT,
.hud_homepoint = 0, .hud_homepoint = SETTING_OSD_HUD_HOMEPOINT_DEFAULT,
.hud_radar_disp = 0, .hud_radar_disp = SETTING_OSD_HUD_RADAR_DISP_DEFAULT,
.hud_radar_range_min = 3, .hud_radar_range_min = SETTING_OSD_HUD_RADAR_RANGE_MIN_DEFAULT,
.hud_radar_range_max = 4000, .hud_radar_range_max = SETTING_OSD_HUD_RADAR_RANGE_MAX_DEFAULT,
.hud_radar_nearest = 0, .hud_radar_nearest = SETTING_OSD_HUD_RADAR_NEAREST_DEFAULT,
.hud_wp_disp = 0, .hud_wp_disp = SETTING_OSD_HUD_WP_DISP_DEFAULT,
.left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE, .left_sidebar_scroll = SETTING_OSD_LEFT_SIDEBAR_SCROLL_DEFAULT,
.right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE, .right_sidebar_scroll = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_DEFAULT,
.sidebar_scroll_arrows = 0, .sidebar_scroll_arrows = SETTING_OSD_SIDEBAR_SCROLL_ARROWS_DEFAULT,
.osd_home_position_arm_screen = true, .sidebar_horizontal_offset = SETTING_OSD_SIDEBAR_HORIZONTAL_OFFSET_DEFAULT,
.left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT,
.right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT,
.osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT,
.pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT,
.pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT,
.units = OSD_UNIT_METRIC, .units = SETTING_OSD_UNITS_DEFAULT,
.main_voltage_decimals = 1, .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT,
.estimations_wind_compensation = true, #ifdef USE_WIND_ESTIMATOR
.coordinate_digits = 9, .estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT,
#endif
.osd_failsafe_switch_layout = false, .coordinate_digits = SETTING_OSD_COORDINATE_DIGITS_DEFAULT,
.plus_code_digits = 11, .osd_failsafe_switch_layout = SETTING_OSD_FAILSAFE_SWITCH_LAYOUT_DEFAULT,
.plus_code_short = 0,
.ahi_width = OSD_AHI_WIDTH * OSD_CHAR_WIDTH, .plus_code_digits = SETTING_OSD_PLUS_CODE_DIGITS_DEFAULT,
.ahi_height = OSD_AHI_HEIGHT * OSD_CHAR_HEIGHT, .plus_code_short = SETTING_OSD_PLUS_CODE_SHORT_DEFAULT,
.ahi_vertical_offset = -OSD_CHAR_HEIGHT,
.ahi_width = SETTING_OSD_AHI_WIDTH_DEFAULT,
.ahi_height = SETTING_OSD_AHI_HEIGHT_DEFAULT,
.ahi_vertical_offset = SETTING_OSD_AHI_VERTICAL_OFFSET_DEFAULT,
.ahi_bordered = SETTING_OSD_AHI_BORDERED_DEFAULT,
.ahi_style = SETTING_OSD_AHI_STYLE_DEFAULT,
.force_grid = SETTING_OSD_FORCE_GRID_DEFAULT,
.stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT
); );
void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
@ -2628,8 +2765,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG; osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG;
osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2); osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2);
osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2); osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2);
osdLayoutsConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2); osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ERROR] = OSD_POS(12, 2);
osdLayoutsConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2); osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ADJUSTMENT] = OSD_POS(12, 2);
osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2); osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2);
osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG; osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG;
osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG; osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG;
@ -2894,6 +3031,8 @@ static void osdResetStats(void)
stats.max_speed = 0; stats.max_speed = 0;
stats.min_voltage = 5000; stats.min_voltage = 5000;
stats.min_rssi = 99; stats.min_rssi = 99;
stats.min_lq = 300;
stats.min_rssi_dbm = 0;
stats.max_altitude = 0; stats.max_altitude = 0;
} }
@ -2926,22 +3065,30 @@ static void osdUpdateStats(void)
if (stats.min_rssi > value) if (stats.min_rssi > value)
stats.min_rssi = value; stats.min_rssi = value;
value = osdGetLQ();
if (stats.min_lq > value)
stats.min_lq = value;
value = osdGetdBm();
if (stats.min_rssi_dbm > value)
stats.min_rssi_dbm = value;
stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude()); stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude());
} }
/* Attention: NTSC screen only has 12 fully visible lines - it is FULL now! */ static void osdShowStatsPage1(void)
static void osdShowStats(void)
{ {
const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS" }; const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS" };
uint8_t top = 1; /* first fully visible line */ uint8_t top = 1; /* first fully visible line */
const uint8_t statNameX = 1; const uint8_t statNameX = 1;
const uint8_t statValuesX = 20; const uint8_t statValuesX = 20;
char buff[10]; char buff[10];
statsPagesCheck = 1;
displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
displayClearScreen(osdDisplayPort); displayClearScreen(osdDisplayPort);
if (osdDisplayIsPAL())
displayWrite(osdDisplayPort, statNameX, top++, " --- STATS ---"); displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2");
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {
displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :");
@ -2962,26 +3109,64 @@ static void osdShowStats(void)
osdFormatAltitudeStr(buff, stats.max_altitude); osdFormatAltitudeStr(buff, stats.max_altitude);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); #if defined(USE_SERIALRX_CRSF)
osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :");
strcat(buff, "V"); itoa(stats.min_lq, buff, 10);
osdLeftAlignString(buff); strcat(buff, "%");
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :");
itoa(stats.min_rssi_dbm, buff, 10);
tfp_sprintf(buff, "%s%c", buff, SYM_DBM);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
#else
displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :");
itoa(stats.min_rssi, buff, 10); itoa(stats.min_rssi, buff, 10);
strcat(buff, "%"); strcat(buff, "%");
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
#endif
displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :");
uint16_t flySeconds = getFlightTime();
uint16_t flyMinutes = flySeconds / 60;
flySeconds %= 60;
uint16_t flyHours = flyMinutes / 60;
flyMinutes %= 60;
tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :");
displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
displayCommitTransaction(osdDisplayPort);
}
static void osdShowStatsPage2(void)
{
uint8_t top = 1; /* first fully visible line */
const uint8_t statNameX = 1;
const uint8_t statValuesX = 20;
char buff[10];
statsPagesCheck = 1;
displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING);
displayClearScreen(osdDisplayPort);
displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 2/2");
displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :");
osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2);
tfp_sprintf(buff, "%s%c", buff, SYM_VOLT);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
if (feature(FEATURE_CURRENT_METER)) { if (feature(FEATURE_CURRENT_METER)) {
displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :");
itoa(stats.max_current, buff, 10); itoa(stats.max_current, buff, 10);
strcat(buff, "A"); tfp_sprintf(buff, "%s%c", buff, SYM_AMP);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :");
itoa(stats.max_power, buff, 10); itoa(stats.max_power, buff, 10);
strcat(buff, "W"); tfp_sprintf(buff, "%s%c", buff, SYM_WATT);
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) {
@ -2990,7 +3175,7 @@ static void osdShowStats(void)
} else { } else {
displayWrite(osdDisplayPort, statNameX, top, "USED WH :"); displayWrite(osdDisplayPort, statNameX, top, "USED WH :");
osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3);
strcat(buff, "\xAB"); // SYM_WH tfp_sprintf(buff, "%s%c", buff, SYM_WH);
} }
displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statValuesX, top++, buff);
@ -3023,15 +3208,6 @@ static void osdShowStats(void)
} }
} }
displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :");
uint16_t flySeconds = getFlightTime();
uint16_t flyMinutes = flySeconds / 60;
flySeconds %= 60;
uint16_t flyHours = flyMinutes / 60;
flyMinutes %= 60;
tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds);
displayWrite(osdDisplayPort, statValuesX, top++, buff);
const float max_gforce = accGetMeasuredMaxG(); const float max_gforce = accGetMeasuredMaxG();
displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); 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);
@ -3041,12 +3217,9 @@ static void osdShowStats(void)
displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:");
osdFormatCentiNumber(buff, acc_extremes[Z].min * 100, 0, 2, 0, 4); osdFormatCentiNumber(buff, acc_extremes[Z].min * 100, 0, 2, 0, 4);
strcat(buff,"/"); strcat(buff,"/");
displayWrite(osdDisplayPort, statValuesX, top, buff); displayWrite(osdDisplayPort, statValuesX - 1, top, buff);
osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3); osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3);
displayWrite(osdDisplayPort, statValuesX + 5, top++, buff); displayWrite(osdDisplayPort, statValuesX + 4, top++, buff);
displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :");
displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]);
displayCommitTransaction(osdDisplayPort); displayCommitTransaction(osdDisplayPort);
} }
@ -3059,8 +3232,8 @@ static void osdShowArmed(void)
char versionBuf[30]; char versionBuf[30];
char *date; char *date;
char *time; char *time;
// We need 12 visible rows // We need 12 visible rows, start row never < first fully visible row 1
uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 12 - 1); uint8_t y = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1;
displayClearScreen(osdDisplayPort); displayClearScreen(osdDisplayPort);
displayWrite(osdDisplayPort, 12, y, "ARMED"); displayWrite(osdDisplayPort, 12, y, "ARMED");
@ -3069,9 +3242,14 @@ static void osdShowArmed(void)
if (strlen(systemConfig()->name) > 0) { if (strlen(systemConfig()->name) > 0) {
osdFormatCraftName(craftNameBuf); osdFormatCraftName(craftNameBuf);
displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig() -> name)) / 2, y, craftNameBuf ); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig() -> name)) / 2, y, craftNameBuf );
y += 2; y += 1;
} }
if (posControl.waypointListValid && posControl.waypointCount > 0) {
displayWrite(osdDisplayPort, 7, y, "*MISSION LOADED*");
}
y += 1;
#if defined(USE_GPS) #if defined(USE_GPS)
if (feature(FEATURE_GPS)) { if (feature(FEATURE_GPS)) {
if (STATE(GPS_FIX_HOME)) { if (STATE(GPS_FIX_HOME)) {
@ -3159,13 +3337,14 @@ static void osdRefresh(timeUs_t currentTimeUs)
osdResetStats(); osdResetStats();
osdShowArmed(); // reset statistic etc osdShowArmed(); // reset statistic etc
uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; uint32_t delay = ARMED_SCREEN_DISPLAY_TIME;
statsPagesCheck = 0;
#if defined(USE_SAFE_HOME) #if defined(USE_SAFE_HOME)
if (isSafeHomeInUse()) if (isSafeHomeInUse())
delay *= 3; delay *= 3;
#endif #endif
osdSetNextRefreshIn(delay); osdSetNextRefreshIn(delay);
} else { } else {
osdShowStats(); // show statistic osdShowStatsPage1(); // show first page of statistic
osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME);
} }
@ -3184,6 +3363,14 @@ static void osdRefresh(timeUs_t currentTimeUs)
if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && DELAYED_REFRESH_RESUME_COMMAND)) { if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && DELAYED_REFRESH_RESUME_COMMAND)) {
displayClearScreen(osdDisplayPort); displayClearScreen(osdDisplayPort);
resumeRefreshAt = 0; resumeRefreshAt = 0;
} else if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && STATS_PAGE1)) {
if (statsPagesCheck == 1) {
osdShowStatsPage1();
}
} else if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && STATS_PAGE2)) {
if (statsPagesCheck == 1) {
osdShowStatsPage2();
}
} else { } else {
displayHeartbeat(osdDisplayPort); displayHeartbeat(osdDisplayPort);
} }
@ -3381,7 +3568,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) {
// Countdown display for remaining Waypoints // Countdown display for remaining Waypoints
tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount);
messages[messageCount++] = messageBuf; messages[messageCount++] = messageBuf;
} else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
// WP hold time countdown in seconds // WP hold time countdown in seconds
@ -3391,8 +3578,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining);
messages[messageCount++] = messageBuf; messages[messageCount++] = messageBuf;
} }
} else { } else {
const char *navStateMessage = navigationStateMessage(); const char *navStateMessage = navigationStateMessage();
if (navStateMessage) { if (navStateMessage) {
messages[messageCount++] = navStateMessage; messages[messageCount++] = navStateMessage;
} }

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

@ -76,6 +76,7 @@
#define OSD_MSG_INVALID_SETTING "INVALID SETTING" #define OSD_MSG_INVALID_SETTING "INVALID SETTING"
#define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE" #define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE"
#define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR" #define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR"
#define OSD_MSG_NO_PREARM "NO PREARM"
#define OSD_MSG_RTH_FS "(RTH)" #define OSD_MSG_RTH_FS "(RTH)"
#define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" #define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)"
#define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" #define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!"
@ -150,8 +151,8 @@ typedef enum {
OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH,
OSD_REMAINING_DISTANCE_BEFORE_RTH, OSD_REMAINING_DISTANCE_BEFORE_RTH,
OSD_HOME_HEADING_ERROR, OSD_HOME_HEADING_ERROR,
OSD_CRUISE_HEADING_ERROR, OSD_COURSE_HOLD_ERROR,
OSD_CRUISE_HEADING_ADJUSTMENT, OSD_COURSE_HOLD_ADJUSTMENT,
OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE, OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE,
OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE, OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE,
OSD_POWER_SUPPLY_IMPEDANCE, OSD_POWER_SUPPLY_IMPEDANCE,
@ -292,7 +293,7 @@ typedef struct osdConfig_s {
float gforce_axis_alarm_min; float gforce_axis_alarm_min;
float gforce_axis_alarm_max; float gforce_axis_alarm_max;
#ifdef USE_SERIALRX_CRSF #ifdef USE_SERIALRX_CRSF
int16_t snr_alarm; //CRSF SNR alarm in dB int8_t snr_alarm; //CRSF SNR alarm in dB
int8_t link_quality_alarm; int8_t link_quality_alarm;
#endif #endif
#ifdef USE_BARO #ifdef USE_BARO
@ -332,13 +333,16 @@ typedef struct osdConfig_s {
uint8_t units; // from osd_unit_e uint8_t units; // from osd_unit_e
uint8_t stats_energy_unit; // from osd_stats_energy_unit_e uint8_t stats_energy_unit; // from osd_stats_energy_unit_e
#ifdef USE_WIND_ESTIMATOR
bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance
#endif
uint8_t coordinate_digits; uint8_t coordinate_digits;
bool osd_failsafe_switch_layout; bool osd_failsafe_switch_layout;
uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE
uint8_t plus_code_short; uint8_t plus_code_short;
uint8_t osd_ahi_style; uint8_t ahi_style;
uint8_t force_grid; // Force a pixel based OSD to use grid mode. 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_bordered; // Only used by the AHI widget
uint8_t ahi_width; // In pixels, only used by the AHI widget uint8_t ahi_width; // In pixels, only used by the AHI widget
@ -348,7 +352,8 @@ typedef struct osdConfig_s {
uint8_t left_sidebar_scroll_step; // How many units each sidebar step represents. 0 means the default value for the scroll type. 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. uint8_t right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar.
bool osd_home_position_arm_screen; 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 crsf_lq_format; uint8_t crsf_lq_format;
} osdConfig_t; } osdConfig_t;

11
src/main/io/osd_canvas.c
View file

@ -354,7 +354,7 @@ static bool osdCanvasDrawArtificialHorizonWidget(displayPort_t *display, display
} }
if (!configured) { if (!configured) {
widgetAHIStyle_e ahiStyle = 0; widgetAHIStyle_e ahiStyle = 0;
switch ((osd_ahi_style_e)osdConfig()->osd_ahi_style) { switch ((osd_ahi_style_e)osdConfig()->ahi_style) {
case OSD_AHI_STYLE_DEFAULT: case OSD_AHI_STYLE_DEFAULT:
ahiStyle = DISPLAY_WIDGET_AHI_STYLE_STAIRCASE; ahiStyle = DISPLAY_WIDGET_AHI_STYLE_STAIRCASE;
break; break;
@ -377,8 +377,13 @@ static bool osdCanvasDrawArtificialHorizonWidget(displayPort_t *display, display
} }
configured = true; configured = true;
} }
// The widget displays 270degs before fixing the bar at the top/bottom
// so that's 135degs each direction. Map that to the configured limit.
const float halfRange = 135.0f;
const float limit = halfRange / 180.0f * M_PIf;
float multiplier = osdConfig()->ahi_style == OSD_AHI_STYLE_DEFAULT ? 1.0f : halfRange / osdConfig()->ahi_max_pitch;
widgetAHIData_t data = { widgetAHIData_t data = {
.pitch = pitchAngle, .pitch = constrainf(pitchAngle * multiplier, -limit, limit),
.roll = rollAngle, .roll = rollAngle,
}; };
if (displayWidgetsDrawAHI(&widgets, instance, &data)) { if (displayWidgetsDrawAHI(&widgets, instance, &data)) {
@ -408,7 +413,7 @@ void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *can
if ((now > nextDrawMinMs && totalError > 0.05f)|| now > nextDrawMaxMs) { if ((now > nextDrawMinMs && totalError > 0.05f)|| now > nextDrawMaxMs) {
if (!osdCanvasDrawArtificialHorizonWidget(display, canvas, p, pitchAngle, rollAngle)) { if (!osdCanvasDrawArtificialHorizonWidget(display, canvas, p, pitchAngle, rollAngle)) {
switch ((osd_ahi_style_e)osdConfig()->osd_ahi_style) { switch ((osd_ahi_style_e)osdConfig()->ahi_style) {
case OSD_AHI_STYLE_DEFAULT: case OSD_AHI_STYLE_DEFAULT:
{ {
int x, y, w, h; int x, y, w, h;

11
src/main/io/osd_common.c
View file

@ -36,6 +36,8 @@
#include "io/osd_common.h" #include "io/osd_common.h"
#include "io/osd_grid.h" #include "io/osd_grid.h"
#include "navigation/navigation.h"
#if defined(USE_OSD) #if defined(USE_OSD)
#define CANVAS_DEFAULT_GRID_ELEMENT_WIDTH OSD_CHAR_WIDTH #define CANVAS_DEFAULT_GRID_ELEMENT_WIDTH OSD_CHAR_WIDTH
@ -152,3 +154,12 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas)
} }
#endif #endif
#ifdef USE_GPS
int16_t osdGet3DSpeed(void)
{
int16_t vert_speed = getEstimatedActualVelocity(Z);
int16_t hor_speed = gpsSol.groundSpeed;
return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
}
#endif

5
src/main/io/osd_common.h
View file

@ -82,3 +82,8 @@ void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, c
// grid slots. // grid slots.
void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading); void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading);
void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas); void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas);
#ifdef USE_GPS
int16_t osdGet3DSpeed(void);
#endif

40
src/main/io/osd_dji_hd.c
View file

@ -58,6 +58,7 @@
#include "io/gps.h" #include "io/gps.h"
#include "io/osd.h" #include "io/osd.h"
#include "io/osd_dji_hd.h" #include "io/osd_dji_hd.h"
#include "io/osd_common.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -68,6 +69,7 @@
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#include "sensors/esc_sensor.h" #include "sensors/esc_sensor.h"
#include "sensors/temperature.h" #include "sensors/temperature.h"
#include "sensors/pitotmeter.h"
#include "msp/msp.h" #include "msp/msp.h"
#include "msp/msp_protocol.h" #include "msp/msp_protocol.h"
@ -118,11 +120,12 @@
* but reuse the packet decoder to minimize code duplication * but reuse the packet decoder to minimize code duplication
*/ */
PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 2);
PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig,
.use_name_for_messages = true, .use_name_for_messages = SETTING_DJI_USE_NAME_FOR_MESSAGES_DEFAULT,
.esc_temperature_source = DJI_OSD_TEMP_ESC, .esc_temperature_source = SETTING_DJI_ESC_TEMP_SOURCE_DEFAULT,
.proto_workarounds = DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA, .proto_workarounds = SETTING_DJI_WORKAROUNDS_DEFAULT,
.speedSource = SETTING_DJI_SPEED_SOURCE_DEFAULT,
); );
// External dependency on looptime // External dependency on looptime
@ -500,6 +503,8 @@ static const char * osdArmingDisabledReasonMessage(void)
return OSD_MESSAGE_STR("CLI"); return OSD_MESSAGE_STR("CLI");
case ARMING_DISABLED_PWM_OUTPUT_ERROR: case ARMING_DISABLED_PWM_OUTPUT_ERROR:
return OSD_MESSAGE_STR("PWM ERR"); return OSD_MESSAGE_STR("PWM ERR");
case ARMING_DISABLED_NO_PREARM:
return OSD_MESSAGE_STR("NO PREARM");
// Cases without message // Cases without message
case ARMING_DISABLED_CMS_MENU: case ARMING_DISABLED_CMS_MENU:
FALLTHROUGH; FALLTHROUGH;
@ -639,13 +644,6 @@ static int32_t osdConvertVelocityToUnit(int32_t vel)
return -1; return -1;
} }
static int16_t osdDJIGet3DSpeed(void)
{
int16_t vert_speed = getEstimatedActualVelocity(Z);
int16_t hor_speed = gpsSol.groundSpeed;
return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed));
}
/** /**
* Converts velocity into a string based on the current unit system. * Converts velocity into a string based on the current unit system.
* @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds) * @param alt Raw velocity (i.e. as taken from gpsSol.groundSpeed in centimeters/seconds)
@ -670,7 +668,7 @@ static void osdDJIFormatThrottlePosition(char *buff, bool autoThr )
thr = rcCommand[THROTTLE]; thr = rcCommand[THROTTLE];
} }
tfp_sprintf(buff, "%3d%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR"); tfp_sprintf(buff, "%3ld%s", (constrain(thr, PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN), "%THR");
} }
/** /**
@ -766,7 +764,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name)
osdDJIFormatThrottlePosition(djibuf,true); osdDJIFormatThrottlePosition(djibuf,true);
break; break;
case 'S': case 'S':
osdDJIFormatVelocityStr(djibuf, osdDJIGet3DSpeed()); osdDJIFormatVelocityStr(djibuf, osdGet3DSpeed());
break; break;
case 'E': case 'E':
osdDJIEfficiencyMahPerKM(djibuf); osdDJIEfficiencyMahPerKM(djibuf);
@ -1004,7 +1002,21 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms
sbufWriteU32(dst, gpsSol.llh.lat); sbufWriteU32(dst, gpsSol.llh.lat);
sbufWriteU32(dst, gpsSol.llh.lon); sbufWriteU32(dst, gpsSol.llh.lon);
sbufWriteU16(dst, gpsSol.llh.alt / 100); sbufWriteU16(dst, gpsSol.llh.alt / 100);
sbufWriteU16(dst, gpsSol.groundSpeed);
int reportedSpeed = 0;
if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_GROUND) {
reportedSpeed = gpsSol.groundSpeed;
} else if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_3D) {
reportedSpeed = osdGet3DSpeed();
} else if (djiOsdConfig()->speedSource == DJI_OSD_SPEED_AIR) {
#ifdef USE_PITOT
reportedSpeed = pitot.airSpeed;
#else
reportedSpeed = 0;
#endif
}
sbufWriteU16(dst, reportedSpeed);
sbufWriteU16(dst, gpsSol.groundCourse); sbufWriteU16(dst, gpsSol.groundCourse);
break; break;

7
src/main/io/osd_dji_hd.h
View file

@ -68,6 +68,12 @@ enum djiOsdTempSource_e {
DJI_OSD_TEMP_BARO = 2 DJI_OSD_TEMP_BARO = 2
}; };
enum djiOsdSpeedSource_e {
DJI_OSD_SPEED_GROUND = 0,
DJI_OSD_SPEED_3D = 1,
DJI_OSD_SPEED_AIR = 2
};
enum djiOsdProtoWorkarounds_e { enum djiOsdProtoWorkarounds_e {
DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA = 1 << 0, DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA = 1 << 0,
}; };
@ -76,6 +82,7 @@ typedef struct djiOsdConfig_s {
uint8_t use_name_for_messages; uint8_t use_name_for_messages;
uint8_t esc_temperature_source; uint8_t esc_temperature_source;
uint8_t proto_workarounds; uint8_t proto_workarounds;
uint8_t speedSource;
} djiOsdConfig_t; } djiOsdConfig_t;
PG_DECLARE(djiOsdConfig_t, djiOsdConfig); PG_DECLARE(djiOsdConfig_t, djiOsdConfig);

5
src/main/io/piniobox.c
View file

@ -33,6 +33,7 @@
#include "fc/fc_msp.h" #include "fc/fc_msp.h"
#include "fc/fc_msp_box.h" #include "fc/fc_msp_box.h"
#include "fc/settings.h"
#include "io/piniobox.h" #include "io/piniobox.h"
@ -40,7 +41,7 @@
PG_REGISTER_WITH_RESET_TEMPLATE(pinioBoxConfig_t, pinioBoxConfig, PG_PINIOBOX_CONFIG, 1); PG_REGISTER_WITH_RESET_TEMPLATE(pinioBoxConfig_t, pinioBoxConfig, PG_PINIOBOX_CONFIG, 1);
PG_RESET_TEMPLATE(pinioBoxConfig_t, pinioBoxConfig, PG_RESET_TEMPLATE(pinioBoxConfig_t, pinioBoxConfig,
{ BOX_PERMANENT_ID_NONE, BOX_PERMANENT_ID_NONE, BOX_PERMANENT_ID_NONE, BOX_PERMANENT_ID_NONE } { SETTING_PINIO_BOX1_DEFAULT, SETTING_PINIO_BOX2_DEFAULT, SETTING_PINIO_BOX3_DEFAULT, SETTING_PINIO_BOX4_DEFAULT }
); );
typedef struct pinioBoxRuntimeConfig_s { typedef struct pinioBoxRuntimeConfig_s {
@ -68,4 +69,4 @@ void pinioBoxUpdate(void)
} }
} }
#endif #endif

6
src/main/io/piniobox.h
View file

@ -26,6 +26,12 @@
#include "common/time.h" #include "common/time.h"
#include "drivers/pinio.h" #include "drivers/pinio.h"
#define BOX_PERMANENT_ID_USER1 47
#define BOX_PERMANENT_ID_USER2 48
#define BOX_PERMANENT_ID_NONE 255 // A permanent ID for no box mode
typedef struct pinioBoxConfig_s { typedef struct pinioBoxConfig_s {
uint8_t permanentId[PINIO_COUNT]; uint8_t permanentId[PINIO_COUNT];
} pinioBoxConfig_t; } pinioBoxConfig_t;

3
src/main/io/serial.c
View file

@ -49,6 +49,7 @@
#include "io/serial.h" #include "io/serial.h"
#include "fc/cli.h" #include "fc/cli.h"
#include "fc/settings.h"
#include "msp/msp_serial.h" #include "msp/msp_serial.h"
@ -147,7 +148,7 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig)
} }
#endif #endif
serialConfig->reboot_character = 'R'; serialConfig->reboot_character = SETTING_REBOOT_CHARACTER_DEFAULT;
} }
baudRate_e lookupBaudRateIndex(uint32_t baudRate) baudRate_e lookupBaudRateIndex(uint32_t baudRate)

6
src/main/io/smartport_master.c
View file

@ -34,6 +34,8 @@ FILE_COMPILE_FOR_SPEED
#include "drivers/serial.h" #include "drivers/serial.h"
#include "drivers/time.h" #include "drivers/time.h"
#include "fc/settings.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/smartport_master.h" #include "io/smartport_master.h"
@ -137,8 +139,8 @@ typedef struct {
PG_REGISTER_WITH_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig, PG_SMARTPORT_MASTER_CONFIG, 0); PG_REGISTER_WITH_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig, PG_SMARTPORT_MASTER_CONFIG, 0);
PG_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig, PG_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig,
.halfDuplex = true, .halfDuplex = SETTING_SMARTPORT_MASTER_HALFDUPLEX_DEFAULT,
.inverted = false .inverted = SETTING_SMARTPORT_MASTER_INVERTED_DEFAULT
); );
static serialPort_t *smartportMasterSerialPort = NULL; static serialPort_t *smartportMasterSerialPort = NULL;

13
src/main/io/vtx.c
View file

@ -36,6 +36,7 @@
#include "fc/config.h" #include "fc/config.h"
#include "fc/rc_modes.h" #include "fc/rc_modes.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/settings.h"
#include "flight/failsafe.h" #include "flight/failsafe.h"
@ -46,12 +47,12 @@
PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 2); PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 2);
PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig,
.band = VTX_SETTINGS_DEFAULT_BAND, .band = SETTING_VTX_BAND_DEFAULT,
.channel = VTX_SETTINGS_DEFAULT_CHANNEL, .channel = SETTING_VTX_CHANNEL_DEFAULT,
.power = VTX_SETTINGS_DEFAULT_POWER, .power = SETTING_VTX_POWER_DEFAULT,
.pitModeChan = VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL, .pitModeChan = SETTING_VTX_PIT_MODE_CHAN_DEFAULT,
.lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF, .lowPowerDisarm = SETTING_VTX_LOW_POWER_DISARM_DEFAULT,
.maxPowerOverride = 0, .maxPowerOverride = SETTING_VTX_MAX_POWER_OVERRIDE_DEFAULT,
); );
typedef enum { typedef enum {

6
src/main/io/vtx_control.c
View file

@ -31,6 +31,7 @@
#include "fc/config.h" #include "fc/config.h"
#include "fc/runtime_config.h" #include "fc/runtime_config.h"
#include "fc/settings.h"
#include "io/beeper.h" #include "io/beeper.h"
#include "io/osd.h" #include "io/osd.h"
@ -42,8 +43,8 @@
PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 3); PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 3);
PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig,
.halfDuplex = true, .halfDuplex = SETTING_VTX_HALFDUPLEX_DEFAULT,
.smartAudioEarlyAkkWorkaroundEnable = true, .smartAudioEarlyAkkWorkaroundEnable = SETTING_VTX_SMARTAUDIO_EARLY_AKK_WORKAROUND_DEFAULT,
); );
static uint8_t locked = 0; static uint8_t locked = 0;
@ -182,4 +183,3 @@ void vtxCyclePower(const uint8_t powerStep)
} }
#endif #endif

6
src/main/io/vtx_smartaudio.c
View file

@ -43,6 +43,8 @@
#include "drivers/time.h" #include "drivers/time.h"
#include "drivers/vtx_common.h" #include "drivers/vtx_common.h"
#include "fc/settings.h"
#include "io/serial.h" #include "io/serial.h"
#include "io/vtx.h" #include "io/vtx.h"
#include "io/vtx_control.h" #include "io/vtx_control.h"
@ -129,8 +131,8 @@ saPowerTable_t saPowerTable[VTX_SMARTAUDIO_MAX_POWER_COUNT] = {
smartAudioDevice_t saDevice = { smartAudioDevice_t saDevice = {
.version = SA_UNKNOWN, .version = SA_UNKNOWN,
.channel = -1, .channel = SETTING_VTX_CHANNEL_DEFAULT,
.power = -1, .power = SETTING_VTX_POWER_DEFAULT,
.mode = 0, .mode = 0,
.freq = 0, .freq = 0,
.orfreq = 0, .orfreq = 0,

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

@ -63,6 +63,7 @@
#define MSP2_INAV_GVAR_STATUS 0x2027 #define MSP2_INAV_GVAR_STATUS 0x2027
#define MSP2_INAV_PROGRAMMING_PID 0x2028 #define MSP2_INAV_PROGRAMMING_PID 0x2028
#define MSP2_INAV_SET_PROGRAMMING_PID 0x2029 #define MSP2_INAV_SET_PROGRAMMING_PID 0x2029
#define MSP2_INAV_PROGRAMMING_PID_STATUS 0x202A
#define MSP2_PID 0x2030 #define MSP2_PID 0x2030
#define MSP2_SET_PID 0x2031 #define MSP2_SET_PID 0x2031
@ -79,4 +80,3 @@
#define MSP2_INAV_SET_SAFEHOME 0x2039 #define MSP2_INAV_SET_SAFEHOME 0x2039
#define MSP2_INAV_MISC2 0x203A #define MSP2_INAV_MISC2 0x203A

677
src/main/navigation/navigation.c Executable file → Normal file
View file

File diff suppressed because it is too large Load diff

7
src/main/navigation/navigation.h
View file

@ -210,6 +210,8 @@ typedef struct navConfig_s {
uint8_t max_bank_angle; // multicopter max banking angle (deg) uint8_t max_bank_angle; // multicopter max banking angle (deg)
uint16_t hover_throttle; // multicopter hover throttle uint16_t hover_throttle; // multicopter hover throttle
uint16_t auto_disarm_delay; // multicopter safety delay for landing detector uint16_t auto_disarm_delay; // multicopter safety delay for landing detector
#ifdef USE_MR_BRAKING_MODE
uint16_t braking_speed_threshold; // above this speed braking routine might kick in uint16_t braking_speed_threshold; // above this speed braking routine might kick in
uint16_t braking_disengage_speed; // below this speed braking will be disengaged uint16_t braking_disengage_speed; // below this speed braking will be disengaged
uint16_t braking_timeout; // Timeout for braking mode uint16_t braking_timeout; // Timeout for braking mode
@ -218,8 +220,11 @@ typedef struct navConfig_s {
uint16_t braking_boost_speed_threshold; // Above this speed braking boost mode can engage uint16_t braking_boost_speed_threshold; // Above this speed braking boost mode can engage
uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage
uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase
#endif
uint8_t posDecelerationTime; // Brake time parameter uint8_t posDecelerationTime; // Brake time parameter
uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC) uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC)
bool slowDownForTurning; // Slow down during WP missions when changing heading on next waypoint
} mc; } mc;
struct { struct {
@ -251,7 +256,7 @@ typedef struct navConfig_s {
uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
bool allow_manual_thr_increase; bool allow_manual_thr_increase;
bool useFwNavYawControl; bool useFwNavYawControl;
uint8_t yawControlDeadband; uint8_t yawControlDeadband;
} fw; } fw;
} navConfig_t; } navConfig_t;

4
src/main/navigation/navigation_fixedwing.c
View file

@ -259,7 +259,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait()) bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
&& (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG)) && (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f) && (distanceToActualTarget > 50.0f)
&& !FLIGHT_MODE(NAV_CRUISE_MODE); && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
// Calculate virtual position for straight movement // Calculate virtual position for straight movement
if (needToCalculateCircularLoiter) { if (needToCalculateCircularLoiter) {
@ -649,7 +649,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
posControl.rcAdjustment[ROLL] = 0; posControl.rcAdjustment[ROLL] = 0;
} }
if (FLIGHT_MODE(NAV_CRUISE_MODE) && posControl.flags.isAdjustingPosition) if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition)
rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband);
//if (navStateFlags & NAV_CTL_YAW) //if (navStateFlags & NAV_CTL_YAW)

5
src/main/navigation/navigation_fw_launch.c
View file

@ -48,6 +48,8 @@
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
#include "io/gps.h"
#define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate
#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms #define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms
#define UNUSED(x) ((void)(x)) #define UNUSED(x) ((void)(x))
@ -316,10 +318,11 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU
const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
const bool isForwardLaunched = isGPSHeadingValid() && (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
applyThrottleIdleLogic(false); applyThrottleIdleLogic(false);
if (isBungeeLaunched || isSwingLaunched) { if (isBungeeLaunched || isSwingLaunched || isForwardLaunched) {
if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) {
return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED
} }

16
src/main/navigation/navigation_multicopter.c
View file

@ -406,7 +406,7 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR
static float getVelocityHeadingAttenuationFactor(void) static float getVelocityHeadingAttenuationFactor(void)
{ {
// In WP mode scale velocity if heading is different from bearing // In WP mode scale velocity if heading is different from bearing
if (navGetCurrentStateFlags() & NAV_AUTO_WP) { if (navConfig()->mc.slowDownForTurning && (navGetCurrentStateFlags() & NAV_AUTO_WP)) {
const int32_t headingError = constrain(wrap_18000(posControl.desiredState.yaw - posControl.actualState.yaw), -9000, 9000); const int32_t headingError = constrain(wrap_18000(posControl.desiredState.yaw - posControl.actualState.yaw), -9000, 9000);
const float velScaling = cos_approx(CENTIDEGREES_TO_RADIANS(headingError)); const float velScaling = cos_approx(CENTIDEGREES_TO_RADIANS(headingError));
@ -438,7 +438,19 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
// Scale velocity to respect max_speed // Scale velocity to respect max_speed
float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY)); float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY));
if (newVelTotal > maxSpeed) {
/*
* We override computed speed with max speed in following cases:
* 1 - computed velocity is > maxSpeed
* 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed
*/
if (
(navGetCurrentStateFlags() & NAV_AUTO_WP &&
!isApproachingLastWaypoint() &&
newVelTotal < maxSpeed &&
!navConfig()->mc.slowDownForTurning
) || newVelTotal > maxSpeed
) {
newVelX = maxSpeed * (newVelX / newVelTotal); newVelX = maxSpeed * (newVelX / newVelTotal);
newVelY = maxSpeed * (newVelY / newVelTotal); newVelY = maxSpeed * (newVelY / newVelTotal);
newVelTotal = maxSpeed; newVelTotal = maxSpeed;

65
src/main/navigation/navigation_pos_estimator.c
View file

@ -37,8 +37,10 @@
#include "drivers/time.h" #include "drivers/time.h"
#include "fc/config.h" #include "fc/config.h"
#include "fc/settings.h"
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/secondary_imu.h"
#include "io/gps.h" #include "io/gps.h"
@ -58,39 +60,39 @@ PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationCo
PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
// Inertial position estimator parameters // Inertial position estimator parameters
.automatic_mag_declination = 1, .automatic_mag_declination = SETTING_INAV_AUTO_MAG_DECL_DEFAULT,
.reset_altitude_type = NAV_RESET_ON_FIRST_ARM, .reset_altitude_type = SETTING_INAV_RESET_ALTITUDE_DEFAULT,
.reset_home_type = NAV_RESET_ON_FIRST_ARM, .reset_home_type = SETTING_INAV_RESET_HOME_DEFAULT,
.gravity_calibration_tolerance = 5, // 5 cm/s/s calibration error accepted (0.5% of gravity) .gravity_calibration_tolerance = SETTING_INAV_GRAVITY_CAL_TOLERANCE_DEFAULT, // 5 cm/s/s calibration error accepted (0.5% of gravity)
.use_gps_velned = 1, // "Disabled" is mandatory with gps_dyn_model = Pedestrian .use_gps_velned = SETTING_INAV_USE_GPS_VELNED_DEFAULT, // "Disabled" is mandatory with gps_dyn_model = Pedestrian
.use_gps_no_baro = 0, // Use GPS altitude if no baro is available on all aircrafts .use_gps_no_baro = SETTING_INAV_USE_GPS_NO_BARO_DEFAULT, // Use GPS altitude if no baro is available on all aircrafts
.allow_dead_reckoning = 0, .allow_dead_reckoning = SETTING_INAV_ALLOW_DEAD_RECKONING_DEFAULT,
.max_surface_altitude = 200, .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT,
.w_xyz_acc_p = 1.0f, .w_xyz_acc_p = SETTING_INAV_W_XYZ_ACC_P_DEFAULT,
.w_z_baro_p = 0.35f, .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT,
.w_z_surface_p = 3.500f, .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT,
.w_z_surface_v = 6.100f, .w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT,
.w_z_gps_p = 0.2f, .w_z_gps_p = SETTING_INAV_W_Z_GPS_P_DEFAULT,
.w_z_gps_v = 0.1f, .w_z_gps_v = SETTING_INAV_W_Z_GPS_V_DEFAULT,
.w_xy_gps_p = 1.0f, .w_xy_gps_p = SETTING_INAV_W_XY_GPS_P_DEFAULT,
.w_xy_gps_v = 2.0f, .w_xy_gps_v = SETTING_INAV_W_XY_GPS_V_DEFAULT,
.w_xy_flow_p = 1.0f, .w_xy_flow_p = SETTING_INAV_W_XY_FLOW_P_DEFAULT,
.w_xy_flow_v = 2.0f, .w_xy_flow_v = SETTING_INAV_W_XY_FLOW_V_DEFAULT,
.w_z_res_v = 0.5f, .w_z_res_v = SETTING_INAV_W_Z_RES_V_DEFAULT,
.w_xy_res_v = 0.5f, .w_xy_res_v = SETTING_INAV_W_XY_RES_V_DEFAULT,
.w_acc_bias = 0.01f, .w_acc_bias = SETTING_INAV_W_ACC_BIAS_DEFAULT,
.max_eph_epv = 1000.0f, .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT,
.baro_epv = 100.0f .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT
); );
#define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; }
@ -214,7 +216,11 @@ void onNewGPSData(void)
if(STATE(GPS_FIX_HOME)){ if(STATE(GPS_FIX_HOME)){
static bool magDeclinationSet = false; static bool magDeclinationSet = false;
if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) { if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) {
imuSetMagneticDeclination(geoCalculateMagDeclination(&newLLH)); const float declination = geoCalculateMagDeclination(&newLLH);
imuSetMagneticDeclination(declination);
#ifdef USE_SECONDARY_IMU
secondaryImuSetMagneticDeclination(declination);
#endif
magDeclinationSet = true; magDeclinationSet = true;
} }
} }
@ -569,7 +575,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
if (ctx->newFlags & EST_GPS_Z_VALID) { if (ctx->newFlags & EST_GPS_Z_VALID) {
// Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution // Trust GPS velocity only if residual/error is less than 2.5 m/s, scale weight according to gaussian distribution
const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z; const float gpsRocResidual = posEstimator.gps.vel.z - posEstimator.est.vel.z;
const float gpsRocScaler = bellCurve(gpsRocResidual, 2.5f); const float gpsRocScaler = bellCurve(gpsRocResidual, 250.0f);
ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt; ctx->estVelCorr.z += gpsRocResidual * positionEstimationConfig()->w_z_gps_v * gpsRocScaler * ctx->dt;
} }
@ -606,6 +612,15 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
return true; return true;
} }
DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate
DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude
DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude
DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level
DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate
DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame
DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed
DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count
return false; return false;
} }

30
src/main/navigation/navigation_private.h
View file

@ -140,9 +140,9 @@ typedef enum {
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3,
NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME,
NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D, NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD,
NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D, NAV_FSM_EVENT_SWITCH_TO_CRUISE,
NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ, NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ,
NAV_FSM_EVENT_COUNT, NAV_FSM_EVENT_COUNT,
} navigationFSMEvent_t; } navigationFSMEvent_t;
@ -187,13 +187,13 @@ typedef enum {
NAV_PERSISTENT_ID_UNUSED_3 = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY NAV_PERSISTENT_ID_UNUSED_3 = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY
NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS = 28, NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS = 28,
NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE = 29, NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE = 29,
NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS = 30, NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS = 30,
NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING = 31, NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING = 31,
NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE = 32, NAV_PERSISTENT_ID_CRUISE_INITIALIZE = 32,
NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS = 33, NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS = 33,
NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING = 34, NAV_PERSISTENT_ID_CRUISE_ADJUSTING = 34,
NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35, NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35,
NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36, NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36,
@ -239,12 +239,12 @@ typedef enum {
NAV_STATE_LAUNCH_WAIT, NAV_STATE_LAUNCH_WAIT,
NAV_STATE_LAUNCH_IN_PROGRESS, NAV_STATE_LAUNCH_IN_PROGRESS,
NAV_STATE_CRUISE_2D_INITIALIZE, NAV_STATE_COURSE_HOLD_INITIALIZE,
NAV_STATE_CRUISE_2D_IN_PROGRESS, NAV_STATE_COURSE_HOLD_IN_PROGRESS,
NAV_STATE_CRUISE_2D_ADJUSTING, NAV_STATE_COURSE_HOLD_ADJUSTING,
NAV_STATE_CRUISE_3D_INITIALIZE, NAV_STATE_CRUISE_INITIALIZE,
NAV_STATE_CRUISE_3D_IN_PROGRESS, NAV_STATE_CRUISE_IN_PROGRESS,
NAV_STATE_CRUISE_3D_ADJUSTING, NAV_STATE_CRUISE_ADJUSTING,
NAV_STATE_COUNT, NAV_STATE_COUNT,
} navigationFSMState_t; } navigationFSMState_t;

34
src/main/programming/logic_condition.c
View file

@ -44,6 +44,8 @@
#include "flight/imu.h" #include "flight/imu.h"
#include "flight/pid.h" #include "flight/pid.h"
#include "drivers/io_port_expander.h" #include "drivers/io_port_expander.h"
#include "io/osd_common.h"
#include "sensors/diagnostics.h"
#include "navigation/navigation.h" #include "navigation/navigation.h"
#include "navigation/navigation_private.h" #include "navigation/navigation_private.h"
@ -317,6 +319,20 @@ static int logicConditionCompute(
return true; return true;
break; break;
case LOGIC_CONDITION_SET_HEADING_TARGET:
temporaryValue = CENTIDEGREES_TO_DEGREES(wrap_36000(DEGREES_TO_CENTIDEGREES(operandA)));
updateHeadingHoldTarget(temporaryValue);
return temporaryValue;
break;
case LOGIC_CONDITION_MODULUS:
if (operandB != 0) {
return constrain(operandA % operandB, INT16_MIN, INT16_MAX);
} else {
return operandA;
}
break;
default: default:
return false; return false;
break; break;
@ -377,7 +393,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99);
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 10 case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100
return getBatteryVoltage(); return getBatteryVoltage();
break; break;
@ -393,7 +409,15 @@ static int logicConditionGetFlightOperandValue(int operand) {
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS:
return gpsSol.numSat; if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) {
return 0;
} else {
return gpsSol.numSat;
}
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1
return STATE(GPS_FIX) ? 1 : 0;
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s
@ -402,7 +426,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
//FIXME align with osdGet3DSpeed //FIXME align with osdGet3DSpeed
case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s
return (int) sqrtf(sq(gpsSol.groundSpeed) + sq((int)getEstimatedActualVelocity(Z))); return osdGet3DSpeed();
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s
@ -418,7 +442,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s case LOGIC_CONDITION_OPERAND_FLIGHT_VERTICAL_SPEED: // cm/s
return constrain(getEstimatedActualVelocity(Z), 0, INT16_MAX); return constrain(getEstimatedActualVelocity(Z), INT16_MIN, INT16_MAX);
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // % case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // %
@ -536,7 +560,7 @@ static int logicConditionGetFlightModeOperandValue(int operand) {
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE: case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE:
return (bool) FLIGHT_MODE(NAV_CRUISE_MODE); return (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD: case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD:

7
src/main/programming/logic_condition.h
View file

@ -68,7 +68,9 @@ typedef enum {
LOGIC_CONDITION_MAP_INPUT = 36, LOGIC_CONDITION_MAP_INPUT = 36,
LOGIC_CONDITION_MAP_OUTPUT = 37, LOGIC_CONDITION_MAP_OUTPUT = 37,
LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38, LOGIC_CONDITION_RC_CHANNEL_OVERRIDE = 38,
LOGIC_CONDITION_LAST = 39, LOGIC_CONDITION_SET_HEADING_TARGET = 39,
LOGIC_CONDITION_MODULUS = 40,
LOGIC_CONDITION_LAST = 41,
} logicOperation_e; } logicOperation_e;
typedef enum logicOperandType_s { typedef enum logicOperandType_s {
@ -117,6 +119,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31 LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32
LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33
LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34
} logicFlightOperands_e; } logicFlightOperands_e;
@ -193,4 +196,4 @@ void logicConditionReset(void);
float getThrottleScale(float globalThrottleScale); float getThrottleScale(float globalThrottleScale);
int16_t getRcCommandOverride(int16_t command[], uint8_t axis); int16_t getRcCommandOverride(int16_t command[], uint8_t axis);
int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue);

Some files were not shown because too many files have changed in this diff Show more