diff --git a/AUTHORS b/AUTHORS index 11085ca78c..839a8a5d8e 100644 --- a/AUTHORS +++ b/AUTHORS @@ -7,6 +7,7 @@ Alberto García Hierro Alex Gorbatchev Alex Zaitsev Alexander Fedorov +Alexander van Saase Alexey Stankevich Andre Bernet Andreas Tacke diff --git a/CMakeLists.txt b/CMakeLists.txt index dc9a5385d3..400e890b7f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,14 @@ endif() 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) add_subdirectory(src/test) else() @@ -29,7 +37,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 2.7.0) +project(INAV VERSION 3.0.0) enable_language(ASM) @@ -49,10 +57,6 @@ if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebI set(IS_RELEASE_BUILD ON) endif() -include(GetGitRevisionDescription) -get_git_head_revision(GIT_REFSPEC GIT_SHA1) -string(SUBSTRING ${GIT_SHA1} 0 8 GIT_REV) - set(FIRMWARE_VERSION ${PROJECT_VERSION}) 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} ) -include(settings) include(openocd) include(svd) -include(main) include(stm32) add_subdirectory(src) diff --git a/README.md b/README.md index cbe75a1b18..f34db4d136 100644 --- a/README.md +++ b/README.md @@ -39,7 +39,7 @@ Tool for Blackbox logs analysis is available [here](https://github.com/iNavFligh ### 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 diff --git a/cmake/settings.cmake b/cmake/settings.cmake index 17d48168b4..5d4e4e3862 100644 --- a/cmake/settings.cmake +++ b/cmake/settings.cmake @@ -4,6 +4,7 @@ set(SETTINGS_GENERATED_H "${SETTINGS_GENERATED}.h") set(SETTINGS_FILE "${MAIN_SRC_DIR}/fc/settings.yaml") set(SETTINGS_GENERATOR "${MAIN_UTILS_DIR}/settings.rb") +include(CMakeParseArguments) function(enable_settings exe name) get_generated_files_dir(dir ${name}) @@ -15,11 +16,26 @@ function(enable_settings exe name) list(APPEND cflags ${options}) list(APPEND cflags ${includes}) 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( - OUTPUT ${dir}/${SETTINGS_GENERATED_H} ${dir}/${SETTINGS_GENERATED_C} + OUTPUT ${output} 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}" DEPENDS ${SETTINGS_GENERATOR} ${SETTINGS_FILE} ) + set(${args_OUTPUTS} ${output} PARENT_SCOPE) endfunction() diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 6e658058d1..ca5cc65f7f 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -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` | | 36 | MAP_INPUT | Scales `Operand A` from [`0` : `Operand B`] to [`0` : `1000`]. Note: input will be constrained and then scaled | | 37 | MAP_OUTPUT | Scales `Operand A` from [`0` : `1000`] to [`0` : `Operand B`]. Note: input will be constrained and then scaled | +| 38 | RC_CHANNEL_OVERRIDE | Overrides channel set by `Operand A` to value of `Operand B` | +| 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 @@ -93,8 +96,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 1 | HOME_DISTANCE | in `meters` | | 2 | TRIP_DISTANCE | in `meters` | | 3 | RSSI | | -| 4 | VBAT | in `Volts * 10`, eg. `12.1V` is `121` | -| 5 | CELL_VOLTAGE | in `Volts * 10`, eg. `12.1V` is `121` | +| 4 | VBAT | in `Volts * 100`, eg. `12.1V` is `1210` | +| 5 | CELL_VOLTAGE | in `Volts * 100`, eg. `12.1V` is `1210` | | 6 | CURRENT | in `Amps * 100`, eg. `9A` is `900` | | 7 | MAH_DRAWN | in `mAh` | | 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 | | 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 | -| 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 @@ -258,4 +262,4 @@ Steps: 1. Normalize range [1000:2000] to [0:1000] by substracting `1000` 2. Scale range [0:1000] to [0:3] 3. Increase range by `1` to have the range of [1:4] -4. Assign LC#2 to VTX power function \ No newline at end of file +4. Assign LC#2 to VTX power function diff --git a/docs/Settings.md b/docs/Settings.md index 110550f1b6..0dfd196cde 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -12,8 +12,8 @@ | 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_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_hz | | | +| acc_notch_cutoff | 1 | | +| acc_notch_hz | 0 | | | 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_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. | | 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. | -| 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_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 | @@ -32,7 +32,7 @@ | 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_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] | | 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 | @@ -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_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 | -| 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` | | 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_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. | -| 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_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 | | 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_meter_offset | 0 | 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_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 | _target default_ | This sets the output offset voltage of the current sensor in millivolts. | +| 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. | -| d_boost_factor | | | -| d_boost_gyro_delta_lpf_hz | | | -| d_boost_max_at_acceleration | | | +| d_boost_factor | 1.25 | | +| d_boost_gyro_delta_lpf_hz | 80 | | +| 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. | | 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. | | 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_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_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_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_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 | +| 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_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_range | `MEDIUM` | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers | -| eleres_freq | | | -| eleres_loc_delay | | | -| eleres_loc_en | | | -| eleres_loc_power | | | -| eleres_signature | | | -| eleres_telemetry_en | | | -| eleres_telemetry_power | | | -| esc_sensor_listen_only | | | +| 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 | 435 | | +| eleres_loc_delay | 240 | | +| eleres_loc_en | OFF | | +| eleres_loc_power | 7 | | +| eleres_signature | 0 | | +| eleres_telemetry_en | OFF | | +| eleres_telemetry_power | 7 | | +| 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_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 | @@ -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_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_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. | | 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_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_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_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 | 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_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 | @@ -113,6 +115,9 @@ | 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_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_roll | 50 | Fixed-wing rate stabilisation FF-gain for ROLL | | 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_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_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_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. | @@ -143,15 +149,15 @@ | 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_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_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_hz | | | +| gyro_notch_cutoff | 1 | | +| 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_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_to_use | | | +| 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 | 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 | 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. | | 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. | @@ -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) | | 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 | +| 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_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 | @@ -167,37 +188,37 @@ | 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_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_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_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_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_w_acc_bias | 0.010 | Weight for accelerometer drift estimation | -| inav_w_xy_flow_p | | | -| inav_w_xy_flow_v | | | -| inav_w_xy_gps_p | 1.000 | 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_res_v | 0.500 | Decay coefficient for estimated velocity when GPS reference for position is lost | -| inav_w_xyz_acc_p | | | -| inav_w_z_baro_p | 0.350 | 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_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_res_v | 0.500 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost | -| inav_w_z_surface_p | | | -| inav_w_z_surface_v | | | +| inav_w_acc_bias | 0.01 | Weight for accelerometer drift estimation | +| inav_w_xy_flow_p | 1.0 | | +| inav_w_xy_flow_v | 2.0 | | +| inav_w_xy_gps_p | 1.0 | Weight of GPS coordinates in estimated UAV position and speed. | +| inav_w_xy_gps_v | 2.0 | Weight of GPS velocity data in estimated UAV speed | +| inav_w_xy_res_v | 0.5 | Decay coefficient for estimated velocity when GPS reference for position is lost | +| inav_w_xyz_acc_p | 1.0 | | +| inav_w_z_baro_p | 0.35 | Weight of barometer measurements in estimated altitude and climb rate | +| 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.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.5 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost | +| inav_w_z_surface_p | 3.5 | | +| 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) | | 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. | | 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. | | 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_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_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 | @@ -209,12 +230,12 @@ | 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_yaw_rate | 100 | Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% | -| mavlink_ext_status_rate | | | -| mavlink_extra1_rate | | | -| mavlink_extra2_rate | | | -| mavlink_extra3_rate | | | -| mavlink_pos_rate | | | -| mavlink_rc_chan_rate | | | +| mavlink_ext_status_rate | 2 | | +| mavlink_extra1_rate | 10 | | +| mavlink_extra2_rate | 2 | | +| mavlink_extra3_rate | 1 | | +| mavlink_pos_rate | 2 | | +| mavlink_rc_chan_rate | 5 | | | 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_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_roll | 30 | Multicopter rate stabilisation I-gain for ROLL | | mc_i_yaw | 45 | Multicopter rate stabilisation I-gain for YAW | -| mc_iterm_relax | | | -| mc_iterm_relax_cutoff | | | +| mc_iterm_relax | RP | | +| mc_iterm_relax_cutoff | 15 | | | mc_p_level | 20 | Multicopter attitude stabilisation P-gain | | mc_p_pitch | 40 | Multicopter rate stabilisation P-gain for PITCH | | 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_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_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_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. | -| 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_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_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_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_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 | @@ -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_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_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_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] | @@ -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_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_i | 0 | 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_i | 2 | I gain of heading trajectory 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_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_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_i | 0 | I gain of altitude PID controller (Fixedwing) | -| nav_fw_pos_z_p | 50 | P gain of altitude PID controller (Fixedwing) | +| nav_fw_pos_z_d | 10 | D gain of altitude PID controller (Fixedwing) | +| nav_fw_pos_z_i | 5 | I 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_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_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_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_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 | @@ -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_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_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_ff | | | +| 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 | 2 | | +| 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_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_i | 50 | I 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_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) | @@ -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_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. | -| opflow_hardware | | Selection of OPFLOW hardware. | -| opflow_scale | | | +| opflow_hardware | NONE | Selection of OPFLOW hardware. | +| opflow_scale | 10.5 | | | 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_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_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_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_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_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_coordinate_digits | | | +| osd_coordinate_digits | 9 | | | osd_crosshairs_style | DEFAULT | To set the visual type for the crosshair | | 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. | @@ -375,49 +397,52 @@ | 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_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_homing | 0 | To display little arrows around the crossair showing where the home point is in the hud | +| osd_hud_homepoint | OFF | To 3D-display the home point location 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_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_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_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_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_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_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_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_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_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_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_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | | osd_units | METRIC | IMPERIAL, METRIC, UK | | 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_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 | -| pinio_box1 | | | -| pinio_box2 | | | -| pinio_box3 | | | -| pinio_box4 | | | +| 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 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | +| pinio_box2 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | +| pinio_box3 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | +| 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. | | pitot_hardware | NONE | Selection of pitot hardware. | -| pitot_lpf_milli_hz | | | -| pitot_scale | | | -| 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 | +| pitot_lpf_milli_hz | 350 | | +| 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 | +| 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_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. | @@ -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_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 | -| 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 | | 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_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 | -| 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_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_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 | | 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_spi_id | | | -| rx_spi_protocol | | | -| rx_spi_rf_channel_count | | | +| rx_spi_id | 0 | | +| rx_spi_protocol | _target default_ | | +| 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. | -| sbus_sync_interval | | | -| 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. | +| sbus_sync_interval | 3000 | | +| 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_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_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) | @@ -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_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 | -| 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_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_pin | Empty string | 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_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 | -32767 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. | +| sim_pin | 0000 | PIN code for the SIM module | +| 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 | | 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_master_halfduplex | | | -| smartport_master_inverted | | | -| spektrum_sat_bind | 0 | 0 = disabled. Used to bind the spektrum satellite to RX | -| srxl2_baud_fast | | | -| srxl2_unit_id | | | +| smartport_master_halfduplex | ON | | +| smartport_master_inverted | OFF | | +| spektrum_sat_bind | `SPEKTRUM_SAT_BIND_DISABLED` | 0 = disabled. Used to bind the spektrum satellite to RX | +| srxl2_baud_fast | ON | | +| srxl2_unit_id | 1 | | | 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_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. | | 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_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_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_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. | | 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. | | 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_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_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_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 | 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_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_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) | | 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_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_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_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_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. | > Note: this table is autogenerated. Do not edit it manually. \ No newline at end of file diff --git a/docs/Telemetry.md b/docs/Telemetry.md index 8c8b406ccf..2b5972370e 100644 --- a/docs/Telemetry.md +++ b/docs/Telemetry.md @@ -123,7 +123,7 @@ The following sensors are transmitted * **0450** : 'Flight Path Vector' or 'Course over ground' in degrees*10 ### 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 diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index a80fdd9feb..6e2d28c864 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -48,6 +48,8 @@ main_sources(COMMON_SRC common/typeconversion.h common/uvarint.c common/uvarint.h + common/circular_queue.c + common/circular_queue.h config/config_eeprom.c config/config_eeprom.h @@ -73,6 +75,8 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro_adxl345.h drivers/accgyro/accgyro_bma280.c drivers/accgyro/accgyro_bma280.h + drivers/accgyro/accgyro_bmi088.c + drivers/accgyro/accgyro_bmi088.h drivers/accgyro/accgyro_bmi160.c drivers/accgyro/accgyro_bmi160.h drivers/accgyro/accgyro_fake.c @@ -99,6 +103,8 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro_mpu6500.h drivers/accgyro/accgyro_mpu9250.c drivers/accgyro/accgyro_mpu9250.h + drivers/accgyro/accgyro_bno055.c + drivers/accgyro/accgyro_bno055.h drivers/adc.c drivers/adc.h @@ -152,6 +158,8 @@ main_sources(COMMON_SRC drivers/compass/compass_mpu9250.h drivers/compass/compass_qmc5883l.c drivers/compass/compass_qmc5883l.h + drivers/compass/compass_rm3100.c + drivers/compass/compass_rm3100.h drivers/compass/compass_msp.c drivers/compass/compass_msp.h @@ -226,9 +234,13 @@ main_sources(COMMON_SRC drivers/rangefinder/rangefinder_srf10.h drivers/rangefinder/rangefinder_vl53l0x.c drivers/rangefinder/rangefinder_vl53l0x.h + drivers/rangefinder/rangefinder_vl53l1x.c + drivers/rangefinder/rangefinder_vl53l1x.h drivers/rangefinder/rangefinder_virtual.c drivers/rangefinder/rangefinder_virtual.h - + drivers/rangefinder/rangefinder_us42.c + drivers/rangefinder/rangefinder_us42.h + drivers/resource.c drivers/resource.h drivers/rcc.c @@ -325,6 +337,8 @@ main_sources(COMMON_SRC flight/dynamic_gyro_notch.h flight/dynamic_lpf.c flight/dynamic_lpf.h + flight/secondary_imu.c + flight/secondary_imu.h io/beeper.c io/beeper.h diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2a98978442..a39aaed510 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -52,6 +52,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -91,18 +92,18 @@ #endif #ifdef SDCARD_DETECT_INVERTED -#define BLACKBOX_INTERVED_CARD_DETECTION 1 +#define BLACKBOX_INVERTED_CARD_DETECTION 1 #else -#define BLACKBOX_INTERVED_CARD_DETECTION 0 +#define BLACKBOX_INVERTED_CARD_DETECTION 0 #endif PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 1); PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, .device = DEFAULT_BLACKBOX_DEVICE, - .rate_num = 1, - .rate_denom = 1, - .invertedCardDetection = BLACKBOX_INTERVED_CARD_DETECTION, + .rate_num = SETTING_BLACKBOX_RATE_NUM_DEFAULT, + .rate_denom = SETTING_BLACKBOX_RATE_DENOM_DEFAULT, + .invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION, ); #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("acc_1G", "%u", acc.dev.acc_1G); +#ifdef USE_ADC BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { blackboxPrintfHeaderLine("vbat_scale", "%u", batteryMetersConfig()->voltage.scale / 10); @@ -1667,6 +1669,7 @@ static bool blackboxWriteSysinfo(void) currentBatteryProfile->voltage.cellWarning / 10, currentBatteryProfile->voltage.cellMax / 10); BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference); +#endif BLACKBOX_PRINT_HEADER_LINE_CUSTOM( //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_type", "%d", gyroConfig()->gyro_soft_lpf_type); 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("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz); +#endif BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_notch_hz, 0); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_notch_cutoff, 1); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); +#ifdef USE_BARO BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware); +#endif +#ifdef USE_MAG 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("motor_pwm_protocol", "%d", motorConfig()->motorPwmProtocol); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->motorPwmRate); diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b484fd4d35..e8f47543eb 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -80,5 +80,8 @@ typedef enum { DEBUG_SPM_VARIO, // Smartport master variometer DEBUG_PCF8574, DEBUG_DYNAMIC_GYRO_LPF, + DEBUG_FW_D, + DEBUG_IMU2, + DEBUG_ALTITUDE, DEBUG_COUNT } debugType_e; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 68311250c7..eaf5770162 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -55,7 +55,7 @@ #define RPY_PIDFF_MAX 200 #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 OTHER_PIDFF_ENTRY(label, ptr) PIDFF_ENTRY(label, ptr, OTHER_PIDDF_MAX) diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 15916826fa..1180932505 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -47,6 +47,7 @@ static const OSD_Entry menuMiscEntries[]= OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS), OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT), #endif + OSD_SETTING_ENTRY("FS PROCEDURE", SETTING_FAILSAFE_PROCEDURE), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 874c5fde29..5b36b61300 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -217,8 +217,8 @@ static const OSD_Entry menuOsdElemsEntries[] = #endif // GPS OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING), OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH), - OSD_ELEMENT_ENTRY("CRS HEAD. ERR", OSD_CRUISE_HEADING_ERROR), - OSD_ELEMENT_ENTRY("CRS HEAD. ADJ", OSD_CRUISE_HEADING_ADJUSTMENT), + OSD_ELEMENT_ENTRY("CRS HLD ERR", OSD_COURSE_HOLD_ERROR), + OSD_ELEMENT_ENTRY("CRS HLD ADJ", OSD_COURSE_HOLD_ADJUSTMENT), #if defined(USE_BARO) || defined(USE_GPS) OSD_ELEMENT_ENTRY("VARIO", OSD_VARIO), OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM), diff --git a/src/main/common/circular_queue.c b/src/main/common/circular_queue.c new file mode 100644 index 0000000000..2a3c8a8bc2 --- /dev/null +++ b/src/main/common/circular_queue.c @@ -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; +} \ No newline at end of file diff --git a/src/main/common/circular_queue.h b/src/main/common/circular_queue.h new file mode 100644 index 0000000000..94bfaffa0d --- /dev/null +++ b/src/main/common/circular_queue.h @@ -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 diff --git a/src/main/common/log.c b/src/main/common/log.c index 284589ca6d..24093c6595 100644 --- a/src/main/common/log.c +++ b/src/main/common/log.c @@ -39,6 +39,7 @@ #include "io/serial.h" #include "fc/config.h" +#include "fc/settings.h" #include "msp/msp.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_RESET_TEMPLATE(logConfig_t, logConfig, + .level = SETTING_LOG_LEVEL_DEFAULT, + .topics = SETTING_LOG_TOPICS_DEFAULT +); + void logInit(void) { const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LOG); diff --git a/src/main/common/maths.c b/src/main/common/maths.c index d3d5a2410d..6d973980cf 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -142,7 +142,7 @@ int32_t applyDeadband(int32_t value, int32_t deadband) 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) return low; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index a5a375e9f8..19c906b8c1 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -88,6 +88,8 @@ #define _ABS_I(x, var) _ABS_II(x, var) #define ABS(x) _ABS_I(x, _CHOOSE_VAR(_abs, __COUNTER__)) +#define power3(x) ((x)*(x)*(x)) + // Floating point Euler angles. typedef struct fp_angles { float roll; @@ -131,7 +133,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu int gcd(int num, int denom); 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); void devClear(stdev_t *dev); diff --git a/src/main/common/time.c b/src/main/common/time.c index 27faeeadfb..f1204c57f4 100644 --- a/src/main/common/time.c +++ b/src/main/common/time.c @@ -33,6 +33,8 @@ #include "drivers/time.h" +#include "fc/settings.h" + // For the "modulo 4" arithmetic to work, we need a leap base year #define REFERENCE_YEAR 2000 // 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_RESET_TEMPLATE(timeConfig_t, timeConfig, - .tz_offset = 0, - .tz_automatic_dst = TZ_AUTO_DST_OFF, + .tz_offset = SETTING_TZ_OFFSET_DEFAULT, + .tz_automatic_dst = SETTING_TZ_AUTOMATIC_DST_DEFAULT, ); static rtcTime_t dateTimeToRtcTime(const dateTime_t *dt) diff --git a/src/main/config/general_settings.c b/src/main/config/general_settings.c index 38b8ecf3e3..47015f8ee6 100644 --- a/src/main/config/general_settings.c +++ b/src/main/config/general_settings.c @@ -28,8 +28,10 @@ #include "config/general_settings.h" +#include "fc/settings.h" + PG_REGISTER_WITH_RESET_TEMPLATE(generalSettings_t, generalSettings, PG_GENERAL_SETTINGS, 0); PG_RESET_TEMPLATE(generalSettings_t, generalSettings, - .appliedDefaults = APPLIED_DEFAULTS_NONE, -); \ No newline at end of file + .appliedDefaults = SETTING_APPLIED_DEFAULTS_DEFAULT, +); diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 6e526ea610..bb7dfa1bcc 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -116,7 +116,8 @@ #define PG_SAFE_HOME_CONFIG 1026 #define PG_DJI_OSD_CONFIG 1027 #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) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/drivers/accgyro/accgyro_bmi088.c b/src/main/drivers/accgyro/accgyro_bmi088.c new file mode 100644 index 0000000000..abe89af1df --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bmi088.c @@ -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 +#include + +#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 */ diff --git a/src/main/drivers/accgyro/accgyro_bmi088.h b/src/main/drivers/accgyro/accgyro_bmi088.h new file mode 100644 index 0000000000..4ffa4acc3d --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bmi088.h @@ -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); diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c new file mode 100644 index 0000000000..fdb73b7250 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -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 +#include +#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 \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h new file mode 100644 index 0000000000..d0c24f6dc4 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -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); \ No newline at end of file diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 2d7a51fc97..eefa81eb10 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -91,6 +91,8 @@ typedef enum { DEVHW_MPU6050, DEVHW_MPU6500, DEVHW_BMI160, + DEVHW_BMI088_GYRO, + DEVHW_BMI088_ACC, DEVHW_ICM20689, /* Combined ACC/GYRO/MAG chips */ @@ -116,6 +118,7 @@ typedef enum { DEVHW_QMC5883, DEVHW_MAG3110, DEVHW_LIS3MDL, + DEVHW_RM3100, /* Temp sensor chips */ DEVHW_LM75_0, @@ -137,6 +140,8 @@ typedef enum { DEVHW_SRF10, DEVHW_HCSR04_I2C, // DIY-style adapter DEVHW_VL53L0X, + DEVHW_VL53L1X, + DEVHW_US42, /* Other hardware */ DEVHW_MS4525, // Pitot meter @@ -146,6 +151,7 @@ typedef enum { DEVHW_SDCARD, // Generic SD-Card DEVHW_IRLOCK, // IR-Lock visual positioning hardware DEVHW_PCF8574, // 8-bit I/O expander + DEVHW_BNO055, // BNO055 IMU } devHardwareType_e; typedef enum { diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index 0531f2af5b..91644b895a 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -174,7 +174,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, 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); } else { diff --git a/src/main/drivers/compass/compass_rm3100.c b/src/main/drivers/compass/compass_rm3100.c new file mode 100644 index 0000000000..e4d7ba3afa --- /dev/null +++ b/src/main/drivers/compass/compass_rm3100.c @@ -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 +#include + +#include + +#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 diff --git a/src/main/drivers/compass/compass_rm3100.h b/src/main/drivers/compass/compass_rm3100.h new file mode 100644 index 0000000000..32f265c630 --- /dev/null +++ b/src/main/drivers/compass/compass_rm3100.h @@ -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); diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 5afe475184..2bb96175a1 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -30,6 +30,7 @@ #include "drivers/display_font_metadata.h" #include "drivers/time.h" +#include "fc/settings.h" #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_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) diff --git a/src/main/drivers/lights_io.c b/src/main/drivers/lights_io.c index 1da350d000..b6f453686e 100644 --- a/src/main/drivers/lights_io.c +++ b/src/main/drivers/lights_io.c @@ -10,7 +10,7 @@ static IO_t lightsIO = DEFIO_IO(NONE); -bool lightsHardwareInit() +bool lightsHardwareInit(void) { lightsIO = IOGetByTag(IO_TAG(LIGHTS_PIN)); diff --git a/src/main/drivers/lights_io.h b/src/main/drivers/lights_io.h index 521210869c..9560320dd3 100644 --- a/src/main/drivers/lights_io.h +++ b/src/main/drivers/lights_io.h @@ -4,7 +4,7 @@ #ifdef USE_LIGHTS -bool lightsHardwareInit(); +bool lightsHardwareInit(void); void lightsHardwareSetStatus(bool status); #endif /* USE_LIGHTS */ diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 244decd89f..d8accf4c31 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -243,7 +243,7 @@ #define SYM_2RSS 0xEA // RSSI 2 #define SYM_DB 0xEB // dB #define SYM_DBM 0xEC // dBm -#define SYM_SRN 0xEE // SNR +#define SYM_SNR 0xEE // SNR #define SYM_MW 0xED // mW #else diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index acfc89df46..48f7a10d5c 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -28,6 +28,7 @@ FILE_COMPILE_FOR_SPEED #include "common/log.h" #include "common/maths.h" +#include "common/circular_queue.h" #include "drivers/io.h" #include "drivers/timer.h" @@ -60,6 +61,10 @@ FILE_COMPILE_FOR_SPEED #define DSHOT_MOTOR_BITLENGTH 20 #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 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; +#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) { 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 if (!isMotorProtocolDigital()) { return; @@ -359,6 +422,9 @@ void pwmCompleteMotorUpdate(void) #ifdef USE_DSHOT if (isMotorProtocolDshot()) { + + executeDShotCommands(); + // Generate DMA buffers for (int index = 0; index < motorCount; index++) { if (motors[index].pwmPort && motors[index].pwmPort->configured) { diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index 2192d6d298..f1f2c1f646 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -20,6 +20,16 @@ #include "drivers/io_types.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); ioTag_t pwmGetMotorPinTag(int motorIndex); @@ -28,6 +38,7 @@ void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteMotorUpdate(void); bool isMotorProtocolDigital(void); +bool isMotorProtocolDshot(void); 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); bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIndex, uint16_t servoPwmRate, uint16_t servoCenterPulse, bool enableOutput); void pwmWriteBeeper(bool onoffBeep); -void beeperPwmInit(ioTag_t tag, uint16_t frequency); \ No newline at end of file +void beeperPwmInit(ioTag_t tag, uint16_t frequency); + +void sendDShotCommand(dshotCommands_e directionSpin); +void initDShotCommands(void); \ No newline at end of file diff --git a/src/main/drivers/rangefinder/rangefinder_us42.c b/src/main/drivers/rangefinder/rangefinder_us42.c new file mode 100644 index 0000000000..eabb571d92 --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_us42.c @@ -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 . + */ + +#include +#include + +#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 \ No newline at end of file diff --git a/src/main/drivers/rangefinder/rangefinder_us42.h b/src/main/drivers/rangefinder/rangefinder_us42.h new file mode 100644 index 0000000000..e6424c6c85 --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_us42.h @@ -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 . + */ + +#pragma once + +#define RANGEFINDER_US42_TASK_PERIOD_MS 100 + +bool us42Detect(rangefinderDev_t *dev); \ No newline at end of file diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c new file mode 100644 index 0000000000..839d06e9dc --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c @@ -0,0 +1,1696 @@ +/* + * 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/. + * + * ================================================================= + * + * Most of the functionality of this library is based on the VL53L1X + * API provided by ST (STSW-IMG009). + * + * The following applies to source code reproduced or derived from + * the API: + * + * ----------------------------------------------------------------- + * + * Copyright (c) 2017, STMicroelectronics - All Rights Reserved + * + * This file : part of VL53L1 Core and : dual licensed, + * either 'STMicroelectronics + * Proprietary license' + * or 'BSD 3-clause "New" or "Revised" License' , at your option. + * + * ******************************************************************************* + * + * 'STMicroelectronics Proprietary license' + * + * ******************************************************************************* + * + * License terms: STMicroelectronics Proprietary in accordance with licensing + * terms at www.st.com/sla0081 + * + * STMicroelectronics confidential + * Reproduction and Communication of this document : strictly prohibited unless + * specifically authorized in writing by STMicroelectronics. + * + * + * ******************************************************************************* + * + * Alternatively, VL53L1 Core may be distributed under the terms of + * 'BSD 3-clause "New" or "Revised" License', in which case the following + * provisions apply instead of the ones mentioned above : + * + * ******************************************************************************* + * + * License terms: BSD 3-clause "New" or "Revised" License. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_VL53L1X) + +#include "build/build_config.h" + +#include "drivers/time.h" +#include "drivers/bus_i2c.h" + +#include "drivers/rangefinder/rangefinder.h" +#include "drivers/rangefinder/rangefinder_vl53l1x.h" + +#include "build/debug.h" + +#define VL53L1X_MAX_RANGE_CM (300) +#define VL53L1X_DETECTION_CONE_DECIDEGREES (270) +#define VL53L1X_TIMING_BUDGET (33) + +#define VL53L1X_IMPLEMENTATION_VER_MAJOR 3 +#define VL53L1X_IMPLEMENTATION_VER_MINOR 4 +#define VL53L1X_IMPLEMENTATION_VER_SUB 0 +#define VL53L1X_IMPLEMENTATION_VER_REVISION 0000 + +typedef int8_t VL53L1X_ERROR; + +#define SOFT_RESET 0x0000 +#define VL53L1_I2C_SLAVE__DEVICE_ADDRESS 0x0001 +#define VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND 0x0008 +#define ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS 0x0016 +#define ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS 0x0018 +#define ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS 0x001A +#define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E +#define MM_CONFIG__INNER_OFFSET_MM 0x0020 +#define MM_CONFIG__OUTER_OFFSET_MM 0x0022 +#define GPIO_HV_MUX__CTRL 0x0030 +#define GPIO__TIO_HV_STATUS 0x0031 +#define SYSTEM__INTERRUPT_CONFIG_GPIO 0x0046 +#define PHASECAL_CONFIG__TIMEOUT_MACROP 0x004B +#define RANGE_CONFIG__TIMEOUT_MACROP_A_HI 0x005E +#define RANGE_CONFIG__VCSEL_PERIOD_A 0x0060 +#define RANGE_CONFIG__VCSEL_PERIOD_B 0x0063 +#define RANGE_CONFIG__TIMEOUT_MACROP_B_HI 0x0061 +#define RANGE_CONFIG__TIMEOUT_MACROP_B_LO 0x0062 +#define RANGE_CONFIG__SIGMA_THRESH 0x0064 +#define RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS 0x0066 +#define RANGE_CONFIG__VALID_PHASE_HIGH 0x0069 +#define VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD 0x006C +#define SYSTEM__THRESH_HIGH 0x0072 +#define SYSTEM__THRESH_LOW 0x0074 +#define SD_CONFIG__WOI_SD0 0x0078 +#define SD_CONFIG__INITIAL_PHASE_SD0 0x007A +#define ROI_CONFIG__USER_ROI_CENTRE_SPAD 0x007F +#define ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE 0x0080 +#define SYSTEM__SEQUENCE_CONFIG 0x0081 +#define VL53L1_SYSTEM__GROUPED_PARAMETER_HOLD 0x0082 +#define SYSTEM__INTERRUPT_CLEAR 0x0086 +#define SYSTEM__MODE_START 0x0087 +#define VL53L1_RESULT__RANGE_STATUS 0x0089 +#define VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0 0x008C +#define RESULT__AMBIENT_COUNT_RATE_MCPS_SD 0x0090 +#define VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0 0x0096 +#define VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0 0x0098 +#define VL53L1_RESULT__OSC_CALIBRATE_VAL 0x00DE +#define VL53L1_FIRMWARE__SYSTEM_STATUS 0x00E5 +#define VL53L1_IDENTIFICATION__MODEL_ID 0x010F +#define VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD 0x013E + +#define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E +#define MM_CONFIG__INNER_OFFSET_MM 0x0020 +#define MM_CONFIG__OUTER_OFFSET_MM 0x0022 + + + +#define VL53L1_ERROR_NONE ((VL53L1X_ERROR) 0) +#define VL53L1_ERROR_CALIBRATION_WARNING ((VL53L1X_ERROR) - 1) + /*!< Warning invalid calibration data may be in used + \a VL53L1_InitData() + \a VL53L1_GetOffsetCalibrationData + \a VL53L1_SetOffsetCalibrationData */ +#define VL53L1_ERROR_MIN_CLIPPED ((VL53L1X_ERROR) - 2) + /*!< Warning parameter passed was clipped to min before to be applied */ + +#define VL53L1_ERROR_UNDEFINED ((VL53L1X_ERROR) - 3) + /*!< Unqualified error */ +#define VL53L1_ERROR_INVALID_PARAMS ((VL53L1X_ERROR) - 4) + /*!< Parameter passed is invalid or out of range */ +#define VL53L1_ERROR_NOT_SUPPORTED ((VL53L1X_ERROR) - 5) + /*!< Function is not supported in current mode or configuration */ +#define VL53L1_ERROR_RANGE_ERROR ((VL53L1X_ERROR) - 6) + /*!< Device report a ranging error interrupt status */ +#define VL53L1_ERROR_TIME_OUT ((VL53L1X_ERROR) - 7) + /*!< Aborted due to time out */ +#define VL53L1_ERROR_MODE_NOT_SUPPORTED ((VL53L1X_ERROR) - 8) + /*!< Asked mode is not supported by the device */ +#define VL53L1_ERROR_BUFFER_TOO_SMALL ((VL53L1X_ERROR) - 9) + /*!< ... */ +#define VL53L1_ERROR_COMMS_BUFFER_TOO_SMALL ((VL53L1X_ERROR) - 10) + /*!< Supplied buffer is larger than I2C supports */ +#define VL53L1_ERROR_GPIO_NOT_EXISTING ((VL53L1X_ERROR) - 11) + /*!< User tried to setup a non-existing GPIO pin */ +#define VL53L1_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED ((VL53L1X_ERROR) - 12) + /*!< unsupported GPIO functionality */ +#define VL53L1_ERROR_CONTROL_INTERFACE ((VL53L1X_ERROR) - 13) + /*!< error reported from IO functions */ +#define VL53L1_ERROR_INVALID_COMMAND ((VL53L1X_ERROR) - 14) + /*!< The command is not allowed in the current device state + * (power down) */ +#define VL53L1_ERROR_DIVISION_BY_ZERO ((VL53L1X_ERROR) - 15) + /*!< In the function a division by zero occurs */ +#define VL53L1_ERROR_REF_SPAD_INIT ((VL53L1X_ERROR) - 16) + /*!< Error during reference SPAD initialization */ +#define VL53L1_ERROR_GPH_SYNC_CHECK_FAIL ((VL53L1X_ERROR) - 17) + /*!< GPH sync interrupt check fail - API out of sync with device*/ +#define VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL ((VL53L1X_ERROR) - 18) + /*!< Stream count check fail - API out of sync with device */ +#define VL53L1_ERROR_GPH_ID_CHECK_FAIL ((VL53L1X_ERROR) - 19) + /*!< GPH ID check fail - API out of sync with device */ +#define VL53L1_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL ((VL53L1X_ERROR) - 20) + /*!< Zone dynamic config stream count check failed - API out of sync */ +#define VL53L1_ERROR_ZONE_GPH_ID_CHECK_FAIL ((VL53L1X_ERROR) - 21) + /*!< Zone dynamic config GPH ID check failed - API out of sync */ + +#define VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL ((VL53L1X_ERROR) - 22) + /*!< Thrown when run_xtalk_extraction fn has 0 succesful samples + * when using the full array to sample the xtalk. In this case there is + * not enough information to generate new Xtalk parm info. The function + * will exit and leave the current xtalk parameters unaltered */ +#define VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL ((VL53L1X_ERROR) - 23) + /*!< Thrown when run_xtalk_extraction fn has found that the + * avg sigma estimate of the full array xtalk sample is > than the + * maximal limit allowed. In this case the xtalk sample is too noisy for + * measurement. The function will exit and leave the current xtalk parameters + * unaltered. */ + + +#define VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL ((VL53L1X_ERROR) - 24) + /*!< Thrown if there one of stages has no valid offset calibration + * samples. A fatal error calibration not valid */ +#define VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL ((VL53L1X_ERROR) - 25) + /*!< Thrown if there one of stages has zero effective SPADS + * Traps the case when MM1 SPADs is zero. + * A fatal error calibration not valid */ +#define VL53L1_ERROR_ZONE_CAL_NO_SAMPLE_FAIL ((VL53L1X_ERROR) - 26) + /*!< Thrown if then some of the zones have no valid samples + * A fatal error calibration not valid */ + +#define VL53L1_ERROR_TUNING_PARM_KEY_MISMATCH ((VL53L1X_ERROR) - 27) + /*!< Thrown if the tuning file key table version does not match with + * expected value. The driver expects the key table version to match + * the compiled default version number in the define + * #VL53L1_TUNINGPARM_KEY_TABLE_VERSION_DEFAULT + * */ + +#define VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS ((VL53L1X_ERROR) - 28) + /*!< Thrown if there are less than 5 good SPADs are available. */ +#define VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH ((VL53L1X_ERROR) - 29) + /*!< Thrown if the final reference rate is greater than + the upper reference rate limit - default is 40 Mcps. + Implies a minimum Q3 (x10) SPAD (5) selected */ +#define VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW ((VL53L1X_ERROR) - 30) + /*!< Thrown if the final reference rate is less than + the lower reference rate limit - default is 10 Mcps. + Implies maximum Q1 (x1) SPADs selected */ + + +#define VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES ((VL53L1X_ERROR) - 31) + /*!< Thrown if there is less than the requested number of + * valid samples. */ +#define VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH ((VL53L1X_ERROR) - 32) + /*!< Thrown if the offset calibration range sigma estimate is greater + * than 8.0 mm. This is the recommended min value to yield a stable + * offset measurement */ +#define VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH ((VL53L1X_ERROR) - 33) + /*!< Thrown when VL53L1_run_offset_calibration() peak rate is greater + than that 50.0Mcps. This is the recommended max rate to avoid + pile-up influencing the offset measurement */ +#define VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW ((VL53L1X_ERROR) - 34) + /*!< Thrown when VL53L1_run_offset_calibration() when one of stages + range has less that 5.0 effective SPADS. This is the recommended + min value to yield a stable offset */ + + +#define VL53L1_WARNING_ZONE_CAL_MISSING_SAMPLES ((VL53L1X_ERROR) - 35) + /*!< Thrown if one of more of the zones have less than + the requested number of valid samples */ +#define VL53L1_WARNING_ZONE_CAL_SIGMA_TOO_HIGH ((VL53L1X_ERROR) - 36) + /*!< Thrown if one or more zones have sigma estimate value greater + * than 8.0 mm. This is the recommended min value to yield a stable + * offset measurement */ +#define VL53L1_WARNING_ZONE_CAL_RATE_TOO_HIGH ((VL53L1X_ERROR) - 37) + /*!< Thrown if one of more zones have peak rate higher than + that 50.0Mcps. This is the recommended max rate to avoid + pile-up influencing the offset measurement */ + + +#define VL53L1_WARNING_XTALK_MISSING_SAMPLES ((VL53L1X_ERROR) - 38) + /*!< Thrown to notify that some of the xtalk samples did not yield + * valid ranging pulse data while attempting to measure + * the xtalk signal in vl53l1_run_xtalk_extract(). This can signify any of + * the zones are missing samples, for further debug information the + * xtalk_results struct should be referred to. This warning is for + * notification only, the xtalk pulse and shape have still been generated + */ +#define VL53L1_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT ((VL53L1X_ERROR) - 39) + /*!< Thrown to notify that some of teh xtalk samples used for gradient + * generation did not yield valid ranging pulse data while attempting to + * measure the xtalk signal in vl53l1_run_xtalk_extract(). This can signify + * that any one of the zones 0-3 yielded no successful samples. The + * xtalk_results struct should be referred to for further debug info. + * This warning is for notification only, the xtalk pulse and shape + * have still been generated. + */ +#define VL53L1_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT ((VL53L1X_ERROR) - 40) +/*!< Thrown to notify that some of the xtalk samples used for gradient + * generation did not pass the sigma limit check while attempting to + * measure the xtalk signal in vl53l1_run_xtalk_extract(). This can signify + * that any one of the zones 0-3 yielded an avg sigma_mm value > the limit. + * The xtalk_results struct should be referred to for further debug info. + * This warning is for notification only, the xtalk pulse and shape + * have still been generated. + */ + +#define VL53L1_ERROR_NOT_IMPLEMENTED ((VL53L1X_ERROR) - 41) + /*!< Tells requested functionality has not been implemented yet or + * not compatible with the device */ +#define VL53L1_ERROR_PLATFORM_SPECIFIC_START ((VL53L1X_ERROR) - 60) + /*!< Tells the starting code for platform */ +/** @} VL53L1_define_Error_group */ + +/**************************************** + * PRIVATE define do not edit + ****************************************/ + +/** + * @brief defines SW Version + */ +typedef struct { + uint8_t major; /*!< major number */ + uint8_t minor; /*!< minor number */ + uint8_t build; /*!< build number */ + uint32_t revision; /*!< revision number */ +} VL53L1X_Version_t; + +/** + * @brief defines packed reading results type + */ +typedef struct { + uint8_t Status; /*!< ResultStatus */ + uint16_t Distance; /*!< ResultDistance */ + uint16_t Ambient; /*!< ResultAmbient */ + uint16_t SigPerSPAD;/*!< ResultSignalPerSPAD */ + uint16_t NumSPADs; /*!< ResultNumSPADs */ +} VL53L1X_Result_t; + +/** + * @brief This function returns the SW driver version + */ +// VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion); + +/** + * @brief This function sets the sensor I2C address used in case multiple devices application, default address 0x52 + */ +// // VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address); + +// /** +// * @brief This function loads the 135 bytes default values to initialize the sensor. +// * @param dev Device address +// * @return 0:success, != 0:failed +// */ +VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev); + +/** + * @brief This function clears the interrupt, to be called after a ranging data reading + * to arm the interrupt for the next data ready event. + */ +static VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev); + +/** + * @brief This function programs the interrupt polarity\n + * 1=active high (default), 0=active low + */ +// VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t IntPol); + +/** + * @brief This function returns the current interrupt polarity\n + * 1=active high (default), 0=active low + */ +static VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pIntPol); + +/** + * @brief This function starts the ranging distance operation\n + * The ranging operation is continuous. The clear interrupt has to be done after each get data to allow the interrupt to raise when the next data is ready\n + * 1=active high (default), 0=active low, use SetInterruptPolarity() to change the interrupt polarity if required. + */ +static VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev); + +/** + * @brief This function stops the ranging. + */ +static VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev); + +/** + * @brief This function checks if the new ranging data is available by polling the dedicated register. + * @param : isDataReady==0 -> not ready; isDataReady==1 -> ready + */ +static VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady); + +/** + * @brief This function programs the timing budget in ms. + * Predefined values = 15, 20, 33, 50, 100(default), 200, 500. + */ +static VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs); + +/** + * @brief This function returns the current timing budget in ms. + */ +static VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudgetInMs); + +/** + * @brief This function programs the distance mode (1=short, 2=long(default)). + * Short mode max distance is limited to 1.3 m but better ambient immunity.\n + * Long mode can range up to 4 m in the dark with 200 ms timing budget. + */ +static VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DistanceMode); + +/** + * @brief This function returns the current distance mode (1=short, 2=long). + */ +static VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *pDistanceMode); + +/** + * @brief This function programs the Intermeasurement period in ms\n + * Intermeasurement period must be >/= timing budget. This condition is not checked by the API, + * the customer has the duty to check the condition. Default = 100 ms + */ +static VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, + uint32_t InterMeasurementInMs); + +/** + * @brief This function returns the Intermeasurement period in ms. + */ +// VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t * pIM); + +/** + * @brief This function returns the boot state of the device (1:booted, 0:not booted) + */ +// VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state); + +/** + * @brief This function returns the sensor id, sensor Id must be 0xEEAC + */ +// VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *id); + +/** + * @brief This function returns the distance measured by the sensor in mm + */ +static VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance); + +/** + * @brief This function returns the returned signal per SPAD in kcps/SPAD. + * With kcps stands for Kilo Count Per Second + */ +// VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalPerSp); + +/** + * @brief This function returns the ambient per SPAD in kcps/SPAD + */ +// VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *amb); + +/** + * @brief This function returns the returned signal in kcps. + */ +// static VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signalRate); + +/** + * @brief This function returns the current number of enabled SPADs + */ +// static VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb); + +/** + * @brief This function returns the ambient rate in kcps + */ +// VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate); + +/** + * @brief This function returns the ranging status error \n + * (0:no error, 1:sigma failed, 2:signal failed, ..., 7:wrap-around) + */ +// VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus); + +/** + * @brief This function returns measurements and the range status in a single read access + */ +// VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult); + +/** + * @brief This function programs the offset correction in mm + * @param OffsetValue:the offset correction value to program in mm + */ +// VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue); + +/** + * @brief This function returns the programmed offset correction value in mm + */ +// VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *Offset); + +/** + * @brief This function programs the xtalk correction value in cps (Count Per Second).\n + * This is the number of photons reflected back from the cover glass in cps. + */ +// VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue); + +/** + * @brief This function returns the current programmed xtalk correction value in cps + */ +// VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *Xtalk); + +/** + * @brief This function programs the threshold detection mode\n + * Example:\n + * VL53L1X_SetDistanceThreshold(dev,100,300,0,1): Below 100 \n + * VL53L1X_SetDistanceThreshold(dev,100,300,1,1): Above 300 \n + * VL53L1X_SetDistanceThreshold(dev,100,300,2,1): Out of window \n + * VL53L1X_SetDistanceThreshold(dev,100,300,3,1): In window \n + * @param dev : device address + * @param ThreshLow(in mm) : the threshold under which one the device raises an interrupt if Window = 0 + * @param ThreshHigh(in mm) : the threshold above which one the device raises an interrupt if Window = 1 + * @param Window detection mode : 0=below, 1=above, 2=out, 3=in + * @param IntOnNoTarget = 0 (No longer used - just use 0) + */ +// VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, +// uint16_t ThreshHigh, uint8_t Window, +// uint8_t IntOnNoTarget); + +/** + * @brief This function returns the window detection mode (0=below; 1=above; 2=out; 3=in) + */ +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window); + +/** + * @brief This function returns the low threshold in mm + */ +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low); + +/** + * @brief This function returns the high threshold in mm + */ +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high); + +/** + * @brief This function programs the ROI (Region of Interest)\n + * The ROI position is centered, only the ROI size can be reprogrammed.\n + * The smallest acceptable ROI size = 4\n + * @param X:ROI Width; Y=ROI Height + */ +// VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y); + +/** + *@brief This function returns width X and height Y + */ +// VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y); + +/** + *@brief This function programs the new user ROI center, please to be aware that there is no check in this function. + *if the ROI center vs ROI size is out of border the ranging function return error #13 + */ +// VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter); + +/** + *@brief This function returns the current user ROI center + */ +// VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter); + +/** + * @brief This function programs a new signal threshold in kcps (default=1024 kcps\n + */ +// VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t signal); + +/** + * @brief This function returns the current signal threshold in kcps + */ +// VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal); + +/** + * @brief This function programs a new sigma threshold in mm (default=15 mm) + */ +// VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t sigma); + +/** + * @brief This function returns the current sigma threshold in mm + */ +// VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *signal); + +/** + * @brief This function performs the temperature calibration. + * It is recommended to call this function any time the temperature might have changed by more than 8 deg C + * without sensor ranging activity for an extended period. + */ +// VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev); + +/** + * @brief This function performs the offset calibration.\n + * The function returns the offset value found and programs the offset compensation into the device. + * @param TargetDistInMm target distance in mm, ST recommended 100 mm + * Target reflectance = grey17% + * @return 0:success, !=0: failed + * @return offset pointer contains the offset found in mm + */ +// int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset); + +/** + * @brief This function performs the xtalk calibration.\n + * The function returns the xtalk value found and programs the xtalk compensation to the device + * @param TargetDistInMm target distance in mm\n + * The target distance : the distance where the sensor start to "under range"\n + * due to the influence of the photons reflected back from the cover glass becoming strong\n + * It's also called inflection point\n + * Target reflectance = grey 17% + * @return 0: success, !=0: failed + * @return xtalk pointer contains the xtalk value found in cps (number of photons in count per second) + */ +// int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk); + + + +const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = { +0x00, /* 0x2d : set bit 2 and 5 to 1 for fast plus mode (1MHz I2C), else don't touch */ +0x00, /* 0x2e : bit 0 if I2C pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */ +0x00, /* 0x2f : bit 0 if GPIO pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */ +0x01, /* 0x30 : set bit 4 to 0 for active high interrupt and 1 for active low (bits 3:0 must be 0x1), use SetInterruptPolarity() */ +0x02, /* 0x31 : bit 1 = interrupt depending on the polarity, use CheckForDataReady() */ +0x00, /* 0x32 : not user-modifiable */ +0x02, /* 0x33 : not user-modifiable */ +0x08, /* 0x34 : not user-modifiable */ +0x00, /* 0x35 : not user-modifiable */ +0x08, /* 0x36 : not user-modifiable */ +0x10, /* 0x37 : not user-modifiable */ +0x01, /* 0x38 : not user-modifiable */ +0x01, /* 0x39 : not user-modifiable */ +0x00, /* 0x3a : not user-modifiable */ +0x00, /* 0x3b : not user-modifiable */ +0x00, /* 0x3c : not user-modifiable */ +0x00, /* 0x3d : not user-modifiable */ +0xff, /* 0x3e : not user-modifiable */ +0x00, /* 0x3f : not user-modifiable */ +0x0F, /* 0x40 : not user-modifiable */ +0x00, /* 0x41 : not user-modifiable */ +0x00, /* 0x42 : not user-modifiable */ +0x00, /* 0x43 : not user-modifiable */ +0x00, /* 0x44 : not user-modifiable */ +0x00, /* 0x45 : not user-modifiable */ +0x20, /* 0x46 : interrupt configuration 0->level low detection, 1-> level high, 2-> Out of window, 3->In window, 0x20-> New sample ready , TBC */ +0x0b, /* 0x47 : not user-modifiable */ +0x00, /* 0x48 : not user-modifiable */ +0x00, /* 0x49 : not user-modifiable */ +0x02, /* 0x4a : not user-modifiable */ +0x0a, /* 0x4b : not user-modifiable */ +0x21, /* 0x4c : not user-modifiable */ +0x00, /* 0x4d : not user-modifiable */ +0x00, /* 0x4e : not user-modifiable */ +0x05, /* 0x4f : not user-modifiable */ +0x00, /* 0x50 : not user-modifiable */ +0x00, /* 0x51 : not user-modifiable */ +0x00, /* 0x52 : not user-modifiable */ +0x00, /* 0x53 : not user-modifiable */ +0xc8, /* 0x54 : not user-modifiable */ +0x00, /* 0x55 : not user-modifiable */ +0x00, /* 0x56 : not user-modifiable */ +0x38, /* 0x57 : not user-modifiable */ +0xff, /* 0x58 : not user-modifiable */ +0x01, /* 0x59 : not user-modifiable */ +0x00, /* 0x5a : not user-modifiable */ +0x08, /* 0x5b : not user-modifiable */ +0x00, /* 0x5c : not user-modifiable */ +0x00, /* 0x5d : not user-modifiable */ +0x01, /* 0x5e : not user-modifiable */ +0xcc, /* 0x5f : not user-modifiable */ +0x0f, /* 0x60 : not user-modifiable */ +0x01, /* 0x61 : not user-modifiable */ +0xf1, /* 0x62 : not user-modifiable */ +0x0d, /* 0x63 : not user-modifiable */ +0x01, /* 0x64 : Sigma threshold MSB (mm in 14.2 format for MSB+LSB), use SetSigmaThreshold(), default value 90 mm */ +0x68, /* 0x65 : Sigma threshold LSB */ +0x00, /* 0x66 : Min count Rate MSB (MCPS in 9.7 format for MSB+LSB), use SetSignalThreshold() */ +0x80, /* 0x67 : Min count Rate LSB */ +0x08, /* 0x68 : not user-modifiable */ +0xb8, /* 0x69 : not user-modifiable */ +0x00, /* 0x6a : not user-modifiable */ +0x00, /* 0x6b : not user-modifiable */ +0x00, /* 0x6c : Intermeasurement period MSB, 32 bits register, use SetIntermeasurementInMs() */ +0x00, /* 0x6d : Intermeasurement period */ +0x0f, /* 0x6e : Intermeasurement period */ +0x89, /* 0x6f : Intermeasurement period LSB */ +0x00, /* 0x70 : not user-modifiable */ +0x00, /* 0x71 : not user-modifiable */ +0x00, /* 0x72 : distance threshold high MSB (in mm, MSB+LSB), use SetD:tanceThreshold() */ +0x00, /* 0x73 : distance threshold high LSB */ +0x00, /* 0x74 : distance threshold low MSB ( in mm, MSB+LSB), use SetD:tanceThreshold() */ +0x00, /* 0x75 : distance threshold low LSB */ +0x00, /* 0x76 : not user-modifiable */ +0x01, /* 0x77 : not user-modifiable */ +0x0f, /* 0x78 : not user-modifiable */ +0x0d, /* 0x79 : not user-modifiable */ +0x0e, /* 0x7a : not user-modifiable */ +0x0e, /* 0x7b : not user-modifiable */ +0x00, /* 0x7c : not user-modifiable */ +0x00, /* 0x7d : not user-modifiable */ +0x02, /* 0x7e : not user-modifiable */ +0xc7, /* 0x7f : ROI center, use SetROI() */ +0xff, /* 0x80 : XY ROI (X=Width, Y=Height), use SetROI() */ +0x9B, /* 0x81 : not user-modifiable */ +0x00, /* 0x82 : not user-modifiable */ +0x00, /* 0x83 : not user-modifiable */ +0x00, /* 0x84 : not user-modifiable */ +0x01, /* 0x85 : not user-modifiable */ +0x00, /* 0x86 : clear interrupt, use ClearInterrupt() */ +0x00 /* 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */ +}; + +// static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0, +// 255, 255, 9, 13, 255, 255, 255, 255, 10, 6, +// 255, 255, 11, 12 +// }; + +static uint8_t _I2CBuffer[256]; + +static int32_t lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE; +static bool lastMeasurementIsNew = false; +static bool isInitialized = false; +static bool isResponding = true; + +#define _I2CWrite(dev, data, size) \ + (busWriteBuf(dev, 0xFF, data, size) ? 0 : -1) + +#define _I2CRead(dev, data, size) \ + (busReadBuf(dev, 0xFF, data, size) ? 0 : -1) + +VL53L1X_ERROR VL53L1_WriteMulti(busDevice_t * Dev, uint16_t index, uint8_t *pdata, uint32_t count) { + int status_int; + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + if (count > sizeof(_I2CBuffer) - 1) { + return VL53L1_ERROR_INVALID_PARAMS; + } + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + memcpy(&_I2CBuffer[2], pdata, count); + status_int = _I2CWrite(Dev, _I2CBuffer, count + 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +// the ranging_sensor_comms.dll will take care of the page selection +VL53L1X_ERROR VL53L1_ReadMulti(busDevice_t * Dev, uint16_t index, uint8_t *pdata, uint32_t count) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, pdata, count); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } +done: + return Status; +} + +VL53L1X_ERROR VL53L1_WrByte(busDevice_t * Dev, uint16_t index, uint8_t data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + _I2CBuffer[2] = data; + + status_int = _I2CWrite(Dev, _I2CBuffer, 3); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +VL53L1X_ERROR VL53L1_WrWord(busDevice_t * Dev, uint16_t index, uint16_t data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + _I2CBuffer[2] = data >> 8; + _I2CBuffer[3] = data & 0x00FF; + + status_int = _I2CWrite(Dev, _I2CBuffer, 4); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +VL53L1X_ERROR VL53L1_WrDWord(busDevice_t * Dev, uint16_t index, uint32_t data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + _I2CBuffer[2] = (data >> 24) & 0xFF; + _I2CBuffer[3] = (data >> 16) & 0xFF; + _I2CBuffer[4] = (data >> 8) & 0xFF; + _I2CBuffer[5] = (data >> 0 ) & 0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 6); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +VL53L1X_ERROR VL53L1_RdByte(busDevice_t * Dev, uint16_t index, uint8_t *data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + if( status_int ){ + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, data, 1); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } +done: + return Status; +} + +VL53L1X_ERROR VL53L1_UpdateByte(busDevice_t * Dev, uint16_t index, uint8_t AndData, uint8_t OrData) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + uint8_t data; + + Status = VL53L1_RdByte(Dev, index, &data); + if (Status) { + goto done; + } + data = (data & AndData) | OrData; + Status = VL53L1_WrByte(Dev, index, data); +done: + return Status; +} + +VL53L1X_ERROR VL53L1_RdWord(busDevice_t * Dev, uint16_t index, uint16_t *data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + + if( status_int ){ + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, _I2CBuffer, 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + + *data = ((uint16_t)_I2CBuffer[0]<<8) + (uint16_t)_I2CBuffer[1]; +done: + return Status; +} + +VL53L1X_ERROR VL53L1_RdDWord(busDevice_t * Dev, uint16_t index, uint32_t *data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, _I2CBuffer, 4); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + + *data = ((uint32_t)_I2CBuffer[0]<<24) + ((uint32_t)_I2CBuffer[1]<<16) + ((uint32_t)_I2CBuffer[2]<<8) + (uint32_t)_I2CBuffer[3]; + +done: + return Status; +} + +// VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion) +// { +// VL53L1X_ERROR Status = 0; + +// pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR; +// pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR; +// pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB; +// pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION; +// return Status; +// } + +// VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address) +// { +// VL53L1X_ERROR status = 0; + +// status = VL53L1_WrByte(dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1); +// return status; +// } + +VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + uint8_t Addr = 0x00, tmp; + + for (Addr = 0x2D; Addr <= 0x87; Addr++){ + status = VL53L1_WrByte(dev, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]); + } + status = VL53L1X_StartRanging(dev); + tmp = 0; + while(tmp==0){ + status = VL53L1X_CheckForDataReady(dev, &tmp); + } + status = VL53L1X_ClearInterrupt(dev); + status = VL53L1X_StopRanging(dev); + status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ + status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ + return status; +} + +static VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CLEAR, 0x01); + return status; +} + +// VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t NewPolarity) +// { +// uint8_t Temp; +// VL53L1X_ERROR status = 0; + +// status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp); +// Temp = Temp & 0xEF; +// status = VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4); +// return status; +// } + +static VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pInterruptPolarity) +{ + uint8_t Temp; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp); + Temp = Temp & 0x10; + *pInterruptPolarity = !(Temp>>4); + return status; +} + +static VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x40); /* Enable VL53L1X */ + return status; +} + +static VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x00); /* Disable VL53L1X */ + return status; +} + +static VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady) +{ + uint8_t Temp; + uint8_t IntPol; + VL53L1X_ERROR status = 0; + + status = VL53L1X_GetInterruptPolarity(dev, &IntPol); + status = VL53L1_RdByte(dev, GPIO__TIO_HV_STATUS, &Temp); + /* Read in the register to check if a new value is available */ + if (status == 0){ + if ((Temp & 1) == IntPol) + *isDataReady = 1; + else + *isDataReady = 0; + } + return status; +} + +static VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs) +{ + uint16_t DM; + VL53L1X_ERROR status=0; + + status = VL53L1X_GetDistanceMode(dev, &DM); + if (DM == 0) + return 1; + else if (DM == 1) { /* Short DistanceMode */ + switch (TimingBudgetInMs) { + case 15: /* only available in short distance mode */ + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x01D); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0027); + break; + case 20: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x0051); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x006E); + break; + case 33: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x00D6); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x006E); + break; + case 50: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x1AE); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x01E8); + break; + case 100: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x02E1); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0388); + break; + case 200: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x03E1); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0496); + break; + case 500: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x0591); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x05C1); + break; + default: + status = 1; + break; + } + } else { + switch (TimingBudgetInMs) { + case 20: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x001E); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0022); + break; + case 33: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x0060); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x006E); + break; + case 50: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x00AD); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x00C6); + break; + case 100: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x01CC); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x01EA); + break; + case 200: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x02D9); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x02F8); + break; + case 500: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x048F); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x04A4); + break; + default: + status = 1; + break; + } + } + return status; +} + +static VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudget) +{ + uint16_t Temp; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &Temp); + switch (Temp) { + case 0x001D : + *pTimingBudget = 15; + break; + case 0x0051 : + case 0x001E : + *pTimingBudget = 20; + break; + case 0x00D6 : + case 0x0060 : + *pTimingBudget = 33; + break; + case 0x1AE : + case 0x00AD : + *pTimingBudget = 50; + break; + case 0x02E1 : + case 0x01CC : + *pTimingBudget = 100; + break; + case 0x03E1 : + case 0x02D9 : + *pTimingBudget = 200; + break; + case 0x0591 : + case 0x048F : + *pTimingBudget = 500; + break; + default: + status = 1; + *pTimingBudget = 0; + } + return status; +} + +static VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DM) +{ + uint16_t TB; + VL53L1X_ERROR status = 0; + + status = VL53L1X_GetTimingBudgetInMs(dev, &TB); + if (status != 0) + return 1; + switch (DM) { + case 1: + status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38); + status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0705); + status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606); + break; + case 2: + status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8); + status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0F0D); + status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E); + break; + default: + status = 1; + break; + } + + if (status == 0) + status = VL53L1X_SetTimingBudgetInMs(dev, TB); + return status; +} + +static VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *DM) +{ + uint8_t TempDM, status=0; + + status = VL53L1_RdByte(dev,PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM); + if (TempDM == 0x14) + *DM=1; + if(TempDM == 0x0A) + *DM=2; + return status; +} + +static VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterMeasMs) +{ + uint16_t ClockPLL; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); + ClockPLL = ClockPLL&0x3FF; + VL53L1_WrDWord(dev, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, + (uint32_t)(ClockPLL * InterMeasMs * 1.075)); + return status; + +} + +// VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t *pIM) +// { +// uint16_t ClockPLL; +// VL53L1X_ERROR status = 0; +// uint32_t tmp; + +// status = VL53L1_RdDWord(dev,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp); +// *pIM = (uint16_t)tmp; +// status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); +// ClockPLL = ClockPLL&0x3FF; +// *pIM= (uint16_t)(*pIM/(ClockPLL*1.065)); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp = 0; + +// status = VL53L1_RdByte(dev,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp); +// *state = tmp; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *sensorId) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp = 0; + +// status = VL53L1_RdWord(dev, VL53L1_IDENTIFICATION__MODEL_ID, &tmp); +// *sensorId = tmp; +// return status; +// } + +static VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = (VL53L1_RdWord(dev, + VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp)); + *distance = tmp; + return status; +} + +// VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalRate) +// { +// VL53L1X_ERROR status = 0; +// uint16_t SpNb=1, signal; + +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal); +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); +// *signalRate = (uint16_t) (200.0*signal/SpNb); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *ambPerSp) +// { +// VL53L1X_ERROR status = 0; +// uint16_t AmbientRate, SpNb = 1; + +// status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate); +// status = VL53L1_RdWord(dev, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); +// *ambPerSp=(uint16_t) (200.0 * AmbientRate / SpNb); +// return status; +// } + +// static VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signal) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp); +// *signal = tmp*8; +// return status; +// } + +// static VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp); +// *spNb = tmp >> 8; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp); +// *ambRate = tmp*8; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus) +// { +// VL53L1X_ERROR status = 0; +// uint8_t RgSt; + +// *rangeStatus = 255; +// status = VL53L1_RdByte(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt); +// RgSt = RgSt & 0x1F; +// if (RgSt < 24) +// *rangeStatus = status_rtn[RgSt]; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult) +// { +// VL53L1X_ERROR status = 0; +// uint8_t Temp[17]; +// uint8_t RgSt = 255; + +// status = VL53L1_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, Temp, 17); +// RgSt = Temp[0] & 0x1F; +// if (RgSt < 24) +// RgSt = status_rtn[RgSt]; +// pResult->Status = RgSt; +// pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8; +// pResult->NumSPADs = Temp[3]; +// pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8; +// pResult->Distance = Temp[13] << 8 | Temp[14]; + +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue) +// { +// VL53L1X_ERROR status = 0; +// int16_t Temp; + +// Temp = (OffsetValue*4); +// VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, +// (uint16_t)Temp); +// VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); +// VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *offset) +// { +// VL53L1X_ERROR status = 0; +// uint16_t Temp; + +// status = VL53L1_RdWord(dev,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp); +// Temp = Temp<<3; +// Temp = Temp>>5; +// *offset = (int16_t)(Temp); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue) +// { +// /* XTalkValue in count per second to avoid float type */ +// VL53L1X_ERROR status = 0; + +// status = VL53L1_WrWord(dev, +// ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS, +// 0x0000); +// status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS, +// 0x0000); +// status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, +// (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */ +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *xtalk ) +// { +// VL53L1X_ERROR status = 0; + +// status = VL53L1_RdWord(dev,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, xtalk); +// *xtalk = (uint16_t)((*xtalk*1000)>>9); /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */ +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, +// uint16_t ThreshHigh, uint8_t Window, +// uint8_t IntOnNoTarget) +// { +// VL53L1X_ERROR status = 0; +// uint8_t Temp = 0; + +// status = VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp); +// Temp = Temp & 0x47; +// if (IntOnNoTarget == 0) { +// status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, +// (Temp | (Window & 0x07))); +// } else { +// status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, +// ((Temp | (Window & 0x07)) | 0x40)); +// } +// status = VL53L1_WrWord(dev, SYSTEM__THRESH_HIGH, ThreshHigh); +// status = VL53L1_WrWord(dev, SYSTEM__THRESH_LOW, ThreshLow); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp; +// status = VL53L1_RdByte(dev,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp); +// *window = (uint16_t)(tmp & 0x7); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev,SYSTEM__THRESH_LOW, &tmp); +// *low = tmp; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev,SYSTEM__THRESH_HIGH, &tmp); +// *high = tmp; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter) +// { +// VL53L1X_ERROR status = 0; +// status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, ROICenter); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp; +// status = VL53L1_RdByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, &tmp); +// *ROICenter = tmp; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y) +// { +// uint8_t OpticalCenter; +// VL53L1X_ERROR status = 0; + +// status =VL53L1_RdByte(dev, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter); +// if (X > 16) +// X = 16; +// if (Y > 16) +// Y = 16; +// if (X > 10 || Y > 10){ +// OpticalCenter = 199; +// } +// status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter); +// status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, +// (Y - 1) << 4 | (X - 1)); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp; + +// status = VL53L1_RdByte(dev,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp); +// *ROI_X = ((uint16_t)tmp & 0x0F) + 1; +// *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t Signal) +// { +// VL53L1X_ERROR status = 0; + +// VL53L1_WrWord(dev,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev, +// RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp); +// *signal = tmp <<3; +// return status; +// } + +// VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t Sigma) +// { +// VL53L1X_ERROR status = 0; + +// if(Sigma>(0xFFFF>>2)){ +// return 1; +// } +// /* 16 bits register 14.2 format */ +// status = VL53L1_WrWord(dev,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2); +// return status; +// } + +// VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *sigma) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; + +// status = VL53L1_RdWord(dev,RANGE_CONFIG__SIGMA_THRESH, &tmp); +// *sigma = tmp >> 2; +// return status; + +// } + +// VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp=0; + +// status = VL53L1_WrByte(dev,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */ +// status = VL53L1_WrByte(dev,0x0B,0x92); +// status = VL53L1X_StartRanging(dev); +// while(tmp==0){ +// status = VL53L1X_CheckForDataReady(dev, &tmp); +// } +// tmp = 0; +// status = VL53L1X_ClearInterrupt(dev); +// status = VL53L1X_StopRanging(dev); +// status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ +// status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ +// return status; +// } + + +// int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset) +// { +// uint8_t i, tmp; +// int16_t AverageDistance = 0; +// uint16_t distance; +// VL53L1X_ERROR status = 0; + +// status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0); +// status = VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); +// status = VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); +// status = VL53L1X_StartRanging(dev); /* Enable VL53L1X sensor */ +// for (i = 0; i < 50; i++) { +// tmp = 0; +// while (tmp == 0){ +// status = VL53L1X_CheckForDataReady(dev, &tmp); +// } +// status = VL53L1X_GetDistance(dev, &distance); +// status = VL53L1X_ClearInterrupt(dev); +// AverageDistance = AverageDistance + distance; +// } +// status = VL53L1X_StopRanging(dev); +// AverageDistance = AverageDistance / 50; +// *offset = TargetDistInMm - AverageDistance; +// status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4); +// return status; +// } + +// int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk) +// { +// uint8_t i, tmp; +// float AverageSignalRate = 0; +// float AverageDistance = 0; +// float AverageSpadNb = 0; +// uint16_t distance = 0, spadNum; +// uint16_t sr; +// VL53L1X_ERROR status = 0; +// uint32_t calXtalk; + +// status = VL53L1_WrWord(dev, 0x0016,0); +// status = VL53L1X_StartRanging(dev); +// for (i = 0; i < 50; i++) { +// tmp = 0; +// while (tmp == 0){ +// status = VL53L1X_CheckForDataReady(dev, &tmp); +// } +// status= VL53L1X_GetSignalRate(dev, &sr); +// status= VL53L1X_GetDistance(dev, &distance); +// status = VL53L1X_ClearInterrupt(dev); +// AverageDistance = AverageDistance + distance; +// status = VL53L1X_GetSpadNb(dev, &spadNum); +// AverageSpadNb = AverageSpadNb + spadNum; +// AverageSignalRate = +// AverageSignalRate + sr; +// } +// status = VL53L1X_StopRanging(dev); +// AverageDistance = AverageDistance / 50; +// AverageSpadNb = AverageSpadNb / 50; +// AverageSignalRate = AverageSignalRate / 50; +// /* Calculate Xtalk value */ +// calXtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb); +// *xtalk = (uint16_t)((calXtalk*1000)>>9); +// status = VL53L1_WrWord(dev, 0x0016, (uint16_t)calXtalk); +// return status; +// } + +static void vl53l1x_Init(rangefinderDev_t * rangefinder) +{ + VL53L1X_ERROR status = VL53L1_ERROR_NONE; + isInitialized = false; + status = VL53L1X_SensorInit(rangefinder->busDev); + if (status == VL53L1_ERROR_NONE) { + VL53L1X_SetDistanceMode(rangefinder->busDev, 2); /* 1=short, 2=long */ + VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, 33); /* in ms possible values [20, 50, 100, 200, 500] */ + VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, RANGEFINDER_VL53L1X_TASK_PERIOD_MS); /* in ms, IM must be > = TB */ + status = VL53L1X_StartRanging(rangefinder->busDev); + } + isInitialized = (status == VL53L1_ERROR_NONE); +} + +void vl53l1x_Update(rangefinderDev_t * rangefinder) +{ + uint16_t Distance; + uint8_t dataReady; + + if (!isInitialized) { + return; + } + + VL53L1X_CheckForDataReady(rangefinder->busDev, &dataReady); + if (dataReady != 0) { + VL53L1X_GetDistance(rangefinder->busDev, &Distance); + lastMeasurementCm = Distance / 10; + lastMeasurementIsNew = true; + } + VL53L1X_ClearInterrupt(rangefinder->busDev); +} + +int32_t vl53l1x_GetDistance(rangefinderDev_t *dev) +{ + UNUSED(dev); + + if (isResponding && isInitialized) { + if (lastMeasurementIsNew) { + lastMeasurementIsNew = false; + return (lastMeasurementCm < VL53L1X_MAX_RANGE_CM) ? lastMeasurementCm : RANGEFINDER_OUT_OF_RANGE; + } + else { + return RANGEFINDER_NO_NEW_DATA; + } + } + else { + return RANGEFINDER_HARDWARE_FAILURE; + } +} + +static bool deviceDetect(busDevice_t * busDev) +{ + for (int retry = 0; retry < 5; retry++) { + uint8_t model_id; + + delay(150); + + VL53L1X_ERROR err = VL53L1_RdByte(busDev, 0x010F, &model_id); + if (err == 0 && model_id == 0xEA) { + return true; + } + }; + + return false; +} + +bool vl53l1xDetect(rangefinderDev_t * rangefinder) +{ + rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_VL53L1X, 0, OWNER_RANGEFINDER); + if (rangefinder->busDev == NULL) { + return false; + } + + if (!deviceDetect(rangefinder->busDev)) { + busDeviceDeInit(rangefinder->busDev); + return false; + } + + rangefinder->delayMs = RANGEFINDER_VL53L1X_TASK_PERIOD_MS; + rangefinder->maxRangeCm = VL53L1X_MAX_RANGE_CM; + rangefinder->detectionConeDeciDegrees = VL53L1X_DETECTION_CONE_DECIDEGREES; + rangefinder->detectionConeExtendedDeciDegrees = VL53L1X_DETECTION_CONE_DECIDEGREES; + + rangefinder->init = &vl53l1x_Init; + rangefinder->update = &vl53l1x_Update; + rangefinder->read = &vl53l1x_GetDistance; + + return true; +} + +#endif diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.h b/src/main/drivers/rangefinder/rangefinder_vl53l1x.h new file mode 100644 index 0000000000..3dc5b63bf0 --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.h @@ -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); diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c index 2734de00ad..592ce4cf64 100644 --- a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c +++ b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c @@ -1081,7 +1081,7 @@ SD_Error_t SD_GetStatus(void) } } else { - ErrorState = SD_CARD_ERROR; + ErrorState = SD_ERROR; } return ErrorState; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 3b04aa4dc6..68f951f687 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -85,6 +85,7 @@ uint8_t cliMode = 0; #include "flight/mixer.h" #include "flight/pid.h" #include "flight/servos.h" +#include "flight/secondary_imu.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -2918,6 +2919,55 @@ static void cliBatch(char *cmdline) } #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) { UNUSED(cmdline); @@ -3666,6 +3716,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), #endif 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) CLI_COMMAND_DEF("safehome", "safe home list", NULL, cliSafeHomes), #endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 5fcf8a6d11..d38554bd21 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -112,11 +112,15 @@ PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, .current_battery_profile_index = 0, - .debug_mode = DEBUG_NONE, - .i2c_speed = I2C_SPEED_400KHZ, - .cpuUnderclock = 0, - .throttle_tilt_compensation_strength = 0, // 0-100, 0 - disabled - .name = { 0 } + .debug_mode = SETTING_DEBUG_MODE_DEFAULT, +#ifdef USE_I2C + .i2c_speed = SETTING_I2C_SPEED_DEFAULT, +#endif +#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); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index bab2d96eef..48dc455e71 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -73,8 +73,12 @@ typedef struct systemConfig_s { uint8_t current_profile_index; uint8_t current_battery_profile_index; uint8_t debug_mode; +#ifdef USE_I2C uint8_t i2c_speed; +#endif +#ifdef USE_UNDERCLOCK uint8_t cpuUnderclock; +#endif uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. char name[MAX_NAME_LENGTH + 1]; } systemConfig_t; diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 49b5b2b227..ba464a1fe6 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -29,6 +29,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_curves.h" +#include "fc/settings.h" 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++) { RESET_CONFIG(controlRateConfig_t, &instance[i], .throttle = { - .rcMid8 = 50, - .rcExpo8 = 0, - .dynPID = 0, - .pa_breakpoint = 1500, - .fixedWingTauMs = 0 + .rcMid8 = SETTING_THR_MID_DEFAULT, + .rcExpo8 = SETTING_THR_EXPO_DEFAULT, + .dynPID = SETTING_TPA_RATE_DEFAULT, + .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT, + .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT }, .stabilized = { - .rcExpo8 = 70, - .rcYawExpo8 = 20, - .rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, - .rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, - .rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT, + .rcExpo8 = SETTING_RC_EXPO_DEFAULT, + .rcYawExpo8 = SETTING_RC_YAW_EXPO_DEFAULT, + .rates[FD_ROLL] = SETTING_ROLL_RATE_DEFAULT, + .rates[FD_PITCH] = SETTING_PITCH_RATE_DEFAULT, + .rates[FD_YAW] = SETTING_YAW_RATE_DEFAULT, }, .manual = { - .rcExpo8 = 70, - .rcYawExpo8 = 20, - .rates[FD_ROLL] = 100, - .rates[FD_PITCH] = 100, - .rates[FD_YAW] = 100 + .rcExpo8 = SETTING_MANUAL_RC_EXPO_DEFAULT, + .rcYawExpo8 = SETTING_MANUAL_RC_YAW_EXPO_DEFAULT, + .rates[FD_ROLL] = SETTING_MANUAL_ROLL_RATE_DEFAULT, + .rates[FD_PITCH] = SETTING_MANUAL_PITCH_RATE_DEFAULT, + .rates[FD_YAW] = SETTING_MANUAL_YAW_RATE_DEFAULT }, .misc = { - .fpvCamAngleDegrees = 0 + .fpvCamAngleDegrees = SETTING_FPV_MIX_DEGREES_DEFAULT } ); } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index b8428347aa..b425148581 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -33,10 +33,8 @@ and so on. */ #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_DEFAULT 20 #define CONTROL_RATE_CONFIG_YAW_RATE_MAX 180 #define CONTROL_RATE_CONFIG_YAW_RATE_MIN 2 -#define CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT 20 #define CONTROL_RATE_CONFIG_TPA_MAX 100 diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 71f758dc40..40c18fd31f 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -38,6 +38,7 @@ FILE_COMPILE_FOR_SPEED #include "drivers/time.h" #include "drivers/system.h" #include "drivers/pwm_output.h" +#include "drivers/accgyro/accgyro_bno055.h" #include "sensors/sensors.h" #include "sensors/diagnostics.h" @@ -89,6 +90,7 @@ FILE_COMPILE_FOR_SPEED #include "flight/failsafe.h" #include "config/feature.h" +#include "common/vector.h" #include "programming/pid.h" // June 2013 V2.2-dev @@ -131,6 +133,9 @@ static uint32_t gyroSyncFailureCount; static disarmReason_t lastDisarmReason = DISARM_NONE; 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) { #ifdef USE_BARO @@ -295,6 +300,22 @@ static void updateArmingStatus(void) 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 */ // 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 @@ -388,11 +409,22 @@ void disarm(disarmReason_t disarmReason) blackboxFinish(); } #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(); logicConditionReset(); + +#ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); +#endif + beeper(BEEPER_DISARMING); // emit disarm tone + + prearmWasReset = false; } } @@ -447,6 +479,21 @@ void releaseSharedTelemetryPorts(void) { void tryArm(void) { 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 if ( !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 ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); logicConditionReset(); + +#ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); +#endif + headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); @@ -506,6 +557,7 @@ void tryArm(void) #else beeper(BEEPER_ARMING); #endif + statsOnArm(); return; diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index a7317e48d1..34eb6fe68b 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -45,4 +45,4 @@ void emergencyArmingUpdate(bool armingSwitchIsOn); bool isCalibrating(void); float getFlightTime(void); -void fcReboot(bool bootLoader); +void fcReboot(bool bootLoader); \ No newline at end of file diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 025861d35e..ce6fecb053 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -99,6 +99,7 @@ #include "flight/pid.h" #include "flight/servos.h" #include "flight/rpm_filter.h" +#include "flight/secondary_imu.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -219,8 +220,12 @@ void init(void) ensureEEPROMContainsValidData(); readEEPROM(); +#ifdef USE_UNDERCLOCK // Re-initialize system clock to their final values (if necessary) systemClockSetup(systemConfig()->cpuUnderclock); +#else + systemClockSetup(false); +#endif #ifdef USE_I2C i2cSetSpeed(systemConfig()->i2c_speed); @@ -673,9 +678,17 @@ void init(void) rcdeviceInit(); #endif // USE_RCDEVICE +#ifdef USE_DSHOT + initDShotCommands(); +#endif + // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; + +#ifdef USE_SECONDARY_IMU + secondaryImuInit(); +#endif fcTasksInit(); #ifdef USE_OSD diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a32961daba..5fdcebc9e1 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -487,7 +487,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, gyroRateDps(i)); } for (int i = 0; i < 3; i++) { +#ifdef USE_MAG sbufWriteU16(dst, mag.magADC[i]); +#else + sbufWriteU16(dst, 0); +#endif } } break; @@ -567,6 +571,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, programmingPids(i)->gains.FF); } break; + case MSP2_INAV_PROGRAMMING_PID_STATUS: + for (int i = 0; i < MAX_PROGRAMMING_PID_COUNT; i++) { + sbufWriteU32(dst, programmingPidGetOutput(i)); + } + break; #endif case MSP2_COMMON_MOTOR_MIXER: 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, 0); +#ifdef USE_MAG sbufWriteU16(dst, compassConfig()->mag_declination / 10); +#else + sbufWriteU16(dst, 0); +#endif +#ifdef USE_ADC sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10); +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); +#endif break; case MSP2_INAV_MISC: @@ -808,8 +828,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif sbufWriteU8(dst, rxConfig()->rssi_channel); +#ifdef USE_MAG sbufWriteU16(dst, compassConfig()->mag_declination / 10); +#else + sbufWriteU16(dst, 0); +#endif +#ifdef USE_ADC sbufWriteU16(dst, batteryMetersConfig()->voltage.scale); sbufWriteU8(dst, batteryMetersConfig()->voltageSource); 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.cellMax); 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.warning); @@ -836,6 +870,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_BATTERY_CONFIG: +#ifdef USE_ADC sbufWriteU16(dst, batteryMetersConfig()->voltage.scale); sbufWriteU8(dst, batteryMetersConfig()->voltageSource); 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.cellMax); 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.scale); @@ -942,10 +986,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_VOLTAGE_METER_CONFIG: +#ifdef USE_ADC sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10); +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); +#endif break; 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, PWM_RANGE_MIDDLE); sbufWriteU16(dst, rxConfig()->mincheck); +#ifdef USE_SPEKTRUM_BIND sbufWriteU8(dst, rxConfig()->spektrum_sat_bind); +#else + sbufWriteU8(dst, 0); +#endif sbufWriteU16(dst, rxConfig()->rx_min_usec); sbufWriteU16(dst, rxConfig()->rx_max_usec); sbufWriteU8(dst, 0); // for compatibility with betaflight (rcInterpolation) sbufWriteU8(dst, 0); // for compatibility with betaflight (rcInterpolationInterval) sbufWriteU16(dst, 0); // for compatibility with betaflight (airModeActivateThreshold) +#ifdef USE_RX_SPI sbufWriteU8(dst, rxConfig()->rx_spi_protocol); sbufWriteU32(dst, rxConfig()->rx_spi_id); 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, rxConfig()->receiverType); break; @@ -1149,7 +1210,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_SENSOR_ALIGNMENT: sbufWriteU8(dst, gyroConfig()->gyro_align); sbufWriteU8(dst, accelerometerConfig()->acc_align); +#ifdef USE_MAG sbufWriteU8(dst, compassConfig()->mag_align); +#else + sbufWriteU8(dst, 0); +#endif #ifdef USE_OPFLOW sbufWriteU8(dst, opticalFlowConfig()->opflow_align); #else @@ -1826,10 +1891,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); #endif +#ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10; 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.cellWarning = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert +#else + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); +#endif } else return MSP_RESULT_ERROR; break; @@ -1864,6 +1936,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); #endif +#ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src); batteryMetersConfigMutable()->voltageSource = 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.cellMax = 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.warning = sbufReadU32(src); @@ -1890,6 +1972,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_BATTERY_CONFIG: if (dataSize == 29) { +#ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src); batteryMetersConfigMutable()->voltageSource = 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.cellMax = 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.scale = sbufReadU16(src); @@ -2533,10 +2625,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_VOLTAGE_METER_CONFIG: if (dataSize >= 4) { +#ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; +#else + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); +#endif } else return MSP_RESULT_ERROR; break; @@ -2565,15 +2664,25 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) rxConfigMutable()->maxcheck = sbufReadU16(src); sbufReadU16(src); // midrc rxConfigMutable()->mincheck = sbufReadU16(src); +#ifdef USE_SPEKTRUM_BIND rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src); +#else + sbufReadU8(src); +#endif rxConfigMutable()->rx_min_usec = sbufReadU16(src); rxConfigMutable()->rx_max_usec = sbufReadU16(src); sbufReadU8(src); // for compatibility with betaflight (rcInterpolation) sbufReadU8(src); // for compatibility with betaflight (rcInterpolationInterval) sbufReadU16(src); // for compatibility with betaflight (airModeActivateThreshold) +#ifdef USE_RX_SPI rxConfigMutable()->rx_spi_protocol = sbufReadU8(src); rxConfigMutable()->rx_spi_id = sbufReadU32(src); rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src); +#else + sbufReadU8(src); + sbufReadU32(src); + sbufReadU8(src); +#endif sbufReadU8(src); // for compatibility with betaflight (fpvCamAngleDegrees) rxConfigMutable()->receiverType = sbufReadU8(src); // Won't be modified if buffer is not large enough } else diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 3afe82c9cb..f517be11f1 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -33,6 +33,8 @@ #include "io/osd.h" +#include "drivers/pwm_output.h" + #include "sensors/diagnostics.h" #include "sensors/sensors.h" @@ -80,12 +82,14 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXOSDALT1, "OSD ALT 1", 42 }, { BOXOSDALT2, "OSD ALT 2", 43 }, { BOXOSDALT3, "OSD ALT 3", 44 }, - { BOXNAVCRUISE, "NAV CRUISE", 45 }, + { BOXNAVCOURSEHOLD, "NAV COURSE HOLD", 45 }, { BOXBRAKING, "MC BRAKING", 46 }, { BOXUSER1, "USER1", BOX_PERMANENT_ID_USER1 }, { BOXUSER2, "USER2", BOX_PERMANENT_ID_USER2 }, { BOXLOITERDIRCHN, "LOITER CHANGE", 49 }, { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 }, + { BOXPREARM, "PREARM", 51 }, + { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -163,6 +167,7 @@ void initActiveBoxIds(void) activeBoxIdCount = 0; activeBoxIds[activeBoxIdCount++] = BOXARM; + activeBoxIds[activeBoxIdCount++] = BOXPREARM; if (sensors(SENSOR_ACC) && STATE(ALTITUDE_CONTROL)) { activeBoxIds[activeBoxIdCount++] = BOXANGLE; @@ -214,7 +219,7 @@ void initActiveBoxIds(void) if (feature(FEATURE_GPS)) { activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; 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) activeBoxIds[activeBoxIdCount++] = BOXMSPRCOVERRIDE; #endif + +#ifdef USE_DSHOT + if(STATE(MULTIROTOR) && isMotorProtocolDshot()) + activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH; +#endif } #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(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD); 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_WP_MODE)), BOXNAVWP); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE); diff --git a/src/main/fc/fc_msp_box.h b/src/main/fc/fc_msp_box.h index cefbeab29f..ac3604d809 100644 --- a/src/main/fc/fc_msp_box.h +++ b/src/main/fc/fc_msp_box.h @@ -19,9 +19,7 @@ #include "fc/rc_modes.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 +#include "io/piniobox.h" typedef struct box_s { const uint8_t boxId; // see boxId_e diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index d11f506231..33d417a1d6 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -47,6 +47,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/wind_estimator.h" +#include "flight/secondary_imu.h" #include "flight/rpm_filter.h" #include "flight/servos.h" #include "flight/dynamic_lpf.h" @@ -375,6 +376,9 @@ void fcTasksInit(void) #if defined(USE_SMARTPORT_MASTER) setTaskEnabled(TASK_SMARTPORT_MASTER, true); #endif +#ifdef USE_SECONDARY_IMU + setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->hardwareType != SECONDARY_IMU_NONE && secondaryImuState.active); +#endif } cfTask_t cfTasks[TASK_COUNT] = { @@ -597,6 +601,14 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE, }, #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 [TASK_RPM_FILTER] = { .taskName = "RPM", diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 434815b69c..c05a390ea4 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -101,7 +101,43 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .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, .data = { .stepConfig = { .step = 1 }} }, { @@ -113,7 +149,11 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .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, .data = { .stepConfig = { .step = 1 }} }, { @@ -128,30 +168,6 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .adjustmentFunction = ADJUSTMENT_ROLL_RATE, .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_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, .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); schedulePidGainsUpdate(); break; - case ADJUSTMENT_PITCH_ROLL_D_FF: - case ADJUSTMENT_PITCH_D_FF: - applyAdjustmentPID(ADJUSTMENT_PITCH_D_FF, getD_FFRefByBank(pidBankMutable(), PID_PITCH), delta); - if (adjustmentFunction == ADJUSTMENT_PITCH_D_FF) { + case ADJUSTMENT_PITCH_ROLL_D: + case ADJUSTMENT_PITCH_D: + applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta); + if (adjustmentFunction == ADJUSTMENT_PITCH_D) { schedulePidGainsUpdate(); break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_D FALLTHROUGH; - case ADJUSTMENT_ROLL_D_FF: - applyAdjustmentPID(ADJUSTMENT_ROLL_D_FF, getD_FFRefByBank(pidBankMutable(), PID_ROLL), delta); + case ADJUSTMENT_ROLL_D: + 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(); break; 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); schedulePidGainsUpdate(); break; - case ADJUSTMENT_YAW_D_FF: - applyAdjustmentPID(ADJUSTMENT_YAW_D_FF, getD_FFRefByBank(pidBankMutable(), PID_YAW), delta); + case ADJUSTMENT_YAW_D: + 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(); break; case ADJUSTMENT_NAV_FW_CRUISE_THR: diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index ce5470eb46..8736978825 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -33,51 +33,55 @@ typedef enum { ADJUSTMENT_YAW_RATE = 5, ADJUSTMENT_PITCH_ROLL_P = 6, ADJUSTMENT_PITCH_ROLL_I = 7, - ADJUSTMENT_PITCH_ROLL_D_FF = 8, - ADJUSTMENT_YAW_P = 9, - ADJUSTMENT_YAW_I = 10, - ADJUSTMENT_YAW_D_FF = 11, - ADJUSTMENT_RATE_PROFILE = 12, // Unused, placeholder for compatibility - ADJUSTMENT_PITCH_RATE = 13, - ADJUSTMENT_ROLL_RATE = 14, - ADJUSTMENT_PITCH_P = 15, - ADJUSTMENT_PITCH_I = 16, - ADJUSTMENT_PITCH_D_FF = 17, - ADJUSTMENT_ROLL_P = 18, - ADJUSTMENT_ROLL_I = 19, - ADJUSTMENT_ROLL_D_FF = 20, - ADJUSTMENT_RC_YAW_EXPO = 21, - ADJUSTMENT_MANUAL_RC_EXPO = 22, - ADJUSTMENT_MANUAL_RC_YAW_EXPO = 23, - ADJUSTMENT_MANUAL_PITCH_ROLL_RATE = 24, - ADJUSTMENT_MANUAL_ROLL_RATE = 25, - ADJUSTMENT_MANUAL_PITCH_RATE = 26, - ADJUSTMENT_MANUAL_YAW_RATE = 27, - ADJUSTMENT_NAV_FW_CRUISE_THR = 28, - ADJUSTMENT_NAV_FW_PITCH2THR = 29, - ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 30, - ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 31, - ADJUSTMENT_LEVEL_P = 32, - ADJUSTMENT_LEVEL_I = 33, - ADJUSTMENT_LEVEL_D = 34, - ADJUSTMENT_POS_XY_P = 35, - ADJUSTMENT_POS_XY_I = 36, - ADJUSTMENT_POS_XY_D = 37, - ADJUSTMENT_POS_Z_P = 38, - ADJUSTMENT_POS_Z_I = 39, - ADJUSTMENT_POS_Z_D = 40, - ADJUSTMENT_HEADING_P = 41, - ADJUSTMENT_VEL_XY_P = 42, - ADJUSTMENT_VEL_XY_I = 43, - ADJUSTMENT_VEL_XY_D = 44, - ADJUSTMENT_VEL_Z_P = 45, - ADJUSTMENT_VEL_Z_I = 46, - ADJUSTMENT_VEL_Z_D = 47, - ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE = 48, - ADJUSTMENT_VTX_POWER_LEVEL = 49, - ADJUSTMENT_TPA = 50, - ADJUSTMENT_TPA_BREAKPOINT = 51, - ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS = 52, + ADJUSTMENT_PITCH_ROLL_D = 8, + ADJUSTMENT_PITCH_ROLL_FF = 9, + ADJUSTMENT_PITCH_P = 10, + ADJUSTMENT_PITCH_I = 11, + ADJUSTMENT_PITCH_D = 12, + ADJUSTMENT_PITCH_FF = 13, + ADJUSTMENT_ROLL_P = 14, + ADJUSTMENT_ROLL_I = 15, + ADJUSTMENT_ROLL_D = 16, + ADJUSTMENT_ROLL_FF = 17, + ADJUSTMENT_YAW_P = 18, + ADJUSTMENT_YAW_I = 19, + ADJUSTMENT_YAW_D = 20, + ADJUSTMENT_YAW_FF = 21, + ADJUSTMENT_RATE_PROFILE = 22, // Unused, placeholder for compatibility + ADJUSTMENT_PITCH_RATE = 23, + ADJUSTMENT_ROLL_RATE = 24, + ADJUSTMENT_RC_YAW_EXPO = 25, + ADJUSTMENT_MANUAL_RC_EXPO = 26, + ADJUSTMENT_MANUAL_RC_YAW_EXPO = 27, + ADJUSTMENT_MANUAL_PITCH_ROLL_RATE = 28, + ADJUSTMENT_MANUAL_ROLL_RATE = 29, + ADJUSTMENT_MANUAL_PITCH_RATE = 30, + ADJUSTMENT_MANUAL_YAW_RATE = 31, + ADJUSTMENT_NAV_FW_CRUISE_THR = 32, + ADJUSTMENT_NAV_FW_PITCH2THR = 33, + ADJUSTMENT_ROLL_BOARD_ALIGNMENT = 34, + ADJUSTMENT_PITCH_BOARD_ALIGNMENT = 35, + ADJUSTMENT_LEVEL_P = 36, + ADJUSTMENT_LEVEL_I = 37, + ADJUSTMENT_LEVEL_D = 38, + ADJUSTMENT_POS_XY_P = 39, + ADJUSTMENT_POS_XY_I = 40, + ADJUSTMENT_POS_XY_D = 41, + ADJUSTMENT_POS_Z_P = 42, + ADJUSTMENT_POS_Z_I = 43, + ADJUSTMENT_POS_Z_D = 44, + ADJUSTMENT_HEADING_P = 45, + ADJUSTMENT_VEL_XY_P = 46, + ADJUSTMENT_VEL_XY_I = 47, + ADJUSTMENT_VEL_XY_D = 48, + ADJUSTMENT_VEL_Z_P = 49, + ADJUSTMENT_VEL_Z_I = 50, + ADJUSTMENT_VEL_Z_D = 51, + 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 } adjustmentFunction_e; diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 1b8664972d..563d181fb6 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -44,6 +44,7 @@ #include "fc/rc_curves.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/pid.h" #include "flight/failsafe.h" @@ -64,29 +65,31 @@ #define AIRMODE_DEADBAND 25 #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_PREARM_TIMEOUT 10000 // Prearm is invalidated after 10 seconds stickPositions_e rcStickPositions; 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, - .deadband = 5, - .yaw_deadband = 5, - .pos_hold_deadband = 10, - .alt_hold_deadband = 50, - .mid_throttle_deadband = 50, - .airmodeHandlingType = STICK_CENTER, - .airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD, + .deadband = SETTING_DEADBAND_DEFAULT, + .yaw_deadband = SETTING_YAW_DEADBAND_DEFAULT, + .pos_hold_deadband = SETTING_POS_HOLD_DEADBAND_DEFAULT, + .alt_hold_deadband = SETTING_ALT_HOLD_DEADBAND_DEFAULT, + .mid_throttle_deadband = SETTING_3D_DEADBAND_THROTTLE_DEFAULT, + .airmodeHandlingType = SETTING_AIRMODE_TYPE_DEFAULT, + .airmodeThrottleThreshold = SETTING_AIRMODE_THROTTLE_THRESHOLD_DEFAULT, ); PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 2); PG_RESET_TEMPLATE(armingConfig_t, armingConfig, - .fixed_wing_auto_arm = 0, - .disarm_kill_switch = 1, - .switchDisarmDelayMs = DEFAULT_RC_SWITCH_DISARM_DELAY_MS, + .fixed_wing_auto_arm = SETTING_FIXED_WING_AUTO_ARM_DEFAULT, + .disarm_kill_switch = SETTING_DISARM_KILL_SWITCH_DEFAULT, + .switchDisarmDelayMs = SETTING_SWITCH_DISARM_DELAY_DEFAULT, + .prearmTimeoutMs = SETTING_PREARM_TIMEOUT_DEFAULT, ); bool areSticksInApModePosition(uint16_t ap_mode) diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index b229de2311..f5258a9c9e 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -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 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 prearmTimeoutMs; // duration for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. } armingConfig_t; PG_DECLARE(armingConfig_t, armingConfig); diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 2336f6e965..48dface10e 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -34,6 +34,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "fc/settings.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(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) { if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) { ENABLE_STATE(AIRMODE_ACTIVE); @@ -134,13 +139,7 @@ void rcModeUpdate(boxBitmask_t *newState) bool isModeActivationConditionPresent(boxId_e modeId) { - for (int index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { - if (modeActivationConditions(index)->modeId == modeId && IS_RANGE_USABLE(&modeActivationConditions(index)->range)) { - return true; - } - } - - return false; + return specifiedConditionCountPerMode[modeId] > 0; } bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) @@ -212,7 +211,7 @@ void updateUsedModeActivationConditionFlags(void) #ifdef USE_NAV isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) || isModeActivationConditionPresent(BOXNAVRTH) || - isModeActivationConditionPresent(BOXNAVCRUISE) || + isModeActivationConditionPresent(BOXNAVCOURSEHOLD) || isModeActivationConditionPresent(BOXNAVWP); #endif } diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 2058c822d3..6b3f877197 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -61,20 +61,22 @@ typedef enum { BOXOSDALT1 = 32, BOXOSDALT2 = 33, BOXOSDALT3 = 34, - BOXNAVCRUISE = 35, + BOXNAVCOURSEHOLD = 35, BOXBRAKING = 36, BOXUSER1 = 37, BOXUSER2 = 38, BOXFPVANGLEMIX = 39, BOXLOITERDIRCHN = 40, BOXMSPRCOVERRIDE = 41, + BOXPREARM = 42, + BOXFLIPOVERAFTERCRASH = 43, CHECKBOX_ITEM_COUNT } boxId_e; // 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; -#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20 +#define MAX_MODE_ACTIVATION_CONDITION_COUNT 40 #define CHANNEL_RANGE_MIN 900 #define CHANNEL_RANGE_MAX 2100 @@ -127,4 +129,4 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range); void updateActivatedModes(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); \ No newline at end of file diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 5ba8c6287e..47d6267cbd 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -36,7 +36,7 @@ const char *armingDisableFlagNames[]= { "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS", "ACC", "ARMSW", "HWFAIL", "BOXFS", "KILLSW", "RX", "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM", - "SETTINGFAIL", "PWMOUT" + "SETTINGFAIL", "PWMOUT", "NOPREARM" }; #endif @@ -57,7 +57,8 @@ const armingFlag_e armDisableReasonsChecklist[] = { ARMING_DISABLED_OSD_MENU, ARMING_DISABLED_ROLLPITCH_NOT_CENTERED, ARMING_DISABLED_SERVO_AUTOTRIM, - ARMING_DISABLED_OOM + ARMING_DISABLED_OOM, + ARMING_DISABLED_NO_PREARM }; armingFlag_e isArmingDisabledReason(void) @@ -153,7 +154,7 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) if (FLIGHT_MODE(NAV_POSHOLD_MODE)) return FLM_POSITION_HOLD; - if (FLIGHT_MODE(NAV_CRUISE_MODE)) + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) return FLM_CRUISE; if (FLIGHT_MODE(NAV_WP_MODE)) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 7b28b46468..9f6ec9efa3 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -23,7 +23,6 @@ typedef enum { WAS_EVER_ARMED = (1 << 3), ARMING_DISABLED_FAILSAFE_SYSTEM = (1 << 7), - ARMING_DISABLED_NOT_LEVEL = (1 << 8), ARMING_DISABLED_SENSORS_CALIBRATING = (1 << 9), ARMING_DISABLED_SYSTEM_OVERLOADED = (1 << 10), @@ -44,6 +43,7 @@ typedef enum { ARMING_DISABLED_OOM = (1 << 25), ARMING_DISABLED_INVALID_SETTING = (1 << 26), 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_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_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | 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; // Arming blockers that can be overriden by emergency arming. @@ -78,25 +78,26 @@ extern const char *armingDisableFlagNames[]; #define ARMING_FLAG(mask) (armingFlags & (mask)) // 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); typedef enum { - ANGLE_MODE = (1 << 0), - HORIZON_MODE = (1 << 1), - HEADING_MODE = (1 << 2), - NAV_ALTHOLD_MODE= (1 << 3), // old BARO - NAV_RTH_MODE = (1 << 4), // old GPS_HOME - NAV_POSHOLD_MODE= (1 << 5), // old GPS_HOLD - HEADFREE_MODE = (1 << 6), - NAV_LAUNCH_MODE = (1 << 7), - MANUAL_MODE = (1 << 8), - FAILSAFE_MODE = (1 << 9), - AUTO_TUNE = (1 << 10), // old G-Tune - NAV_WP_MODE = (1 << 11), - NAV_CRUISE_MODE = (1 << 12), - FLAPERON = (1 << 13), - TURN_ASSISTANT = (1 << 14), + ANGLE_MODE = (1 << 0), + HORIZON_MODE = (1 << 1), + HEADING_MODE = (1 << 2), + NAV_ALTHOLD_MODE = (1 << 3), // old BARO + NAV_RTH_MODE = (1 << 4), // old GPS_HOME + NAV_POSHOLD_MODE = (1 << 5), // old GPS_HOLD + HEADFREE_MODE = (1 << 6), + NAV_LAUNCH_MODE = (1 << 7), + MANUAL_MODE = (1 << 8), + FAILSAFE_MODE = (1 << 9), + AUTO_TUNE = (1 << 10), // old G-Tune + NAV_WP_MODE = (1 << 11), + NAV_COURSE_HOLD_MODE = (1 << 12), + FLAPERON = (1 << 13), + TURN_ASSISTANT = (1 << 14), + FLIP_OVER_AFTER_CRASH = (1 << 15), } flightModeFlags_e; extern uint32_t flightModeFlags; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a382e6c7b1..6e4e29da99 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -4,16 +4,19 @@ tables: - name: gyro_lpf values: ["256HZ", "188HZ", "98HZ", "42HZ", "20HZ", "10HZ"] - name: acc_hardware - values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"] + values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware - values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"] + values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X", "US42"] enum: rangefinderType_e + - name: secondary_imu_hardware + values: ["NONE", "BNO055"] + enum: secondaryImuType_e - name: mag_hardware - values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"] + values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"] enum: magSensor_e - name: opflow_hardware - values: ["NONE", "PMW3901", "CXOF", "MSP", "FAKE"] + values: ["NONE", "CXOF", "MSP", "FAKE"] enum: opticalFlowSensor_e - name: baro_hardware values: ["NONE", "AUTO", "BMP085", "MS5611", "BMP280", "MS5607", "LPS25H", "SPL06", "BMP388", "DPS310", "MSP", "FAKE"] @@ -84,7 +87,7 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -152,6 +155,9 @@ tables: - name: djiOsdTempSource values: ["ESC", "IMU", "BARO"] enum: djiOsdTempSource_e + - name: djiOsdSpeedSource + values: ["GROUND", "3D", "AIR"] + enum: djiOsdSpeedSource_e - name: nav_overrides_motor_stop enum: navOverridesMotorStop_e values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] @@ -165,11 +171,11 @@ groups: members: - name: looptime description: "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." - default_value: "1000" + default_value: 1000 max: 9000 - name: gyro_sync description: "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" - default_value: "OFF" + default_value: ON field: gyroSync type: bool - name: align_gyro @@ -179,13 +185,13 @@ groups: type: uint8_t table: alignment - name: gyro_hardware_lpf - description: "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." - default_value: "42HZ" + description: "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." + default_value: "256HZ" field: gyro_lpf table: gyro_lpf - name: gyro_lpf_hz description: "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." - default_value: "60" + default_value: 60 field: gyro_soft_lpf_hz max: 200 - name: gyro_lpf_type @@ -195,72 +201,74 @@ groups: table: filter_type - name: moron_threshold description: "When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered." - default_value: "32" + default_value: 32 field: gyroMovementCalibrationThreshold max: 128 - name: gyro_notch_hz field: gyro_notch_hz max: 500 + default_value: 0 - name: gyro_notch_cutoff field: gyro_notch_cutoff min: 1 max: 500 + default_value: 1 - name: gyro_stage2_lowpass_hz description: "Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz)" - default_value: "0" + default_value: 0 field: gyro_stage2_lowpass_hz min: 0 max: 500 - name: gyro_stage2_lowpass_type description: "Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." - default_value: "`BIQUAD`" + default_value: BIQUAD field: gyro_stage2_lowpass_type table: filter_type - name: gyro_use_dyn_lpf description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position." - default_value: "OFF" + default_value: OFF field: useDynamicLpf type: bool - name: gyro_dyn_lpf_min_hz description: "Minimum frequency of the gyro Dynamic LPF" - default_value: "200" + default_value: 200 field: gyroDynamicLpfMinHz min: 40 max: 400 - name: gyro_dyn_lpf_max_hz description: "Maximum frequency of the gyro Dynamic LPF" - default_value: "500" + default_value: 500 field: gyroDynamicLpfMaxHz min: 40 max: 1000 - name: gyro_dyn_lpf_curve_expo description: "Expo value for the throttle-to-frequency mapping for Dynamic LPF" - default_value: "5" + default_value: 5 field: gyroDynamicLpfCurveExpo min: 1 max: 10 - name: dynamic_gyro_notch_enabled description: "Enable/disable dynamic gyro notch also known as Matrix Filter" - default_value: "`OFF`" + default_value: OFF field: dynamicGyroNotchEnabled condition: USE_DYNAMIC_FILTERS type: bool - name: dynamic_gyro_notch_range description: "Range for dynamic gyro notches. `MEDIUM` for 5\", `HIGH` for 3\" and `MEDIUM`/`LOW` for 7\" and bigger propellers" - default_value: "`MEDIUM`" + default_value: "MEDIUM" field: dynamicGyroNotchRange condition: USE_DYNAMIC_FILTERS table: dynamicFilterRangeTable - name: dynamic_gyro_notch_q description: "Q factor for dynamic notches" - default_value: "120" + default_value: 120 field: dynamicGyroNotchQ condition: USE_DYNAMIC_FILTERS min: 1 max: 1000 - name: dynamic_gyro_notch_min_hz description: "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`" - default_value: "150" + default_value: 150 field: dynamicGyroNotchMinHz condition: USE_DYNAMIC_FILTERS min: 30 @@ -269,6 +277,7 @@ groups: condition: USE_DUAL_GYRO min: 0 max: 1 + default_value: 0 - name: PG_ADC_CHANNEL_CONFIG type: adcChannelConfig_t @@ -277,25 +286,25 @@ groups: members: - name: vbat_adc_channel description: "ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled" - default_value: "-" + default_value: :target field: adcFunctionChannel[ADC_BATTERY] min: ADC_CHN_NONE max: ADC_CHN_MAX - name: rssi_adc_channel description: "ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled" - default_value: "-" + default_value: :target field: adcFunctionChannel[ADC_RSSI] min: ADC_CHN_NONE max: ADC_CHN_MAX - name: current_adc_channel description: "ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled" - default_value: "-" + default_value: :target field: adcFunctionChannel[ADC_CURRENT] min: ADC_CHN_NONE max: ADC_CHN_MAX - name: airspeed_adc_channel description: "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" - default_value: "0" + default_value: :target field: adcFunctionChannel[ADC_AIRSPEED] min: ADC_CHN_NONE max: ADC_CHN_MAX @@ -307,9 +316,11 @@ groups: - name: acc_notch_hz min: 0 max: 255 + default_value: 0 - name: acc_notch_cutoff min: 1 max: 255 + default_value: 1 - name: align_acc description: "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." default_value: "DEFAULT" @@ -322,7 +333,7 @@ groups: table: acc_hardware - name: acc_lpf_hz description: "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." - default_value: "15" + default_value: 15 min: 0 max: 200 - name: acc_lpf_type @@ -332,37 +343,37 @@ groups: table: filter_type - name: acczero_x description: "Calculated value after '6 position avanced calibration'. See Wiki page." - default_value: "0" + default_value: 0 field: accZero.raw[X] min: INT16_MIN max: INT16_MAX - name: acczero_y description: "Calculated value after '6 position avanced calibration'. See Wiki page." - default_value: "0" + default_value: 0 field: accZero.raw[Y] min: INT16_MIN max: INT16_MAX - name: acczero_z description: "Calculated value after '6 position avanced calibration'. See Wiki page." - default_value: "0" + default_value: 0 field: accZero.raw[Z] min: INT16_MIN max: INT16_MAX - name: accgain_x description: "Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page." - default_value: "4096" + default_value: 4096 field: accGain.raw[X] min: 1 max: 8192 - name: accgain_y description: "Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page." - default_value: "4096" + default_value: 4096 field: accGain.raw[Y] min: 1 max: 8192 - name: accgain_z description: "Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page." - default_value: "4096" + default_value: 4096 field: accGain.raw[Z] min: 1 max: 8192 @@ -378,7 +389,7 @@ groups: default_value: "NONE" - name: rangefinder_median_filter description: "3-point median filtering for rangefinder readouts" - default_value: "OFF" + default_value: OFF field: use_median_filtering type: bool @@ -389,18 +400,111 @@ groups: members: - name: opflow_hardware description: "Selection of OPFLOW hardware." - default: "NONE" + default_value: NONE table: opflow_hardware - name: opflow_scale min: 0 max: 10000 + default_value: 10.5 - name: align_opflow description: "Optical flow module alignment (default CW0_DEG_FLIP)" - default_value: "5" + default_value: CW0FLIP field: opflow_align type: uint8_t table: alignment + - name: PG_SECONDARY_IMU + type: secondaryImuConfig_t + headers: ["flight/secondary_imu.h"] + condition: USE_SECONDARY_IMU + members: + - name: imu2_hardware + description: "Selection of a Secondary IMU hardware type. NONE disables this functionality" + default_value: "NONE" + field: hardwareType + table: secondary_imu_hardware + - name: imu2_use_for_osd_heading + description: "If set to ON, Secondary IMU data will be used for Analog OSD heading" + default_value: OFF + field: useForOsdHeading + type: bool + - name: imu2_use_for_osd_ahi + description: "If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon" + field: useForOsdAHI + default_value: OFF + type: bool + - name: imu2_use_for_stabilized + description: "If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH)" + field: useForStabilized + default_value: OFF + type: bool + - name: imu2_align_roll + description: "Roll alignment for Secondary IMU. 1/10 of a degree" + field: rollDeciDegrees + default_value: 0 + min: -1800 + max: 3600 + - name: imu2_align_pitch + description: "Pitch alignment for Secondary IMU. 1/10 of a degree" + field: pitchDeciDegrees + default_value: 0 + min: -1800 + max: 3600 + - name: imu2_align_yaw + description: "Yaw alignment for Secondary IMU. 1/10 of a degree" + field: yawDeciDegrees + default_value: 0 + min: -1800 + max: 3600 + - name: imu2_gain_acc_x + description: "Secondary IMU ACC calibration data" + field: calibrationOffsetAcc[X] + default_value: 0 + min: INT16_MIN + max: INT16_MAX + - name: imu2_gain_acc_y + field: calibrationOffsetAcc[Y] + description: "Secondary IMU ACC calibration data" + default_value: 0 + min: INT16_MIN + max: INT16_MAX + - name: imu2_gain_acc_z + field: calibrationOffsetAcc[Z] + description: "Secondary IMU ACC calibration data" + default_value: 0 + min: INT16_MIN + max: INT16_MAX + - name: imu2_gain_mag_x + field: calibrationOffsetMag[X] + description: "Secondary IMU MAG calibration data" + default_value: 0 + min: INT16_MIN + max: INT16_MAX + - name: imu2_gain_mag_y + field: calibrationOffsetMag[Y] + description: "Secondary IMU MAG calibration data" + default_value: 0 + min: INT16_MIN + max: INT16_MAX + - name: imu2_gain_mag_z + field: calibrationOffsetMag[Z] + description: "Secondary IMU MAG calibration data" + default_value: 0 + min: INT16_MIN + max: INT16_MAX + - name: imu2_radius_acc + field: calibrationRadiusAcc + description: "Secondary IMU MAG calibration data" + default_value: 0 + min: INT16_MIN + max: INT16_MAX + - name: imu2_radius_mag + field: calibrationRadiusMag + description: "Secondary IMU MAG calibration data" + default_value: 0 + min: INT16_MIN + max: INT16_MAX + - name: PG_COMPASS_CONFIG type: compassConfig_t headers: ["sensors/compass.h"] @@ -418,71 +522,72 @@ groups: table: mag_hardware - name: mag_declination description: "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." - default_value: "0" + default_value: 0 min: -18000 max: 18000 - name: magzero_x description: "Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed." - default_value: "0" + default_value: :zero field: magZero.raw[X] min: INT16_MIN max: INT16_MAX - name: magzero_y description: "Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed." - default_value: "0" + default_value: :zero field: magZero.raw[Y] min: INT16_MIN max: INT16_MAX - name: magzero_z description: "Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed." - default_value: "0" + default_value: :zero field: magZero.raw[Z] min: INT16_MIN max: INT16_MAX - name: maggain_x description: "Magnetometer calibration X gain. If 1024, no calibration or calibration failed" - default_value: "1024" + default_value: 1024 field: magGain[X] min: INT16_MIN max: INT16_MAX - name: maggain_y description: "Magnetometer calibration Y gain. If 1024, no calibration or calibration failed" - default_value: "1024" + default_value: 1024 field: magGain[Y] min: INT16_MIN max: INT16_MAX - name: maggain_z description: "Magnetometer calibration Z gain. If 1024, no calibration or calibration failed" - default_value: "1024" + default_value: 1024 field: magGain[Z] min: INT16_MIN max: INT16_MAX - name: mag_calibration_time description: "Adjust how long time the Calibration of mag will last." - default_value: "30" + default_value: 30 field: magCalibrationTimeLimit - min: 30 + min: 20 max: 120 - name: mag_to_use description: "Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target" condition: USE_DUAL_MAG min: 0 max: 1 + default_value: 0 - name: align_mag_roll description: "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." - default_value: "0" + default_value: 0 field: rollDeciDegrees min: -1800 max: 3600 - name: align_mag_pitch description: "Same as align_mag_roll, but for the pitch axis." - default_value: "0" + default_value: 0 field: pitchDeciDegrees min: -1800 max: 3600 - name: align_mag_yaw description: "Same as align_mag_roll, but for the yaw axis." - default_value: "0" + default_value: 0 field: yawDeciDegrees min: -1800 max: 3600 @@ -498,12 +603,12 @@ groups: table: baro_hardware - name: baro_median_filter description: "3-point median filtering for barometer readouts. No reason to change this setting" - default_value: "ON" + default_value: ON field: use_median_filtering type: bool - name: baro_cal_tolerance description: "Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]." - default_value: "150" + default_value: 150 field: baro_calibration_tolerance min: 0 max: 1000 @@ -520,9 +625,11 @@ groups: - name: pitot_lpf_milli_hz min: 0 max: 10000 + default_value: 350 - name: pitot_scale min: 0 max: 100 + default_value: 1.0 - name: PG_RX_CONFIG type: rxConfig_t @@ -530,40 +637,40 @@ groups: members: - name: receiver_type description: "Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL`" - default_value: "`TARGET dependent`" + default_value: :target field: receiverType table: receiver_type - name: min_check description: "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." - default_value: "1100" + default_value: 1100 field: mincheck min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: max_check description: "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." - default_value: "1900" + default_value: 1900 field: maxcheck min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: rssi_source description: "Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP`" - default_value: "`AUTO`" + default_value: "AUTO" field: rssi_source table: rssi_source - name: rssi_channel description: "RX channel containing the RSSI signal" - default_value: "0" + default_value: 0 min: 0 max: MAX_SUPPORTED_RC_CHANNEL_COUNT - name: rssi_min description: "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)." - default_value: "0" + default_value: 0 field: rssiMin min: RSSI_VISIBLE_VALUE_MIN max: RSSI_VISIBLE_VALUE_MAX - name: rssi_max description: "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." - default_value: "100" + default_value: 100 field: rssiMax min: RSSI_VISIBLE_VALUE_MIN max: RSSI_VISIBLE_VALUE_MAX @@ -571,53 +678,60 @@ groups: field: sbusSyncInterval min: 500 max: 10000 + default_value: 3000 - name: rc_filter_frequency description: "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" - default_value: "50" + default_value: 50 field: rcFilterFrequency min: 0 max: 100 - name: serialrx_provider description: "When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section." - default_value: "SPEK1024" + default_value: :target condition: USE_SERIAL_RX table: serial_rx - name: serialrx_inverted description: "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)." - default_value: "OFF" + default_value: OFF condition: USE_SERIAL_RX type: bool - name: rx_spi_protocol condition: USE_RX_SPI table: rx_spi_protocol + default_value: :target - name: rx_spi_id condition: USE_RX_SPI min: 0 max: 0 + default_value: :zero - name: rx_spi_rf_channel_count + condition: USE_RX_SPI min: 0 max: 8 + default_value: :zero - name: spektrum_sat_bind description: "0 = disabled. Used to bind the spektrum satellite to RX" - default_value: "0" condition: USE_SPEKTRUM_BIND min: SPEKTRUM_SAT_BIND_DISABLED max: SPEKTRUM_SAT_BIND_MAX + default_value: :SPEKTRUM_SAT_BIND_DISABLED - name: srxl2_unit_id condition: USE_SERIALRX_SRXL2 min: 0 max: 15 + default_value: 1 - name: srxl2_baud_fast condition: USE_SERIALRX_SRXL2 - table: off_on + type: bool + default_value: ON - name: rx_min_usec description: "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." - default_value: "885" + default_value: 885 min: PWM_PULSE_MIN max: PWM_PULSE_MAX - name: rx_max_usec description: "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." - default_value: "2115" + default_value: 2115 min: PWM_PULSE_MIN max: PWM_PULSE_MAX - name: serialrx_halfduplex @@ -627,7 +741,7 @@ groups: table: tristate - name: msp_override_channels description: "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." - default_value: "0" + default_value: 0 field: mspOverrideChannels condition: USE_MSP_RC_OVERRIDE min: 0 @@ -640,24 +754,24 @@ groups: members: - name: blackbox_rate_num description: "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" - default_value: "1" + default_value: 1 field: rate_num min: 1 max: 65535 - name: blackbox_rate_denom description: "Blackbox logging rate denominator. See blackbox_rate_num." - default_value: "1" + default_value: 1 field: rate_denom min: 1 max: 65535 - name: blackbox_device description: "Selection of where to write blackbox data" - default_value: "SPIFLASH" + default_value: :target field: device table: blackbox_device - name: sdcard_detect_inverted description: "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." - default_value: "`TARGET dependent`" + default_value: :target field: invertedCardDetection condition: USE_SDCARD type: bool @@ -668,31 +782,31 @@ groups: members: - name: max_throttle description: "This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000." - default_value: "1850" + default_value: 1850 field: maxthrottle min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: min_command description: "This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once." - default_value: "1000" + default_value: 1000 field: mincommand min: 0 max: PWM_RANGE_MAX - name: motor_pwm_rate description: "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." - default_value: "400" + default_value: 400 field: motorPwmRate min: 50 max: 32000 - name: motor_accel_time description: "Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000]" - default_value: "0" + default_value: 0 field: motorAccelTimeMs min: 0 max: 1000 - name: motor_decel_time description: "Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000]" - default_value: "0" + default_value: 0 field: motorDecelTimeMs min: 0 max: 1000 @@ -703,20 +817,29 @@ groups: table: motor_pwm_protocol - name: throttle_scale description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%" - default_value: "1.000" + default_value: 1.0 field: throttleScale min: 0 max: 1 - name: throttle_idle description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle." - default_value: "15" + default_value: 15 field: throttleIdle min: 0 max: 30 - name: motor_poles field: motorPoleCount + description: "The number of motor poles. Required to compute motor RPM" min: 4 max: 255 + default_value: 14 + - name: flip_over_after_crash_power_factor + field: flipOverAfterPowerFactor + default_value: 65 + description: "flip over after crash power factor" + condition: "USE_DSHOT" + min: 0 + max: 100 - name: PG_FAILSAFE_CONFIG type: failsafeConfig_t @@ -724,27 +847,27 @@ groups: members: - name: failsafe_delay description: "Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay)." - default_value: "5" + default_value: 5 min: 0 max: 200 - name: failsafe_recovery_delay description: "Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay)." - default_value: "5" + default_value: 5 min: 0 max: 200 - name: failsafe_off_delay description: "Time in deciseconds to wait before turning off motors when failsafe is activated. 0 = No timeout. See [Failsafe documentation](Failsafe.md#failsafe_off_delay)." - default_value: "200" + default_value: 200 min: 0 max: 200 - name: failsafe_throttle description: "Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle)." - default_value: "1000" + default_value: 1000 min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: failsafe_throttle_low_delay description: "If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout" - default_value: "100" + default_value: 0 min: 0 max: 300 - name: failsafe_procedure @@ -753,28 +876,28 @@ groups: table: failsafe_procedure - name: failsafe_stick_threshold description: "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." - default_value: "50" + default_value: 50 field: failsafe_stick_motion_threshold min: 0 max: 500 - name: failsafe_fw_roll_angle description: "Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll" - default_value: "-200" + default_value: -200 min: -800 max: 800 - name: failsafe_fw_pitch_angle description: "Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb" - default_value: "100" + default_value: 100 min: -800 max: 800 - name: failsafe_fw_yaw_rate description: "Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn" - default_value: "-45" + default_value: -45 min: -1000 max: 1000 - name: failsafe_min_distance description: "If failsafe happens when craft is closer than this distance in centimeters from home, failsafe will not execute regular failsafe_procedure, but will execute procedure specified in failsafe_min_distance_procedure instead. 0 = Normal failsafe_procedure always taken." - default_value: "0" + default_value: 0 min: 0 max: 65000 - name: failsafe_min_distance_procedure @@ -783,7 +906,7 @@ groups: table: failsafe_procedure - name: failsafe_mission description: "If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode" - default_value: "ON" + default_value: ON type: bool - name: PG_LIGHTS_CONFIG @@ -793,18 +916,18 @@ groups: members: - name: failsafe_lights description: "Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]." - default_value: "ON" + default_value: ON field: failsafe.enabled type: bool - name: failsafe_lights_flash_period description: "Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]." - default_value: "1000" + default_value: 1000 field: failsafe.flash_period min: 40 max: 65535 - name: failsafe_lights_flash_on_time description: "Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]." - default_value: "100" + default_value: 100 field: failsafe.flash_on_time min: 20 max: 65535 @@ -815,19 +938,19 @@ groups: members: - name: align_board_roll description: "Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc" - default_value: "0" + default_value: :zero field: rollDeciDegrees min: -1800 max: 3600 - name: align_board_pitch description: "Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc" - default_value: "0" + default_value: :zero field: pitchDeciDegrees min: -1800 max: 3600 - name: align_board_yaw description: "Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc" - default_value: "0" + default_value: :zero field: yawDeciDegrees min: -1800 max: 3600 @@ -838,26 +961,27 @@ groups: members: - name: vbat_meter_type description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running" - default_value: "`ADC`" + condition: USE_ADC + default_value: ADC field: voltage.type table: voltage_sensor type: uint8_t - name: vbat_scale description: "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." - default_value: "1100" + default_value: :target field: voltage.scale condition: USE_ADC min: VBAT_SCALE_MIN max: VBAT_SCALE_MAX - name: current_meter_scale description: "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." - default_value: "400" + default_value: :target field: current.scale min: -10000 max: 10000 - name: current_meter_offset description: "This sets the output offset voltage of the current sensor in millivolts." - default_value: "0" + default_value: :target field: current.offset min: -32768 max: 32767 @@ -875,24 +999,24 @@ groups: type: uint8_t - name: cruise_power description: "Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit" - default_value: "0" + default_value: 0 field: cruise_power min: 0 max: 4294967295 - name: idle_power description: "Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit" - default_value: "0" + default_value: 0 field: idle_power min: 0 max: 65535 - name: rth_energy_margin description: "Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation" - default_value: "5" + default_value: 5 min: 0 max: 100 - name: thr_comp_weight description: "Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage)" - default_value: "0.692" + default_value: 1 field: throttle_compensation_weight min: 0 max: 2 @@ -903,55 +1027,55 @@ groups: value_type: BATTERY_CONFIG_VALUE members: - name: bat_cells - description: "Number of cells of the battery (0 = autodetect), see battery documentation" - default_value: "0" + description: "Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected." + default_value: 0 field: cells condition: USE_ADC min: 0 - max: 8 + max: 12 - name: vbat_cell_detect_voltage - description: "Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V" - default_value: "430" + description: "Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units." + default_value: 425 field: voltage.cellDetect condition: USE_ADC min: 100 max: 500 - name: vbat_max_cell_voltage description: "Maximum voltage per cell in 0.01V units, default is 4.20V" - default_value: "420" + default_value: 420 field: voltage.cellMax condition: USE_ADC min: 100 max: 500 - name: vbat_min_cell_voltage description: "Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V)" - default_value: "330" + default_value: 330 field: voltage.cellMin condition: USE_ADC min: 100 max: 500 - name: vbat_warning_cell_voltage description: "Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V)" - default_value: "350" + default_value: 350 field: voltage.cellWarning condition: USE_ADC min: 100 max: 500 - name: battery_capacity description: "Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity." - default_value: "0" + default_value: 0 field: capacity.value min: 0 max: 4294967295 - name: battery_capacity_warning description: "If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink." - default_value: "0" + default_value: 0 field: capacity.warning min: 0 max: 4294967295 - name: battery_capacity_critical description: "If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps." - default_value: "0" + default_value: 0 field: capacity.critical min: 0 max: 4294967295 @@ -967,29 +1091,29 @@ groups: members: - name: motor_direction_inverted description: "Use if you need to inverse yaw motor direction." - default_value: "OFF" + default_value: OFF field: motorDirectionInverted type: bool - name: platform_type description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" - default_value: "\"MULTIROTOR\"" + default_value: "MULTIROTOR" field: platformType type: uint8_t table: platform_type - name: has_flaps description: "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" - default_value: "OFF" + default_value: OFF field: hasFlaps type: bool - name: model_preview_type description: "ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons." - default_value: "-1" + default_value: -1 field: appliedMixerPreset min: -1 max: INT16_MAX - name: fw_min_throttle_down_pitch description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)" - default_value: "0" + default_value: 0 field: fwMinThrottleDownPitchAngle min: 0 max: 450 @@ -999,19 +1123,19 @@ groups: members: - name: 3d_deadband_low description: "Low value of throttle deadband for 3D mode (when stick is in the 3d_deadband_throttle range, the fixed values of 3d_deadband_low / _high are used instead)" - default_value: "1406" + default_value: 1406 field: deadband_low min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: 3d_deadband_high description: "High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead)" - default_value: "1514" + default_value: 1514 field: deadband_high min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: 3d_neutral description: "Neutral (stop) throttle value for 3D mode" - default_value: "1460" + default_value: 1460 field: neutral min: PWM_RANGE_MIN max: PWM_RANGE_MAX @@ -1027,30 +1151,30 @@ groups: table: servo_protocol - name: servo_center_pulse description: "Servo midpoint" - default_value: "1500" + default_value: 1500 field: servoCenterPulse min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: servo_pwm_rate description: "Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz." - default_value: "50" + default_value: 50 field: servoPwmRate min: 50 max: 498 - name: servo_lpf_hz description: "Selects the servo PWM output cutoff frequency. Value is in [Hz]" - default_value: "20" + default_value: 20 field: servo_lowpass_freq min: 0 max: 400 - name: flaperon_throw_offset description: "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." - default_value: "200" + default_value: 200 min: FLAPERON_THROW_MIN max: FLAPERON_THROW_MAX - name: tri_unarmed_servo description: "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." - default_value: "ON" + default_value: ON type: bool - name: PG_CONTROL_RATE_PROFILES @@ -1060,43 +1184,43 @@ groups: members: - name: thr_mid description: "Throttle value when the stick is set to mid-position. Used in the throttle curve calculation." - default_value: "50" + default_value: 50 field: throttle.rcMid8 min: 0 max: 100 - name: thr_expo description: "Throttle exposition value" - default_value: "0" + default_value: 0 field: throttle.rcExpo8 min: 0 max: 100 - name: tpa_rate description: "Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate." - default_value: "0" + default_value: 0 field: throttle.dynPID min: 0 max: CONTROL_RATE_CONFIG_TPA_MAX - name: tpa_breakpoint description: "See tpa_rate." - default_value: "1500" + default_value: 1500 field: throttle.pa_breakpoint min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: fw_tpa_time_constant description: "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" - default_value: "0" + default_value: 0 field: throttle.fixedWingTauMs min: 0 max: 5000 - name: rc_expo description: "Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`)" - default_value: "70" + default_value: 70 field: stabilized.rcExpo8 min: 0 max: 100 - name: rc_yaw_expo description: "Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`)" - default_value: "20" + default_value: 20 field: stabilized.rcYawExpo8 min: 0 max: 100 @@ -1104,49 +1228,49 @@ groups: # Rate 180 (1800dps) is max. value gyro can measure reliably - name: roll_rate description: "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." - default_value: "20" + default_value: 20 field: stabilized.rates[FD_ROLL] min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX - name: pitch_rate description: "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." - default_value: "20" + default_value: 20 field: stabilized.rates[FD_PITCH] min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX - name: yaw_rate description: "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." - default_value: "20" + default_value: 20 field: stabilized.rates[FD_YAW] min: CONTROL_RATE_CONFIG_YAW_RATE_MIN max: CONTROL_RATE_CONFIG_YAW_RATE_MAX - name: manual_rc_expo description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]" - default_value: "70" + default_value: 70 field: manual.rcExpo8 min: 0 max: 100 - name: manual_rc_yaw_expo description: "Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100]" - default_value: "20" + default_value: 20 field: manual.rcYawExpo8 min: 0 max: 100 - name: manual_roll_rate description: "Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]%" - default_value: "100" + default_value: 100 field: manual.rates[FD_ROLL] min: 0 max: 100 - name: manual_pitch_rate description: "Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]%" - default_value: "100" + default_value: 100 field: manual.rates[FD_PITCH] min: 0 max: 100 - name: manual_yaw_rate description: "Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]%" - default_value: "100" + default_value: 100 field: manual.rates[FD_YAW] min: 0 max: 100 @@ -1154,6 +1278,7 @@ groups: field: misc.fpvCamAngleDegrees min: 0 max: 50 + default_value: 0 - name: PG_SERIAL_CONFIG type: serialConfig_t @@ -1161,7 +1286,7 @@ groups: members: - name: reboot_character description: "Special character used to trigger reboot" - default_value: "82" + default_value: 82 min: 48 max: 126 @@ -1171,38 +1296,38 @@ groups: members: - name: imu_dcm_kp description: "Inertial Measurement Unit KP Gain for accelerometer measurements" - default_value: "2500" + default_value: 2500 field: dcm_kp_acc max: UINT16_MAX - name: imu_dcm_ki description: "Inertial Measurement Unit KI Gain for accelerometer measurements" - default_value: "50" + default_value: 50 field: dcm_ki_acc max: UINT16_MAX - name: imu_dcm_kp_mag description: "Inertial Measurement Unit KP Gain for compass measurements" - default_value: "10000" + default_value: 10000 field: dcm_kp_mag max: UINT16_MAX - name: imu_dcm_ki_mag description: "Inertial Measurement Unit KI Gain for compass measurements" - default_value: "0" + default_value: 0 field: dcm_ki_mag max: UINT16_MAX - name: small_angle description: "If the aircraft tilt angle exceed this value the copter will refuse to arm." - default_value: "25" + default_value: 25 min: 0 max: 180 - name: imu_acc_ignore_rate description: "Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes" - default_value: "0" + default_value: 0 field: acc_ignore_rate min: 0 max: 20 - name: imu_acc_ignore_slope description: "Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight)" - default_value: "0" + default_value: 0 field: acc_ignore_slope min: 0 max: 5 @@ -1212,18 +1337,24 @@ groups: members: - name: fixed_wing_auto_arm description: "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." - default_value: "OFF" + default_value: OFF type: bool - name: disarm_kill_switch description: "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." - default_value: "ON" + default_value: ON type: bool - name: switch_disarm_delay description: "Delay before disarming when requested by switch (ms) [0-1000]" - default_value: "250" + default_value: 250 field: switchDisarmDelayMs min: 0 max: 1000 + - name: prearm_timeout + description: "Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout." + default_value: 10000 + field: prearmTimeoutMs + min: 0 + max: 10000 - name: PG_GENERAL_SETTINGS headers: ["config/general_settings.h"] @@ -1231,7 +1362,7 @@ groups: members: - name: applied_defaults description: "Internal (configurator) hint. Should not be changed manually" - default_value: "0" + default_value: 0 field: appliedDefaults type: uint8_t min: 0 @@ -1244,26 +1375,26 @@ groups: members: - name: rpm_gyro_filter_enabled description: "Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV" - default_value: "`OFF`" + default_value: OFF field: gyro_filter_enabled type: bool - name: rpm_gyro_harmonics description: "Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine" - default_value: "1" + default_value: 1 field: gyro_harmonics type: uint8_t min: 1 max: 3 - name: rpm_gyro_min_hz description: "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`" - default_value: "150" + default_value: 100 field: gyro_min_hz type: uint8_t min: 30 max: 200 - name: rpm_gyro_q description: "Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting" - default_value: "500" + default_value: 500 field: gyro_q type: uint16_t min: 1 @@ -1292,22 +1423,22 @@ groups: type: uint8_t - name: gps_auto_config description: "Enable automatic configuration of UBlox GPS receivers." - default_value: "ON" + default_value: ON field: autoConfig type: bool - name: gps_auto_baud description: "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" - default_value: "ON" + default_value: ON field: autoBaud type: bool - name: gps_ublox_use_galileo description: "Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]." - default_value: "OFF" + default_value: OFF field: ubloxUseGalileo type: bool - name: gps_min_sats description: "Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count." - default_value: "6" + default_value: 6 field: gpsMinSats min: 5 max: 10 @@ -1318,27 +1449,27 @@ groups: members: - name: deadband description: "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." - default_value: "5" + default_value: 5 min: 0 max: 32 - name: yaw_deadband description: "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." - default_value: "5" + default_value: 5 min: 0 max: 100 - name: pos_hold_deadband description: "Stick deadband in [r/c points], applied after r/c deadband and expo" - default_value: "20" + default_value: 10 min: 2 max: 250 - name: alt_hold_deadband description: "Defines the deadband of throttle during alt_hold [r/c points]" - default_value: "50" + default_value: 50 min: 10 max: 250 - name: 3d_deadband_throttle description: "Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter." - default_value: "50" + default_value: 50 field: mid_throttle_deadband min: 0 max: 200 @@ -1349,7 +1480,7 @@ groups: table: airmodeHandlingType - name: airmode_throttle_threshold description: "Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used" - default_value: "1300" + default_value: 1300 field: airmodeThrottleThreshold min: 1000 max: 2000 @@ -1361,206 +1492,224 @@ groups: members: - name: mc_p_pitch description: "Multicopter rate stabilisation P-gain for PITCH" - default_value: "40" + default_value: 40 field: bank_mc.pid[PID_PITCH].P min: 0 max: 200 - name: mc_i_pitch description: "Multicopter rate stabilisation I-gain for PITCH" - default_value: "30" + default_value: 30 field: bank_mc.pid[PID_PITCH].I min: 0 max: 200 - name: mc_d_pitch description: "Multicopter rate stabilisation D-gain for PITCH" - default_value: "23" + default_value: 23 field: bank_mc.pid[PID_PITCH].D min: 0 max: 200 - name: mc_cd_pitch description: "Multicopter Control Derivative gain for PITCH" - default_value: "60" + default_value: 60 field: bank_mc.pid[PID_PITCH].FF min: 0 max: 200 - name: mc_p_roll description: "Multicopter rate stabilisation P-gain for ROLL" - default_value: "40" + default_value: 40 field: bank_mc.pid[PID_ROLL].P min: 0 max: 200 - name: mc_i_roll description: "Multicopter rate stabilisation I-gain for ROLL" - default_value: "30" + default_value: 30 field: bank_mc.pid[PID_ROLL].I min: 0 max: 200 - name: mc_d_roll description: "Multicopter rate stabilisation D-gain for ROLL" - default_value: "23" + default_value: 23 field: bank_mc.pid[PID_ROLL].D min: 0 max: 200 - name: mc_cd_roll description: "Multicopter Control Derivative gain for ROLL" - default_value: "60" + default_value: 60 field: bank_mc.pid[PID_ROLL].FF min: 0 max: 200 - name: mc_p_yaw description: "Multicopter rate stabilisation P-gain for YAW" - default_value: "85" + default_value: 85 field: bank_mc.pid[PID_YAW].P min: 0 max: 200 - name: mc_i_yaw description: "Multicopter rate stabilisation I-gain for YAW" - default_value: "45" + default_value: 45 field: bank_mc.pid[PID_YAW].I min: 0 max: 200 - name: mc_d_yaw description: "Multicopter rate stabilisation D-gain for YAW" - default_value: "0" + default_value: 0 field: bank_mc.pid[PID_YAW].D min: 0 max: 200 - name: mc_cd_yaw description: "Multicopter Control Derivative gain for YAW" - default_value: "60" + default_value: 60 field: bank_mc.pid[PID_YAW].FF min: 0 max: 200 - name: mc_p_level description: "Multicopter attitude stabilisation P-gain" - default_value: "20" + default_value: 20 field: bank_mc.pid[PID_LEVEL].P min: 0 max: 200 - name: mc_i_level description: "Multicopter attitude stabilisation low-pass filter cutoff" - default_value: "15" + default_value: 15 field: bank_mc.pid[PID_LEVEL].I min: 0 max: 200 - name: mc_d_level description: "Multicopter attitude stabilisation HORIZON transition point" - default_value: "75" + default_value: 75 field: bank_mc.pid[PID_LEVEL].D min: 0 max: 200 - name: fw_p_pitch description: "Fixed-wing rate stabilisation P-gain for PITCH" - default_value: "5" + default_value: 5 field: bank_fw.pid[PID_PITCH].P min: 0 max: 200 - name: fw_i_pitch description: "Fixed-wing rate stabilisation I-gain for PITCH" - default_value: "7" + default_value: 7 field: bank_fw.pid[PID_PITCH].I min: 0 max: 200 + - name: fw_d_pitch + description: "Fixed wing rate stabilisation D-gain for PITCH" + default_value: 0 + field: bank_fw.pid[PID_PITCH].D + min: 0 + max: 200 - name: fw_ff_pitch description: "Fixed-wing rate stabilisation FF-gain for PITCH" - default_value: "50" + default_value: 50 field: bank_fw.pid[PID_PITCH].FF min: 0 max: 200 - name: fw_p_roll description: "Fixed-wing rate stabilisation P-gain for ROLL" - default_value: "5" + default_value: 5 field: bank_fw.pid[PID_ROLL].P min: 0 max: 200 - name: fw_i_roll description: "Fixed-wing rate stabilisation I-gain for ROLL" - default_value: "7" + default_value: 7 field: bank_fw.pid[PID_ROLL].I min: 0 max: 200 + - name: fw_d_roll + description: "Fixed wing rate stabilisation D-gain for ROLL" + default_value: 0 + field: bank_fw.pid[PID_ROLL].D + min: 0 + max: 200 - name: fw_ff_roll description: "Fixed-wing rate stabilisation FF-gain for ROLL" - default_value: "50" + default_value: 50 field: bank_fw.pid[PID_ROLL].FF min: 0 max: 200 - name: fw_p_yaw description: "Fixed-wing rate stabilisation P-gain for YAW" - default_value: "6" + default_value: 6 field: bank_fw.pid[PID_YAW].P min: 0 max: 200 - name: fw_i_yaw description: "Fixed-wing rate stabilisation I-gain for YAW" - default_value: "10" + default_value: 10 field: bank_fw.pid[PID_YAW].I min: 0 max: 200 + - name: fw_d_yaw + description: "Fixed wing rate stabilisation D-gain for YAW" + default_value: 0 + field: bank_fw.pid[PID_YAW].D + min: 0 + max: 200 - name: fw_ff_yaw description: "Fixed-wing rate stabilisation FF-gain for YAW" - default_value: "60" + default_value: 60 field: bank_fw.pid[PID_YAW].FF min: 0 max: 200 - name: fw_p_level description: "Fixed-wing attitude stabilisation P-gain" - default_value: "20" + default_value: 20 field: bank_fw.pid[PID_LEVEL].P min: 0 max: 200 - name: fw_i_level description: "Fixed-wing attitude stabilisation low-pass filter cutoff" - default_value: "5" + default_value: 5 field: bank_fw.pid[PID_LEVEL].I min: 0 max: 200 - name: fw_d_level description: "Fixed-wing attitude stabilisation HORIZON transition point" - default_value: "75" + default_value: 75 field: bank_fw.pid[PID_LEVEL].D min: 0 max: 200 - name: max_angle_inclination_rll description: "Maximum inclination in level (angle) mode (ROLL axis). 100=10°" - default_value: "300" + default_value: 300 field: max_angle_inclination[FD_ROLL] min: 100 max: 900 - name: max_angle_inclination_pit description: "Maximum inclination in level (angle) mode (PITCH axis). 100=10°" - default_value: "300" + default_value: 300 field: max_angle_inclination[FD_PITCH] min: 100 max: 900 - name: dterm_lpf_hz description: "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" - default_value: "40" + default_value: 40 min: 0 max: 500 - name: dterm_lpf_type description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." - default_value: "`BIQUAD`" + default_value: "BIQUAD" field: dterm_lpf_type table: filter_type - name: dterm_lpf2_hz description: "Cutoff frequency for stage 2 D-term low pass filter" - default_value: "0" + default_value: 0 min: 0 max: 500 - name: dterm_lpf2_type description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." - default_value: "`BIQUAD`" + default_value: "BIQUAD" field: dterm_lpf2_type table: filter_type - name: yaw_lpf_hz description: "Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below)" - default_value: "30" + default_value: 0 min: 0 max: 200 - name: fw_iterm_throw_limit description: "Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely." - default_value: "165" + default_value: 165 field: fixedWingItermThrowLimit min: FW_ITERM_THROW_LIMIT_MIN max: FW_ITERM_THROW_LIMIT_MAX @@ -1572,126 +1721,134 @@ groups: table: direction - name: fw_reference_airspeed description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present." - default_value: "1000" + default_value: 1000 field: fixedWingReferenceAirspeed min: 1 max: 5000 - name: fw_turn_assist_yaw_gain description: "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" - default_value: "1" + default_value: 1 field: fixedWingCoordinatedYawGain min: 0 max: 2 - name: fw_turn_assist_pitch_gain description: "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" - default_value: "1" + default_value: 1 field: fixedWingCoordinatedPitchGain min: 0 max: 2 - name: fw_iterm_limit_stick_position description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side" - default_value: "0.5" + default_value: 0.5 field: fixedWingItermLimitOnStickPosition min: 0 max: 1 + - name: fw_yaw_iterm_freeze_bank_angle + description: "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." + default_value: 0 + field: fixedWingYawItermBankFreeze + min: 0 + max: 90 - name: pidsum_limit description: "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" - default_value: "500" + default_value: 500 field: pidSumLimit min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - name: pidsum_limit_yaw description: "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" - default_value: "400" + default_value: 350 field: pidSumLimitYaw min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - name: iterm_windup description: "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)" - default_value: "50" + default_value: 50 field: itermWindupPointPercent min: 0 max: 90 - name: rate_accel_limit_roll_pitch description: "Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting." - default_value: "0" + default_value: 0 field: axisAccelerationLimitRollPitch max: 500000 - name: rate_accel_limit_yaw description: "Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting." - default_value: "10000" + default_value: 10000 field: axisAccelerationLimitYaw max: 500000 - name: heading_hold_rate_limit description: "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." - default_value: "90" min: HEADING_HOLD_RATE_LIMIT_MIN max: HEADING_HOLD_RATE_LIMIT_MAX + default_value: 90 - name: nav_mc_pos_z_p description: "P gain of altitude PID controller (Multirotor)" - default_value: "50" field: bank_mc.pid[PID_POS_Z].P condition: USE_NAV min: 0 max: 255 + default_value: 50 - name: nav_mc_vel_z_p description: "P gain of velocity PID controller" - default_value: "100" field: bank_mc.pid[PID_VEL_Z].P condition: USE_NAV min: 0 max: 255 + default_value: 100 - name: nav_mc_vel_z_i description: "I gain of velocity PID controller" - default_value: "50" field: bank_mc.pid[PID_VEL_Z].I condition: USE_NAV min: 0 max: 255 + default_value: 50 - name: nav_mc_vel_z_d description: "D gain of velocity PID controller" - default_value: "10" + default_value: 10 field: bank_mc.pid[PID_VEL_Z].D condition: USE_NAV min: 0 max: 255 + default_value: 10 - name: nav_mc_pos_xy_p description: "Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity" - default_value: "65" field: bank_mc.pid[PID_POS_XY].P condition: USE_NAV min: 0 max: 255 + default_value: 65 - name: nav_mc_vel_xy_p description: "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" - default_value: "40" field: bank_mc.pid[PID_VEL_XY].P condition: USE_NAV min: 0 max: 255 + default_value: 40 - name: nav_mc_vel_xy_i description: "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" - default_value: "15" field: bank_mc.pid[PID_VEL_XY].I condition: USE_NAV min: 0 max: 255 + default_value: 15 - name: nav_mc_vel_xy_d description: "D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target." - default_value: "100" field: bank_mc.pid[PID_VEL_XY].D condition: USE_NAV min: 0 max: 255 + default_value: 100 - name: nav_mc_vel_xy_ff field: bank_mc.pid[PID_VEL_XY].FF condition: USE_NAV min: 0 max: 255 + default_value: 40 - name: nav_mc_heading_p description: "P gain of Heading Hold controller (Multirotor)" - default_value: "60" + default_value: 60 field: bank_mc.pid[PID_HEADING].P condition: USE_NAV min: 0 @@ -1700,139 +1857,145 @@ groups: field: navVelXyDTermLpfHz min: 0 max: 100 + default_value: 2 - name: nav_mc_vel_xy_dterm_attenuation description: "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." - default_value: "90" field: navVelXyDtermAttenuation min: 0 max: 100 + default_value: 90 - name: nav_mc_vel_xy_dterm_attenuation_start description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins" - default_value: 10" + default_value: 10 field: navVelXyDtermAttenuationStart min: 0 max: 100 - name: nav_mc_vel_xy_dterm_attenuation_end description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum" - default_value: "60" + default_value: 60 field: navVelXyDtermAttenuationEnd min: 0 max: 100 - name: nav_fw_pos_z_p description: "P gain of altitude PID controller (Fixedwing)" - default_value: "50" + default_value: 40 field: bank_fw.pid[PID_POS_Z].P condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_z_i description: "I gain of altitude PID controller (Fixedwing)" - default_value: "0" + default_value: 5 field: bank_fw.pid[PID_POS_Z].I condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_z_d description: "D gain of altitude PID controller (Fixedwing)" - default_value: "0" + default_value: 10 field: bank_fw.pid[PID_POS_Z].D condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_xy_p description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH" - default_value: "75" + default_value: 75 field: bank_fw.pid[PID_POS_XY].P condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_xy_i description: "I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero" - default_value: "5" + default_value: 5 field: bank_fw.pid[PID_POS_XY].I condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_xy_d description: "D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero" - default_value: "8" + default_value: 8 field: bank_fw.pid[PID_POS_XY].D condition: USE_NAV min: 0 max: 255 - name: nav_fw_heading_p description: "P gain of Heading Hold controller (Fixedwing)" - default_value: "60" + default_value: 60 field: bank_fw.pid[PID_HEADING].P condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_hdg_p description: "P gain of heading PID controller. (Fixedwing, rovers, boats)" - default_value: "60" + default_value: 30 field: bank_fw.pid[PID_POS_HEADING].P condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_hdg_i description: "I gain of heading trajectory PID controller. (Fixedwing, rovers, boats)" - default_value: "0" + default_value: 2 field: bank_fw.pid[PID_POS_HEADING].I condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_hdg_d description: "D gain of heading trajectory PID controller. (Fixedwing, rovers, boats)" - default_value: "0" + default_value: 0 field: bank_fw.pid[PID_POS_HEADING].D condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_hdg_pidsum_limit description: "Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats)" - default_value: "350" + default_value: 350 field: navFwPosHdgPidsumLimit min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - name: mc_iterm_relax field: iterm_relax table: iterm_relax + default_value: RP - name: mc_iterm_relax_cutoff field: iterm_relax_cutoff min: 1 max: 100 + default_value: 15 - name: d_boost_factor field: dBoostFactor condition: USE_D_BOOST min: 1 max: 3 + default_value: 1.25 - name: d_boost_max_at_acceleration field: dBoostMaxAtAlleceleration condition: USE_D_BOOST min: 1000 max: 16000 + default_value: 7500 - name: d_boost_gyro_delta_lpf_hz field: dBoostGyroDeltaLpfHz condition: USE_D_BOOST min: 10 max: 250 + default_value: 80 - name: antigravity_gain description: "Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allowed to double during rapid throttle movements" - default_value: "1" + default_value: 1 field: antigravityGain condition: USE_ANTIGRAVITY min: 1 max: 20 - name: antigravity_accelerator description: "" - default_value: "1" + default_value: 1 field: antigravityAccelerator condition: USE_ANTIGRAVITY min: 1 max: 20 - name: antigravity_cutoff_lpf_hz description: "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" - default_value: "15" + default_value: 15 field: antigravityCutoff condition: USE_ANTIGRAVITY min: 1 @@ -1841,42 +2004,43 @@ groups: description: "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`" field: pidControllerType table: pidTypeTable + default_value: AUTO - name: mc_cd_lpf_hz description: "Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky" - default_value: "30" + default_value: 30 field: controlDerivativeLpfHz min: 0 max: 200 - name: setpoint_kalman_enabled description: "Enable Kalman filter on the PID controller setpoint" - default_value: "OFF" + default_value: OFF condition: USE_GYRO_KALMAN field: kalmanEnabled type: bool - name: setpoint_kalman_q description: "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" - default_value: "100" + default_value: 100 field: kalman_q condition: USE_GYRO_KALMAN min: 1 max: 16000 - name: setpoint_kalman_w description: "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" - default_value: "4" + default_value: 4 field: kalman_w condition: USE_GYRO_KALMAN min: 1 max: 40 - name: setpoint_kalman_sharpness description: "Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets" - default_value: "100" + default_value: 100 field: kalman_sharpness condition: USE_GYRO_KALMAN min: 1 max: 16000 - name: fw_level_pitch_trim description: "Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level" - default_value: "0" + default_value: 0 field: fixedWingLevelTrim min: -10 max: 10 @@ -1887,31 +2051,31 @@ groups: members: - name: fw_autotune_overshoot_time description: "Time [ms] to detect sustained overshoot" - default_value: "100" + default_value: 100 field: fw_overshoot_time min: 50 max: 500 - name: fw_autotune_undershoot_time description: "Time [ms] to detect sustained undershoot" - default_value: "200" + default_value: 200 field: fw_undershoot_time min: 50 max: 500 - name: fw_autotune_threshold description: "Threshold [%] of max rate to consider overshoot/undershoot detection" - default_value: "50" + default_value: 50 field: fw_max_rate_threshold min: 0 max: 100 - name: fw_autotune_ff_to_p_gain description: "FF to P gain (strength relationship) [%]" - default_value: "10" + default_value: 10 field: fw_ff_to_p_gain min: 0 max: 100 - name: fw_autotune_ff_to_i_tc description: "FF to I time (defines time for I to reach the same level of response as FF) [ms]" - default_value: "600" + default_value: 600 field: fw_ff_to_i_time_constant min: 100 max: 5000 @@ -1922,26 +2086,27 @@ groups: members: - name: inav_auto_mag_decl description: "Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored." - default_value: "ON" + default_value: ON field: automatic_mag_declination type: bool - name: inav_gravity_cal_tolerance description: "Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value." - default_value: "5" + default_value: 5 field: gravity_calibration_tolerance min: 0 max: 255 - name: inav_use_gps_velned description: "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." - default_value: "ON" + default_value: ON field: use_gps_velned type: bool - name: inav_use_gps_no_baro field: use_gps_no_baro type: bool + default_value: OFF - name: inav_allow_dead_reckoning description: "Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation" - default_value: "OFF" + default_value: OFF field: allow_dead_reckoning type: bool - name: inav_reset_altitude @@ -1956,7 +2121,7 @@ groups: table: reset_type - name: inav_max_surface_altitude description: "Max allowed altitude for surface following mode. [cm]" - default_value: "200" + default_value: 200 field: max_surface_altitude min: 0 max: 1000 @@ -1964,57 +2129,61 @@ groups: field: w_z_surface_p min: 0 max: 100 + default_value: 3.5 - name: inav_w_z_surface_v field: w_z_surface_v min: 0 max: 100 + default_value: 6.1 - name: inav_w_xy_flow_p field: w_xy_flow_p min: 0 max: 100 + default_value: 1.0 - name: inav_w_xy_flow_v field: w_xy_flow_v min: 0 max: 100 + default_value: 2.0 - name: inav_w_z_baro_p description: "Weight of barometer measurements in estimated altitude and climb rate" - default_value: "0.350" field: w_z_baro_p min: 0 max: 10 + default_value: 0.35 - name: inav_w_z_gps_p description: "Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes" - default_value: "0.200" field: w_z_gps_p min: 0 max: 10 + default_value: 0.2 - name: inav_w_z_gps_v description: "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" - default_value: "0.500" field: w_z_gps_v min: 0 max: 10 + default_value: 0.1 - name: inav_w_xy_gps_p description: "Weight of GPS coordinates in estimated UAV position and speed." - default_value: "1.000" + default_value: 1.0 field: w_xy_gps_p min: 0 max: 10 - name: inav_w_xy_gps_v description: "Weight of GPS velocity data in estimated UAV speed" - default_value: "2.000" + default_value: 2.0 field: w_xy_gps_v min: 0 max: 10 - name: inav_w_z_res_v description: "Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost" - default_value: "0.500" + default_value: 0.5 field: w_z_res_v min: 0 max: 10 - name: inav_w_xy_res_v description: "Decay coefficient for estimated velocity when GPS reference for position is lost" - default_value: "0.500" + default_value: 0.5 field: w_xy_res_v min: 0 max: 10 @@ -2022,21 +2191,22 @@ groups: field: w_xyz_acc_p min: 0 max: 1 + default_value: 1.0 - name: inav_w_acc_bias description: "Weight for accelerometer drift estimation" - default_value: "0.010" + default_value: 0.01 field: w_acc_bias min: 0 max: 1 - name: inav_max_eph_epv description: "Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm]" - default_value: "1000.000" + default_value: 1000 field: max_eph_epv min: 0 max: 9999 - name: inav_baro_epv description: "Uncertainty value for barometric sensor [cm]" - default_value: "100.000" + default_value: 100 field: baro_epv min: 0 max: 9999 @@ -2048,12 +2218,12 @@ groups: members: - name: nav_disarm_on_landing description: "If set to ON, iNav disarms the FC after landing" - default_value: "OFF" + default_value: OFF field: general.flags.disarm_on_landing type: bool - name: nav_use_midthr_for_althold description: "If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude" - default_value: "OFF" + default_value: OFF field: general.flags.use_thr_mid_for_althold type: bool - name: nav_extra_arming_safety @@ -2068,72 +2238,72 @@ groups: table: nav_user_control_mode - name: nav_position_timeout description: "If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable)" - default_value: "5" + default_value: 5 field: general.pos_failure_timeout min: 0 max: 10 - name: nav_wp_radius description: "Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius" - default_value: "100" + default_value: 100 field: general.waypoint_radius min: 10 max: 10000 - name: nav_wp_safe_distance description: "First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check." - default_value: "10000" + default_value: 10000 field: general.waypoint_safe_distance max: 65000 - name: nav_auto_speed description: "Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only]" - default_value: "300" + default_value: 300 field: general.max_auto_speed min: 10 max: 2000 - name: nav_auto_climb_rate description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]" - default_value: "500" + default_value: 500 field: general.max_auto_climb_rate min: 10 max: 2000 - name: nav_manual_speed description: "Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]" - default_value: "500" + default_value: 500 field: general.max_manual_speed min: 10 max: 2000 - name: nav_manual_climb_rate description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" - default_value: "200" + default_value: 200 field: general.max_manual_climb_rate min: 10 max: 2000 - name: nav_landing_speed description: "Vertical descent velocity during the RTH landing phase. [cm/s]" - default_value: "200" + default_value: 200 field: general.land_descent_rate min: 100 max: 2000 - name: nav_land_slowdown_minalt description: "Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm]" - default_value: "500" + default_value: 500 field: general.land_slowdown_minalt min: 50 max: 1000 - name: nav_land_slowdown_maxalt description: "Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm]" - default_value: "2000" + default_value: 2000 field: general.land_slowdown_maxalt min: 500 max: 4000 - name: nav_emerg_landing_speed description: "Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s]" - default_value: "500" + default_value: 500 field: general.emerg_descent_rate min: 100 max: 2000 - name: nav_min_rth_distance description: "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." - default_value: "500" + default_value: 500 field: general.min_rth_distance min: 0 max: 5000 @@ -2144,17 +2314,17 @@ groups: table: nav_overrides_motor_stop - name: nav_rth_climb_first description: "If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way." - default_value: "ON" + default_value: ON field: general.flags.rth_climb_first type: bool - name: nav_rth_climb_ignore_emerg description: "If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status." - default_value: "OFF" + default_value: OFF field: general.flags.rth_climb_ignore_emerg type: bool - name: nav_rth_tail_first description: "If set to ON drone will return tail-first. Obviously meaningless for airplanes." - default_value: "OFF" + default_value: OFF field: general.flags.rth_tail_first type: bool - name: nav_rth_allow_landing @@ -2174,192 +2344,200 @@ groups: type: bool - name: nav_rth_abort_threshold description: "RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm]" - default_value: "50000" + default_value: 50000 field: general.rth_abort_threshold max: 65000 - name: nav_max_terrain_follow_alt field: general.max_terrain_follow_altitude + default_value: "100" + description: "Max allowed above the ground altitude for terrain following mode" max: 1000 + default_value: 100 - name: nav_rth_altitude description: "Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters)" - default_value: "1000" + default_value: 1000 field: general.rth_altitude max: 65000 - name: nav_rth_home_altitude description: "Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm]" - default_value: "0" + default_value: 0 field: general.rth_home_altitude max: 65000 - name: safehome_max_distance description: "In order for a safehome to be used, it must be less than this distance (in cm) from the arming point." - default_value: "20000" + default_value: 20000 field: general.safehome_max_distance min: 0 max: 65000 - name: nav_mc_bank_angle description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude" - default_value: "30" + default_value: 30 field: mc.max_bank_angle min: 15 max: 45 - name: nav_mc_hover_thr description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering." - default_value: "1500" + default_value: 1500 field: mc.hover_throttle min: 1000 max: 2000 - name: nav_mc_auto_disarm_delay description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)" - default_value: "2000" + default_value: 2000 field: mc.auto_disarm_delay min: 100 max: 10000 - name: nav_mc_braking_speed_threshold description: "min speed in cm/s above which braking can happen" - default_value: "100" + default_value: 100 field: mc.braking_speed_threshold condition: USE_MR_BRAKING_MODE min: 0 max: 1000 - name: nav_mc_braking_disengage_speed description: "braking is disabled when speed goes below this value" - default_value: "75" + default_value: 75 field: mc.braking_disengage_speed condition: USE_MR_BRAKING_MODE min: 0 max: 1000 - name: nav_mc_braking_timeout description: "timeout in ms for braking" - default_value: "2000" + default_value: 2000 field: mc.braking_timeout condition: USE_MR_BRAKING_MODE min: 100 max: 5000 - name: nav_mc_braking_boost_factor description: "acceleration factor for BOOST phase" - default_value: "100" + default_value: 100 field: mc.braking_boost_factor condition: USE_MR_BRAKING_MODE min: 0 max: 200 - name: nav_mc_braking_boost_timeout description: "how long in ms BOOST phase can happen" - default_value: "750" + default_value: 750 field: mc.braking_boost_timeout condition: USE_MR_BRAKING_MODE min: 0 max: 5000 - name: nav_mc_braking_boost_speed_threshold description: "BOOST can be enabled when speed is above this value" - default_value: "150" + default_value: 150 field: mc.braking_boost_speed_threshold condition: USE_MR_BRAKING_MODE min: 100 max: 1000 - name: nav_mc_braking_boost_disengage_speed description: "BOOST will be disabled when speed goes below this value" - default_value: "100" + default_value: 100 field: mc.braking_boost_disengage_speed condition: USE_MR_BRAKING_MODE min: 0 max: 1000 - name: nav_mc_braking_bank_angle description: "max angle that MR is allowed to bank in BOOST mode" - default_value: "40" + default_value: 40 field: mc.braking_bank_angle condition: USE_MR_BRAKING_MODE min: 15 max: 60 - name: nav_mc_pos_deceleration_time description: "Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting" - default_value: "120" + default_value: 120 field: mc.posDecelerationTime condition: USE_NAV min: 0 max: 255 - name: nav_mc_pos_expo description: "Expo for PosHold control" - default_value: "10" + default_value: 10 field: mc.posResponseExpo condition: USE_NAV min: 0 max: 255 + - name: nav_mc_wp_slowdown + description: "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." + default_value: ON + field: mc.slowDownForTurning + type: bool - name: nav_fw_cruise_thr description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )" - default_value: "1400" + default_value: 1400 field: fw.cruise_throttle min: 1000 max: 2000 - name: nav_fw_min_thr description: "Minimum throttle for flying wing in GPS assisted modes" - default_value: "1200" + default_value: 1200 field: fw.min_throttle min: 1000 max: 2000 - name: nav_fw_max_thr description: "Maximum throttle for flying wing in GPS assisted modes" - default_value: "1700" + default_value: 1700 field: fw.max_throttle min: 1000 max: 2000 - name: nav_fw_bank_angle description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll" - default_value: "20" + default_value: 35 field: fw.max_bank_angle min: 5 max: 80 - name: nav_fw_climb_angle description: "Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit" - default_value: "20" + default_value: 20 field: fw.max_climb_angle min: 5 max: 80 - name: nav_fw_dive_angle description: "Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit" - default_value: "15" + default_value: 15 field: fw.max_dive_angle min: 5 max: 80 - name: nav_fw_pitch2thr description: "Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr)" - default_value: "10" + default_value: 10 field: fw.pitch_to_throttle min: 0 max: 100 - name: nav_fw_pitch2thr_smoothing description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh." - default_value: "6" + default_value: 6 field: fw.pitch_to_throttle_smooth min: 0 max: 9 - name: nav_fw_pitch2thr_threshold description: "Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]" - default_value: "50" + default_value: 50 field: fw.pitch_to_throttle_thresh min: 0 max: 900 - name: nav_fw_loiter_radius description: "PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]" - default_value: "7500" + default_value: 7500 field: fw.loiter_radius min: 0 - max: 10000 + max: 30000 - name: nav_fw_cruise_speed description: "Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s" - default_value: "0" + default_value: 0 field: fw.cruise_speed min: 0 max: 65535 - name: nav_fw_control_smoothness description: "How smoothly the autopilot controls the airplane to correct the navigation error" - default_value: "0" + default_value: 0 field: fw.control_smoothness min: 0 max: 9 - name: nav_fw_land_dive_angle description: "Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees" - default_value: "2" + default_value: 2 field: fw.land_dive_angle condition: NAV_FIXED_WING_LANDING min: -20 @@ -2367,100 +2545,100 @@ groups: - name: nav_fw_launch_velocity description: "Forward velocity threshold for swing-launch detection [cm/s]" - default_value: "300" + default_value: 300 field: fw.launch_velocity_thresh min: 100 max: 10000 - name: nav_fw_launch_accel description: "Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s" - default_value: "1863" + default_value: 1863 field: fw.launch_accel_thresh min: 1000 max: 20000 - name: nav_fw_launch_max_angle description: "Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]" - default_value: "45" + default_value: 45 field: fw.launch_max_angle min: 5 max: 180 - name: nav_fw_launch_detect_time description: "Time for which thresholds have to breached to consider launch happened [ms]" - default_value: "40" + default_value: 40 field: fw.launch_time_thresh min: 10 max: 1000 - name: nav_fw_launch_thr description: "Launch throttle - throttle to be set during launch sequence (pwm units)" - default_value: "1700" + default_value: 1700 field: fw.launch_throttle min: 1000 max: 2000 - name: nav_fw_launch_idle_thr description: "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)" - default_value: "1000" + default_value: 1000 field: fw.launch_idle_throttle min: 1000 max: 2000 - name: nav_fw_launch_motor_delay description: "Delay between detected launch and launch sequence start and throttling up (ms)" - default_value: "500" + default_value: 500 field: fw.launch_motor_timer min: 0 max: 5000 - name: nav_fw_launch_spinup_time description: "Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller" - default_value: "100" + default_value: 100 field: fw.launch_motor_spinup_time min: 0 max: 1000 - name: nav_fw_launch_end_time description: "Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms]" - default_value: "2000" + default_value: 3000 field: fw.launch_end_time min: 0 max: 5000 - name: nav_fw_launch_min_time description: "Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]." - default_value: "0" + default_value: 0 field: fw.launch_min_time min: 0 max: 60000 - name: nav_fw_launch_timeout description: "Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms)" - default_value: "5000" + default_value: 5000 field: fw.launch_timeout max: 60000 - name: nav_fw_launch_max_altitude description: "Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]." - default_value: "0" + default_value: 0 field: fw.launch_max_altitude min: 0 max: 60000 - name: nav_fw_launch_climb_angle description: "Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit" - default_value: "18" + default_value: 18 field: fw.launch_climb_angle min: 5 max: 45 - name: nav_fw_cruise_yaw_rate description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]" - default_value: "20" + default_value: 20 field: fw.cruise_yaw_rate min: 0 max: 60 - name: nav_fw_allow_manual_thr_increase description: "Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing" - default_value: "OFF" + default_value: OFF field: fw.allow_manual_thr_increase type: bool - name: nav_use_fw_yaw_control description: "Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats" - default_value: "OFF" + default_value: OFF field: fw.useFwNavYawControl type: bool - name: nav_fw_yaw_deadband description: "Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course" - default_value: "0" + default_value: 0 field: fw.yawControlDeadband min: 0 max: 90 @@ -2472,27 +2650,27 @@ groups: members: - name: telemetry_switch description: "Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed." - default_value: "OFF" + default_value: OFF type: bool - name: telemetry_inverted description: "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." - default_value: "OFF" + default_value: OFF type: bool - name: frsky_default_latitude description: "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." - default_value: "0.000" + default_value: 0 field: gpsNoFixLatitude min: -90 max: 90 - name: frsky_default_longitude description: "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." - default_value: "0.000" + default_value: 0 field: gpsNoFixLongitude min: -180 max: 180 - name: frsky_coordinates_format description: "D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA" - default_value: "0" + default_value: 0 field: frsky_coordinate_format min: 0 max: FRSKY_FORMAT_NMEA @@ -2504,26 +2682,26 @@ groups: type: uint8_t - name: frsky_vfas_precision description: "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" - default_value: "0" + default_value: 0 min: FRSKY_VFAS_PRECISION_LOW max: FRSKY_VFAS_PRECISION_HIGH - name: frsky_pitch_roll description: "S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data" - default_value: "OFF" + default_value: OFF type: bool - name: report_cell_voltage description: "S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON" - default_value: "OFF" + default_value: OFF type: bool - name: hott_alarm_sound_interval description: "Battery alarm delay in seconds for Hott telemetry" - default_value: "5" + default_value: 5 field: hottAlarmSoundInterval min: 0 max: 120 - name: telemetry_halfduplex description: "S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details" - default_value: "OFF" + default_value: ON field: halfDuplex type: bool - name: smartport_fuel_unit @@ -2535,7 +2713,7 @@ groups: table: smartport_fuel_unit - name: ibus_telemetry_type description: "Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details." - default_value: "0" + default_value: 0 field: ibusTelemetryType min: 0 max: 255 @@ -2547,36 +2725,31 @@ groups: table: ltm_rates - name: sim_ground_station_number description: "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." - default_value: "Empty string" + default_value: "" field: simGroundStationNumber condition: USE_TELEMETRY_SIM - type: string - max: 16 - name: sim_pin description: "PIN code for the SIM module" - default_value: "Empty string" + default_value: "0000" field: simPin condition: USE_TELEMETRY_SIM - type: string - max: 8 - name: sim_transmit_interval description: "Text message transmission interval in seconds for SIM module. Minimum value: 10" - default_value: "60" + default_value: 60 field: simTransmitInterval condition: USE_TELEMETRY_SIM type: uint16_t min: SIM_MIN_TRANSMIT_INTERVAL max: 65535 - name: sim_transmit_flags - description: "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" - default_value: "F" + description: "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`" + default_value: :SIM_TX_FLAG_FAILSAFE field: simTransmitFlags condition: USE_TELEMETRY_SIM - type: string - max: SIM_N_TX_FLAGS + max: 63 - name: acc_event_threshold_high description: "Acceleration threshold [cm/s/s] for impact / high g event text messages sent by SIM module. Acceleration values greater than 4 g can occur in fixed wing flight without an impact, so a setting of 4000 or greater is suggested. 0 = detection off." - default_value: "0" + default_value: 0 field: accEventThresholdHigh condition: USE_TELEMETRY_SIM type: uint16_t @@ -2584,7 +2757,7 @@ groups: max: 65535 - name: acc_event_threshold_low description: "Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. A setting of less than 100 is suggested. Valid values: [0-900], 0 = detection off." - default_value: "0" + default_value: 0 field: accEventThresholdLow condition: USE_TELEMETRY_SIM type: uint16_t @@ -2592,7 +2765,7 @@ groups: max: 900 - name: acc_event_threshold_neg_x description: "Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. Suggested value for fixed wing: 1100. 0 = detection off." - default_value: "0" + default_value: 0 field: accEventThresholdNegX condition: USE_TELEMETRY_SIM type: uint16_t @@ -2600,7 +2773,7 @@ groups: max: 65535 - name: sim_low_altitude description: "Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`." - default_value: "0" + default_value: -32767 field: simLowAltitude condition: USE_TELEMETRY_SIM type: int16_t @@ -2611,31 +2784,37 @@ groups: type: uint8_t min: 0 max: 255 + default_value: 2 - name: mavlink_rc_chan_rate field: mavlink.rc_channels_rate type: uint8_t min: 0 max: 255 + default_value: 5 - name: mavlink_pos_rate field: mavlink.position_rate type: uint8_t min: 0 max: 255 + default_value: 2 - name: mavlink_extra1_rate field: mavlink.extra1_rate type: uint8_t min: 0 max: 255 + default_value: 10 - name: mavlink_extra2_rate field: mavlink.extra2_rate type: uint8_t min: 0 max: 255 + default_value: 2 - name: mavlink_extra3_rate field: mavlink.extra3_rate type: uint8_t min: 0 max: 255 + default_value: 1 - name: PG_ELERES_CONFIG type: eleresConfig_t headers: ["rx/eleres.h"] @@ -2645,27 +2824,34 @@ groups: field: eleresFreq min: 415 max: 450 + default_value: 435 - name: eleres_telemetry_en field: eleresTelemetryEn type: bool + default_value: OFF - name: eleres_telemetry_power field: eleresTelemetryPower min: 0 max: 7 + default_value: 7 - name: eleres_loc_en field: eleresLocEn type: bool + default_value: OFF - name: eleres_loc_power field: eleresLocPower min: 0 max: 7 + default_value: 7 - name: eleres_loc_delay field: eleresLocDelay min: 30 max: 1800 + default_value: 240 - name: eleres_signature field: eleresSignature max: UINT32_MAX + default_value: :zero - name: PG_LED_STRIP_CONFIG type: ledStripConfig_t @@ -2674,7 +2860,7 @@ groups: members: - name: ledstrip_visual_beeper description: "" - default_value: "OFF" + default_value: OFF type: bool - name: PG_OSD_CONFIG @@ -2690,7 +2876,7 @@ groups: type: uint8_t - name: osd_row_shiftdown description: "Number of rows to shift the OSD display (increase if top rows are cut off)" - default_value: "0" + default_value: 0 field: row_shiftdown min: 0 max: 1 @@ -2709,92 +2895,92 @@ groups: - name: osd_rssi_alarm description: "Value below which to make the OSD RSSI indicator blink" - default_value: "20" + default_value: 20 field: rssi_alarm min: 0 max: 100 - name: osd_time_alarm description: "Value above which to make the OSD flight time indicator blink (minutes)" - default_value: "10" + default_value: 10 field: time_alarm min: 0 max: 600 - name: osd_alt_alarm description: "Value above which to make the OSD relative altitude indicator blink (meters)" - default_value: "100" + default_value: 100 field: alt_alarm min: 0 max: 10000 - name: osd_dist_alarm description: "Value above which to make the OSD distance from home indicator blink (meters)" - default_value: "1000" + default_value: 1000 field: dist_alarm min: 0 max: 50000 - name: osd_neg_alt_alarm description: "Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters)" - default_value: "5" + default_value: 5 field: neg_alt_alarm min: 0 max: 10000 - name: osd_current_alarm description: "Value above which the OSD current consumption element will start blinking. Measured in full Amperes." - default_value: "0" + default_value: 0 field: current_alarm min: 0 max: 255 - name: osd_gforce_alarm description: "Value above which the OSD g force indicator will blink (g)" - default_value: "5" + default_value: 5 field: gforce_alarm min: 0 max: 20 - name: osd_gforce_axis_alarm_min description: "Value under which the OSD axis g force indicators will blink (g)" - default_value: "-5" + default_value: -5 field: gforce_axis_alarm_min min: -20 max: 20 - name: osd_gforce_axis_alarm_max description: "Value above which the OSD axis g force indicators will blink (g)" - default_value: "5" + default_value: 5 field: gforce_axis_alarm_max min: -20 max: 20 - name: osd_imu_temp_alarm_min description: "Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "-200" + default_value: -200 field: imu_temp_alarm_min min: -550 max: 1250 - name: osd_imu_temp_alarm_max description: "Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "600" + default_value: 600 field: imu_temp_alarm_max min: -550 max: 1250 - name: osd_esc_temp_alarm_max description: "Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "900" + default_value: 900 field: esc_temp_alarm_max min: -550 max: 1500 - name: osd_esc_temp_alarm_min description: "Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "-200" + default_value: -200 field: esc_temp_alarm_min min: -550 max: 1500 - name: osd_baro_temp_alarm_min description: "Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "-200" + default_value: -200 field: baro_temp_alarm_min condition: USE_BARO min: -550 max: 1250 - name: osd_baro_temp_alarm_max description: "Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "600" + default_value: 600 field: baro_temp_alarm_max condition: USE_BARO min: -550 @@ -2802,14 +2988,14 @@ groups: - name: osd_snr_alarm condition: USE_SERIALRX_CRSF description: "Value below which Crossfire SNR Alarm pops-up. (dB)" - default_value: "4" + default_value: 4 field: snr_alarm - min: -12 - max: 8 + min: -20 + max: 10 - name: osd_link_quality_alarm condition: USE_SERIALRX_CRSF description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%" - default_value: "70" + default_value: 70 field: link_quality_alarm min: 0 max: 100 @@ -2821,15 +3007,17 @@ groups: table: osd_alignment type: uint8_t - - name: osd_artificial_horizon_reverse_roll + - name: osd_ahi_reverse_roll field: ahi_reverse_roll type: bool - - name: osd_artificial_horizon_max_pitch + default_value: OFF + - name: osd_ahi_max_pitch description: "Max pitch, in degrees, for OSD artificial horizon" - default_value: "20" + default_value: 20 field: ahi_max_pitch min: 10 max: 90 + default_value: 20 - name: osd_crosshairs_style description: "To set the visual type for the crosshair" default_value: "DEFAULT" @@ -2844,77 +3032,77 @@ groups: type: uint8_t - name: osd_horizon_offset description: "To vertically adjust the whole OSD and AHI and scrolling bars" - default_value: "0" + default_value: 0 field: horizon_offset min: -2 max: 2 - name: osd_camera_uptilt description: "Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal" - default_value: "0" + default_value: 0 field: camera_uptilt min: -40 max: 80 - name: osd_camera_fov_h description: "Horizontal field of view for the camera in degres" - default_value: "135" + default_value: 135 field: camera_fov_h min: 60 max: 150 - name: osd_camera_fov_v description: "Vertical field of view for the camera in degres" - default_value: "85" + default_value: 85 field: camera_fov_v min: 30 max: 120 - name: osd_hud_margin_h description: "Left and right margins for the hud area" - default_value: "3" + default_value: 3 field: hud_margin_h min: 0 max: 4 - name: osd_hud_margin_v description: "Top and bottom margins for the hud area" - default_value: "3" + default_value: 3 field: hud_margin_v min: 1 max: 3 - name: osd_hud_homing description: "To display little arrows around the crossair showing where the home point is in the hud" - default_value: "0" + default_value: OFF field: hud_homing type: bool - name: osd_hud_homepoint description: "To 3D-display the home point location in the hud" - default_value: "0" + default_value: OFF field: hud_homepoint type: bool - name: osd_hud_radar_disp description: "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" - default_value: "0" + default_value: 0 field: hud_radar_disp min: 0 max: 4 - name: osd_hud_radar_range_min description: "In meters, radar aircrafts closer than this will not be displayed in the hud" - default_value: "3" + default_value: 3 field: hud_radar_range_min min: 1 max: 30 - name: osd_hud_radar_range_max description: "In meters, radar aircrafts further away than this will not be displayed in the hud" - default_value: 4000" + default_value: 4000 field: hud_radar_range_max min: 100 max: 9990 - name: osd_hud_radar_nearest description: "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." - default_value: "0" + default_value: 0 field: hud_radar_nearest min: 0 max: 4000 - name: osd_hud_wp_disp description: "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)" - default_value: "0" + default_value: 0 field: hud_wp_disp min: 0 @@ -2923,16 +3111,19 @@ groups: field: left_sidebar_scroll table: osd_sidebar_scroll type: uint8_t + default_value: NONE - name: osd_right_sidebar_scroll field: right_sidebar_scroll table: osd_sidebar_scroll type: uint8_t + default_value: NONE - name: osd_sidebar_scroll_arrows field: sidebar_scroll_arrows type: bool + default_value: OFF - name: osd_main_voltage_decimals description: "Number of decimals for the battery voltages displayed in the OSD [1-2]." - default_value: "1" + default_value: 1 field: main_voltage_decimals min: 1 max: 2 @@ -2940,23 +3131,24 @@ groups: field: coordinate_digits min: 8 max: 11 + default_value: 9 - name: osd_estimations_wind_compensation description: "Use wind estimation for remaining flight time/distance estimation" - default_value: "ON" + default_value: ON condition: USE_WIND_ESTIMATOR field: estimations_wind_compensation type: bool - name: osd_failsafe_switch_layout description: "If enabled the OSD automatically switches to the first layout during failsafe" - default_value: "OFF" + default_value: OFF type: bool - name: osd_plus_code_digits description: "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." field: plus_code_digits - default_value: 10 + default_value: 11 min: 10 max: 13 - name: osd_plus_code_short @@ -2964,24 +3156,25 @@ groups: field: plus_code_short default_value: "0" table: osd_plus_code_short - type: uint8_t - name: osd_ahi_style description: "Sets OSD Artificial Horizon style \"DEFAULT\" or \"LINE\" for the FrSky Graphical OSD." + field: ahi_style default_value: "DEFAULT" table: osd_ahi_style + type: uint8_t - name: osd_force_grid field: force_grid type: bool - default_value: "OFF" + default_value: OFF description: Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) - name: osd_ahi_bordered field: ahi_bordered type: bool description: Shows a border/corners around the AHI region (pixel OSD only) - default_value: "OFF" + default_value: OFF - name: osd_ahi_width field: ahi_width @@ -3000,7 +3193,7 @@ groups: min: -128 max: 127 description: AHI vertical offset from center (pixel OSD only) - default_value: 0 + default_value: -18 - name: osd_sidebar_horizontal_offset field: sidebar_horizontal_offset @@ -3023,9 +3216,24 @@ groups: - name: osd_home_position_arm_screen type: bool - default_value: "ON" + default_value: ON description: Should home position coordinates be displayed on the arming screen. + - name: osd_pan_servo_index + description: 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. + field: pan_servo_index + min: 0 + max: 10 + default_value: 0 + + - name: osd_pan_servo_pwm2centideg + description: 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. + field: pan_servo_pwm2centideg + default_value: 0 + min: -36 + max: 36 + + - name: PG_SYSTEM_CONFIG type: systemConfig_t headers: ["fc/config.h"] @@ -3037,7 +3245,7 @@ groups: table: i2c_speed - name: cpu_underclock description: "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" - default_value: "OFF" + default_value: OFF field: cpuUnderclock condition: USE_UNDERCLOCK type: bool @@ -3047,13 +3255,13 @@ groups: table: debug_modes - name: throttle_tilt_comp_str description: "Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled." - default_value: "0" + default_value: 0 field: throttle_tilt_compensation_strength min: 0 max: 100 - name: name description: "Craft name" - default_value: "Empty string" + default_value: "" - name: PG_MODE_ACTIVATION_OPERATOR_CONFIG type: modeActivationOperatorConfig_t @@ -3073,20 +3281,21 @@ groups: members: - name: stats description: "General switch of the statistics recording feature (a.k.a. odometer)" - default_value: "OFF" + default_value: OFF field: stats_enabled type: bool - name: stats_total_time description: "Total flight time [in seconds]. The value is updated on every disarm when \"stats\" are enabled." - default_value: "0" + default_value: 0 max: INT32_MAX - name: stats_total_dist description: "Total flight distance [in meters]. The value is updated on every disarm when \"stats\" are enabled." - default_value: "0" + default_value: 0 max: INT32_MAX - name: stats_total_energy max: INT32_MAX condition: USE_ADC + default_value: 0 - name: PG_TIME_CONFIG type: timeConfig_t @@ -3094,7 +3303,7 @@ groups: members: - name: tz_offset description: "Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs" - default_value: "0" + default_value: 0 min: -1440 max: 1440 - name: tz_automatic_dst @@ -3109,7 +3318,7 @@ groups: members: - name: display_force_sw_blink description: "OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON" - default_value: "OFF" + default_value: OFF field: force_sw_blink type: bool @@ -3120,12 +3329,12 @@ groups: members: - name: vtx_halfduplex description: "Use half duplex UART to communicate with the VTX, using only a TX pin in the FC." - default_value: "ON" + default_value: ON field: halfDuplex type: bool - name: vtx_smartaudio_early_akk_workaround description: "Enable workaround for early AKK SAudio-enabled VTX bug." - default_value: "ON" + default_value: ON field: smartAudioEarlyAkkWorkaroundEnable type: bool @@ -3136,19 +3345,19 @@ groups: members: - name: vtx_band description: "Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race." - default_value: "4" + default_value: 4 field: band min: VTX_SETTINGS_NO_BAND max: VTX_SETTINGS_MAX_BAND - name: vtx_channel description: "Channel to use within the configured `vtx_band`. Valid values are [1, 8]." - default_value: "1" + default_value: 1 field: channel min: VTX_SETTINGS_MIN_CHANNEL max: VTX_SETTINGS_MAX_CHANNEL - name: vtx_power description: "VTX RF power level to use. The exact number of mw depends on the VTX hardware." - default_value: "1" + default_value: 1 field: power min: VTX_SETTINGS_MIN_POWER max: VTX_SETTINGS_MAX_POWER @@ -3162,9 +3371,10 @@ groups: field: pitModeChan min: VTX_SETTINGS_MIN_CHANNEL max: VTX_SETTINGS_MAX_CHANNEL + default_value: 1 - name: vtx_max_power_override description: "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" - default_value: "0" + default_value: 0 field: maxPowerOverride min: 0 max: 10000 @@ -3176,23 +3386,35 @@ groups: members: - name: pinio_box1 field: permanentId[0] + description: "Mode assignment for PINIO#1" + default_value: "target specific" min: 0 max: 255 + default_value: :BOX_PERMANENT_ID_NONE type: uint8_t - name: pinio_box2 field: permanentId[1] + default_value: "target specific" + description: "Mode assignment for PINIO#1" min: 0 max: 255 + default_value: :BOX_PERMANENT_ID_NONE type: uint8_t - name: pinio_box3 field: permanentId[2] + default_value: "target specific" + description: "Mode assignment for PINIO#1" min: 0 max: 255 + default_value: :BOX_PERMANENT_ID_NONE type: uint8_t - name: pinio_box4 field: permanentId[3] + default_value: "target specific" + description: "Mode assignment for PINIO#1" min: 0 max: 255 + default_value: :BOX_PERMANENT_ID_NONE type: uint8_t - name: PG_LOG_CONFIG @@ -3204,13 +3426,13 @@ groups: field: level table: log_level description: "Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage." - default_value: "`ERROR`" + default_value: "ERROR" - name: log_topics field: topics min: 0 max: UINT32_MAX description: "Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage." - default_value: "0" + default_value: 0 - name: PG_ESC_SENSOR_CONFIG type: escSensorConfig_t @@ -3218,8 +3440,11 @@ groups: condition: USE_ESC_SENSOR members: - name: esc_sensor_listen_only + default_value: OFF + description: "Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case" field: listenOnly type: bool + default_value: OFF - name: PG_SMARTPORT_MASTER_CONFIG type: smartportMasterConfig_t @@ -3229,9 +3454,11 @@ groups: - name: smartport_master_halfduplex field: halfDuplex type: bool + default_value: ON - name: smartport_master_inverted field: inverted type: bool + default_value: OFF - name: PG_DJI_OSD_CONFIG type: djiOsdConfig_t @@ -3243,9 +3470,10 @@ groups: field: proto_workarounds min: 0 max: UINT8_MAX + default_value: 1 - name: dji_use_name_for_messages description: "Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance" - default_value: "ON" + default_value: ON field: use_name_for_messages type: bool - name: dji_esc_temp_source @@ -3254,3 +3482,9 @@ groups: field: esc_temperature_source table: djiOsdTempSource type: uint8_t + - name: dji_speed_source + description: "Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR" + default_value: "GROUND" + field: speedSource + table: djiOsdSpeedSource + type: uint8_t diff --git a/src/main/fc/stats.c b/src/main/fc/stats.c index 43534a62a0..f34e2d949f 100644 --- a/src/main/fc/stats.c +++ b/src/main/fc/stats.c @@ -3,6 +3,7 @@ #ifdef USE_STATS +#include "fc/settings.h" #include "fc/stats.h" #include "sensors/battery.h" @@ -20,11 +21,11 @@ PG_REGISTER_WITH_RESET_TEMPLATE(statsConfig_t, statsConfig, PG_STATS_CONFIG, 1); PG_RESET_TEMPLATE(statsConfig_t, statsConfig, - .stats_enabled = 0, - .stats_total_time = 0, - .stats_total_dist = 0, + .stats_enabled = SETTING_STATS_DEFAULT, + .stats_total_time = SETTING_STATS_TOTAL_TIME_DEFAULT, + .stats_total_dist = SETTING_STATS_TOTAL_DIST_DEFAULT, #ifdef USE_ADC - .stats_total_energy = 0 + .stats_total_energy = SETTING_STATS_TOTAL_ENERGY_DEFAULT #endif ); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 6ac2783c18..7a0c3612d7 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -40,6 +40,7 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "fc/controlrate_profile.h" +#include "fc/settings.h" #include "flight/failsafe.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_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, - .failsafe_delay = 5, // 0.5 sec - .failsafe_recovery_delay = 5, // 0.5 seconds (plus 200ms explicit delay) - .failsafe_off_delay = 200, // 20sec - .failsafe_throttle = 1000, // default throttle off. - .failsafe_throttle_low_delay = 0, // default throttle low delay for "just disarm" on failsafe condition - .failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING, // default full failsafe procedure - .failsafe_fw_roll_angle = -200, // 20 deg left - .failsafe_fw_pitch_angle = 100, // 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_stick_motion_threshold = 50, - .failsafe_min_distance = 0, // No minimum distance for failsafe by default - .failsafe_min_distance_procedure = FAILSAFE_PROCEDURE_DROP_IT, // default minimum distance failsafe procedure - .failsafe_mission = true, // Enable failsafe in WP mode or not + .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT, // 0.5 sec + .failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT, // 0.5 seconds (plus 200ms explicit delay) + .failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT, // 20sec + .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off. + .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT, // default throttle low delay for "just disarm" on failsafe condition + .failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT, // default full failsafe procedure + .failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT, // 20 deg left + .failsafe_fw_pitch_angle = SETTING_FAILSAFE_FW_PITCH_ANGLE_DEFAULT, // 10 deg dive (yes, positive means dive) + .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 = SETTING_FAILSAFE_STICK_THRESHOLD_DEFAULT, + .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default + .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure + .failsafe_mission = SETTING_FAILSAFE_MISSION_DEFAULT, // Enable failsafe in WP mode or not ); typedef enum { diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d75f416f8f..3401e3b00c 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -45,6 +45,7 @@ FILE_COMPILE_FOR_SPEED #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/hil.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_RESET_TEMPLATE(imuConfig_t, imuConfig, - .dcm_kp_acc = 2500, // 0.25 * 10000 - .dcm_ki_acc = 50, // 0.005 * 10000 - .dcm_kp_mag = 10000, // 1.00 * 10000 - .dcm_ki_mag = 0, // 0.00 * 10000 - .small_angle = 25, - .acc_ignore_rate = 0, - .acc_ignore_slope = 0 + .dcm_kp_acc = SETTING_IMU_DCM_KP_DEFAULT, // 0.25 * 10000 + .dcm_ki_acc = SETTING_IMU_DCM_KI_DEFAULT, // 0.005 * 10000 + .dcm_kp_mag = SETTING_IMU_DCM_KP_MAG_DEFAULT, // 1.00 * 10000 + .dcm_ki_mag = SETTING_IMU_DCM_KI_MAG_DEFAULT, // 0.00 * 10000 + .small_angle = SETTING_SMALL_ANGLE_DEFAULT, + .acc_ignore_rate = SETTING_IMU_ACC_IGNORE_RATE_DEFAULT, + .acc_ignore_slope = SETTING_IMU_ACC_IGNORE_SLOPE_DEFAULT ); STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) @@ -156,8 +157,13 @@ void imuInit(void) gpsHeadingInitialized = 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 imuSetMagneticDeclination(deg + min / 60.0f); quaternionInitUnit(&orientation); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index fd0b83a468..af1ca40a20 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -42,6 +42,8 @@ FILE_COMPILE_FOR_SPEED #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/controlrate_profile.h" +#include "fc/settings.h" #include "flight/failsafe.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_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, - .deadband_low = 1406, - .deadband_high = 1514, - .neutral = 1460 + .deadband_low = SETTING_3D_DEADBAND_LOW_DEFAULT, + .deadband_high = SETTING_3D_DEADBAND_HIGH_DEFAULT, + .neutral = SETTING_3D_NEUTRAL_DEFAULT ); PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3); PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, - .motorDirectionInverted = 0, - .platformType = PLATFORM_MULTIROTOR, - .hasFlaps = false, - .appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only - .fwMinThrottleDownPitchAngle = 0 + .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, + .platformType = SETTING_PLATFORM_TYPE_DEFAULT, + .hasFlaps = SETTING_HAS_FLAPS_DEFAULT, + .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only + .fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT ); #ifdef BRUSHED_MOTORS @@ -99,22 +101,28 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, #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, - .motorPwmProtocol = DEFAULT_PWM_PROTOCOL, - .motorPwmRate = DEFAULT_PWM_RATE, - .maxthrottle = DEFAULT_MAX_THROTTLE, - .mincommand = 1000, - .motorAccelTimeMs = 0, - .motorDecelTimeMs = 0, - .throttleIdle = 15.0f, - .throttleScale = 1.0f, - .motorPoleCount = 14 // Most brushless motors that we use are 14 poles + .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, + .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT, + .maxthrottle = SETTING_MAX_THROTTLE_DEFAULT, + .mincommand = SETTING_MIN_COMMAND_DEFAULT, + .motorAccelTimeMs = SETTING_MOTOR_ACCEL_TIME_DEFAULT, + .motorDecelTimeMs = SETTING_MOTOR_DECEL_TIME_DEFAULT, + .throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT, + .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT, + .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); +#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); static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn; @@ -184,7 +192,7 @@ void mixerUpdateStateFlags(void) } else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) { ENABLE_STATE(MULTIROTOR); ENABLE_STATE(ALTITUDE_CONTROL); - } + } if (mixerConfig()->hasFlaps) { ENABLE_STATE(FLAPERON_AVAILABLE); @@ -266,7 +274,7 @@ void mixerInit(void) void mixerResetDisarmedMotors(void) { int motorZeroCommand; - + if (feature(FEATURE_REVERSIBLE_MOTORS)) { motorZeroCommand = reversibleMotorsConfig()->neutral; throttleRangeMin = throttleDeadbandHigh; @@ -319,6 +327,85 @@ static uint16_t handleOutputScaling( } 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 void FAST_CODE writeMotors(void) @@ -443,6 +530,13 @@ static int getReversibleMotorsThrottleDeadband(void) 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] // Allow direct stick input to motors in passthrough mode on airplanes 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)) { throttleRangeMin = throttleIdleValue; throttleRangeMax = motorConfig()->maxthrottle; - mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); + mixerThrottleCommand = constrain(logicConditionValuesByType[LOGIC_CONDITION_OVERRIDE_THROTTLE], throttleRangeMin, throttleRangeMax); } else #endif if (feature(FEATURE_REVERSIBLE_MOTORS)) { @@ -519,7 +613,7 @@ void FAST_CODE mixTable(const float dT) mixerThrottleCommand = ((mixerThrottleCommand - throttleRangeMin) * motorConfig()->throttleScale) + throttleRangeMin; #endif // 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); } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index dfe1abedb5..7f5088fafc 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -92,6 +92,7 @@ typedef struct motorConfig_s { float throttleIdle; // Throttle IDLE value based on min_command, max_throttle, in percent 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 flipOverAfterPowerFactor; // Power factor from 0 to 100% of flip over after crash } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 23fde837b7..b8c4b94e24 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -40,11 +40,13 @@ FILE_COMPILE_FOR_SPEED #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/rpm_filter.h" +#include "flight/secondary_imu.h" #include "flight/kalman.h" #include "io/gps.h" @@ -104,6 +106,7 @@ typedef struct { uint16_t pidSumLimit; filterApply4FnPtr ptermFilterApplyFn; bool itermLimitActive; + bool itermFreezeActive; biquadFilter_t rateTargetFilter; } pidState_t; @@ -161,38 +164,38 @@ PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { .pid = { - [PID_ROLL] = { 40, 30, 23, 60 }, - [PID_PITCH] = { 40, 30, 23, 60 }, - [PID_YAW] = { 85, 45, 0, 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] = { SETTING_MC_P_PITCH_DEFAULT, SETTING_MC_I_PITCH_DEFAULT, SETTING_MC_D_PITCH_DEFAULT, SETTING_MC_CD_PITCH_DEFAULT }, + [PID_YAW] = { SETTING_MC_P_YAW_DEFAULT, SETTING_MC_I_YAW_DEFAULT, SETTING_MC_D_YAW_DEFAULT, SETTING_MC_CD_YAW_DEFAULT }, [PID_LEVEL] = { - .P = 20, // Self-level strength - .I = 15, // Self-leveing low-pass frequency (0 - disabled) - .D = 75, // 75% horizon strength + .P = SETTING_MC_P_LEVEL_DEFAULT, // Self-level strength + .I = SETTING_MC_I_LEVEL_DEFAULT, // Self-leveing low-pass frequency (0 - disabled) + .D = SETTING_MC_D_LEVEL_DEFAULT, // 75% horizon strength .FF = 0, }, - [PID_HEADING] = { 60, 0, 0, 0 }, + [PID_HEADING] = { SETTING_NAV_MC_HEADING_P_DEFAULT, 0, 0, 0 }, [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, .D = 0, .FF = 0, }, [PID_VEL_XY] = { - .P = 40, // NAV_VEL_XY_P * 20 - .I = 15, // NAV_VEL_XY_I * 100 - .D = 100, // NAV_VEL_XY_D * 100 - .FF = 40, // NAV_VEL_XY_D * 100 + .P = SETTING_NAV_MC_VEL_XY_P_DEFAULT, // NAV_VEL_XY_P * 20 + .I = SETTING_NAV_MC_VEL_XY_I_DEFAULT, // NAV_VEL_XY_I * 100 + .D = SETTING_NAV_MC_VEL_XY_D_DEFAULT, // NAV_VEL_XY_D * 100 + .FF = SETTING_NAV_MC_VEL_XY_FF_DEFAULT, // NAV_VEL_XY_D * 100 }, [PID_POS_Z] = { - .P = 50, // NAV_POS_Z_P * 100 - .I = 0, // not used - .D = 0, // not used + .P = SETTING_NAV_MC_POS_Z_P_DEFAULT, // NAV_POS_Z_P * 100 + .I = 0, // not used + .D = 0, // not used .FF = 0, }, [PID_VEL_Z] = { - .P = 100, // NAV_VEL_Z_P * 66.7 - .I = 50, // NAV_VEL_Z_I * 20 - .D = 10, // NAV_VEL_Z_D * 100 + .P = SETTING_NAV_MC_VEL_Z_P_DEFAULT, // NAV_VEL_Z_P * 66.7 + .I = SETTING_NAV_MC_VEL_Z_I_DEFAULT, // NAV_VEL_Z_I * 20 + .D = SETTING_NAV_MC_VEL_Z_D_DEFAULT, // NAV_VEL_Z_D * 100 .FF = 0, }, [PID_POS_HEADING] = { @@ -206,82 +209,94 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_fw = { .pid = { - [PID_ROLL] = { 5, 7, 0, 50 }, - [PID_PITCH] = { 5, 7, 0, 50 }, - [PID_YAW] = { 6, 10, 0, 60 }, + [PID_ROLL] = { SETTING_FW_P_ROLL_DEFAULT, SETTING_FW_I_ROLL_DEFAULT, 0, SETTING_FW_FF_ROLL_DEFAULT }, + [PID_PITCH] = { SETTING_FW_P_PITCH_DEFAULT, SETTING_FW_I_PITCH_DEFAULT, 0, SETTING_FW_FF_PITCH_DEFAULT }, + [PID_YAW] = { SETTING_FW_P_YAW_DEFAULT, SETTING_FW_I_YAW_DEFAULT, 0, SETTING_FW_FF_YAW_DEFAULT }, [PID_LEVEL] = { - .P = 20, // Self-level strength - .I = 5, // Self-leveing low-pass frequency (0 - disabled) - .D = 75, // 75% horizon strength + .P = SETTING_FW_P_LEVEL_DEFAULT, // Self-level strength + .I = SETTING_FW_I_LEVEL_DEFAULT, // Self-leveing low-pass frequency (0 - disabled) + .D = SETTING_FW_D_LEVEL_DEFAULT, // 75% horizon strength .FF = 0, }, - [PID_HEADING] = { 60, 0, 0, 0 }, + [PID_HEADING] = { SETTING_NAV_FW_HEADING_P_DEFAULT, 0, 0, 0 }, [PID_POS_Z] = { - .P = 40, // FW_POS_Z_P * 10 - .I = 5, // FW_POS_Z_I * 10 - .D = 10, // FW_POS_Z_D * 10 + .P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 10 + .I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 10 + .D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 10 .FF = 0, }, [PID_POS_XY] = { - .P = 75, // FW_POS_XY_P * 100 - .I = 5, // FW_POS_XY_I * 100 - .D = 8, // FW_POS_XY_D * 100 + .P = SETTING_NAV_FW_POS_XY_P_DEFAULT, // FW_POS_XY_P * 100 + .I = SETTING_NAV_FW_POS_XY_I_DEFAULT, // FW_POS_XY_I * 100 + .D = SETTING_NAV_FW_POS_XY_D_DEFAULT, // FW_POS_XY_D * 100 .FF = 0, }, [PID_POS_HEADING] = { - .P = 30, - .I = 2, - .D = 0, + .P = SETTING_NAV_FW_POS_HDG_P_DEFAULT, + .I = SETTING_NAV_FW_POS_HDG_I_DEFAULT, + .D = SETTING_NAV_FW_POS_HDG_D_DEFAULT, .FF = 0 } } }, - .dterm_lpf_type = 1, //Default to BIQUAD - .dterm_lpf_hz = 40, - .dterm_lpf2_type = 1, //Default to BIQUAD - .dterm_lpf2_hz = 0, // Off by default - .yaw_lpf_hz = 0, + .dterm_lpf_type = SETTING_DTERM_LPF_TYPE_DEFAULT, + .dterm_lpf_hz = SETTING_DTERM_LPF_HZ_DEFAULT, + .dterm_lpf2_type = SETTING_DTERM_LPF2_TYPE_DEFAULT, + .dterm_lpf2_hz = SETTING_DTERM_LPF2_HZ_DEFAULT, + .yaw_lpf_hz = SETTING_YAW_LPF_HZ_DEFAULT, - .itermWindupPointPercent = 50, // Percent + .itermWindupPointPercent = SETTING_ITERM_WINDUP_DEFAULT, - .axisAccelerationLimitYaw = 10000, // dps/s - .axisAccelerationLimitRollPitch = 0, // dps/s + .axisAccelerationLimitYaw = SETTING_RATE_ACCEL_LIMIT_YAW_DEFAULT, + .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_PITCH] = 300, // 30 degrees - .pidSumLimit = PID_SUM_LIMIT_DEFAULT, - .pidSumLimitYaw = PID_SUM_LIMIT_YAW_DEFAULT, + .max_angle_inclination[FD_ROLL] = SETTING_MAX_ANGLE_INCLINATION_RLL_DEFAULT, + .max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT, + .pidSumLimit = SETTING_PIDSUM_LIMIT_DEFAULT, + .pidSumLimitYaw = SETTING_PIDSUM_LIMIT_YAW_DEFAULT, - .fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT, - .fixedWingReferenceAirspeed = 1000, - .fixedWingCoordinatedYawGain = 1.0f, - .fixedWingCoordinatedPitchGain = 1.0f, - .fixedWingItermLimitOnStickPosition = 0.5f, + .fixedWingItermThrowLimit = SETTING_FW_ITERM_THROW_LIMIT_DEFAULT, + .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT, + .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT, + .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT, + .fixedWingItermLimitOnStickPosition = SETTING_FW_ITERM_LIMIT_STICK_POSITION_DEFAULT, + .fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT, - .loiter_direction = NAV_LOITER_RIGHT, - .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, - .navVelXyDtermAttenuation = 90, - .navVelXyDtermAttenuationStart = 10, - .navVelXyDtermAttenuationEnd = 60, - .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT, - .iterm_relax = ITERM_RELAX_RP, - .dBoostFactor = 1.25f, - .dBoostMaxAtAlleceleration = 7500.0f, - .dBoostGyroDeltaLpfHz = D_BOOST_GYRO_LPF_HZ, - .antigravityGain = 1.0f, - .antigravityAccelerator = 1.0f, - .antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, - .pidControllerType = PID_TYPE_AUTO, - .navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT, - .controlDerivativeLpfHz = 30, - .kalman_q = 100, - .kalman_w = 4, - .kalman_sharpness = 100, - .kalmanEnabled = 0, - .fixedWingLevelTrim = 0, + .loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT, + .navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT, + .navVelXyDtermAttenuation = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_DEFAULT, + .navVelXyDtermAttenuationStart = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_DEFAULT, + .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_DEFAULT, + .iterm_relax_cutoff = SETTING_MC_ITERM_RELAX_CUTOFF_DEFAULT, + .iterm_relax = SETTING_MC_ITERM_RELAX_DEFAULT, + +#ifdef USE_D_BOOST + .dBoostFactor = SETTING_D_BOOST_FACTOR_DEFAULT, + .dBoostMaxAtAlleceleration = SETTING_D_BOOST_MAX_AT_ACCELERATION_DEFAULT, + .dBoostGyroDeltaLpfHz = SETTING_D_BOOST_GYRO_DELTA_LPF_HZ_DEFAULT, +#endif + +#ifdef USE_ANTIGRAVITY + .antigravityGain = SETTING_ANTIGRAVITY_GAIN_DEFAULT, + .antigravityAccelerator = SETTING_ANTIGRAVITY_ACCELERATOR_DEFAULT, + .antigravityCutoff = SETTING_ANTIGRAVITY_CUTOFF_LPF_HZ_DEFAULT, +#endif + + .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) @@ -481,7 +496,7 @@ void updatePIDCoefficients() // Airplanes - scale all PIDs according to TPA 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].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].kCD = 0.0f; pidState[axis].kT = 0.0f; @@ -495,7 +510,7 @@ void updatePIDCoefficients() pidState[axis].kFF = 0.0f; // 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)); } else { 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()) 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)) { /* * 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); } +#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]); +#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); @@ -608,66 +637,6 @@ static float pTermProcess(pidState_t *pidState, float rateError, float 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 static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) { @@ -694,10 +663,100 @@ static float applyDBoost(pidState_t *pidState, float dT) { } #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) { const float rateError = pidState->rateTarget - pidState->gyroRate; const float newPTerm = pTermProcess(pidState, rateError, dT); + const float newDTerm = dTermProcess(pidState, dT); const float rateTargetDelta = pidState->rateTarget - pidState->previousRateTarget; 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); - // 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 const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; 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 * 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; targetRates.x = 0.0f; @@ -889,8 +933,9 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState) airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // 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); - float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn; + bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60)); + float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget)); + float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); } @@ -951,6 +996,24 @@ void checkItermLimitingActive(pidState_t *pidState) 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) { if (!pidFiltersConfigured) { @@ -1003,8 +1066,10 @@ void FAST_CODE pidController(float dT) levelingEnabled = false; } - if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - pidTurnAssistant(pidState); + if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { + 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 } @@ -1021,6 +1086,8 @@ void FAST_CODE pidController(float dT) // Step 4: Run gyro-driven control checkItermLimitingActive(&pidState[axis]); + checkItermFreezingActive(&pidState[axis], axis); + pidControllerApplyFn(&pidState[axis], axis, dT); } } @@ -1135,12 +1202,3 @@ const pidBank_t * pidBank(void) { pidBank_t * pidBankMutable(void) { 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; - } -} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 83b7b6380c..3f3d5a3bcc 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -76,7 +76,7 @@ typedef enum { typedef enum { PID_TYPE_NONE = 0, // Not used in the current platform/mixer/configuration 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 } 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 fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. float fixedWingItermLimitOnStickPosition; //Do not allow Iterm to grow when stick position is above this point + uint16_t fixedWingYawItermBankFreeze; // Freeze yaw Iterm when bank angle is more than this many degrees uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise) 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; // Enable iterm suppression during stick input +#ifdef USE_D_BOOST float dBoostFactor; float dBoostMaxAtAlleceleration; uint8_t dBoostGyroDeltaLpfHz; +#endif + +#ifdef USE_ANTIGRAVITY float antigravityGain; float antigravityAccelerator; uint8_t antigravityCutoff; +#endif uint16_t navFwPosHdgPidsumLimit; uint8_t controlDerivativeLpfHz; + +#ifdef USE_GYRO_KALMAN uint16_t kalman_q; uint16_t kalman_w; uint16_t kalman_sharpness; uint8_t kalmanEnabled; +#endif float fixedWingLevelTrim; } pidProfile_t; @@ -201,4 +210,3 @@ void autotuneUpdateState(void); void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput); pidType_e pidIndexGetType(pidIndex_e pidIndex); -uint16_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex); diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 9db1f15284..684f42d1b7 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -40,11 +40,10 @@ #include "fc/rc_controls.h" #include "fc/rc_adjustments.h" #include "fc/runtime_config.h" +#include "fc/settings.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_DECREASE_STEP 8 // 8% #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_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, - .fw_overshoot_time = AUTOTUNE_FIXED_WING_OVERSHOOT_TIME, - .fw_undershoot_time = AUTOTUNE_FIXED_WING_UNDERSHOOT_TIME, - .fw_max_rate_threshold = 50, - .fw_ff_to_p_gain = 10, - .fw_ff_to_i_time_constant = AUTOTUNE_FIXED_WING_INTEGRATOR_TC, + .fw_overshoot_time = SETTING_FW_AUTOTUNE_OVERSHOOT_TIME_DEFAULT, + .fw_undershoot_time = SETTING_FW_AUTOTUNE_UNDERSHOOT_TIME_DEFAULT, + .fw_max_rate_threshold = SETTING_FW_AUTOTUNE_THRESHOLD_DEFAULT, + .fw_ff_to_p_gain = SETTING_FW_AUTOTUNE_FF_TO_P_GAIN_DEFAULT, + .fw_ff_to_i_time_constant = SETTING_FW_AUTOTUNE_FF_TO_I_TC_DEFAULT, ); typedef enum { @@ -230,15 +229,15 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa switch (axis) { case FD_ROLL: - blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D_FF, tuneCurrent[axis].gainFF); + blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF); break; case FD_PITCH: - blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D_FF, tuneCurrent[axis].gainFF); + blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF); break; case FD_YAW: - blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D_FF, tuneCurrent[axis].gainFF); + blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF); break; } } diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 6a9604651e..183220461e 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -38,6 +38,7 @@ #include "flight/mixer.h" #include "sensors/esc_sensor.h" #include "fc/config.h" +#include "fc/settings.h" #ifdef USE_RPM_FILTER @@ -48,10 +49,10 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1); PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, - .gyro_filter_enabled = 0, - .gyro_harmonics = 1, - .gyro_min_hz = 100, - .gyro_q = 500, ); + .gyro_filter_enabled = SETTING_RPM_GYRO_FILTER_ENABLED_DEFAULT, + .gyro_harmonics = SETTING_RPM_GYRO_HARMONICS_DEFAULT, + .gyro_min_hz = SETTING_RPM_GYRO_MIN_HZ_DEFAULT, + .gyro_q = SETTING_RPM_GYRO_Q_DEFAULT, ); typedef struct { @@ -203,4 +204,4 @@ float rpmFilterGyroApply(uint8_t axis, float input) return rpmGyroApplyFn(&gyroRpmFilters, axis, input); } -#endif \ No newline at end of file +#endif diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c new file mode 100644 index 0000000000..93cd11b9d1 --- /dev/null +++ b/src/main/flight/secondary_imu.c @@ -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 +} diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h new file mode 100644 index 0000000000..b674bc9c93 --- /dev/null +++ b/src/main/flight/secondary_imu.h @@ -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); \ No newline at end of file diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 918a20ce99..df58e6f649 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -44,6 +44,7 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "fc/controlrate_profile.h" +#include "fc/settings.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -57,12 +58,12 @@ PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1); PG_RESET_TEMPLATE(servoConfig_t, servoConfig, - .servoCenterPulse = 1500, - .servoPwmRate = 50, // 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_protocol = SERVO_TYPE_PWM, - .flaperon_throw_offset = FLAPERON_THROW_DEFAULT, - .tri_unarmed_servo = 1 + .servoCenterPulse = SETTING_SERVO_CENTER_PULSE_DEFAULT, + .servoPwmRate = SETTING_SERVO_PWM_RATE_DEFAULT, // Default for analog servos + .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 = SETTING_SERVO_PROTOCOL_DEFAULT, + .flaperon_throw_offset = SETTING_FLAPERON_THROW_OFFSET_DEFAULT, + .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT ); PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index fd0294bebc..3d693deba8 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -58,6 +58,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" typedef struct { 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_RESET_TEMPLATE(gpsConfig_t, gpsConfig, - .provider = GPS_UBLOX, - .sbasMode = SBAS_NONE, - .autoConfig = GPS_AUTOCONFIG_ON, - .autoBaud = GPS_AUTOBAUD_ON, - .dynModel = GPS_DYNMODEL_AIR_1G, - .gpsMinSats = 6, - .ubloxUseGalileo = false + .provider = SETTING_GPS_PROVIDER_DEFAULT, + .sbasMode = SETTING_GPS_SBAS_MODE_DEFAULT, + .autoConfig = SETTING_GPS_AUTO_CONFIG_DEFAULT, + .autoBaud = SETTING_GPS_AUTO_BAUD_DEFAULT, + .dynModel = SETTING_GPS_DYN_MODEL_DEFAULT, + .gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT, + .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT ); void gpsSetState(gpsState_e state) @@ -192,6 +193,8 @@ static void gpsResetSolution(void) { gpsSol.eph = 9999; gpsSol.epv = 9999; + gpsSol.numSat = 0; + gpsSol.hdop = 9999; gpsSol.fixType = GPS_NO_FIX; diff --git a/src/main/io/lights.c b/src/main/io/lights.c index 6eb06f914d..2329aa8bf0 100644 --- a/src/main/io/lights.c +++ b/src/main/io/lights.c @@ -31,9 +31,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(lightsConfig_t, lightsConfig, PG_LIGHTS_CONFIG, PG_RESET_TEMPLATE(lightsConfig_t, lightsConfig, .failsafe = { - .enabled = true, - .flash_period = 1000, - .flash_on_time = 100 + .enabled = SETTING_FAILSAFE_LIGHTS_DEFAULT, + .flash_period = SETTING_FAILSAFE_LIGHTS_FLASH_PERIOD_DEFAULT, + .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); } -void lightsInit() +void lightsInit(void) { lightsHardwareInit(); } diff --git a/src/main/io/lights.h b/src/main/io/lights.h index 8d68ec5ffb..935a540a81 100644 --- a/src/main/io/lights.h +++ b/src/main/io/lights.h @@ -33,6 +33,6 @@ typedef struct lightsConfig_s { PG_DECLARE(lightsConfig_t, lightsConfig); void lightsUpdate(timeUs_t currentTimeUs); -void lightsInit(); +void lightsInit(void); #endif /* USE_LIGHTS */ diff --git a/src/main/io/osd.c b/src/main/io/osd.c old mode 100755 new mode 100644 index 3612ef00d5..7c8825e3a1 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -87,6 +87,8 @@ FILE_COMPILE_FOR_SPEED #include "flight/pid.h" #include "flight/rth_estimator.h" #include "flight/wind_estimator.h" +#include "flight/secondary_imu.h" +#include "flight/servos.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -115,6 +117,8 @@ FILE_COMPILE_FOR_SPEED #define GFORCE_FILTER_TC 0.2 #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 ARMED_SCREEN_DISPLAY_TIME 1500 // ms @@ -154,6 +158,8 @@ typedef struct statistic_s { int16_t max_current; // /100 int16_t max_power; // /100 int16_t min_rssi; + int16_t min_lq; // for CRSF + int16_t min_rssi_dbm; // for CRSF int32_t max_altitude; uint32_t max_distance; } statistic_t; @@ -166,6 +172,7 @@ static bool refreshWaitForResumeCmdRelease; static bool fullRedraw = false; static uint8_t armState; +static uint8_t statsPagesCheck = 0; typedef struct osdMapData_s { uint32_t scale; @@ -185,7 +192,7 @@ static bool osdDisplayHasCanvas; #define AH_MAX_PITCH_DEFAULT 20 // Specify default maximum AHI pitch value displayed (degrees) -PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 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); static int digitCount(int32_t value) @@ -526,6 +533,21 @@ static uint16_t osdConvertRSSI(void) 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 * @param label to display @@ -712,6 +734,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR(OSD_MSG_CLI_ACTIVE); case ARMING_DISABLED_PWM_OUTPUT_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 case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; @@ -785,7 +809,7 @@ static const char * navigationStateMessage(void) break; case MW_NAV_STATE_RTH_START: 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); case MW_NAV_STATE_RTH_ENROUTE: return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); @@ -937,12 +961,36 @@ static inline int32_t osdGetAltitudeMsl(void) static bool osdIsHeadingValid(void) { +#ifdef USE_SECONDARY_IMU + if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) { + return true; + } else { + return isImuHeadingValid(); + } +#else return isImuHeadingValid(); +#endif } 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; +#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). @@ -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); } -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 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); } -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; char buff[4]; @@ -1192,7 +1275,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char * elemAttr = TEXT_ATTRIBUTES_NONE; 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); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); } @@ -1304,6 +1387,9 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); if (!STATE(GPS_FIX)) { + if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + strcpy(buff + 2, "X!"); + } TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } break; @@ -1334,7 +1420,11 @@ static bool osdDrawSingleElement(uint8_t item) } 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); } } else { @@ -1397,18 +1487,18 @@ static bool osdDrawSingleElement(uint8_t item) 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, " "); return true; } 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] = '-'; - } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { + } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError())); if (ABS(herr) > 99) strcpy(buff + 1, ">99"); @@ -1421,11 +1511,11 @@ static bool osdDrawSingleElement(uint8_t item) break; } - case OSD_CRUISE_HEADING_ADJUSTMENT: + case OSD_COURSE_HOLD_ADJUSTMENT: { 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, " "); return true; } @@ -1434,7 +1524,7 @@ static bool osdDrawSingleElement(uint8_t item) if (!ARMING_FLAG(ARMED)) { 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); } @@ -1525,7 +1615,11 @@ static bool osdDrawSingleElement(uint8_t item) timeUs_t currentTimeUs = micros(); static int32_t timeSeconds = -1; if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { +#ifdef USE_WIND_ESTIMATOR timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); +#else + timeSeconds = calculateRemainingFlightTimeBeforeRTH(false); +#endif updatedTimestamp = currentTimeUs; } if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { @@ -1552,7 +1646,11 @@ static bool osdDrawSingleElement(uint8_t item) timeUs_t currentTimeUs = micros(); static int32_t distanceMeters = -1; if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { +#ifdef USE_WIND_ESTIMATOR distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); +#else + distanceMeters = calculateRemainingDistanceBeforeRTH(false); +#endif updatedTimestamp = currentTimeUs; } buff[0] = SYM_TRIP_DIST; @@ -1585,10 +1683,10 @@ static bool osdDrawSingleElement(uint8_t item) p = "RTH "; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) p = "HOLD"; - else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) - p = "3CRS"; - else if (FLIGHT_MODE(NAV_CRUISE_MODE)) - p = "CRS "; + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) + p = "CRUZ"; + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) + p = "CRSH"; else if (FLIGHT_MODE(NAV_WP_MODE)) p = " WP "; else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) { @@ -1601,6 +1699,8 @@ static bool osdDrawSingleElement(uint8_t item) p = "ANGL"; else if (FLIGHT_MODE(HORIZON_MODE)) p = "HOR "; + else if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) + p = "TURT"; displayWrite(osdDisplayPort, elemPosX, elemPosY, p); return true; @@ -1642,7 +1742,9 @@ static bool osdDrawSingleElement(uint8_t item) return true; } +#if defined(USE_SERIALRX_CRSF) case OSD_CRSF_RSSI_DBM: + { if (rxLinkStatistics.activeAnt == 0) { buff[0] = SYM_RSSI; tfp_sprintf(buff + 1, "%4d%c", rxLinkStatistics.uplinkRSSI, SYM_DBM); @@ -1657,12 +1759,12 @@ static bool osdDrawSingleElement(uint8_t item) } } break; - -#if defined(USE_SERIALRX_CRSF) - case OSD_CRSF_LQ: { - buff[0] = SYM_BLANK; - int16_t statsLQ = rxLinkStatistics.uplinkLQ; - int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); + } + case OSD_CRSF_LQ: + { + buff[0] = SYM_BLANK; + int16_t statsLQ = rxLinkStatistics.uplinkLQ; + int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); if (rxLinkStatistics.rfMode == 2) { if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) { tfp_sprintf(buff, "%5d%s", scaledLQ, "%"); @@ -1672,9 +1774,9 @@ static bool osdDrawSingleElement(uint8_t item) } else { if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) { tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%"); - } else { + } else { tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); - } + } } if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1684,31 +1786,32 @@ static bool osdDrawSingleElement(uint8_t item) break; } - case OSD_CRSF_SNR_DB: { - const char* showsnr = "-12"; - const char* hidesnr = " "; - int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR; - if (osdSNR_Alarm <= osdConfig()->snr_alarm) { - buff[0] = SYM_SRN; - tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB); - } - else if (osdSNR_Alarm > osdConfig()->snr_alarm) { - if (cmsInMenu) { - buff[0] = SYM_SRN; - tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); - } else { - buff[0] = SYM_BLANK; - tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); + case OSD_CRSF_SNR_DB: + { + const char* showsnr = "-20"; + const char* hidesnr = " "; + int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR; + if (osdSNR_Alarm > osdConfig()->snr_alarm) { + if (cmsInMenu) { + buff[0] = SYM_SNR; + tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); + } else { + buff[0] = SYM_BLANK; + tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); + } + } else if (osdSNR_Alarm <= osdConfig()->snr_alarm) { + buff[0] = SYM_SNR; + tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB); } + break; } - break; - } -#endif - case OSD_CRSF_TX_POWER: { - tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); - break; - } + case OSD_CRSF_TX_POWER: + { + tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); + break; + } +#endif case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair @@ -1762,9 +1865,6 @@ static bool osdDrawSingleElement(uint8_t item) gpsLocation_t wp2; 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 j = posControl.activeWaypointIndex + i; 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: { - float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + float rollAngle; + 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) { rollAngle = -rollAngle; } @@ -1857,35 +1970,35 @@ static bool osdDrawSingleElement(uint8_t item) #endif 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; 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; 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; 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; 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; 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; 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; 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; case OSD_HEADING_P: @@ -1969,7 +2082,7 @@ static bool osdDrawSingleElement(uint8_t item) return true; 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; case OSD_NAV_FW_PITCH2THR: @@ -2189,12 +2302,21 @@ static bool osdDrawSingleElement(uint8_t item) } break; } - 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) { - 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); } break; @@ -2545,69 +2667,84 @@ void osdDrawNextElement(void) } PG_RESET_TEMPLATE(osdConfig_t, osdConfig, - .rssi_alarm = 20, - .time_alarm = 10, - .alt_alarm = 100, - .dist_alarm = 1000, - .neg_alt_alarm = 5, - .current_alarm = 0, - .imu_temp_alarm_min = -200, - .imu_temp_alarm_max = 600, - .esc_temp_alarm_min = -200, - .esc_temp_alarm_max = 900, - .gforce_alarm = 5, - .gforce_axis_alarm_min = -5, - .gforce_axis_alarm_max = 5, + .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT, + .time_alarm = SETTING_OSD_TIME_ALARM_DEFAULT, + .alt_alarm = SETTING_OSD_ALT_ALARM_DEFAULT, + .dist_alarm = SETTING_OSD_DIST_ALARM_DEFAULT, + .neg_alt_alarm = SETTING_OSD_NEG_ALT_ALARM_DEFAULT, + .current_alarm = SETTING_OSD_CURRENT_ALARM_DEFAULT, + .imu_temp_alarm_min = SETTING_OSD_IMU_TEMP_ALARM_MIN_DEFAULT, + .imu_temp_alarm_max = SETTING_OSD_IMU_TEMP_ALARM_MAX_DEFAULT, + .esc_temp_alarm_min = SETTING_OSD_ESC_TEMP_ALARM_MIN_DEFAULT, + .esc_temp_alarm_max = SETTING_OSD_ESC_TEMP_ALARM_MAX_DEFAULT, + .gforce_alarm = SETTING_OSD_GFORCE_ALARM_DEFAULT, + .gforce_axis_alarm_min = SETTING_OSD_GFORCE_AXIS_ALARM_MIN_DEFAULT, + .gforce_axis_alarm_max = SETTING_OSD_GFORCE_AXIS_ALARM_MAX_DEFAULT, #ifdef USE_BARO - .baro_temp_alarm_min = -200, - .baro_temp_alarm_max = 600, + .baro_temp_alarm_min = SETTING_OSD_BARO_TEMP_ALARM_MIN_DEFAULT, + .baro_temp_alarm_max = SETTING_OSD_BARO_TEMP_ALARM_MAX_DEFAULT, #endif #ifdef USE_SERIALRX_CRSF - .snr_alarm = 4, - .crsf_lq_format = OSD_CRSF_LQ_TYPE1, - .link_quality_alarm = 70, + .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, + .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, + .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT, #endif #ifdef USE_TEMPERATURE_SENSOR - .temp_label_align = OSD_ALIGN_LEFT, + .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT, #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_max_pitch = AH_MAX_PITCH_DEFAULT, - .crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT, - .horizon_offset = 0, - .camera_uptilt = 0, - .camera_fov_h = 135, - .camera_fov_v = 85, - .hud_margin_h = 3, - .hud_margin_v = 3, - .hud_homing = 0, - .hud_homepoint = 0, - .hud_radar_disp = 0, - .hud_radar_range_min = 3, - .hud_radar_range_max = 4000, - .hud_radar_nearest = 0, - .hud_wp_disp = 0, - .left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE, - .right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE, - .sidebar_scroll_arrows = 0, - .osd_home_position_arm_screen = true, + .ahi_reverse_roll = SETTING_OSD_AHI_REVERSE_ROLL_DEFAULT, + .ahi_max_pitch = SETTING_OSD_AHI_MAX_PITCH_DEFAULT, + .crosshairs_style = SETTING_OSD_CROSSHAIRS_STYLE_DEFAULT, + .horizon_offset = SETTING_OSD_HORIZON_OFFSET_DEFAULT, + .camera_uptilt = SETTING_OSD_CAMERA_UPTILT_DEFAULT, + .camera_fov_h = SETTING_OSD_CAMERA_FOV_H_DEFAULT, + .camera_fov_v = SETTING_OSD_CAMERA_FOV_V_DEFAULT, + .hud_margin_h = SETTING_OSD_HUD_MARGIN_H_DEFAULT, + .hud_margin_v = SETTING_OSD_HUD_MARGIN_V_DEFAULT, + .hud_homing = SETTING_OSD_HUD_HOMING_DEFAULT, + .hud_homepoint = SETTING_OSD_HUD_HOMEPOINT_DEFAULT, + .hud_radar_disp = SETTING_OSD_HUD_RADAR_DISP_DEFAULT, + .hud_radar_range_min = SETTING_OSD_HUD_RADAR_RANGE_MIN_DEFAULT, + .hud_radar_range_max = SETTING_OSD_HUD_RADAR_RANGE_MAX_DEFAULT, + .hud_radar_nearest = SETTING_OSD_HUD_RADAR_NEAREST_DEFAULT, + .hud_wp_disp = SETTING_OSD_HUD_WP_DISP_DEFAULT, + .left_sidebar_scroll = SETTING_OSD_LEFT_SIDEBAR_SCROLL_DEFAULT, + .right_sidebar_scroll = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_DEFAULT, + .sidebar_scroll_arrows = SETTING_OSD_SIDEBAR_SCROLL_ARROWS_DEFAULT, + .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, - .main_voltage_decimals = 1, + .units = SETTING_OSD_UNITS_DEFAULT, + .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, - .estimations_wind_compensation = true, - .coordinate_digits = 9, +#ifdef USE_WIND_ESTIMATOR + .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, - .plus_code_short = 0, + .osd_failsafe_switch_layout = SETTING_OSD_FAILSAFE_SWITCH_LAYOUT_DEFAULT, - .ahi_width = OSD_AHI_WIDTH * OSD_CHAR_WIDTH, - .ahi_height = OSD_AHI_HEIGHT * OSD_CHAR_HEIGHT, - .ahi_vertical_offset = -OSD_CHAR_HEIGHT, + .plus_code_digits = SETTING_OSD_PLUS_CODE_DIGITS_DEFAULT, + .plus_code_short = SETTING_OSD_PLUS_CODE_SHORT_DEFAULT, + + .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) @@ -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_AUTO_THR] = OSD_POS(6, 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_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2); + osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ERROR] = 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_CURRENT_DRAW] = OSD_POS(2, 3) | 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.min_voltage = 5000; stats.min_rssi = 99; + stats.min_lq = 300; + stats.min_rssi_dbm = 0; stats.max_altitude = 0; } @@ -2926,22 +3065,30 @@ static void osdUpdateStats(void) if (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()); } -/* Attention: NTSC screen only has 12 fully visible lines - it is FULL now! */ -static void osdShowStats(void) +static void osdShowStatsPage1(void) { const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS" }; 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); - if (osdDisplayIsPAL()) - displayWrite(osdDisplayPort, statNameX, top++, " --- STATS ---"); + + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2"); if (feature(FEATURE_GPS)) { displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); @@ -2962,26 +3109,64 @@ static void osdShowStats(void) osdFormatAltitudeStr(buff, stats.max_altitude); displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); - osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - strcat(buff, "V"); - osdLeftAlignString(buff); +#if defined(USE_SERIALRX_CRSF) + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(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 :"); itoa(stats.min_rssi, buff, 10); strcat(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)) { displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); itoa(stats.max_current, buff, 10); - strcat(buff, "A"); + tfp_sprintf(buff, "%s%c", buff, SYM_AMP); displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); itoa(stats.max_power, buff, 10); - strcat(buff, "W"); + tfp_sprintf(buff, "%s%c", buff, SYM_WATT); displayWrite(osdDisplayPort, statValuesX, top++, buff); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { @@ -2990,7 +3175,7 @@ static void osdShowStats(void) } else { displayWrite(osdDisplayPort, statNameX, top, "USED WH :"); 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); @@ -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(); displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); 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:"); osdFormatCentiNumber(buff, acc_extremes[Z].min * 100, 0, 2, 0, 4); strcat(buff,"/"); - displayWrite(osdDisplayPort, statValuesX, top, buff); + displayWrite(osdDisplayPort, statValuesX - 1, top, buff); osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3); - displayWrite(osdDisplayPort, statValuesX + 5, top++, buff); - - displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); - displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); + displayWrite(osdDisplayPort, statValuesX + 4, top++, buff); displayCommitTransaction(osdDisplayPort); } @@ -3059,8 +3232,8 @@ static void osdShowArmed(void) char versionBuf[30]; char *date; char *time; - // We need 12 visible rows - uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 12 - 1); + // We need 12 visible rows, start row never < first fully visible row 1 + uint8_t y = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; displayClearScreen(osdDisplayPort); displayWrite(osdDisplayPort, 12, y, "ARMED"); @@ -3069,9 +3242,14 @@ static void osdShowArmed(void) if (strlen(systemConfig()->name) > 0) { osdFormatCraftName(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 (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { @@ -3159,13 +3337,14 @@ static void osdRefresh(timeUs_t currentTimeUs) osdResetStats(); osdShowArmed(); // reset statistic etc uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; + statsPagesCheck = 0; #if defined(USE_SAFE_HOME) if (isSafeHomeInUse()) delay *= 3; #endif osdSetNextRefreshIn(delay); } else { - osdShowStats(); // show statistic + osdShowStatsPage1(); // show first page of statistic osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); } @@ -3184,6 +3363,14 @@ static void osdRefresh(timeUs_t currentTimeUs) if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && DELAYED_REFRESH_RESUME_COMMAND)) { displayClearScreen(osdDisplayPort); 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 { 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 (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { // 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; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { // 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); messages[messageCount++] = messageBuf; } - } else { - const char *navStateMessage = navigationStateMessage(); + } else { + const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { messages[messageCount++] = navStateMessage; } diff --git a/src/main/io/osd.h b/src/main/io/osd.h index d740b5de52..1dc185b0d1 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -76,6 +76,7 @@ #define OSD_MSG_INVALID_SETTING "INVALID SETTING" #define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE" #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_EMERG_LANDING_FS "(EMERGENCY LANDING)" #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_DISTANCE_BEFORE_RTH, OSD_HOME_HEADING_ERROR, - OSD_CRUISE_HEADING_ERROR, - OSD_CRUISE_HEADING_ADJUSTMENT, + OSD_COURSE_HOLD_ERROR, + OSD_COURSE_HOLD_ADJUSTMENT, OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE, OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE, OSD_POWER_SUPPLY_IMPEDANCE, @@ -292,7 +293,7 @@ typedef struct osdConfig_s { float gforce_axis_alarm_min; float gforce_axis_alarm_max; #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; #endif #ifdef USE_BARO @@ -332,13 +333,16 @@ typedef struct osdConfig_s { uint8_t units; // from osd_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 +#endif + uint8_t coordinate_digits; bool osd_failsafe_switch_layout; uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE - uint8_t plus_code_short; - uint8_t osd_ahi_style; + uint8_t plus_code_short; + uint8_t ahi_style; uint8_t force_grid; // Force a pixel based OSD to use grid mode. uint8_t ahi_bordered; // Only used by the AHI widget uint8_t ahi_width; // In pixels, only used by the AHI widget @@ -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 right_sidebar_scroll_step; // Same as left_sidebar_scroll_step, but for the right sidebar. bool osd_home_position_arm_screen; - + uint8_t pan_servo_index; // Index of the pan servo used for home direction offset + int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm uint8_t crsf_lq_format; } osdConfig_t; diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index a13e66bc47..5044e93c26 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -354,7 +354,7 @@ static bool osdCanvasDrawArtificialHorizonWidget(displayPort_t *display, display } if (!configured) { 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: ahiStyle = DISPLAY_WIDGET_AHI_STYLE_STAIRCASE; break; @@ -377,8 +377,13 @@ static bool osdCanvasDrawArtificialHorizonWidget(displayPort_t *display, display } 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 = { - .pitch = pitchAngle, + .pitch = constrainf(pitchAngle * multiplier, -limit, limit), .roll = rollAngle, }; 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 (!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: { int x, y, w, h; diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index c797565265..d785eac979 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -36,6 +36,8 @@ #include "io/osd_common.h" #include "io/osd_grid.h" +#include "navigation/navigation.h" + #if defined(USE_OSD) #define CANVAS_DEFAULT_GRID_ELEMENT_WIDTH OSD_CHAR_WIDTH @@ -152,3 +154,12 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas) } #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 \ No newline at end of file diff --git a/src/main/io/osd_common.h b/src/main/io/osd_common.h index 054f458a63..2bed67d569 100644 --- a/src/main/io/osd_common.h +++ b/src/main/io/osd_common.h @@ -82,3 +82,8 @@ void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, c // grid slots. void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading); void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas); + +#ifdef USE_GPS +int16_t osdGet3DSpeed(void); +#endif + diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index f506c8b9cf..ed72a973d3 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -58,6 +58,7 @@ #include "io/gps.h" #include "io/osd.h" #include "io/osd_dji_hd.h" +#include "io/osd_common.h" #include "rx/rx.h" @@ -68,6 +69,7 @@ #include "sensors/acceleration.h" #include "sensors/esc_sensor.h" #include "sensors/temperature.h" +#include "sensors/pitotmeter.h" #include "msp/msp.h" #include "msp/msp_protocol.h" @@ -118,11 +120,12 @@ * 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, - .use_name_for_messages = true, - .esc_temperature_source = DJI_OSD_TEMP_ESC, - .proto_workarounds = DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA, + .use_name_for_messages = SETTING_DJI_USE_NAME_FOR_MESSAGES_DEFAULT, + .esc_temperature_source = SETTING_DJI_ESC_TEMP_SOURCE_DEFAULT, + .proto_workarounds = SETTING_DJI_WORKAROUNDS_DEFAULT, + .speedSource = SETTING_DJI_SPEED_SOURCE_DEFAULT, ); // External dependency on looptime @@ -500,6 +503,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR("CLI"); case ARMING_DISABLED_PWM_OUTPUT_ERROR: return OSD_MESSAGE_STR("PWM ERR"); + case ARMING_DISABLED_NO_PREARM: + return OSD_MESSAGE_STR("NO PREARM"); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; @@ -639,13 +644,6 @@ static int32_t osdConvertVelocityToUnit(int32_t vel) 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. * @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]; } - 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); break; case 'S': - osdDJIFormatVelocityStr(djibuf, osdDJIGet3DSpeed()); + osdDJIFormatVelocityStr(djibuf, osdGet3DSpeed()); break; case 'E': 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.lon); 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); break; diff --git a/src/main/io/osd_dji_hd.h b/src/main/io/osd_dji_hd.h index d105b275b6..8405323167 100644 --- a/src/main/io/osd_dji_hd.h +++ b/src/main/io/osd_dji_hd.h @@ -68,6 +68,12 @@ enum djiOsdTempSource_e { 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 { 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 esc_temperature_source; uint8_t proto_workarounds; + uint8_t speedSource; } djiOsdConfig_t; PG_DECLARE(djiOsdConfig_t, djiOsdConfig); diff --git a/src/main/io/piniobox.c b/src/main/io/piniobox.c index b8b01ad329..4fbcc657e4 100644 --- a/src/main/io/piniobox.c +++ b/src/main/io/piniobox.c @@ -33,6 +33,7 @@ #include "fc/fc_msp.h" #include "fc/fc_msp_box.h" +#include "fc/settings.h" #include "io/piniobox.h" @@ -40,7 +41,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pinioBoxConfig_t, pinioBoxConfig, PG_PINIOBOX_CONFIG, 1); 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 { @@ -68,4 +69,4 @@ void pinioBoxUpdate(void) } } -#endif \ No newline at end of file +#endif diff --git a/src/main/io/piniobox.h b/src/main/io/piniobox.h index 74ba398a32..638fd04b4e 100644 --- a/src/main/io/piniobox.h +++ b/src/main/io/piniobox.h @@ -26,6 +26,12 @@ #include "common/time.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 { uint8_t permanentId[PINIO_COUNT]; } pinioBoxConfig_t; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index d8dc546c6e..7c6360c43c 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -49,6 +49,7 @@ #include "io/serial.h" #include "fc/cli.h" +#include "fc/settings.h" #include "msp/msp_serial.h" @@ -147,7 +148,7 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig) } #endif - serialConfig->reboot_character = 'R'; + serialConfig->reboot_character = SETTING_REBOOT_CHARACTER_DEFAULT; } baudRate_e lookupBaudRateIndex(uint32_t baudRate) diff --git a/src/main/io/smartport_master.c b/src/main/io/smartport_master.c index 95e1c74b22..ade2b00acc 100644 --- a/src/main/io/smartport_master.c +++ b/src/main/io/smartport_master.c @@ -34,6 +34,8 @@ FILE_COMPILE_FOR_SPEED #include "drivers/serial.h" #include "drivers/time.h" +#include "fc/settings.h" + #include "io/serial.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_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig, - .halfDuplex = true, - .inverted = false + .halfDuplex = SETTING_SMARTPORT_MASTER_HALFDUPLEX_DEFAULT, + .inverted = SETTING_SMARTPORT_MASTER_INVERTED_DEFAULT ); static serialPort_t *smartportMasterSerialPort = NULL; diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index f4b999a534..eabe0d1f00 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -36,6 +36,7 @@ #include "fc/config.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/failsafe.h" @@ -46,12 +47,12 @@ PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 2); PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, - .band = VTX_SETTINGS_DEFAULT_BAND, - .channel = VTX_SETTINGS_DEFAULT_CHANNEL, - .power = VTX_SETTINGS_DEFAULT_POWER, - .pitModeChan = VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL, - .lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF, - .maxPowerOverride = 0, + .band = SETTING_VTX_BAND_DEFAULT, + .channel = SETTING_VTX_CHANNEL_DEFAULT, + .power = SETTING_VTX_POWER_DEFAULT, + .pitModeChan = SETTING_VTX_PIT_MODE_CHAN_DEFAULT, + .lowPowerDisarm = SETTING_VTX_LOW_POWER_DISARM_DEFAULT, + .maxPowerOverride = SETTING_VTX_MAX_POWER_OVERRIDE_DEFAULT, ); typedef enum { diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index f80b992c29..9a3c2ef633 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -31,6 +31,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "io/beeper.h" #include "io/osd.h" @@ -42,8 +43,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 3); PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, - .halfDuplex = true, - .smartAudioEarlyAkkWorkaroundEnable = true, + .halfDuplex = SETTING_VTX_HALFDUPLEX_DEFAULT, + .smartAudioEarlyAkkWorkaroundEnable = SETTING_VTX_SMARTAUDIO_EARLY_AKK_WORKAROUND_DEFAULT, ); static uint8_t locked = 0; @@ -182,4 +183,3 @@ void vtxCyclePower(const uint8_t powerStep) } #endif - diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index cb057f0fc5..31030573d5 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -43,6 +43,8 @@ #include "drivers/time.h" #include "drivers/vtx_common.h" +#include "fc/settings.h" + #include "io/serial.h" #include "io/vtx.h" #include "io/vtx_control.h" @@ -129,8 +131,8 @@ saPowerTable_t saPowerTable[VTX_SMARTAUDIO_MAX_POWER_COUNT] = { smartAudioDevice_t saDevice = { .version = SA_UNKNOWN, - .channel = -1, - .power = -1, + .channel = SETTING_VTX_CHANNEL_DEFAULT, + .power = SETTING_VTX_POWER_DEFAULT, .mode = 0, .freq = 0, .orfreq = 0, diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 70e394ccb3..d3e7d14585 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -63,6 +63,7 @@ #define MSP2_INAV_GVAR_STATUS 0x2027 #define MSP2_INAV_PROGRAMMING_PID 0x2028 #define MSP2_INAV_SET_PROGRAMMING_PID 0x2029 +#define MSP2_INAV_PROGRAMMING_PID_STATUS 0x202A #define MSP2_PID 0x2030 #define MSP2_SET_PID 0x2031 @@ -79,4 +80,3 @@ #define MSP2_INAV_SET_SAFEHOME 0x2039 #define MSP2_INAV_MISC2 0x203A - diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c old mode 100755 new mode 100644 index 904da3abbe..ba74e49a91 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -39,6 +39,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -91,98 +92,103 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 9); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 10); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { .flags = { - .use_thr_mid_for_althold = 0, - .extra_arming_safety = NAV_EXTRA_ARMING_SAFETY_ON, - .user_control_mode = NAV_GPS_ATTI, - .rth_alt_control_mode = NAV_RTH_AT_LEAST_ALT, - .rth_climb_first = 1, // Climb first, turn after reaching safe altitude - .rth_climb_ignore_emerg = 0, // Ignore GPS loss on initial climb - .rth_tail_first = 0, - .disarm_on_landing = 0, - .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, - .rth_alt_control_override = 0, // Override RTH Altitude and rth_climb_first settings using Pitch and Roll stick - .nav_overrides_motor_stop = NOMS_ALL_NAV, + .use_thr_mid_for_althold = SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD_DEFAULT, + .extra_arming_safety = SETTING_NAV_EXTRA_ARMING_SAFETY_DEFAULT, + .user_control_mode = SETTING_NAV_USER_CONTROL_MODE_DEFAULT, + .rth_alt_control_mode = SETTING_NAV_RTH_ALT_MODE_DEFAULT, + .rth_climb_first = SETTING_NAV_RTH_CLIMB_FIRST_DEFAULT, // Climb first, turn after reaching safe altitude + .rth_climb_ignore_emerg = SETTING_NAV_RTH_CLIMB_IGNORE_EMERG_DEFAULT, // Ignore GPS loss on initial climb + .rth_tail_first = SETTING_NAV_RTH_TAIL_FIRST_DEFAULT, + .disarm_on_landing = SETTING_NAV_DISARM_ON_LANDING_DEFAULT, + .rth_allow_landing = SETTING_NAV_RTH_ALLOW_LANDING_DEFAULT, + .rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick + .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT, }, // General navigation parameters - .pos_failure_timeout = 5, // 5 sec - .waypoint_radius = 100, // 2m diameter - .waypoint_safe_distance = 10000, // centimeters - first waypoint should be closer than this - .max_auto_speed = 300, // 3 m/s = 10.8 km/h - .max_auto_climb_rate = 500, // 5 m/s - .max_manual_speed = 500, - .max_manual_climb_rate = 200, - .land_descent_rate = 200, // centimeters/s - .land_slowdown_minalt = 500, // altitude in centimeters - .land_slowdown_maxalt = 2000, // altitude in meters - .emerg_descent_rate = 500, // centimeters/s - .min_rth_distance = 500, // centimeters, if closer than this land immediately - .rth_altitude = 1000, // altitude in centimeters - .rth_home_altitude = 0, // altitude in centimeters - .rth_abort_threshold = 50000, // centimeters - 500m should be safe for all aircraft - .max_terrain_follow_altitude = 100, // max altitude in centimeters in terrain following mode - .safehome_max_distance = 20000, // Max distance that a safehome is from the arming point + .pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT, // 5 sec + .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter + .waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT, // centimeters - first waypoint should be closer than this + .max_auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // 3 m/s = 10.8 km/h + .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s + .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, + .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT, + .land_descent_rate = SETTING_NAV_LANDING_SPEED_DEFAULT, // centimeters/s + .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters + .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters + .emerg_descent_rate = SETTING_NAV_EMERG_LANDING_SPEED_DEFAULT, // centimeters/s + .min_rth_distance = SETTING_NAV_MIN_RTH_DISTANCE_DEFAULT, // centimeters, if closer than this land immediately + .rth_altitude = SETTING_NAV_RTH_ALTITUDE_DEFAULT, // altitude in centimeters + .rth_home_altitude = SETTING_NAV_RTH_HOME_ALTITUDE_DEFAULT, // altitude in centimeters + .rth_abort_threshold = SETTING_NAV_RTH_ABORT_THRESHOLD_DEFAULT, // centimeters - 500m should be safe for all aircraft + .max_terrain_follow_altitude = SETTING_NAV_MAX_TERRAIN_FOLLOW_ALT_DEFAULT, // max altitude in centimeters in terrain following mode + .safehome_max_distance = SETTING_SAFEHOME_MAX_DISTANCE_DEFAULT, // Max distance that a safehome is from the arming point }, // MC-specific .mc = { - .max_bank_angle = 30, // degrees - .hover_throttle = 1500, - .auto_disarm_delay = 2000, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed - .braking_speed_threshold = 100, // Braking can become active above 1m/s - .braking_disengage_speed = 75, // Stop when speed goes below 0.75m/s - .braking_timeout = 2000, // Timeout barking after 2s - .braking_boost_factor = 100, // A 100% boost by default - .braking_boost_timeout = 750, // Timout boost after 750ms - .braking_boost_speed_threshold = 150, // Boost can happen only above 1.5m/s - .braking_boost_disengage_speed = 100, // Disable boost at 1m/s - .braking_bank_angle = 40, // Max braking angle - .posDecelerationTime = 120, // posDecelerationTime * 100 - .posResponseExpo = 10, // posResponseExpo * 100 + .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees + .hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT, + .auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed + +#ifdef USE_MR_BRAKING_MODE + .braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT, // Braking can become active above 1m/s + .braking_disengage_speed = SETTING_NAV_MC_BRAKING_DISENGAGE_SPEED_DEFAULT, // Stop when speed goes below 0.75m/s + .braking_timeout = SETTING_NAV_MC_BRAKING_TIMEOUT_DEFAULT, // Timeout barking after 2s + .braking_boost_factor = SETTING_NAV_MC_BRAKING_BOOST_FACTOR_DEFAULT, // A 100% boost by default + .braking_boost_timeout = SETTING_NAV_MC_BRAKING_BOOST_TIMEOUT_DEFAULT, // Timout boost after 750ms + .braking_boost_speed_threshold = SETTING_NAV_MC_BRAKING_BOOST_SPEED_THRESHOLD_DEFAULT, // Boost can happen only above 1.5m/s + .braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT, // Disable boost at 1m/s + .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle +#endif + + .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100 + .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100 + .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT, }, // Fixed wing .fw = { - .max_bank_angle = 35, // degrees - .max_climb_angle = 20, // degrees - .max_dive_angle = 15, // degrees - .cruise_throttle = 1400, - .cruise_speed = 0, // cm/s - .control_smoothness = 0, - .max_throttle = 1700, - .min_throttle = 1200, - .pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle) - .pitch_to_throttle_smooth = 6, - .pitch_to_throttle_thresh = 50, - .loiter_radius = 7500, // 75m + .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees + .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees + .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees + .cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT, + .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s + .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT, + .max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT, + .min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT, + .pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT, // pwm units per degree of pitch (10pwm units ~ 1% throttle) + .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT, + .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT, + .loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m //Fixed wing landing - .land_dive_angle = 2, // 2 degrees dive by default + .land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default // Fixed wing launch - .launch_velocity_thresh = 300, // 3 m/s - .launch_accel_thresh = 1.9f * 981, // cm/s/s (1.9*G) - .launch_time_thresh = 40, // 40ms - .launch_throttle = 1700, - .launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP - .launch_motor_timer = 500, // ms - .launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch - .launch_end_time = 3000, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode - .launch_min_time = 0, // ms, min time in launch mode - .launch_timeout = 5000, // ms, timeout for launch procedure - .launch_max_altitude = 0, // cm, altitude where to consider launch ended - .launch_climb_angle = 18, // 18 degrees - .launch_max_angle = 45, // 45 deg - .cruise_yaw_rate = 20, // 20dps - .allow_manual_thr_increase = false, - .useFwNavYawControl = 0, - .yawControlDeadband = 0, + .launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s + .launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G) + .launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms + .launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT, + .launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP + .launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms + .launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch + .launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode + .launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode + .launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure + .launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended + .launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees + .launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg + .cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps + .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT, + .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT, + .yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT, } ); @@ -238,12 +244,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState); @@ -278,15 +284,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mapToFlightModes = 0, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, - .onEvent = { - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + .onEvent = { + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -300,9 +306,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_ALTHOLD_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_ALTHOLD_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -315,14 +321,14 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_ALTHOLD_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_ALTHOLD_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -336,9 +342,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_HOLD_INFINIT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -351,131 +357,131 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_HOLD_INFINIT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, - /** CRUISE_2D mode ************************************************/ - [NAV_STATE_CRUISE_2D_INITIALIZE] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE, + /** CRUISE_HOLD mode ************************************************/ + [NAV_STATE_COURSE_HOLD_INITIALIZE] = { + .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE, + .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE, .timeoutMs = 0, .stateFlags = NAV_REQUIRE_ANGLE, - .mapToFlightModes = NAV_CRUISE_MODE, + .mapToFlightModes = NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_2D_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, - [NAV_STATE_CRUISE_2D_IN_PROGRESS] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS, + [NAV_STATE_COURSE_HOLD_IN_PROGRESS] = { + .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS, + .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS, .timeoutMs = 10, .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_CRUISE_MODE, + .mapToFlightModes = NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_2D_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ] = NAV_STATE_CRUISE_2D_ADJUSTING, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_COURSE_HOLD_ADJUSTING, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, - [NAV_STATE_CRUISE_2D_ADJUSTING] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING, + [NAV_STATE_COURSE_HOLD_ADJUSTING] = { + .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING, + .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING, .timeoutMs = 10, .stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS, - .mapToFlightModes = NAV_CRUISE_MODE, + .mapToFlightModes = NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_2D_IN_PROGRESS, - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_2D_ADJUSTING, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_ADJUSTING, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, /** CRUISE_3D mode ************************************************/ - [NAV_STATE_CRUISE_3D_INITIALIZE] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_STATE_CRUISE_INITIALIZE] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_INITIALIZE, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE, .timeoutMs = 0, .stateFlags = NAV_REQUIRE_ANGLE, - .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE, + .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_3D_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, - [NAV_STATE_CRUISE_3D_IN_PROGRESS] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS, + [NAV_STATE_CRUISE_IN_PROGRESS] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS, .timeoutMs = 10, .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, - .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE, + .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_3D_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ] = NAV_STATE_CRUISE_3D_ADJUSTING, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_CRUISE_ADJUSTING, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, - [NAV_STATE_CRUISE_3D_ADJUSTING] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING, + [NAV_STATE_CRUISE_ADJUSTING] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_ADJUSTING, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING, .timeoutMs = 10, .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT, - .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE, + .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_3D_IN_PROGRESS, - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_3D_ADJUSTING, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_ADJUSTING, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, @@ -489,11 +495,11 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_RTH_START, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -506,14 +512,14 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_RTH_CLIMB, .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HEAD_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HEAD_HOME, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -526,14 +532,14 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -546,15 +552,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LAND_SETTLE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING, [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -567,13 +573,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -586,12 +592,12 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, @@ -604,8 +610,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -618,11 +624,11 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LANDED, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_FINISHED, // re-process state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_FINISHED, // re-process state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, @@ -636,10 +642,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, } }, @@ -652,11 +658,11 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP) - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP) + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, } }, @@ -669,15 +675,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_WP_ENROUTE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_REACHED, // successfully reached waypoint - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_REACHED, // successfully reached waypoint + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -690,19 +696,19 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -715,15 +721,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_HOLD_TIMED, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOLD_TIME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, // successfully reached waypoint - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOLD_TIME, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, // successfully reached waypoint + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -736,15 +742,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_RTH_LAND, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_RTH_LAND, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -757,8 +763,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, } }, @@ -771,13 +777,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_WP_ENROUTE, .mwError = MW_NAV_ERROR_FINISH, .onEvent = { - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -790,15 +796,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -812,10 +818,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_EMERGENCY_LANDING, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there) + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there) } }, @@ -828,10 +834,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_EMERGENCY_LANDING, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there) + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there) } }, @@ -844,8 +850,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LANDED, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -858,9 +864,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_WAIT, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_WAIT, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -873,10 +879,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_WAIT, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_WAIT, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -889,10 +895,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, }; @@ -1009,7 +1015,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( } ///////////////// -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState) { const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); @@ -1029,7 +1035,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState) { const timeMs_t currentTimeMs = millis(); @@ -1039,7 +1045,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n DEBUG_SET(DEBUG_CRUISE, 2, 0); if (posControl.flags.isAdjustingPosition) { - return NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ; + return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ; } // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile @@ -1058,8 +1064,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm] - if ((previousState == NAV_STATE_CRUISE_2D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_2D_ADJUSTING) - || (previousState == NAV_STATE_CRUISE_3D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_3D_ADJUSTING) + if ((previousState == NAV_STATE_COURSE_HOLD_INITIALIZE) || (previousState == NAV_STATE_COURSE_HOLD_ADJUSTING) + || (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING) || posControl.flags.isAdjustingHeading) { calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance); DEBUG_SET(DEBUG_CRUISE, 2, 1); @@ -1073,7 +1079,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n return NAV_FSM_EVENT_NONE; } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState) { UNUSED(previousState); DEBUG_SET(DEBUG_CRUISE, 0, 3); @@ -1090,27 +1096,27 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(nav return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState) { if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState); - return navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(previousState); + return navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(previousState); } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState) { navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState); - return navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(previousState); + return navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(previousState); } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState) { navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState); - return navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(previousState); + return navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(previousState); } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState) @@ -3105,7 +3111,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void switchNavigationFlightModes(void) { const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_CRUISE_MODE) & (~enabledNavFlightModes); + const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -3222,13 +3228,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // PH has priority over CRUISE // CRUISE has priority on AH - if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) { - if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) - return NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D; + if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) + return NAV_FSM_EVENT_SWITCH_TO_CRUISE; - if (FLIGHT_MODE(NAV_CRUISE_MODE) || (canActivatePosHold)) - return NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D; + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold)) + return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD; } @@ -3310,8 +3316,8 @@ bool navigationTerrainFollowingEnabled(void) navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) { - const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE)); - const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)); + const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)); + const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)); if (usedBypass) { *usedBypass = false; @@ -3472,7 +3478,10 @@ void navigationUsePIDs(void) multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f; multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart / 100.0f; multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd / 100.0f; + +#ifdef USE_MR_BRAKING_MODE multicopterPosXyCoefficients.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f; +#endif // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z navPidInit( diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 8d37cd1400..004685cd92 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -210,6 +210,8 @@ typedef struct navConfig_s { uint8_t max_bank_angle; // multicopter max banking angle (deg) uint16_t hover_throttle; // multicopter hover throttle 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_disengage_speed; // below this speed braking will be disengaged 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_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 +#endif + uint8_t posDecelerationTime; // Brake time parameter 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; 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 cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled bool allow_manual_thr_increase; - bool useFwNavYawControl; + bool useFwNavYawControl; uint8_t yawControlDeadband; } fw; } navConfig_t; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 51c25d77cc..40f2f49b0b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -259,7 +259,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait()) && (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG)) && (distanceToActualTarget > 50.0f) - && !FLIGHT_MODE(NAV_CRUISE_MODE); + && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE); // Calculate virtual position for straight movement if (needToCalculateCircularLoiter) { @@ -649,7 +649,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, 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); //if (navStateFlags & NAV_CTL_YAW) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 0b3c1633be..66ee070c1f 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -48,6 +48,8 @@ #include "navigation/navigation.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 LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms #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 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); - if (isBungeeLaunched || isSwingLaunched) { + if (isBungeeLaunched || isSwingLaunched || isForwardLaunched) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED } diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 52235c5930..a0beb4149c 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -406,7 +406,7 @@ bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcR static float getVelocityHeadingAttenuationFactor(void) { // 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 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 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); newVelY = maxSpeed * (newVelY / newVelTotal); newVelTotal = maxSpeed; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 94f612afa5..700909aaa4 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -37,8 +37,10 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/settings.h" #include "flight/imu.h" +#include "flight/secondary_imu.h" #include "io/gps.h" @@ -58,39 +60,39 @@ PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationCo PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, // Inertial position estimator parameters - .automatic_mag_declination = 1, - .reset_altitude_type = NAV_RESET_ON_FIRST_ARM, - .reset_home_type = NAV_RESET_ON_FIRST_ARM, - .gravity_calibration_tolerance = 5, // 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_no_baro = 0, // Use GPS altitude if no baro is available on all aircrafts - .allow_dead_reckoning = 0, + .automatic_mag_declination = SETTING_INAV_AUTO_MAG_DECL_DEFAULT, + .reset_altitude_type = SETTING_INAV_RESET_ALTITUDE_DEFAULT, + .reset_home_type = SETTING_INAV_RESET_HOME_DEFAULT, + .gravity_calibration_tolerance = SETTING_INAV_GRAVITY_CAL_TOLERANCE_DEFAULT, // 5 cm/s/s calibration error accepted (0.5% of gravity) + .use_gps_velned = SETTING_INAV_USE_GPS_VELNED_DEFAULT, // "Disabled" is mandatory with gps_dyn_model = Pedestrian + .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 = 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_v = 6.100f, + .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT, + .w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT, - .w_z_gps_p = 0.2f, - .w_z_gps_v = 0.1f, + .w_z_gps_p = SETTING_INAV_W_Z_GPS_P_DEFAULT, + .w_z_gps_v = SETTING_INAV_W_Z_GPS_V_DEFAULT, - .w_xy_gps_p = 1.0f, - .w_xy_gps_v = 2.0f, + .w_xy_gps_p = SETTING_INAV_W_XY_GPS_P_DEFAULT, + .w_xy_gps_v = SETTING_INAV_W_XY_GPS_V_DEFAULT, - .w_xy_flow_p = 1.0f, - .w_xy_flow_v = 2.0f, + .w_xy_flow_p = SETTING_INAV_W_XY_FLOW_P_DEFAULT, + .w_xy_flow_v = SETTING_INAV_W_XY_FLOW_V_DEFAULT, - .w_z_res_v = 0.5f, - .w_xy_res_v = 0.5f, + .w_z_res_v = SETTING_INAV_W_Z_RES_V_DEFAULT, + .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, - .baro_epv = 100.0f + .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, + .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT ); #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } @@ -214,7 +216,11 @@ void onNewGPSData(void) if(STATE(GPS_FIX_HOME)){ static bool magDeclinationSet = false; 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; } } @@ -569,7 +575,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) 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 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; } @@ -606,6 +612,15 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) 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; } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 6e7c4d3c0c..9848dfefbe 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -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_HOVER_ABOVE_HOME, - NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D, - NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D, - NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ, + NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD, + NAV_FSM_EVENT_SWITCH_TO_CRUISE, + NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ, NAV_FSM_EVENT_COUNT, } navigationFSMEvent_t; @@ -187,13 +187,13 @@ typedef enum { NAV_PERSISTENT_ID_UNUSED_3 = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS = 28, - NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE = 29, - NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS = 30, - NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING = 31, + NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE = 29, + NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS = 30, + NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING = 31, - NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE = 32, - NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS = 33, - NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING = 34, + NAV_PERSISTENT_ID_CRUISE_INITIALIZE = 32, + NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS = 33, + NAV_PERSISTENT_ID_CRUISE_ADJUSTING = 34, NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35, NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36, @@ -239,12 +239,12 @@ typedef enum { NAV_STATE_LAUNCH_WAIT, NAV_STATE_LAUNCH_IN_PROGRESS, - NAV_STATE_CRUISE_2D_INITIALIZE, - NAV_STATE_CRUISE_2D_IN_PROGRESS, - NAV_STATE_CRUISE_2D_ADJUSTING, - NAV_STATE_CRUISE_3D_INITIALIZE, - NAV_STATE_CRUISE_3D_IN_PROGRESS, - NAV_STATE_CRUISE_3D_ADJUSTING, + NAV_STATE_COURSE_HOLD_INITIALIZE, + NAV_STATE_COURSE_HOLD_IN_PROGRESS, + NAV_STATE_COURSE_HOLD_ADJUSTING, + NAV_STATE_CRUISE_INITIALIZE, + NAV_STATE_CRUISE_IN_PROGRESS, + NAV_STATE_CRUISE_ADJUSTING, NAV_STATE_COUNT, } navigationFSMState_t; diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index d4465c4cec..89a1fc6fe6 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -44,6 +44,8 @@ #include "flight/imu.h" #include "flight/pid.h" #include "drivers/io_port_expander.h" +#include "io/osd_common.h" +#include "sensors/diagnostics.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -317,6 +319,20 @@ static int logicConditionCompute( return true; 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: return false; break; @@ -377,7 +393,7 @@ static int logicConditionGetFlightOperandValue(int operand) { return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); break; - case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 10 + case LOGIC_CONDITION_OPERAND_FLIGHT_VBAT: // V / 100 return getBatteryVoltage(); break; @@ -393,7 +409,15 @@ static int logicConditionGetFlightOperandValue(int operand) { break; 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; case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s @@ -402,7 +426,7 @@ static int logicConditionGetFlightOperandValue(int operand) { //FIXME align with osdGet3DSpeed case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s - return (int) sqrtf(sq(gpsSol.groundSpeed) + sq((int)getEstimatedActualVelocity(Z))); + return osdGet3DSpeed(); break; case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s @@ -418,7 +442,7 @@ static int logicConditionGetFlightOperandValue(int operand) { break; 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; case LOGIC_CONDITION_OPERAND_FLIGHT_TROTTLE_POS: // % @@ -536,7 +560,7 @@ static int logicConditionGetFlightModeOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE: - return (bool) FLIGHT_MODE(NAV_CRUISE_MODE); + return (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE); break; case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD: diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index fc38839d71..005c7a85f4 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -68,7 +68,9 @@ typedef enum { LOGIC_CONDITION_MAP_INPUT = 36, LOGIC_CONDITION_MAP_OUTPUT = 37, 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; typedef enum logicOperandType_s { @@ -117,6 +119,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33 + LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34 } logicFlightOperands_e; @@ -193,4 +196,4 @@ void logicConditionReset(void); float getThrottleScale(float globalThrottleScale); int16_t getRcCommandOverride(int16_t command[], uint8_t axis); -int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); \ No newline at end of file +int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); diff --git a/src/main/programming/pid.c b/src/main/programming/pid.c index 09916fbb1b..8b284087eb 100644 --- a/src/main/programming/pid.c +++ b/src/main/programming/pid.c @@ -110,7 +110,7 @@ void programmingPidInit(void) } } -int programmingPidGetOutput(uint8_t i) { +int32_t programmingPidGetOutput(uint8_t i) { return programmingPidState[constrain(i, 0, MAX_PROGRAMMING_PID_COUNT)].output; } diff --git a/src/main/programming/pid.h b/src/main/programming/pid.h index 8645de8f13..0d55b952c6 100644 --- a/src/main/programming/pid.h +++ b/src/main/programming/pid.h @@ -51,4 +51,4 @@ typedef struct programmingPidState_s { void programmingPidUpdateTask(timeUs_t currentTimeUs); void programmingPidInit(void); void programmingPidReset(void); -int programmingPidGetOutput(uint8_t i); \ No newline at end of file +int32_t programmingPidGetOutput(uint8_t i); \ No newline at end of file diff --git a/src/main/rx/eleres.c b/src/main/rx/eleres.c index 638818c531..705c4b7e15 100755 --- a/src/main/rx/eleres.c +++ b/src/main/rx/eleres.c @@ -72,12 +72,12 @@ PG_REGISTER_WITH_RESET_TEMPLATE(eleresConfig_t, eleresConfig, PG_ELERES_CONFIG, 0); PG_RESET_TEMPLATE(eleresConfig_t, eleresConfig, - .eleresFreq = 435, - .eleresTelemetryEn = 0, - .eleresTelemetryPower = 7, - .eleresLocEn = 0, - .eleresLocPower = 7, - .eleresLocDelay = 240 + .eleresFreq = SETTING_ELERES_FREQ_DEFAULT, + .eleresTelemetryEn = SETTING_ELERES_TELEMETRY_EN_DEFAULT, + .eleresTelemetryPower = SETTING_ELERES_TELEMETRY_POWER_DEFAULT, + .eleresLocEn = SETTING_ELERES_LOC_EN_DEFAULT, + .eleresLocPower = SETTING_ELERES_LOC_POWER_DEFAULT, + .eleresLocDelay = SETTING_ELERES_LOC_DELAY_DEFAULT ); static uint8_t hoppingChannel = 1; @@ -784,8 +784,6 @@ void eleresInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) channelHopping(1); rfMode = RECEIVE; localizerTime = millis() + (1000L * eleresConfig()->eleresLocDelay); - - return true; } uint8_t eleresBind(void) diff --git a/src/main/rx/ghst_protocol.h b/src/main/rx/ghst_protocol.h index e40986384b..e5616c02fc 100644 --- a/src/main/rx/ghst_protocol.h +++ b/src/main/rx/ghst_protocol.h @@ -65,6 +65,8 @@ typedef enum { GHST_DL_LINK_STAT = 0x21, GHST_DL_VTX_STAT = 0x22, GHST_DL_PACK_STAT = 0x23, // Battery (Pack) Status + GHST_DL_GPS_PRIMARY = 0x25, // Primary GPS data (position) + GHST_DL_GPS_SECONDARY = 0x26 // Secondary GPS data (auxiliary) } ghstDl_e; #define GHST_RC_CTR_VAL_12BIT 0x7C0 // servo center for 12 bit values (0x3e0 << 1) @@ -72,9 +74,12 @@ typedef enum { #define GHST_FRAME_SIZE 14 // including addr, type, len, crc, and payload -#define GHST_PAYLOAD_SIZE_MAX 14 +#define GHST_PAYLOAD_SIZE_MAX 14 -#define GHST_FRAME_SIZE_MAX 24 +#define GHST_FRAME_SIZE_MAX 24 + +#define GPS_FLAGS_FIX 0x01 +#define GPS_FLAGS_FIX_HOME 0x02 typedef struct ghstFrameDef_s { uint8_t addr; diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f028e73330..c8e1184594 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -45,6 +45,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" +#include "fc/settings.h" #include "flight/failsafe.h" @@ -125,26 +126,32 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 9); PG_RESET_TEMPLATE(rxConfig_t, rxConfig, .receiverType = DEFAULT_RX_TYPE, .rcmap = {0, 1, 3, 2}, // Default to AETR map - .halfDuplex = TRISTATE_AUTO, + .halfDuplex = SETTING_SERIALRX_HALFDUPLEX_DEFAULT, .serialrx_provider = SERIALRX_PROVIDER, +#ifdef USE_RX_SPI .rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL, - .spektrum_sat_bind = 0, - .serialrx_inverted = 0, - .mincheck = 1100, - .maxcheck = 1900, - .rx_min_usec = RX_MIN_USEX, // any of first 4 channels below this value will trigger rx loss detection - .rx_max_usec = 2115, // any of first 4 channels above this value will trigger rx loss detection - .rssi_channel = 0, - .rssiMin = RSSI_VISIBLE_VALUE_MIN, - .rssiMax = RSSI_VISIBLE_VALUE_MAX, - .sbusSyncInterval = SBUS_DEFAULT_INTERFRAME_DELAY_US, - .rcFilterFrequency = 50, -#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - .mspOverrideChannels = 15, #endif - .rssi_source = RSSI_SOURCE_AUTO, - .srxl2_unit_id = 1, - .srxl2_baud_fast = 1, +#ifdef USE_SPEKTRUM_BIND + .spektrum_sat_bind = SETTING_SPEKTRUM_SAT_BIND_DEFAULT, +#endif + .serialrx_inverted = SETTING_SERIALRX_INVERTED_DEFAULT, + .mincheck = SETTING_MIN_CHECK_DEFAULT, + .maxcheck = SETTING_MAX_CHECK_DEFAULT, + .rx_min_usec = SETTING_RX_MIN_USEC_DEFAULT, // any of first 4 channels below this value will trigger rx loss detection + .rx_max_usec = SETTING_RX_MAX_USEC_DEFAULT, // any of first 4 channels above this value will trigger rx loss detection + .rssi_channel = SETTING_RSSI_CHANNEL_DEFAULT, + .rssiMin = SETTING_RSSI_MIN_DEFAULT, + .rssiMax = SETTING_RSSI_MAX_DEFAULT, + .sbusSyncInterval = SETTING_SBUS_SYNC_INTERVAL_DEFAULT, + .rcFilterFrequency = SETTING_RC_FILTER_FREQUENCY_DEFAULT, +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + .mspOverrideChannels = SETTING_MSP_OVERRIDE_CHANNELS_DEFAULT, +#endif + .rssi_source = SETTING_RSSI_SOURCE_DEFAULT, +#ifdef USE_SERIALRX_SRXL2 + .srxl2_unit_id = SETTING_SRXL2_UNIT_ID_DEFAULT, + .srxl2_baud_fast = SETTING_SRXL2_BAUD_FAST_DEFAULT, +#endif ); void resetAllRxChannelRangeConfigurations(void) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 0aee1e8353..dcc1804f5d 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -114,11 +114,15 @@ typedef struct rxConfig_s { uint8_t serialrx_provider; // Type of UART-based receiver (rxSerialReceiverType_e enum). Only used if receiverType is RX_TYPE_SERIAL uint8_t serialrx_inverted; // Flip the default inversion of the protocol - e.g. sbus (Futaba, FrSKY) is inverted if this is false, uninverted if it's true. Support for uninverted OpenLRS (and modified FrSKY) receivers. uint8_t halfDuplex; // allow rx to operate in half duplex mode. From tristate_e. +#ifdef USE_RX_SPI uint8_t rx_spi_protocol; // type of SPI receiver protocol (rx_spi_protocol_e enum). Only used if receiverType is RX_TYPE_SPI uint32_t rx_spi_id; uint8_t rx_spi_rf_channel_count; +#endif +#ifdef USE_SPEKTRUM_BIND uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot +#endif uint8_t rssi_channel; uint8_t rssiMin; // minimum RSSI sent by the RX - [RSSI_VISIBLE_VALUE_MIN, RSSI_VISIBLE_VALUE_MAX] uint8_t rssiMax; // maximum RSSI sent by the RX - [RSSI_VISIBLE_VALUE_MIN, RSSI_VISIBLE_VALUE_MAX] @@ -130,8 +134,10 @@ typedef struct rxConfig_s { uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness) uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active uint8_t rssi_source; +#ifdef USE_SERIALRX_SRXL2 uint8_t srxl2_unit_id; uint8_t srxl2_baud_fast; +#endif } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 233eaec99c..0ad456967c 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -119,6 +119,9 @@ typedef enum { #endif #ifdef USE_IRLOCK TASK_IRLOCK, +#endif +#ifdef USE_SECONDARY_IMU + TASK_SECONDARY_IMU, #endif /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 9ff00c4529..aeb6af2965 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -50,6 +50,7 @@ FILE_COMPILE_FOR_SPEED #include "drivers/accgyro/accgyro_adxl345.h" #include "drivers/accgyro/accgyro_mma845x.h" #include "drivers/accgyro/accgyro_bma280.h" +#include "drivers/accgyro/accgyro_bmi088.h" #include "drivers/accgyro/accgyro_bmi160.h" #include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_fake.h" @@ -57,6 +58,7 @@ FILE_COMPILE_FOR_SPEED #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "io/beeper.h" @@ -92,22 +94,22 @@ PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELER void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) { RESET_CONFIG_2(accelerometerConfig_t, instance, - .acc_align = ALIGN_DEFAULT, - .acc_hardware = ACC_AUTODETECT, - .acc_lpf_hz = 15, - .acc_notch_hz = 0, - .acc_notch_cutoff = 1, - .acc_soft_lpf_type = FILTER_BIQUAD + .acc_align = SETTING_ALIGN_ACC_DEFAULT, + .acc_hardware = SETTING_ACC_HARDWARE_DEFAULT, + .acc_lpf_hz = SETTING_ACC_LPF_HZ_DEFAULT, + .acc_notch_hz = SETTING_ACC_NOTCH_HZ_DEFAULT, + .acc_notch_cutoff = SETTING_ACC_NOTCH_CUTOFF_DEFAULT, + .acc_soft_lpf_type = SETTING_ACC_LPF_TYPE_DEFAULT ); RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accZero, - .raw[X] = 0, - .raw[Y] = 0, - .raw[Z] = 0 + .raw[X] = SETTING_ACCZERO_X_DEFAULT, + .raw[Y] = SETTING_ACCZERO_Y_DEFAULT, + .raw[Z] = SETTING_ACCZERO_Z_DEFAULT ); RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accGain, - .raw[X] = 4096, - .raw[Y] = 4096, - .raw[Z] = 4096 + .raw[X] = SETTING_ACCGAIN_X_DEFAULT, + .raw[Y] = SETTING_ACCGAIN_Y_DEFAULT, + .raw[Z] = SETTING_ACCGAIN_Z_DEFAULT ); } @@ -240,6 +242,19 @@ static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) FALLTHROUGH; #endif +#if defined(USE_IMU_BMI088) + case ACC_BMI088: + if (bmi088AccDetect(dev)) { + accHardware = ACC_BMI088; + break; + } + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (accHardwareToUse != ACC_AUTODETECT) { + break; + } + FALLTHROUGH; +#endif + #ifdef USE_IMU_ICM20689 case ACC_ICM20689: if (icm20689AccDetect(dev)) { diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 47f669b83a..a650b4be95 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -45,7 +45,8 @@ typedef enum { ACC_MPU9250 = 9, ACC_BMI160 = 10, ACC_ICM20689 = 11, - ACC_FAKE = 12, + ACC_BMI088 = 12, + ACC_FAKE = 13, ACC_MAX = ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index bb1325aeeb..63532a3986 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -44,6 +44,7 @@ #include "drivers/time.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "sensors/barometer.h" #include "sensors/sensors.h" @@ -56,21 +57,16 @@ baro_t baro; // barometer access functions +#ifdef USE_BARO + PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3); -#ifdef USE_BARO -#define BARO_HARDWARE_DEFAULT BARO_AUTODETECT -#else -#define BARO_HARDWARE_DEFAULT BARO_NONE -#endif PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, - .baro_hardware = BARO_HARDWARE_DEFAULT, - .use_median_filtering = 1, - .baro_calibration_tolerance = 150 + .baro_hardware = SETTING_BARO_HARDWARE_DEFAULT, + .use_median_filtering = SETTING_BARO_MEDIAN_FILTER_DEFAULT, + .baro_calibration_tolerance = SETTING_BARO_CAL_TOLERANCE_DEFAULT ); -#ifdef USE_BARO - static zeroCalibrationScalar_t zeroCalibration; static float baroGroundAltitude = 0; static float baroGroundPressure = 101325.0f; // 101325 pascal, 1 standard atmosphere diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 2e365fb115..3c04c86e46 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -37,14 +37,6 @@ typedef enum { BARO_MAX = BARO_FAKE } baroSensor_e; -typedef struct barometerConfig_s { - uint8_t baro_hardware; // Barometer hardware to use - uint8_t use_median_filtering; // Use 3-point median filtering - uint16_t baro_calibration_tolerance; // Baro calibration tolerance (cm at sea level) -} barometerConfig_t; - -PG_DECLARE(barometerConfig_t, barometerConfig); - typedef struct baro_s { baroDev_t dev; int32_t BaroAlt; @@ -54,6 +46,17 @@ typedef struct baro_s { extern baro_t baro; +#ifdef USE_BARO + +typedef struct barometerConfig_s { + uint8_t baro_hardware; // Barometer hardware to use + uint8_t use_median_filtering; // Use 3-point median filtering + uint16_t baro_calibration_tolerance; // Baro calibration tolerance (cm at sea level) +} barometerConfig_t; + +PG_DECLARE(barometerConfig_t, barometerConfig); + + bool baroInit(void); bool baroIsCalibrationComplete(void); void baroStartCalibration(void); @@ -62,3 +65,5 @@ int32_t baroCalculateAltitude(void); int32_t baroGetLatestAltitude(void); int16_t baroGetTemperature(void); bool baroIsHealthy(void); + +#endif diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 01d2582ded..fe54342561 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -39,6 +39,7 @@ #include "fc/fc_core.h" #include "fc/runtime_config.h" #include "fc/stats.h" +#include "fc/settings.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -97,20 +98,22 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) { for (int i = 0; i < MAX_BATTERY_PROFILE_COUNT; i++) { RESET_CONFIG(batteryProfile_t, &instance[i], - .cells = 0, +#ifdef USE_ADC + .cells = SETTING_BAT_CELLS_DEFAULT, .voltage = { - .cellDetect = 430, - .cellMax = 420, - .cellMin = 330, - .cellWarning = 350 + .cellDetect = SETTING_VBAT_CELL_DETECT_VOLTAGE_DEFAULT, + .cellMax = SETTING_VBAT_MAX_CELL_VOLTAGE_DEFAULT, + .cellMin = SETTING_VBAT_MIN_CELL_VOLTAGE_DEFAULT, + .cellWarning = SETTING_VBAT_WARNING_CELL_VOLTAGE_DEFAULT }, +#endif .capacity = { - .value = 0, - .warning = 0, - .critical = 0, - .unit = BAT_CAPACITY_UNIT_MAH, + .value = SETTING_BATTERY_CAPACITY_DEFAULT, + .warning = SETTING_BATTERY_CAPACITY_WARNING_DEFAULT, + .critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT, + .unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT, } ); } @@ -120,24 +123,26 @@ PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_B PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, +#ifdef USE_ADC .voltage = { - .type = VOLTAGE_SENSOR_ADC, + .type = SETTING_VBAT_METER_TYPE_DEFAULT, .scale = VBAT_SCALE_DEFAULT, }, +#endif .current = { - .type = CURRENT_SENSOR_ADC, + .type = SETTING_CURRENT_METER_TYPE_DEFAULT, .scale = CURRENT_METER_SCALE, .offset = CURRENT_METER_OFFSET }, - .voltageSource = BAT_VOLTAGE_RAW, + .voltageSource = SETTING_BAT_VOLTAGE_SRC_DEFAULT, - .cruise_power = 0, - .idle_power = 0, - .rth_energy_margin = 5, + .cruise_power = SETTING_CRUISE_POWER_DEFAULT, + .idle_power = SETTING_IDLE_POWER_DEFAULT, + .rth_energy_margin = SETTING_RTH_ENERGY_MARGIN_DEFAULT, - .throttle_compensation_weight = 1.0f + .throttle_compensation_weight = SETTING_THR_COMP_WEIGHT_DEFAULT ); @@ -150,6 +155,7 @@ void batteryInit(void) batteryCriticalVoltage = 0; } +#ifdef USE_ADC // profileDetect() profile sorting compare function static int profile_compare(profile_comp_t *a, profile_comp_t *b) { if (a->max_voltage < b->max_voltage) @@ -182,6 +188,7 @@ static int8_t profileDetect(void) { // No matching profile found return -1; } +#endif void setBatteryProfile(uint8_t profileIndex) { @@ -202,6 +209,7 @@ void activateBatteryProfile(void) } } +#ifdef USE_ADC static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) { static pt1Filter_t vbatFilterState; @@ -266,7 +274,11 @@ void batteryUpdate(timeUs_t timeDelta) batteryCellCount = currentBatteryProfile->cells; else { batteryCellCount = (vbat / currentBatteryProfile->voltage.cellDetect) + 1; - if (batteryCellCount > 8) batteryCellCount = 8; // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) + // Assume there are no 7S, 9S and 11S batteries so round up to 8S, 10S and 12S respectively + if (batteryCellCount == 7 || batteryCellCount == 9 || batteryCellCount == 11) { + batteryCellCount += 1; + } + batteryCellCount = MIN(batteryCellCount, 12); } batteryFullVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMax; @@ -340,6 +352,7 @@ void batteryUpdate(timeUs_t timeDelta) } } } +#endif batteryState_e getBatteryState(void) { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 353f537d01..94201fb28c 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -69,10 +69,12 @@ typedef enum { typedef struct batteryMetersConfig_s { +#ifdef USE_ADC struct { uint16_t scale; voltageSensor_e type; } voltage; +#endif struct { int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A @@ -92,6 +94,7 @@ typedef struct batteryMetersConfig_s { typedef struct batteryProfile_s { +#ifdef USE_ADC uint8_t cells; struct { @@ -100,6 +103,7 @@ typedef struct batteryProfile_s { uint16_t cellMin; // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.3V) uint16_t cellWarning; // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.5V) } voltage; +#endif struct { uint32_t value; // mAh or mWh (see capacity.unit) @@ -151,10 +155,13 @@ int32_t getPower(void); int32_t getMAhDrawn(void); int32_t getMWhDrawn(void); +#ifdef USE_ADC void batteryUpdate(timeUs_t timeDelta); -void currentMeterUpdate(timeUs_t timeDelta); void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta); void powerMeterUpdate(timeUs_t timeDelta); +#endif + +void currentMeterUpdate(timeUs_t timeDelta); uint8_t calculateBatteryPercentage(void); float calculateThrottleCompensationFactor(void); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 90a108109d..8e1da170b9 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -40,6 +40,7 @@ #include "drivers/compass/compass_qmc5883l.h" #include "drivers/compass/compass_mpu9250.h" #include "drivers/compass/compass_lis3mdl.h" +#include "drivers/compass/compass_rm3100.h" #include "drivers/compass/compass_msp.h" #include "drivers/io.h" #include "drivers/light_led.h" @@ -47,6 +48,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "io/gps.h" #include "io/beeper.h" @@ -58,27 +60,24 @@ mag_t mag; // mag access functions +#ifdef USE_MAG + PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 4); -#ifdef USE_MAG -#define MAG_HARDWARE_DEFAULT MAG_AUTODETECT -#else -#define MAG_HARDWARE_DEFAULT MAG_NONE -#endif PG_RESET_TEMPLATE(compassConfig_t, compassConfig, - .mag_align = ALIGN_DEFAULT, - .mag_hardware = MAG_HARDWARE_DEFAULT, - .mag_declination = 0, - .mag_to_use = 0, - .magCalibrationTimeLimit = 30, - .rollDeciDegrees = 0, - .pitchDeciDegrees = 0, - .yawDeciDegrees = 0, - .magGain = {1024, 1024, 1024}, + .mag_align = SETTING_ALIGN_MAG_DEFAULT, + .mag_hardware = SETTING_MAG_HARDWARE_DEFAULT, + .mag_declination = SETTING_MAG_DECLINATION_DEFAULT, +#ifdef USE_DUAL_MAG + .mag_to_use = SETTING_MAG_TO_USE_DEFAULT, +#endif + .magCalibrationTimeLimit = SETTING_MAG_CALIBRATION_TIME_DEFAULT, + .rollDeciDegrees = SETTING_ALIGN_MAG_ROLL_DEFAULT, + .pitchDeciDegrees = SETTING_ALIGN_MAG_PITCH_DEFAULT, + .yawDeciDegrees = SETTING_ALIGN_MAG_YAW_DEFAULT, + .magGain = {SETTING_MAGGAIN_X_DEFAULT, SETTING_MAGGAIN_Y_DEFAULT, SETTING_MAGGAIN_Z_DEFAULT}, ); -#ifdef USE_MAG - static uint8_t magUpdatedAtLeastOnce = 0; bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) @@ -264,6 +263,22 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) } FALLTHROUGH; + case MAG_RM3100: +#ifdef USE_MAG_RM3100 + if (rm3100MagDetect(dev)) { +#ifdef MAG_RM3100_ALIGN + dev->magAlign.onBoard = MAG_RM3100_ALIGN; +#endif + magHardware = MAG_RM3100; + break; + } +#endif + /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */ + if (magHardwareToUse != MAG_AUTODETECT) { + break; + } + FALLTHROUGH; + case MAG_FAKE: #ifdef USE_FAKE_MAG if (fakeMagDetect(dev)) { @@ -350,7 +365,7 @@ void compassUpdate(timeUs_t currentTimeUs) static sensorCalibrationState_t calState; static timeUs_t calStartedAt = 0; static int16_t magPrev[XYZ_AXIS_COUNT]; - static int magGain[XYZ_AXIS_COUNT] = {-4096, -4096, -4096}; + static int magAxisDeviation[XYZ_AXIS_COUNT]; // Check magZero if ( @@ -381,6 +396,7 @@ void compassUpdate(timeUs_t currentTimeUs) compassConfigMutable()->magZero.raw[axis] = 0; compassConfigMutable()->magGain[axis] = 1024; magPrev[axis] = 0; + magAxisDeviation[axis] = 0; // Gain is based on the biggest absolute deviation from the mag zero point. Gain computation starts at 0 } beeper(BEEPER_ACTION_SUCCESS); @@ -400,9 +416,9 @@ void compassUpdate(timeUs_t currentTimeUs) diffMag += (mag.magADC[axis] - magPrev[axis]) * (mag.magADC[axis] - magPrev[axis]); avgMag += (mag.magADC[axis] + magPrev[axis]) * (mag.magADC[axis] + magPrev[axis]) / 4.0f; - const int32_t sample = ABS(mag.magADC[axis]); - if (sample > magGain[axis]) { - magGain[axis] = sample; + // Find the biggest sample deviation together with sample' sign + if (ABS(mag.magADC[axis]) > ABS(magAxisDeviation[axis])) { + magAxisDeviation[axis] = mag.magADC[axis]; } } @@ -429,7 +445,7 @@ void compassUpdate(timeUs_t currentTimeUs) * It is dirty, but worth checking if this will solve the problem of changing mag vector when UAV is tilted */ for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - compassConfigMutable()->magGain[axis] = magGain[axis] - compassConfig()->magZero.raw[axis]; + compassConfigMutable()->magGain[axis] = ABS(magAxisDeviation[axis] - compassConfig()->magZero.raw[axis]); } calStartedAt = 0; diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index f715638107..baba7bf6a1 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -26,7 +26,6 @@ #include "sensors/sensors.h" - // Type of magnetometer used/detected typedef enum { MAG_NONE = 0, @@ -42,7 +41,8 @@ typedef enum { MAG_IST8308 = 10, MAG_LIS3MDL = 11, MAG_MSP = 12, - MAG_FAKE = 13, + MAG_RM3100 = 13, + MAG_FAKE = 14, MAG_MAX = MAG_FAKE } magSensor_e; @@ -53,6 +53,8 @@ typedef struct mag_s { extern mag_t mag; +#ifdef USE_MAG + typedef struct compassConfig_s { int16_t mag_declination; // Get your magnetic declination from here : http://magnetic-declination.com/ // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. @@ -60,7 +62,9 @@ typedef struct compassConfig_s { uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device flightDynamicsTrims_t magZero; int16_t magGain[XYZ_AXIS_COUNT]; +#ifdef USE_DUAL_MAG uint8_t mag_to_use; +#endif uint8_t magCalibrationTimeLimit; // Time for compass calibration (seconds) int16_t rollDeciDegrees; // Alignment for external mag on the roll (X) axis (0.1deg) int16_t pitchDeciDegrees; // Alignment for external mag on the pitch (Y) axis (0.1deg) @@ -74,3 +78,5 @@ bool compassInit(void); void compassUpdate(timeUs_t currentTimeUs); bool compassIsReady(void); bool compassIsHealthy(void); + +#endif diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index bd6e9886ec..672428b615 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -46,6 +46,7 @@ #include "io/serial.h" #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #if defined(USE_ESC_SENSOR) @@ -80,7 +81,7 @@ static bool escSensorDataNeedsUpdate; PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 1); PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, .currentOffset = 0, // UNUSED - .listenOnly = 0, + .listenOnly = SETTING_ESC_SENSOR_LISTEN_ONLY_DEFAULT, ); static int getTelemetryMotorCount(void) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 3428d118a4..6a8710bff2 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -52,6 +52,7 @@ FILE_COMPILE_FOR_SPEED #include "drivers/accgyro/accgyro_adxl345.h" #include "drivers/accgyro/accgyro_mma845x.h" #include "drivers/accgyro/accgyro_bma280.h" +#include "drivers/accgyro/accgyro_bmi088.h" #include "drivers/accgyro/accgyro_bmi160.h" #include "drivers/accgyro/accgyro_icm20689.h" #include "drivers/accgyro/accgyro_fake.h" @@ -60,6 +61,7 @@ FILE_COMPILE_FOR_SPEED #include "fc/config.h" #include "fc/runtime_config.h" #include "fc/rc_controls.h" +#include "fc/settings.h" #include "io/beeper.h" #include "io/statusindicator.h" @@ -105,26 +107,30 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, - .gyro_lpf = GYRO_LPF_256HZ, - .gyro_soft_lpf_hz = 60, - .gyro_soft_lpf_type = FILTER_BIQUAD, - .gyro_align = ALIGN_DEFAULT, - .gyroMovementCalibrationThreshold = 32, - .looptime = 1000, - .gyroSync = 1, - .gyro_to_use = 0, - .gyro_notch_hz = 0, - .gyro_notch_cutoff = 1, - .gyro_stage2_lowpass_hz = 0, - .gyro_stage2_lowpass_type = FILTER_BIQUAD, - .useDynamicLpf = 0, - .gyroDynamicLpfMinHz = 200, - .gyroDynamicLpfMaxHz = 500, - .gyroDynamicLpfCurveExpo = 5, - .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM, - .dynamicGyroNotchQ = 120, - .dynamicGyroNotchMinHz = 150, - .dynamicGyroNotchEnabled = 0, + .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, + .gyro_soft_lpf_hz = SETTING_GYRO_LPF_HZ_DEFAULT, + .gyro_soft_lpf_type = SETTING_GYRO_LPF_TYPE_DEFAULT, + .gyro_align = SETTING_ALIGN_GYRO_DEFAULT, + .gyroMovementCalibrationThreshold = SETTING_MORON_THRESHOLD_DEFAULT, + .looptime = SETTING_LOOPTIME_DEFAULT, + .gyroSync = SETTING_GYRO_SYNC_DEFAULT, +#ifdef USE_DUAL_GYRO + .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT, +#endif + .gyro_notch_hz = SETTING_GYRO_NOTCH_HZ_DEFAULT, + .gyro_notch_cutoff = SETTING_GYRO_NOTCH_CUTOFF_DEFAULT, + .gyro_stage2_lowpass_hz = SETTING_GYRO_STAGE2_LOWPASS_HZ_DEFAULT, + .gyro_stage2_lowpass_type = SETTING_GYRO_STAGE2_LOWPASS_TYPE_DEFAULT, + .useDynamicLpf = SETTING_GYRO_USE_DYN_LPF_DEFAULT, + .gyroDynamicLpfMinHz = SETTING_GYRO_DYN_LPF_MIN_HZ_DEFAULT, + .gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT, + .gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT, +#ifdef USE_DYNAMIC_FILTERS + .dynamicGyroNotchRange = SETTING_DYNAMIC_GYRO_NOTCH_RANGE_DEFAULT, + .dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT, + .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT, + .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT, +#endif ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) @@ -207,6 +213,15 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard FALLTHROUGH; #endif +#ifdef USE_IMU_BMI088 + case GYRO_BMI088: + if (bmi088GyroDetect(dev)) { + gyroHardware = GYRO_BMI088; + break; + } + FALLTHROUGH; +#endif + #ifdef USE_IMU_ICM20689 case GYRO_ICM20689: if (icm20689GyroDetect(dev)) { @@ -526,4 +541,4 @@ void gyroUpdateDynamicLpf(float cutoffFreq) { biquadFilterUpdate(&gyroLpfState[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF); } } -} \ No newline at end of file +} diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 737055af1f..6aa3c11414 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -36,6 +36,7 @@ typedef enum { GYRO_MPU9250, GYRO_BMI160, GYRO_ICM20689, + GYRO_BMI088, GYRO_FAKE } gyroSensor_e; @@ -60,12 +61,14 @@ extern gyro_t gyro; typedef struct gyroConfig_s { sensor_align_e gyro_align; // gyro alignment uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. - uint8_t gyroSync; // Enable interrupt based loop + bool gyroSync; // Enable interrupt based loop uint16_t looptime; // imu loop time in us uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_soft_lpf_hz; uint8_t gyro_soft_lpf_type; +#ifdef USE_DUAL_GYRO uint8_t gyro_to_use; +#endif uint16_t gyro_notch_hz; uint16_t gyro_notch_cutoff; uint16_t gyro_stage2_lowpass_hz; @@ -74,10 +77,12 @@ typedef struct gyroConfig_s { uint16_t gyroDynamicLpfMinHz; uint16_t gyroDynamicLpfMaxHz; uint8_t gyroDynamicLpfCurveExpo; +#ifdef USE_DYNAMIC_FILTERS uint8_t dynamicGyroNotchRange; uint16_t dynamicGyroNotchQ; uint16_t dynamicGyroNotchMinHz; uint8_t dynamicGyroNotchEnabled; +#endif } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c index 942bc4ff84..9c1b6f265e 100755 --- a/src/main/sensors/opflow.c +++ b/src/main/sensors/opflow.c @@ -50,6 +50,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "sensors/boardalignment.h" #include "sensors/gyro.h" @@ -75,12 +76,12 @@ static float opflowCalibrationFlowAcc; #define OPFLOW_UPDATE_TIMEOUT_US 200000 // At least 5Hz updates required #define OPFLOW_CALIBRATE_TIME_MS 30000 // 30 second calibration time -PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 2); PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, - .opflow_hardware = OPFLOW_NONE, - .opflow_align = CW0_DEG_FLIP, - .opflow_scale = 10.5f, + .opflow_hardware = SETTING_OPFLOW_HARDWARE_DEFAULT, + .opflow_align = SETTING_ALIGN_OPFLOW_DEFAULT, + .opflow_scale = SETTING_OPFLOW_SCALE_DEFAULT, ); static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse) diff --git a/src/main/sensors/opflow.h b/src/main/sensors/opflow.h index 52fc487f7e..afe94ec79a 100755 --- a/src/main/sensors/opflow.h +++ b/src/main/sensors/opflow.h @@ -30,10 +30,9 @@ typedef enum { OPFLOW_NONE = 0, - OPFLOW_PMW3901 = 1, - OPFLOW_CXOF = 2, - OPFLOW_MSP = 3, - OPFLOW_FAKE = 4, + OPFLOW_CXOF = 1, + OPFLOW_MSP = 2, + OPFLOW_FAKE = 3, } opticalFlowSensor_e; typedef enum { diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 60fbaed019..a330be79bb 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -38,6 +38,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "scheduler/protothreads.h" @@ -58,9 +59,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTME #define PITOT_HARDWARE_DEFAULT PITOT_NONE #endif PG_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, - .pitot_hardware = PITOT_HARDWARE_DEFAULT, - .pitot_lpf_milli_hz = 350, - .pitot_scale = 1.00f + .pitot_hardware = SETTING_PITOT_HARDWARE_DEFAULT, + .pitot_lpf_milli_hz = SETTING_PITOT_LPF_MILLI_HZ_DEFAULT, + .pitot_scale = SETTING_PITOT_SCALE_DEFAULT ); bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 29955a42d5..cec9b00faf 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -40,10 +40,13 @@ #include "drivers/rangefinder/rangefinder_srf10.h" #include "drivers/rangefinder/rangefinder_hcsr04_i2c.h" #include "drivers/rangefinder/rangefinder_vl53l0x.h" +#include "drivers/rangefinder/rangefinder_vl53l1x.h" #include "drivers/rangefinder/rangefinder_virtual.h" +#include "drivers/rangefinder/rangefinder_us42.h" #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "sensors/sensors.h" #include "sensors/rangefinder.h" @@ -64,8 +67,8 @@ rangefinder_t rangefinder; PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 1); PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, - .rangefinder_hardware = RANGEFINDER_NONE, - .use_median_filtering = 0, + .rangefinder_hardware = SETTING_RANGEFINDER_HARDWARE_DEFAULT, + .use_median_filtering = SETTING_RANGEFINDER_MEDIAN_FILTER_DEFAULT, ); const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void) @@ -131,6 +134,15 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; + case RANGEFINDER_VL53L1X: +#if defined(USE_RANGEFINDER_VL53L1X) + if (vl53l1xDetect(dev)) { + rangefinderHardware = RANGEFINDER_VL53L1X; + rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VL53L1X_TASK_PERIOD_MS)); + } +#endif + break; + case RANGEFINDER_MSP: #if defined(USE_RANGEFINDER_MSP) if (virtualRangefinderDetect(dev, &rangefinderMSPVtable)) { @@ -149,7 +161,16 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; - case RANGEFINDER_NONE: + case RANGEFINDER_US42: +#ifdef USE_RANGEFINDER_US42 + if (us42Detect(dev)) { + rangefinderHardware = RANGEFINDER_US42; + rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_US42_TASK_PERIOD_MS)); + } +#endif + break; + + case RANGEFINDER_NONE: rangefinderHardware = RANGEFINDER_NONE; break; } @@ -277,4 +298,3 @@ bool rangefinderIsHealthy(void) return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS; } #endif - diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 2cd7d01e1c..568c01ae12 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -30,6 +30,8 @@ typedef enum { RANGEFINDER_MSP = 5, RANGEFINDER_UNUSED = 6, // Was UIB RANGEFINDER_BENEWAKE = 7, + RANGEFINDER_VL53L1X = 8, + RANGEFINDER_US42 = 9, } rangefinderType_e; typedef struct rangefinderConfig_s { diff --git a/src/main/target/AIKONF4/target.h b/src/main/target/AIKONF4/target.h index 6e77fde6ad..d0e13b8a12 100644 --- a/src/main/target/AIKONF4/target.h +++ b/src/main/target/AIKONF4/target.h @@ -103,6 +103,7 @@ #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS #define PITOT_I2C_BUS DEFAULT_I2C_BUS #define PCA9685_I2C_BUS DEFAULT_I2C_BUS +#define BNO055_I2C_BUS DEFAULT_I2C_BUS #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h index edf6973b54..596a0b54fd 100644 --- a/src/main/target/AIRBOTF4/target.h +++ b/src/main/target/AIRBOTF4/target.h @@ -64,8 +64,8 @@ #define USE_PITOT_ADC #define PITOT_I2C_BUS BUS_I2C2 - #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define M25P16_CS_PIN PB3 #define M25P16_SPI_BUS BUS_SPI3 diff --git a/src/main/target/AIRBOTF7/target.h b/src/main/target/AIRBOTF7/target.h index 8b4a267710..4a554cdd56 100644 --- a/src/main/target/AIRBOTF7/target.h +++ b/src/main/target/AIRBOTF7/target.h @@ -79,7 +79,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index cc1e81203f..ad553d6aad 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -63,9 +63,6 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[PITCH].P = 36; failsafeConfigMutable()->failsafe_delay = 2; failsafeConfigMutable()->failsafe_off_delay = 0; - controlRateProfilesMutable(0)->stabilized.rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT; - controlRateProfilesMutable(0)->stabilized.rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT; - controlRateProfilesMutable(0)->stabilized.rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT; parseRcChannels("TAER1234"); *primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 0cae3b81cb..9d7718d640 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -64,6 +64,7 @@ #define MAG_MPU9250_ALIGN CW0_DEG #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 45e4fe08d9..0a4522d312 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -66,6 +66,7 @@ #define AK8963_SPI_BUS BUS_SPI3 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index cfd82fc9c7..ad5fdf17a7 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -53,6 +53,7 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index 16baaca9cc..1e6d6c9f42 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -49,6 +49,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index ca42a0ad2e..07e09434a1 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -49,6 +49,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ASGARD32F4/target.h b/src/main/target/ASGARD32F4/target.h index 49c8dd10c3..d9018a5cd9 100644 --- a/src/main/target/ASGARD32F4/target.h +++ b/src/main/target/ASGARD32F4/target.h @@ -168,3 +168,4 @@ #define PITOT_I2C_BUS BUS_I2C2 #define PCA9685_I2C_BUS BUS_I2C2 #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ASGARD32F7/target.h b/src/main/target/ASGARD32F7/target.h index f26650eeeb..97e2ad210e 100644 --- a/src/main/target/ASGARD32F7/target.h +++ b/src/main/target/ASGARD32F7/target.h @@ -172,3 +172,4 @@ #define PCA9685_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 7f7ed3dea6..7963cae436 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -56,6 +56,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_OSD #define USE_MAX7456 diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 2f50dc866e..1c4cc3fe68 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -110,8 +110,8 @@ //#define SBUS_TELEMETRY_UART SERIAL_PORT_USART1 // XXX #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_CURRENT_METER | FEATURE_TELEMETRY ) // XXX -#define USE_SPEKTRUM_BIND -#define BIND_PIN UART2_RX_PIN +//#define USE_SPEKTRUM_BIND +//#define BIND_PIN UART2_RX_PIN #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index cf4727c7b3..0b33a58755 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -130,6 +130,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 06cca4cf11..321b5e280a 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -53,6 +53,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index ad03b71bcb..eae3c8ebdd 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -39,6 +39,7 @@ #define L3GD20_CS_PIN PE3 #define IMU_L3GD20_ALIGN CW270_DEG +#define IMU_LSM303DLHC_ALIGN CW0_DEG #define IMU_MPU6050_ALIGN CW0_DEG #define USE_BARO diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h index 2b8f3c0145..e5f5aa5a4f 100644 --- a/src/main/target/CLRACINGF4AIR/target.h +++ b/src/main/target/CLRACINGF4AIR/target.h @@ -116,6 +116,7 @@ #define I2C2_SDA PB11 #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #endif #define USE_ADC diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index d852c19a06..eff71edd24 100755 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -56,6 +56,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C3 +#define BNO055_I2C_BUS BUS_I2C3 #ifdef QUANTON #define IMU_MPU6000_ALIGN CW90_DEG diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 57e6a98eee..8347b58cd3 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -124,4 +124,5 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/DALRCF405/target.c b/src/main/target/DALRCF405/target.c index a8d8deb8f3..1dbe7eac09 100644 --- a/src/main/target/DALRCF405/target.c +++ b/src/main/target/DALRCF405/target.c @@ -28,9 +28,9 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S2 (2,2) DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 (2,6) DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 (2,1) (2.3 2.6) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 (2,4) (2.2) - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 (1,2) - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 (1,5) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S5 (2,4) (2.2) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S6 (1,2) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S7 (2,3) DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 (2,7) DEF_TIM(TIM4, CH1, PB6, TIM_USE_LED, 0, 0), // LED STRIP(1,0) diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h index b478f8e238..0b37dc2c11 100644 --- a/src/main/target/DALRCF405/target.h +++ b/src/main/target/DALRCF405/target.h @@ -96,6 +96,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04_I2C diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h index 534042b8da..ca29604c6c 100644 --- a/src/main/target/DALRCF722DUAL/target.h +++ b/src/main/target/DALRCF722DUAL/target.h @@ -82,7 +82,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 #define USE_SPI diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 6c2ebc7925..6dbb58442f 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -56,6 +56,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 diff --git a/src/main/target/FALCORE/target.h b/src/main/target/FALCORE/target.h index d8480eda73..cdb8ba4636 100755 --- a/src/main/target/FALCORE/target.h +++ b/src/main/target/FALCORE/target.h @@ -135,4 +135,5 @@ #define TARGET_IO_PORTD 0xFFFF #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h index d2285e8a38..73e3e0fee9 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.h +++ b/src/main/target/FF_F35_LIGHTNING/target.h @@ -127,6 +127,7 @@ #define PITOT_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_GPS | FEATURE_TELEMETRY) diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h index c3818458fd..a0ea4428da 100644 --- a/src/main/target/FIREWORKSV2/target.h +++ b/src/main/target/FIREWORKSV2/target.h @@ -231,6 +231,8 @@ #if defined(OMNIBUSF4V6) #define PCA9685_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #else #define PCA9685_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #endif diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 91fd29491b..40c0fd199e 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -65,6 +65,8 @@ // *************** Temperature sensor ***************** #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 + // *************** BARO ***************************** #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/FLYWOOF411/target.h b/src/main/target/FLYWOOF411/target.h index 675513def2..34d110d7a5 100644 --- a/src/main/target/FLYWOOF411/target.h +++ b/src/main/target/FLYWOOF411/target.h @@ -76,6 +76,8 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define BNO055_I2C_BUS BUS_I2C1 + #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 diff --git a/src/main/target/FLYWOOF7DUAL/target.h b/src/main/target/FLYWOOF7DUAL/target.h index 419d5ba9e4..10b8d285bb 100644 --- a/src/main/target/FLYWOOF7DUAL/target.h +++ b/src/main/target/FLYWOOF7DUAL/target.h @@ -113,6 +113,8 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define BNO055_I2C_BUS BUS_I2C1 + #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 diff --git a/src/main/target/FOXEERF405/target.h b/src/main/target/FOXEERF405/target.h index c4c38aa3c7..2b8d4e2ac0 100644 --- a/src/main/target/FOXEERF405/target.h +++ b/src/main/target/FOXEERF405/target.h @@ -117,6 +117,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define BNO055_I2C_BUS BUS_I2C1 + /*** ADC ***/ #define USE_ADC #define ADC_CHANNEL_1_PIN PC0 diff --git a/src/main/target/FOXEERF722DUAL/target.h b/src/main/target/FOXEERF722DUAL/target.h index e1ac91c271..b339cef7a9 100644 --- a/src/main/target/FOXEERF722DUAL/target.h +++ b/src/main/target/FOXEERF722DUAL/target.h @@ -125,6 +125,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define BNO055_I2C_BUS BUS_I2C1 + /*** ADC ***/ #define USE_ADC #define ADC_CHANNEL_1_PIN PC0 diff --git a/src/main/target/FRSKYPILOT/target.h b/src/main/target/FRSKYPILOT/target.h index a92057f4ee..87bd0b072b 100644 --- a/src/main/target/FRSKYPILOT/target.h +++ b/src/main/target/FRSKYPILOT/target.h @@ -123,7 +123,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C3 - +#define BNO055_I2C_BUS BUS_I2C3 #define PITOT_I2C_BUS BUS_I2C3 #define USE_RANGEFINDER diff --git a/src/main/target/FRSKY_ROVERF7/target.h b/src/main/target/FRSKY_ROVERF7/target.h index 30955d0bfd..6347cd9bd9 100755 --- a/src/main/target/FRSKY_ROVERF7/target.h +++ b/src/main/target/FRSKY_ROVERF7/target.h @@ -66,7 +66,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 // *************** SPI2 Flash *********************** diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h index c445959610..d5ccf15a95 100644 --- a/src/main/target/FURYF4OSD/target.h +++ b/src/main/target/FURYF4OSD/target.h @@ -134,6 +134,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define BNO055_I2C_BUS DEFAULT_I2C_BUS #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP diff --git a/src/main/target/HGLRCF722/target.c b/src/main/target/HGLRCF722/target.c index ebabbd28a1..6924d5ac93 100644 --- a/src/main/target/HGLRCF722/target.c +++ b/src/main/target/HGLRCF722/target.c @@ -42,8 +42,8 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 2, 5) DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2 DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2) DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0) DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2 diff --git a/src/main/target/HGLRCF722/target.h b/src/main/target/HGLRCF722/target.h index c745a853be..440a657c1b 100644 --- a/src/main/target/HGLRCF722/target.h +++ b/src/main/target/HGLRCF722/target.h @@ -80,7 +80,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER diff --git a/src/main/target/IFLIGHTF4_SUCCEXD/target.h b/src/main/target/IFLIGHTF4_SUCCEXD/target.h index e47b9c7638..aee436548c 100644 --- a/src/main/target/IFLIGHTF4_SUCCEXD/target.h +++ b/src/main/target/IFLIGHTF4_SUCCEXD/target.h @@ -112,6 +112,7 @@ #define PITOT_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP diff --git a/src/main/target/IFLIGHTF4_TWING/target.h b/src/main/target/IFLIGHTF4_TWING/target.h index cbb9d29428..ab5d0bf5f3 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.h +++ b/src/main/target/IFLIGHTF4_TWING/target.h @@ -79,6 +79,7 @@ #define RANGEFINDER_I2C_BUS BUS_I2C1 #define PCA9685_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 // *************** OSD ***************************** #define USE_SPI_DEVICE_2 diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h index bac28ceea0..2fb765bce7 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.h +++ b/src/main/target/IFLIGHTF7_TWING/target.h @@ -80,6 +80,7 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 // *************** FLASH ************************** #define M25P16_CS_PIN PB9 diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 80013075a3..134889a0f3 100755 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -65,6 +65,7 @@ # define USE_MAG_LIS3MDL # define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 # define USE_BARO # define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index 69fb42d612..da8f8c9d1a 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -179,3 +179,5 @@ #define TARGET_IO_PORTE 0xffff #define MAX_PWM_OUTPUT_PORTS 6 + +#define BNO055_I2C_BUS BUS_I2C1 \ No newline at end of file diff --git a/src/main/target/KAKUTEF7MINIV3/target.h b/src/main/target/KAKUTEF7MINIV3/target.h index a32f9655c9..7ecc7a3d1e 100644 --- a/src/main/target/KAKUTEF7MINIV3/target.h +++ b/src/main/target/KAKUTEF7MINIV3/target.h @@ -167,6 +167,7 @@ #define RANGEFINDER_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 /*** Used pins ***/ #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/KFC32F3_INAV/CMakeLists.txt b/src/main/target/KFC32F3_INAV/CMakeLists.txt deleted file mode 100644 index 1af4cecc06..0000000000 --- a/src/main/target/KFC32F3_INAV/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -target_stm32f303xc(KFC32F3_INAV SKIP_RELEASES) diff --git a/src/main/target/KFC32F3_INAV/hardware_setup.c b/src/main/target/KFC32F3_INAV/hardware_setup.c deleted file mode 100755 index c12e7dfde1..0000000000 --- a/src/main/target/KFC32F3_INAV/hardware_setup.c +++ /dev/null @@ -1,53 +0,0 @@ -/* - * 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 . - */ - -#include -#include -#include - -#include "platform.h" - -#include "build/build_config.h" - -#include "drivers/time.h" -#include "drivers/bus_spi.h" -#include "drivers/io.h" -#include "drivers/io_impl.h" - -void initialisePreBootHardware(void) -{ - // setup CS pins - IOInit(DEFIO_IO(PB5), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); - IOConfigGPIO(DEFIO_IO(PB5), IOCFG_OUT_PP); - IOHi(DEFIO_IO(PB5)); - - IOInit(DEFIO_IO(PC15), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); - IOConfigGPIO(DEFIO_IO(PC15), IOCFG_OUT_PP); - IOHi(DEFIO_IO(PC15)); - - IOInit(DEFIO_IO(PB12), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); - IOConfigGPIO(DEFIO_IO(PB12), IOCFG_OUT_PP); - IOHi(DEFIO_IO(PB12)); - - IOInit(DEFIO_IO(PB2), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); - IOConfigGPIO(DEFIO_IO(PB2), IOCFG_OUT_PP); - IOHi(DEFIO_IO(PB2)); - - IOInit(DEFIO_IO(PA7), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); - IOConfigGPIO(DEFIO_IO(PA7), IOCFG_OUT_PP); - IOHi(DEFIO_IO(PA7)); -} \ No newline at end of file diff --git a/src/main/target/KFC32F3_INAV/target.c b/src/main/target/KFC32F3_INAV/target.c deleted file mode 100755 index a32e395332..0000000000 --- a/src/main/target/KFC32F3_INAV/target.c +++ /dev/null @@ -1,42 +0,0 @@ -/* - * 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 . - */ - -#include - -#include -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" -#include "drivers/timer.h" - -const timerHardware_t timerHardware[] = -{ - DEF_TIM(TIM2, CH1, PA0, TIM_USE_PPM, 0), // PPM - - DEF_TIM(TIM8, CH3, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM2 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM7 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM8 - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // S1_out - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // S2_out -}; - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KFC32F3_INAV/target.h b/src/main/target/KFC32F3_INAV/target.h deleted file mode 100755 index 5d3dacf3ba..0000000000 --- a/src/main/target/KFC32F3_INAV/target.h +++ /dev/null @@ -1,146 +0,0 @@ -/* - * 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 . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "KFCi" -#define USE_HARDWARE_PREBOOT_SETUP - -#define USE_DJI_HD_OSD - -#define LED0 PC13 -#define LED0_INVERTED - -#define LED1 PC14 -#define LED1_INVERTED - -#define BEEPER PA13 -#define BEEPER_INVERTED - -#define USE_EXTI -#define GYRO_INT_EXTI PC5 -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - -#define BUS_SPI_SPEED_MAX BUS_SPEED_SLOW - -#define USE_IMU_MPU6000 -#define IMU_MPU6000_ALIGN CW90_DEG -#define MPU6000_CS_PIN PB5 -#define MPU6000_SPI_BUS BUS_SPI2 - - -#define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL - -#define USE_SPI -#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 - -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define USE_FLASHFS - -#define USE_FLASH_M25P16 -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_BUS BUS_SPI2 -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - -#define USE_BARO -#define USE_BARO_MS5611 -#define MS5611_SPI_BUS BUS_SPI2 -#define MS5611_CS_PIN PB2 - -#define USE_OSD -#define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI2 -#define MAX7456_CS_PIN PA7 - -//#define RFM_SPI SPI2 -//#define RFM_SPI_CS_PIN PC15 -//#define RFM_IRQ_PIN PB3 -//#define RFM_SPI_CLK (SPI_CLOCK_STANDARD*2) -//#define RFM_RESTORE_CLK (SPI_CLOCK_FAST) - -#define USE_RX_SPI -#define USE_RX_ELERES -#define RX_NSS_PIN PC15 -#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOC -#define RX_SCK_PIN PB13 -#define RX_MOSI_PIN PB15 -#define RX_MISO_PIN PB14 -#define RX_SPI_INSTANCE SPI2 -#define RX_IRQ_PIN PB3 - -#define USE_VCP -#define USE_UART1 -#define USE_UART2 -#define SERIAL_PORT_COUNT 3 - -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define UART2_TX_PIN PA2 -#define UART2_RX_PIN PA3 - -#define USE_I2C -#define USE_I2C_DEVICE_1 - -#define I2C1_SCL PA15 -#define I2C1_SDA PA14 - -#define USE_PITOT_MS4525 -#define PITOT_I2C_BUS BUS_I2C1 - -#define BOARD_HAS_VOLTAGE_DIVIDER -#define USE_ADC -#define ADC_INSTANCE ADC2 -#define ADC_CHANNEL_1_PIN PA4 -#define ADC_CHANNEL_2_PIN PA6 -#define ADC_CHANNEL_3_PIN PA5 -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define RSSI_ADC_CHANNEL ADC_CHN_2 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 - -//#define USE_LED_STRIP -//#define WS2811_PIN PA8 -//#define WS2811_DMA_STREAM DMA1_Channel2 -//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER - -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_OSD | FEATURE_VBAT) -#define DEFAULT_RX_TYPE RX_TYPE_PPM - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -// Number of available PWM outputs -#define MAX_PWM_OUTPUT_PORTS 10 -#define TARGET_MOTOR_COUNT 10 - -// IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h index f4312c06fc..f1ca6b7b00 100644 --- a/src/main/target/MAMBAF405US/target.h +++ b/src/main/target/MAMBAF405US/target.h @@ -56,6 +56,8 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define BNO055_I2C_BUS BUS_I2C2 + //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h index aa324c685c..20bb2e4873 100644 --- a/src/main/target/MAMBAF722/target.h +++ b/src/main/target/MAMBAF722/target.h @@ -56,6 +56,8 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define BNO055_I2C_BUS BUS_I2C2 + //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 37e7f78096..a70cc55583 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -162,6 +162,8 @@ #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define BNO055_I2C_BUS DEFAULT_I2C_BUS + #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP #define USE_RANGEFINDER_HCSR04_I2C diff --git a/src/main/target/MATEKF405CAN/CMakeLists.txt b/src/main/target/MATEKF405CAN/CMakeLists.txt new file mode 100644 index 0000000000..29a924b4e6 --- /dev/null +++ b/src/main/target/MATEKF405CAN/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f405xg(MATEKF405CAN) diff --git a/src/main/target/SPRACINGF3NEO/config.c b/src/main/target/MATEKF405CAN/config.c similarity index 58% rename from src/main/target/SPRACINGF3NEO/config.c rename to src/main/target/MATEKF405CAN/config.c index 922e391a27..f3f22b74a9 100644 --- a/src/main/target/SPRACINGF3NEO/config.c +++ b/src/main/target/MATEKF405CAN/config.c @@ -15,25 +15,17 @@ * along with Cleanflight. If not, see . */ -#include #include - #include "platform.h" - +#include "config/config_master.h" +#include "config/feature.h" #include "io/serial.h" -#include "sensors/sensors.h" -#include "sensors/compass.h" -#include "sensors/barometer.h" - -#include "telemetry/telemetry.h" void targetConfiguration(void) { - barometerConfigMutable()->baro_hardware = BARO_AUTODETECT; - compassConfigMutable()->mag_hardware = MAG_AUTODETECT; - serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So Bluetooth users don't have to change anything. - serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; - serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(GPS_UART)].functionMask = FUNCTION_GPS; - //telemetryConfigMutable()->halfDuplex = 1; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_GPS; + // serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].gps_baudrateIndex = BAUD_115200; + } diff --git a/src/main/target/MATEKF405CAN/target.c b/src/main/target/MATEKF405CAN/target.c new file mode 100644 index 0000000000..c8e7a088a3 --- /dev/null +++ b/src/main/target/MATEKF405CAN/target.c @@ -0,0 +1,44 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#include +#include +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S1 D(2,2,7) + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S2 D(2,3,7) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 1), // S3 D(2,4,7) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S4 D(2,7,7) + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S5 D(1,7,5) + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 1, 0), // S6 D(1,2,5) + + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S7 D(1,0,2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 1, 0), // S8 D(1,3,2) + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED, 0, 0), //2812LED D(1,5,3) + + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); + diff --git a/src/main/target/MATEKF405CAN/target.h b/src/main/target/MATEKF405CAN/target.h new file mode 100644 index 0000000000..f2c0999ee8 --- /dev/null +++ b/src/main/target/MATEKF405CAN/target.h @@ -0,0 +1,175 @@ +/* + * This file is part of INAV. + * + * INAV is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * INAV is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with INAV. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "MF4C" +#define USBD_PRODUCT_STRING "Matek_F405CAN" + +#define LED0 PA14 //Blue +#define LED1 PA13 //Green + +#define BEEPER PA8 +#define BEEPER_INVERTED +#define BEEPER_PWM +#define BEEPER_PWM_FREQUENCY 2500 + +// *************** SPI1 Gyro & ACC ******************* +#define USE_SPI +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define USE_IMU_MPU6500 +#define IMU_MPU6500_ALIGN CW180_DEG_FLIP +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + + +// *************** I2C /Baro/Mag ********************* +#define USE_I2C +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 + +#define USE_BARO +#define BARO_I2C_BUS BUS_I2C2 +#define USE_BARO_BMP280 +#define USE_BARO_MS5611 +#define USE_BARO_DPS310 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_AK8963 +#define USE_MAG_AK8975 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL + +#define USE_RANGEFINDER +#define USE_RANGEFINDER_HCSR04_I2C +#define RANGEFINDER_I2C_BUS BUS_I2C2 + +#define PITOT_I2C_BUS BUS_I2C2 +#define TEMPERATURE_I2C_BUS BUS_I2C2 +#define PCA9685_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 + + +// *************** SPI2 RM3100 ************************** +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_MAG_RM3100 +#define MAG_RM3100_ALIGN CW0_DEG_FLIP +#define RM3100_CS_PIN PB12 +#define RM3100_SPI_BUS BUS_SPI2 + +// *************** SPI3 SD Card ******************** +#define USE_SDCARD +#define USE_SDCARD_SPI +#define SDCARD_SPI_BUS BUS_SPI3 +#define SDCARD_CS_PIN PC14 + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PB5 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define USE_UART2 +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART4 +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define USE_UART5 +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define USE_SOFTSERIAL1 +#define SOFTSERIAL_1_TX_PIN PA2 //TX2 pad +#define SOFTSERIAL_1_RX_PIN PA2 //TX2 pad + +#define SERIAL_PORT_COUNT 7 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART2 + +// *************** ADC *************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define ADC_CHANNEL_3_PIN PC2 +#define ADC_CHANNEL_4_PIN PC3 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 +#define AIRSPEED_ADC_CHANNEL ADC_CHN_4 + +// *************** LED2812 ************************ +#define USE_LED_STRIP +#define WS2811_PIN PA15 +#define WS2811_DMA_HANDLER_IDENTIFER DMA1_ST5_HANDLER +#define WS2811_DMA_STREAM DMA1_Stream5 +#define WS2811_DMA_CHANNEL DMA_Channel_3 + +// *************** OTHERS ************************* +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_GPS | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) +#define VBAT_SCALE 2100 //1K:20K + +#define USE_SPEKTRUM_BIND +#define BIND_PIN PA3 // RX2 + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 81681b94f5..75afdea322 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -72,11 +72,11 @@ #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04_I2C +#define USE_RANGEFINDER_US42 #define RANGEFINDER_I2C_BUS BUS_I2C2 - #define PITOT_I2C_BUS BUS_I2C2 - #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 // *************** SPI2 OSD *************************** diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index b12f1e596a..ac6962a350 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -121,6 +121,7 @@ #define USE_BARO_DPS310 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF411SE/CMakeLists.txt b/src/main/target/MATEKF411SE/CMakeLists.txt index fa289c23ff..79d7f68c5a 100644 --- a/src/main/target/MATEKF411SE/CMakeLists.txt +++ b/src/main/target/MATEKF411SE/CMakeLists.txt @@ -1,2 +1,3 @@ target_stm32f411xe(MATEKF411SE) +target_stm32f411xe(MATEKF411SE_PINIO) target_stm32f411xe(MATEKF411SE_FD_SFTSRL1) diff --git a/src/main/target/MATEKF411SE/config.c b/src/main/target/MATEKF411SE/config.c index 07f6de4697..38bf22dd8e 100755 --- a/src/main/target/MATEKF411SE/config.c +++ b/src/main/target/MATEKF411SE/config.c @@ -25,4 +25,7 @@ void targetConfiguration(void) { pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; +#ifdef MATEKF411SE_PINIO + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +#endif } diff --git a/src/main/target/MATEKF411SE/target.c b/src/main/target/MATEKF411SE/target.c index 879acd6131..1a2da60c6d 100755 --- a/src/main/target/MATEKF411SE/target.c +++ b/src/main/target/MATEKF411SE/target.c @@ -35,7 +35,9 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 pad -softserial_tx2 DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM +#ifndef MATEKF411SE_PINIO DEF_TIM(TIM2, CH3, PB10, TIM_USE_ANY, 0, 0), // softserial_rx1 - LED 2812 D(1,1,3) +#endif }; diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index 7e322e8401..59d59e42de 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -113,6 +113,7 @@ #define PITOT_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP @@ -135,7 +136,7 @@ #define AIRSPEED_ADC_CHANNEL ADC_CHN_4 // *************** LED2812 ************************ -#ifndef MATEKF411SE_FD_SFTSRL1 +#if !defined(MATEKF411SE_PINIO) && !defined(MATEKF411SE_FD_SFTSRL1) #define USE_LED_STRIP #define WS2811_PIN PB10 #endif @@ -144,6 +145,9 @@ #define USE_PINIO #define USE_PINIOBOX #define PINIO1_PIN PA13 // Camera switcher +#ifdef MATEKF411SE_PINIO +#define PINIO2_PIN PB10 // External PINIO (LED pad) +#endif // *************** OTHERS ************************* #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL ) diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index a99a60afc1..331ad65821 100755 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -159,3 +159,5 @@ #define USE_DSHOT #define USE_ESC_SENSOR #define USE_SERIALSHOT + +#define BNO055_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h index 24c471860e..ca9dd843a6 100755 --- a/src/main/target/MATEKF722PX/target.h +++ b/src/main/target/MATEKF722PX/target.h @@ -66,7 +66,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h index 7942a30e21..ecff68fa66 100644 --- a/src/main/target/MATEKF722SE/target.h +++ b/src/main/target/MATEKF722SE/target.h @@ -193,4 +193,6 @@ #define MAX_PWM_OUTPUT_PORTS 8 #define USE_DSHOT #define USE_SERIALSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR + +#define BNO055_I2C_BUS BUS_I2C1 \ No newline at end of file diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h index 48dde29e1e..8f5b256026 100644 --- a/src/main/target/MATEKF765/target.h +++ b/src/main/target/MATEKF765/target.h @@ -86,7 +86,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 - +#define BNO055_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h index b8ac4303eb..cc3409c8f5 100644 --- a/src/main/target/MATEKH743/target.h +++ b/src/main/target/MATEKH743/target.h @@ -157,7 +157,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 - +#define BNO055_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 386eadf120..81d44eb6a2 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -120,4 +120,5 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 3a222d8acc..3338db998e 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -119,8 +119,8 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 -#define USE_LED_STRIP -#define WS2811_PIN PA8 +//#define USE_LED_STRIP +//#define WS2811_PIN PA8 //#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 0c3666a9d3..2a4383ab4d 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -100,6 +100,7 @@ #define USE_MAG_AK8975 #define TEMPERATURE_I2C_BUS I2C_EXT_BUS +#define BNO055_I2C_BUS I2C_EXT_BUS #define USE_BARO diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index d013a36a6c..d82ba047b1 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -158,6 +158,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04_I2C diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h index b14e0c219e..241322b5d6 100644 --- a/src/main/target/OMNIBUSF7NXT/target.h +++ b/src/main/target/OMNIBUSF7NXT/target.h @@ -59,6 +59,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define USE_BARO_LPS25H diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index c60d00116b..9a68d246b1 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -72,8 +72,8 @@ #define VBAT_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 -#define USE_LED_STRIP -#define WS2811_PIN PB8 // TIM16_CH1 +//#define USE_LED_STRIP +//#define WS2811_PIN PB8 // TIM16_CH1 // #define USE_TRANSPONDER // #define TRANSPONDER_PIN PA8 diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index be2c511c19..360d097c69 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -56,6 +56,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/RMDO/CMakeLists.txt b/src/main/target/RMDO/CMakeLists.txt deleted file mode 100644 index ed40102c52..0000000000 --- a/src/main/target/RMDO/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -target_stm32f303xc(RMDO COMPILE_DEFINITIONS "SPRACINGF3" SKIP_RELEASES) diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c deleted file mode 100644 index 59769ba505..0000000000 --- a/src/main/target/RMDO/target.c +++ /dev/null @@ -1,48 +0,0 @@ -/* - * 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 . - */ - -#include - -#include -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" -#include "drivers/timer.h" - -const timerHardware_t timerHardware[] = { - DEF_TIM(TIM16, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - DEF_TIM(TIM17, CH1, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - DEF_TIM(TIM4, CH1, PA11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PA11 - DEF_TIM(TIM4, CH2, PA12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PA12 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 - PB8 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 - PB9 - DEF_TIM(TIM15, CH1, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM7 - PA2 - DEF_TIM(TIM15, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM8 - PA3 - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), // GPIO_TIMER / LED_STRIP - - DEF_TIM(TIM2, CH1, PA0, TIM_USE_PWM | TIM_USE_PPM, 0), // RC_CH1 - PA0 - *TIM2_CH1 - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, 0), // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0), // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0), // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_PWM, 0), // RC_CH5 - PB4 - *TIM3_CH1 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, 0), // RC_CH6 - PB5 - *TIM3_CH2 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0), // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0), // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N -}; - - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h deleted file mode 100644 index fe467f8296..0000000000 --- a/src/main/target/RMDO/target.h +++ /dev/null @@ -1,125 +0,0 @@ -/* - * 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 . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "RMDO" // Ready Made RC DoDo - -#define LED0 PB3 - -#define BEEPER PC15 -#define BEEPER_INVERTED - -#define USE_EXTI -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - -#define USE_IMU_MPU6050 -#define IMU_MPU6050_ALIGN CW270_DEG -#define MPU6050_I2C_BUS BUS_I2C1 - - -#define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP280 - -#define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL - -#define USE_FLASHFS -#define USE_FLASH_M25P16 - -#define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) -#define RANGEFINDER_HCSR04_ECHO_PIN PB1 // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) - -#define USE_UART1 -#define USE_UART2 -#define USE_UART3 -#define USE_SOFTSERIAL1 -#define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 - -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 - -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) - -#define SOFTSERIAL_1_RX_PIN PB4 -#define SOFTSERIAL_1_TX_PIN PB5 -#define SOFTSERIAL_2_RX_PIN PB0 -#define SOFTSERIAL_2_TX_PIN PB1 - -#define USE_I2C -#define USE_I2C_DEVICE_1 - -#define USE_SPI -#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 - -#define M25P16_CS_GPIO GPIOB -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_BUS BUS_SPI2 - -#define BOARD_HAS_VOLTAGE_DIVIDER -#define USE_ADC -#define ADC_INSTANCE ADC2 -#define ADC_CHANNEL_1_PIN PA4 -#define ADC_CHANNEL_2_PIN PA5 -#define ADC_CHANNEL_3_PIN PB2 -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define RSSI_ADC_CHANNEL ADC_CHN_3 - -#define USE_LED_STRIP - -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 - -#undef NAV_MAX_WAYPOINTS -#define NAV_MAX_WAYPOINTS 30 - -#define USE_SPEKTRUM_BIND -// USART3, -#define BIND_PIN PB11 - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -#undef USE_GPS_PROTO_NAZA -#undef USE_TELEMETRY_HOTT -#undef USE_TELEMETRY_SMARTPORT -#undef USE_TELEMETRY_IBUS - -// Number of available PWM outputs -#define MAX_PWM_OUTPUT_PORTS 12 - -// IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) diff --git a/src/main/target/RUSH_BLADE_F7/target.h b/src/main/target/RUSH_BLADE_F7/target.h index ca0175f10d..cdae1860b3 100644 --- a/src/main/target/RUSH_BLADE_F7/target.h +++ b/src/main/target/RUSH_BLADE_F7/target.h @@ -66,7 +66,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index f716369b74..c1d18603d5 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -53,6 +53,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h index 1365b7aec5..c51b558832 100644 --- a/src/main/target/SPEEDYBEEF4/target.h +++ b/src/main/target/SPEEDYBEEF4/target.h @@ -124,6 +124,8 @@ #define SERIAL_PORT_COUNT 6 #endif +#define BNO055_I2C_BUS BUS_I2C1 + /*** BARO & MAG ***/ #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/SPEEDYBEEF7/target.h b/src/main/target/SPEEDYBEEF7/target.h index a5c8b9150e..71e855af0a 100644 --- a/src/main/target/SPEEDYBEEF7/target.h +++ b/src/main/target/SPEEDYBEEF7/target.h @@ -120,7 +120,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 - +#define BNO055_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER diff --git a/src/main/target/SPRACINGF3NEO/CMakeLists.txt b/src/main/target/SPRACINGF3NEO/CMakeLists.txt deleted file mode 100644 index d93726a632..0000000000 --- a/src/main/target/SPRACINGF3NEO/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -target_stm32f303xc(SPRACINGF3NEO SKIP_RELEASES) diff --git a/src/main/target/SPRACINGF3NEO/target.c b/src/main/target/SPRACINGF3NEO/target.c deleted file mode 100755 index 6a932ead1b..0000000000 --- a/src/main/target/SPRACINGF3NEO/target.c +++ /dev/null @@ -1,46 +0,0 @@ -/* - * 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 . - */ - -#include - -#include - -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" -#include "drivers/timer.h" - -const timerHardware_t timerHardware[] = { - DEF_TIM(TIM15, CH2, PA3, TIM_USE_PWM | TIM_USE_PPM, 0), // PWM1 / PPM / UART2 RX - DEF_TIM(TIM15, CH1, PA2, TIM_USE_PWM, 0), // PWM2 - - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC1 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC2 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC3 - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC4 - - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // ESC5 (FW motor) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // ESC6 (FW motor) - - - DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0), // PWM3 - PB10 - *TIM2_CH3, UART3_TX (AF7) - DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0), // PWM4 - PB11 - *TIM2_CH4, UART3_RX (AF7) - - DEF_TIM(TIM16, CH1, PB8, TIM_USE_ANY, 0), - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), -}; - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h deleted file mode 100755 index 9bec2fda3c..0000000000 --- a/src/main/target/SPRACINGF3NEO/target.h +++ /dev/null @@ -1,163 +0,0 @@ -/* - * 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 . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "SP3N" - -#define LED0 PB9 -#define LED1 PB2 - -#define BEEPER PC15 -#define BEEPER_INVERTED - -#define USE_EXTI -#define GYRO_INT_EXTI PC13 -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - -#define USE_IMU_MPU6500 -#define IMU_MPU6500_ALIGN CW0_DEG - -#define USE_IMU_MPU9250 -#define IMU_MPU9250_ALIGN CW0_DEG - -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_BUS BUS_SPI1 - -#define MPU9250_CS_PIN SPI1_NSS_PIN -#define MPU9250_SPI_BUS BUS_SPI1 - -#define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP280 -#define USE_BARO_MS5611 - -#define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_MPU9250 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL - -#define USE_VCP -#define USE_UART1 -#define USE_UART2 -#define USE_UART3 -#define USE_UART4 -#define USE_UART5 -//#define USE_SOFTSERIAL1 -//#define USE_SOFTSERIAL2 -//#define SERIAL_PORT_COUNT 8 -#define SERIAL_PORT_COUNT 6 - -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define UART2_TX_PIN PA2 -#define UART2_RX_PIN PA3 - -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 - -#define USE_I2C -#define USE_I2C_DEVICE_1 - -#define USE_SPI -#define USE_SPI_DEVICE_1 // MPU -#define USE_SPI_DEVICE_2 // SDCard -#define USE_SPI_DEVICE_3 // External (MAX7456 & RTC6705) - -#define SPI1_NSS_PIN PA4 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define SPI3_NSS_PIN PA15 -#define SPI3_SCK_PIN PB3 -#define SPI3_MISO_PIN PB4 -#define SPI3_MOSI_PIN PB5 - -#undef USE_VTX_FFPV -#undef USE_VTX_SMARTAUDIO // Disabled due to flash size -#undef USE_VTX_TRAMP // Disabled due to flash size - -#undef USE_PITOT // Disabled due to RAM size -#undef USE_PITOT_ADC // Disabled due to RAM size - -#define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI3 -#define MAX7456_CS_PIN PA15 - -#define USE_SDCARD -#define USE_SDCARD_SPI -#define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PC14 -#define SDCARD_SPI_BUS BUS_SPI2 -#define SDCARD_CS_PIN SPI2_NSS_PIN - -#define USE_ADC -#define ADC_INSTANCE ADC1 -#define ADC_CHANNEL_1_PIN PC1 -#define ADC_CHANNEL_2_PIN PC2 -#define ADC_CHANNEL_3_PIN PC0 -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define RSSI_ADC_CHANNEL ADC_CHN_3 - -#define USE_LED_STRIP -#define WS2811_PIN PA8 - -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT - -#define USE_OSD - -#define DEFAULT_RX_TYPE RX_TYPE_SERIAL -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP) -#define SERIALRX_UART SERIAL_PORT_USART2 -#define GPS_UART SERIAL_PORT_USART3 -#define TELEMETRY_UART SERIAL_PORT_USART5 -#define SERIALRX_PROVIDER SERIALRX_SBUS - -#define BUTTONS -#define BUTTON_A_PIN PD2 - -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - -#define BINDPLUG_PIN PD2 - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -// Number of available PWM outputs -#define MAX_PWM_OUTPUT_PORTS 10 - -// IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#undef USE_TELEMETRY_FRSKY diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index c1da04f60b..efe89add1e 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -59,6 +59,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_VCP diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h index aead3cbe59..9bbc2a7455 100644 --- a/src/main/target/SPRACINGF7DUAL/target.h +++ b/src/main/target/SPRACINGF7DUAL/target.h @@ -62,6 +62,8 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define BNO055_I2C_BUS BUS_I2C1 + #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG #define USE_MAG_HMC5883 diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index dc423616f2..7f05ef7ef4 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -68,6 +68,7 @@ #define USE_MAG_QMC5883 #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h index b7799bfb81..820431f3e3 100644 --- a/src/main/target/YUPIF7/target.h +++ b/src/main/target/YUPIF7/target.h @@ -15,7 +15,6 @@ * along with Cleanflight. If not, see . */ - #pragma once #define TARGET_BOARD_IDENTIFIER "YPF7" #define USBD_PRODUCT_STRING "YUPIF7" @@ -37,19 +36,18 @@ #define ENSURE_MPU_DATA_READY_IS_LOW // ICM 20689 -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_BUS BUS_SPI1 - -#define USE_IMU_MPU6500 -#define IMU_MPU6500_ALIGN CW90_DEG +#define USE_IMU_ICM20689 +#define IMU_ICM20689_ALIGN CW90_DEG +#define ICM20689_CS_PIN SPI1_NSS_PIN +#define ICM20689_SPI_BUS BUS_SPI1 #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_MAG_QMC5883 - #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 @@ -57,8 +55,10 @@ #define USE_BARO_BMP280 // Serial ports +#define USB_IO #define USE_VCP #define VBUS_SENSING_PIN PA8 +#define VBUS_SENSING_ENABLED #define USE_UART1 #define UART1_RX_PIN PA10 @@ -147,6 +147,7 @@ #define USE_ESC_SENSOR // Default configuration +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART6 #define TELEMETRY_UART SERIAL_PORT_USART1 @@ -162,3 +163,4 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) + diff --git a/src/main/target/common.h b/src/main/target/common.h index 9d0ddc3346..7344fd21f3 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -97,7 +97,9 @@ #define USE_RANGEFINDER_MSP #define USE_RANGEFINDER_BENEWAKE #define USE_RANGEFINDER_VL53L0X +#define USE_RANGEFINDER_VL53L1X #define USE_RANGEFINDER_HCSR04_I2C +#define USE_RANGEFINDER_US42 // Allow default optic flow boards #define USE_OPFLOW @@ -123,7 +125,6 @@ #define USE_PWM_DRIVER_PCA9685 -#define USE_TELEMETRY_SIM #define USE_FRSKYOSD #define USE_DJI_HD_OSD #define USE_SMARTPORT_MASTER @@ -137,7 +138,19 @@ #define USE_I2C_IO_EXPANDER +#define USE_GPS_PROTO_NMEA +#define USE_GPS_PROTO_MTK + +#define USE_TELEMETRY_SIM +#define USE_TELEMETRY_HOTT +#define USE_TELEMETRY_MAVLINK +#define USE_MSP_OVER_TELEMETRY + #define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol +#define USE_SERIALRX_SUMD +#define USE_SERIALRX_SUMH +#define USE_SERIALRX_XBUS +#define USE_SERIALRX_JETIEXBUS #define USE_TELEMETRY_SRXL #define USE_SPEKTRUM_CMS_TELEMETRY //#define USE_SPEKTRUM_VTX_CONTROL //Some functions from betaflight still not implemented @@ -148,6 +161,9 @@ #define USE_SERIALRX_GHST #define USE_TELEMETRY_GHST +#define USE_SECONDARY_IMU +#define USE_IMU_BNO055 + #else // MCU_FLASH_SIZE < 256 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR #endif @@ -160,23 +176,14 @@ #define USE_STATS #define USE_CMS #define CMS_MENU_OSD -#define USE_GPS_PROTO_NMEA -#define USE_GPS_PROTO_MTK #define NAV_GPS_GLITCH_DETECTION #define NAV_NON_VOLATILE_WAYPOINT_STORAGE -#define USE_TELEMETRY_HOTT #define USE_TELEMETRY_IBUS -#define USE_TELEMETRY_MAVLINK #define USE_TELEMETRY_SMARTPORT #define USE_TELEMETRY_CRSF -#define USE_MSP_OVER_TELEMETRY // These are rather exotic serial protocols #define USE_RX_MSP //#define USE_MSP_RC_OVERRIDE -#define USE_SERIALRX_SUMD -#define USE_SERIALRX_SUMH -#define USE_SERIALRX_XBUS -#define USE_SERIALRX_JETIEXBUS #define USE_SERIALRX_CRSF #define USE_PWM_SERVO_DRIVER #define USE_SERIAL_PASSTHROUGH diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index ad8736d0db..bc32325ced 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -80,6 +80,15 @@ BUSDEV_REGISTER_I2C(busdev_bmi160, DEVHW_BMI160, BMI160_I2C_BUS, 0x68, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI160_ALIGN); #endif #endif + + #if defined(USE_IMU_BMI088) + #if defined(BMI088_SPI_BUS) + BUSDEV_REGISTER_SPI(busdev_bmi088_gyro, DEVHW_BMI088_GYRO, BMI088_SPI_BUS, BMI088_GYRO_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI088_ALIGN); + BUSDEV_REGISTER_SPI(busdev_bmi088_acc, DEVHW_BMI088_ACC, BMI088_SPI_BUS, BMI088_ACC_CS_PIN, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI088_ALIGN); + #elif defined(BMI088_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_bmi088, DEVHW_BMI088, BMI088_I2C_BUS, 0x68, GYRO_INT_EXTI, DEVFLAGS_NONE, IMU_BMI088_ALIGN); + #endif + #endif #endif @@ -229,6 +238,12 @@ #endif BUSDEV_REGISTER_I2C(busdev_ist8308, DEVHW_IST8308, IST8308_I2C_BUS, 0x0C, NONE, DEVFLAGS_NONE, 0); #endif + +#if defined(USE_MAG_RM3100) + #if defined(RM3100_SPI_BUS) + BUSDEV_REGISTER_SPI(busdev_rm3100, DEVHW_RM3100, RM3100_SPI_BUS, RM3100_CS_PIN, NONE, DEVFLAGS_NONE, 0); + #endif +#endif #endif @@ -295,6 +310,24 @@ #endif #endif +#if defined(USE_RANGEFINDER_VL53L1X) + #if !defined(VL53L1X_I2C_BUS) && defined(RANGEFINDER_I2C_BUS) + #define VL53L1X_I2C_BUS RANGEFINDER_I2C_BUS + #endif + + #if defined(VL53L1X_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_vl53l1x, DEVHW_VL53L1X, VL53L1X_I2C_BUS, 0x29, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); + #endif +#endif + +#if defined(USE_RANGEFINDER_US42) + #if !defined(US42_I2C_BUS) && defined(RANGEFINDER_I2C_BUS) + #define US42_I2C_BUS RANGEFINDER_I2C_BUS + #endif + #if defined(US42_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_us42, DEVHW_US42, US42_I2C_BUS, 0x70, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); // Requires null data to passthrough + #endif +#endif /** AIRSPEED SENSORS **/ @@ -370,4 +403,11 @@ BUSDEV_REGISTER_I2C(busdev_pcf8574, DEVHW_PCF8574, PCF8574_I2C_BUS, 0x20, NONE, DEVFLAGS_NONE, 0); #endif +#ifdef USE_IMU_BNO055 +#ifndef BNO055_I2C_BUS + #define BNO055_I2C_BUS BUS_I2C1 +#endif + BUSDEV_REGISTER_I2C(busdev_bno055, DEVHW_BNO055, BNO055_I2C_BUS, 0x29, NONE, DEVFLAGS_NONE, 0); +#endif + #endif // USE_TARGET_HARDWARE_DESCRIPTORS diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 8ac00c39f9..439ca8f2d0 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -246,7 +246,11 @@ static void crsfFrameBatterySensor(sbuf_t *dst) // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); - crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V + if (telemetryConfig()->report_cell_voltage) { + crsfSerialize16(dst, getBatteryAverageCellVoltage() / 10); + } else { + crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V + } crsfSerialize16(dst, getAmperage() / 10); const uint8_t batteryRemainingPercentage = calculateBatteryPercentage(); crsfSerialize8(dst, (getMAhDrawn() >> 16)); @@ -327,9 +331,9 @@ static void crsfFrameFlightMode(sbuf_t *dst) flightMode = "RTH"; } else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) { flightMode = "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)) { flightMode = "3CRS"; - } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { + } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { flightMode = "CRS"; } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { flightMode = "AH"; diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 373a055a87..0896f412de 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -47,7 +47,7 @@ uint16_t frskyGetFlightMode(void) // thousands column if (FLIGHT_MODE(NAV_RTH_MODE)) tmpi += 1000; - if (FLIGHT_MODE(NAV_CRUISE_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow tmpi += 8000; else if (FLIGHT_MODE(NAV_WP_MODE)) tmpi += 2000; diff --git a/src/main/telemetry/ghst.c b/src/main/telemetry/ghst.c index dc9c02579e..c2aca3ac51 100644 --- a/src/main/telemetry/ghst.c +++ b/src/main/telemetry/ghst.c @@ -43,6 +43,7 @@ #include "drivers/nvic.h" +#include "fc/config.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -51,6 +52,8 @@ #include "io/gps.h" #include "io/serial.h" +#include "navigation/navigation.h" + #include "rx/rx.h" #include "rx/ghst.h" #include "rx/ghst_protocol.h" @@ -67,6 +70,7 @@ #define GHST_CYCLETIME_US 100000 // 10x/sec #define GHST_FRAME_PACK_PAYLOAD_SIZE 10 +#define GHST_FRAME_GPS_PAYLOAD_SIZE 10 #define GHST_FRAME_LENGTH_CRC 1 #define GHST_FRAME_LENGTH_TYPE 1 @@ -112,11 +116,51 @@ void ghstFramePackTelemetry(sbuf_t *dst) sbufWriteU8(dst, 0x00); // tbd3 } + +// GPS data, primary, positional data +void ghstFrameGpsPrimaryTelemetry(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE); + sbufWriteU8(dst, GHST_DL_GPS_PRIMARY); + + sbufWriteU32(dst, gpsSol.llh.lat); + sbufWriteU32(dst, gpsSol.llh.lon); + + // constrain alt. from -32,000m to +32,000m, units of meters + const int16_t altitude = (constrain(getEstimatedActualPosition(Z), -32000 * 100, 32000 * 100) / 100); + sbufWriteU16(dst, altitude); +} + +// GPS data, secondary, auxiliary data +void ghstFrameGpsSecondaryTelemetry(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, GHST_FRAME_GPS_PAYLOAD_SIZE + GHST_FRAME_LENGTH_CRC + GHST_FRAME_LENGTH_TYPE); + sbufWriteU8(dst, GHST_DL_GPS_SECONDARY); + + sbufWriteU16(dst, gpsSol.groundSpeed); // speed in 0.1m/s + sbufWriteU16(dst, gpsSol.groundCourse); // degrees * 10 + sbufWriteU8(dst, gpsSol.numSat); + + sbufWriteU16(dst, (uint16_t) (GPS_distanceToHome / 10)); // use units of 10m to increase range of U16 to 655.36km + sbufWriteU16(dst, GPS_directionToHome); + + uint8_t gpsFlags = 0; + if(STATE(GPS_FIX)) + gpsFlags |= GPS_FLAGS_FIX; + if(STATE(GPS_FIX_HOME)) + gpsFlags |= GPS_FLAGS_FIX_HOME; + sbufWriteU8(dst, gpsFlags); +} + // schedule array to decide how often each type of frame is sent typedef enum { GHST_FRAME_START_INDEX = 0, GHST_FRAME_PACK_INDEX = GHST_FRAME_START_INDEX, // Battery (Pack) data - GHST_SCHEDULE_COUNT_MAX + GHST_FRAME_GPS_PRIMARY_INDEX, // GPS, primary values (Lat, Long, Alt) + GHST_FRAME_GPS_SECONDARY_INDEX, // GPS, secondary values (Sat Count, HDOP, etc.) + GHST_SCHEDULE_COUNT_MAX } ghstFrameTypeIndex_e; static uint8_t ghstScheduleCount; @@ -136,6 +180,19 @@ static void processGhst(void) ghstFramePackTelemetry(dst); ghstFinalize(dst); } + + if (currentSchedule & BIT(GHST_FRAME_GPS_PRIMARY_INDEX)) { + ghstInitializeFrame(dst); + ghstFrameGpsPrimaryTelemetry(dst); + ghstFinalize(dst); + } + + if (currentSchedule & BIT(GHST_FRAME_GPS_SECONDARY_INDEX)) { + ghstInitializeFrame(dst); + ghstFrameGpsSecondaryTelemetry(dst); + ghstFinalize(dst); + } + ghstScheduleIndex = (ghstScheduleIndex + 1) % ghstScheduleCount; } @@ -152,6 +209,14 @@ void initGhstTelemetry(void) if (isBatteryVoltageConfigured() || isAmperageConfigured()) { ghstSchedule[index++] = BIT(GHST_FRAME_PACK_INDEX); } + +#ifdef USE_GPS + if (feature(FEATURE_GPS)) { + ghstSchedule[index++] = BIT(GHST_FRAME_GPS_PRIMARY_INDEX); + ghstSchedule[index++] = BIT(GHST_FRAME_GPS_SECONDARY_INDEX); + } +#endif + ghstScheduleCount = (uint8_t)index; } diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 8a9a5c8590..184c947ead 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -168,7 +168,7 @@ void ltm_sframe(sbuf_t *dst) lt_flightmode = LTM_MODE_RTH; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) lt_flightmode = LTM_MODE_GPSHOLD; - else if (FLIGHT_MODE(NAV_CRUISE_MODE)) + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) lt_flightmode = LTM_MODE_CRUISE; else if (FLIGHT_MODE(NAV_LAUNCH_MODE)) lt_flightmode = LTM_MODE_LAUNCH; diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index 8fe6b7c53c..1d4ef3fdd9 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -63,15 +63,6 @@ #define SIM_RESPONSE_CODE_CMT ('C' << 24 | 'M' << 16 | 'T' << 8 | ':') -typedef enum { - SIM_TX_FLAG = (1 << 0), - SIM_TX_FLAG_FAILSAFE = (1 << 1), - SIM_TX_FLAG_GPS = (1 << 2), - SIM_TX_FLAG_ACC = (1 << 3), - SIM_TX_FLAG_LOW_ALT = (1 << 4), - SIM_TX_FLAG_RESPONSE = (1 << 5) -} simTxFlags_e; - typedef enum { SIM_MODULE_NOT_DETECTED = 0, SIM_MODULE_NOT_REGISTERED, @@ -124,7 +115,6 @@ static uint8_t simResponse[SIM_RESPONSE_BUFFER_SIZE + 1]; static int atCommandStatus = SIM_AT_OK; static bool simWaitAfterResponse = false; static uint8_t readState = SIM_READSTATE_RESPONSE; -static uint8_t transmitFlags = 0; static timeMs_t t_lastMessageSent = 0; static uint8_t lastMessageTriggeredBy = 0; static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED; @@ -146,7 +136,7 @@ static bool isGroundStationNumberDefined(void) { static bool checkGroundStationNumber(uint8_t* rv) { int i; - const uint8_t* gsn = telemetryConfig()->simGroundStationNumber; + const char* gsn = telemetryConfig()->simGroundStationNumber; int digitsToCheck = strlen((char*)gsn); if (gsn[0] == '+') { @@ -177,7 +167,7 @@ static bool checkGroundStationNumber(uint8_t* rv) static void readOriginatingNumber(uint8_t* rv) { int i; - uint8_t* gsn = telemetryConfigMutable()->simGroundStationNumber; + char* gsn = telemetryConfigMutable()->simGroundStationNumber; if (gsn[0] != '\0') return; for (i = 0; i < 15 && rv[i] != '\"'; i++) @@ -189,7 +179,7 @@ static void readTransmitFlags(const uint8_t* fs) { int i; - transmitFlags = 0; + uint8_t transmitFlags = 0; for (i = 0; i < SIM_N_TX_FLAGS && fs[i] != '\0'; i++) { switch (fs[i]) { case 'T': case 't': @@ -209,6 +199,8 @@ static void readTransmitFlags(const uint8_t* fs) break; } } + + telemetryConfigMutable()->simTransmitFlags = transmitFlags; } static void requestSendSMS(uint8_t trigger) @@ -337,7 +329,7 @@ static void transmit(void) if (gpsSol.fixType != GPS_NO_FIX && FLIGHT_MODE(SIM_LOW_ALT_WARNING_MODES) && getAltitudeMeters() < telemetryConfig()->simLowAltitude) triggers |= SIM_TX_FLAG_LOW_ALT; - triggers &= transmitFlags; + triggers &= telemetryConfig()->simTransmitFlags; if (!triggers) return; @@ -508,7 +500,6 @@ static void configureSimTelemetryPort(void) sim_t_stateChange = millis() + SIM_STARTUP_DELAY_MS; simTelemetryState = SIM_STATE_INIT; readState = SIM_READSTATE_RESPONSE; - readTransmitFlags(telemetryConfig()->simTransmitFlags); simEnabled = true; } diff --git a/src/main/telemetry/sim.h b/src/main/telemetry/sim.h index 081f74e036..299981e623 100644 --- a/src/main/telemetry/sim.h +++ b/src/main/telemetry/sim.h @@ -23,6 +23,15 @@ #define SIM_DEFAULT_TX_FLAGS "f" #define SIM_PIN "0000" +typedef enum { + SIM_TX_FLAG = (1 << 0), + SIM_TX_FLAG_FAILSAFE = (1 << 1), + SIM_TX_FLAG_GPS = (1 << 2), + SIM_TX_FLAG_ACC = (1 << 3), + SIM_TX_FLAG_LOW_ALT = (1 << 4), + SIM_TX_FLAG_RESPONSE = (1 << 5) +} simTxFlags_e; + void handleSimTelemetry(void); void initSimTelemetry(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 98531e65e3..6246c3f47f 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -34,6 +34,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "io/serial.h" @@ -53,38 +54,43 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 5); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, - .gpsNoFixLatitude = 0, - .gpsNoFixLongitude = 0, - .telemetry_switch = 0, - .telemetry_inverted = 0, - .frsky_coordinate_format = FRSKY_FORMAT_DMS, - .frsky_unit = FRSKY_UNIT_METRICS, - .frsky_vfas_precision = 0, - .frsky_pitch_roll = 0, - .report_cell_voltage = 0, - .hottAlarmSoundInterval = 5, - .halfDuplex = 1, - .smartportFuelUnit = SMARTPORT_FUEL_UNIT_MAH, - .ibusTelemetryType = 0, - .ltmUpdateRate = LTM_RATE_NORMAL, - .simTransmitInterval = SIM_DEFAULT_TRANSMIT_INTERVAL, - .simTransmitFlags = SIM_DEFAULT_TX_FLAGS, - .simLowAltitude = INT16_MIN, - .simPin = SIM_PIN, - .accEventThresholdHigh = 0, - .accEventThresholdLow = 0, - .accEventThresholdNegX = 0, + .gpsNoFixLatitude = SETTING_FRSKY_DEFAULT_LATITUDE_DEFAULT, + .gpsNoFixLongitude = SETTING_FRSKY_DEFAULT_LONGITUDE_DEFAULT, + .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, + .telemetry_inverted = SETTING_TELEMETRY_INVERTED_DEFAULT, + .frsky_coordinate_format = SETTING_FRSKY_COORDINATES_FORMAT_DEFAULT, + .frsky_unit = SETTING_FRSKY_UNIT_DEFAULT, + .frsky_vfas_precision = SETTING_FRSKY_VFAS_PRECISION_DEFAULT, + .frsky_pitch_roll = SETTING_FRSKY_PITCH_ROLL_DEFAULT, + .report_cell_voltage = SETTING_REPORT_CELL_VOLTAGE_DEFAULT, + .hottAlarmSoundInterval = SETTING_HOTT_ALARM_SOUND_INTERVAL_DEFAULT, + .halfDuplex = SETTING_TELEMETRY_HALFDUPLEX_DEFAULT, + .smartportFuelUnit = SETTING_SMARTPORT_FUEL_UNIT_DEFAULT, + .ibusTelemetryType = SETTING_IBUS_TELEMETRY_TYPE_DEFAULT, + .ltmUpdateRate = SETTING_LTM_UPDATE_RATE_DEFAULT, + +#ifdef USE_TELEMETRY_SIM + .simTransmitInterval = SETTING_SIM_TRANSMIT_INTERVAL_DEFAULT, + .simTransmitFlags = SETTING_SIM_TRANSMIT_FLAGS_DEFAULT, + .simLowAltitude = SETTING_SIM_LOW_ALTITUDE_DEFAULT, + .simPin = SETTING_SIM_PIN_DEFAULT, + .simGroundStationNumber = SETTING_SIM_GROUND_STATION_NUMBER_DEFAULT, + + .accEventThresholdHigh = SETTING_ACC_EVENT_THRESHOLD_HIGH_DEFAULT, + .accEventThresholdLow = SETTING_ACC_EVENT_THRESHOLD_LOW_DEFAULT, + .accEventThresholdNegX = SETTING_ACC_EVENT_THRESHOLD_NEG_X_DEFAULT, +#endif .mavlink = { - .extended_status_rate = 2, - .rc_channels_rate = 5, - .position_rate = 2, - .extra1_rate = 10, - .extra2_rate = 2, - .extra3_rate = 1 + .extended_status_rate = SETTING_MAVLINK_EXT_STATUS_RATE_DEFAULT, + .rc_channels_rate = SETTING_MAVLINK_RC_CHAN_RATE_DEFAULT, + .position_rate = SETTING_MAVLINK_POS_RATE_DEFAULT, + .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, + .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, + .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 717e9fec03..242f5cc83e 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -68,14 +68,18 @@ typedef struct telemetryConfig_s { smartportFuelUnit_e smartportFuelUnit; uint8_t ibusTelemetryType; uint8_t ltmUpdateRate; + +#ifdef USE_TELEMETRY_SIM + int16_t simLowAltitude; + char simGroundStationNumber[16]; + char simPin[8]; uint16_t simTransmitInterval; - uint8_t simTransmitFlags[4]; + uint8_t simTransmitFlags; + uint16_t accEventThresholdHigh; uint16_t accEventThresholdLow; uint16_t accEventThresholdNegX; - int16_t simLowAltitude; - uint8_t simGroundStationNumber[16]; - uint8_t simPin[8]; +#endif struct { uint8_t extended_status_rate; uint8_t rc_channels_rate; @@ -98,4 +102,3 @@ void telemetryCheckState(void); void telemetryProcess(timeUs_t currentTimeUs); bool telemetryDetermineEnabledState(portSharing_e portSharing); - diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index 42567db71a..1f6e7dcd00 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -27,6 +27,8 @@ set_property(SOURCE telemetry_hott_unittest.cc PROPERTY depends set_property(SOURCE time_unittest.cc PROPERTY depends "drivers/time.c") +set_property(SOURCE circular_queue_unittest.cc PROPERTY depends "common/circular_queue.c") + function(unit_test src) get_filename_component(basename ${src} NAME) string(REPLACE ".cc" "" name ${basename} ) @@ -41,9 +43,13 @@ function(unit_test src) endif() list(TRANSFORM deps PREPEND "${MAIN_DIR}/") add_executable(${name} ${src} ${deps}) - target_include_directories(${name} PRIVATE . ${MAIN_DIR}) + set(gen_name ${name}_gen) + get_generated_files_dir(gen ${gen_name}) + target_include_directories(${name} PRIVATE . ${MAIN_DIR} ${gen}) target_compile_definitions(${name} PRIVATE ${test_definitions}) - set_target_properties(${name} PROPERTIES COMPILE_FLAGS "-pthread -Wall -Wextra -Wno-extern-c-compat -ggdb3 -O0") + target_compile_options(${name} PRIVATE -pthread -Wall -Wextra -Wno-extern-c-compat -ggdb3 -O0) + enable_settings(${name} ${gen_name} OUTPUTS setting_files SETTINGS_CXX g++) + target_sources(${name} PRIVATE ${setting_files}) target_link_libraries(${name} gtest_main) gtest_discover_tests(${name}) add_custom_target("run-${name}" "${name}" DEPENDS ${name}) diff --git a/src/test/unit/circular_queue_unittest.cc b/src/test/unit/circular_queue_unittest.cc new file mode 100644 index 0000000000..64dd5d0488 --- /dev/null +++ b/src/test/unit/circular_queue_unittest.cc @@ -0,0 +1,166 @@ +#include "unittest_macros.h" +#include "gtest/gtest.h" +#include + +#include +#include + +extern "C" { + #include "common/circular_queue.h" +} +#include +#include + + +TEST(CircularQueue, ElementsAreCorrect){ + srand (time(NULL)); + + circularBuffer_t buffer; + + const size_t bufferSize = 16; + uint8_t buff[bufferSize]; + + circularBufferInit(&buffer, buff, bufferSize, sizeof(char)); + + const uint8_t testBurst = 3; + + const uint8_t dataToInsertSize[testBurst] = {5, 10, 3}; + const uint8_t dataToRemoveSize[testBurst] = {3, 7, 2}; + + std::queue queue; + + for(uint8_t j=0; jqueue; + + EXPECT_EQ(circularBufferIsEmpty(&buffer),true); + EXPECT_EQ(circularBufferIsFull(&buffer),false); + + for(uint8_t j=0; jqueue; + + EXPECT_EQ(circularBufferIsEmpty(&buffer),true); + EXPECT_EQ(circularBufferIsFull(&buffer),false); + + for(uint8_t j=0; j '0', true => '1' }[default_value] + else + default_value = table_values.index default_value + end + + when type == "string" + default_value = "{ #{[*default_value.bytes, 0] * ', '} }" + + when default_value.is_a?(Float) + default_value = default_value.to_s + ?f + + end + min, max = resolve_range(member) setting_name = "SETTING_#{name.upcase}" + buf << "#define #{setting_name}_DEFAULT #{default_value}\n" unless default_value.nil? buf << "#define #{setting_name} #{ii}\n" buf << "#define #{setting_name}_MIN #{min}\n" buf << "#define #{setting_name}_MAX #{max}\n" @@ -444,6 +471,12 @@ class Generator end end + # When this file is compiled in unit tests, some of the tables + # are not used and generate warnings, causing the test to fail + # with -Werror. Silence them + + buf << "#pragma GCC diagnostic ignored \"-Wunused-const-variable\"\n" + # Write PGN arrays pgn_steps = [] pgns = [] @@ -629,44 +662,47 @@ class Generator return !cond || @true_conditions.include?(cond) end - def foreach_enabled_member - @data["groups"].each do |group| - if is_condition_enabled(group["condition"]) - group["members"].each do |member| - if is_condition_enabled(member["condition"]) - yield group, member + def foreach_enabled_member &block + enum = Enumerator.new do |yielder| + groups.each do |group| + if is_condition_enabled(group["condition"]) + group["members"].each do |member| + if is_condition_enabled(member["condition"]) + yielder.yield group, member + end end end end end + block_given? ? enum.each(&block) : enum end - def foreach_enabled_group - last = nil - foreach_enabled_member do |group, member| - if last != group - last = group - yield group + def foreach_enabled_group &block + enum = Enumerator.new do |yielder| + last = nil + foreach_enabled_member do |group, member| + if last != group + last = group + yielder.yield group + end end end + block_given? ? enum.each(&block) : enum end - def foreach_member - @data["groups"].each do |group| - group["members"].each do |member| - yield group, member + def foreach_member &block + enum = Enumerator.new do |yielder| + @data["groups"].each do |group| + group["members"].each do |member| + yielder.yield group, member + end end end + block_given? ? enum.each(&block) : enum end - def foreach_group - last = nil - foreach_member do |group, member| - if last != group - last = group - yield group - end - end + def groups + @data["groups"] end def initialize_tables @@ -795,25 +831,27 @@ class Generator if !group["name"] raise "Missing group name" end + if !member["name"] raise "Missing member name in group #{group["name"]}" end + table = member["table"] if table if !@tables[table] raise "Member #{member["name"]} references non-existing table #{table}" end + @used_tables << table end + if !member["field"] member["field"] = member["name"] end + typ = member["type"] - if !typ - pending_types[member] = group - elsif typ == "bool" + if typ == "bool" has_booleans = true - member["type"] = "uint8_t" member["table"] = OFF_ON_TABLE["name"] end end @@ -825,7 +863,7 @@ class Generator @used_tables << OFF_ON_TABLE["name"] end - resolve_types pending_types unless !pending_types + resolve_all_types foreach_enabled_member do |group, member| @count += 1 @max_name_length = [@max_name_length, member["name"].length].max @@ -835,9 +873,82 @@ class Generator end end + def validate_default_values + foreach_enabled_member do |_, member| + name = member["name"] + type = member["type"] + default_value = member["default_value"] + + next if %i[ zero target ].include? default_value + + case + when type == "bool" + raise "Member #{name} has an invalid default value" unless [ false, true ].include? default_value + + when member.has_key?("table") + table_name = member["table"] + table_values = @tables[table_name]["values"] + raise "Member #{name} has an invalid default value" unless table_values.include? default_value + + when type =~ /\A(?u?)int(?8|16|32|64)_t\Z/ + unsigned = !$~[:unsigned].empty? + bitsize = $~[:bitsize].to_i + type_range = unsigned ? 0..(2**bitsize-1) : (-2**(bitsize-1)+1)..(2**(bitsize-1)-1) + raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' + raise "Member #{name} default value has an invalid type, integer or symbol expected" unless default_value.is_a? Integer or default_value.is_a? Symbol + raise "Member #{name} default value is outside type's storage range, min #{type_range.min}, max #{type_range.max}" unless default_value.is_a? Symbol or type_range === default_value + + when type == "float" + raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' + raise "Member #{name} default value has an invalid type, numeric or symbol expected" unless default_value.is_a? Numeric or default_value.is_a? Symbol + + when type == "string" + max = member["max"].to_i + raise "Member #{name} default value has an invalid type, string expected" unless default_value.is_a? String + raise "Member #{name} default value is too long (max #{max} chars)" if default_value.bytesize > max + + else + raise "Unexpected type for member #{name}: #{type.inspect}" + end + end + end + + def scan_types(stderr) + types = Hash.new + # gcc 6-9 + stderr.scan(/var_(\d+).*?['’], which is of non-class type ['‘](.*)['’]/).each do |m| + member_idx = m[0].to_i + type = m[1] + types[member_idx] = type + end + # clang + stderr.scan(/member reference base type '(.*?)'.*?is not a structure or union.*? var_(\d+)/m).each do |m| + member_idx = m[1].to_i + type = m[0] + types[member_idx] = type + end + return types + end + + def resolve_all_types() + loop do + pending = Hash.new + foreach_enabled_member do |group, member| + if !member["type"] + pending[member] = group + end + end + + if pending.empty? + # All types resolved + break + end + + resolve_types(pending) + end + end + def resolve_types(pending) - # TODO: Loop to avoid reaching the maximum number - # of errors printed by the compiler. prog = StringIO.new prog << "int main() {\n" ii = 0 @@ -853,9 +964,13 @@ class Generator prog << "return 0;\n" prog << "};\n" stderr = compile_test_file(prog) - stderr.scan(/var_(\d+).*?', which is of non-class type '(.*)'/).each do |m| - member = members[m[0].to_i] - case m[1] + types = scan_types(stderr) + if types.empty? + raise "No types resolved from #{stderr}" + end + types.each do |idx, type| + member = members[idx] + case type when /^int8_t/ # {aka signed char}" typ = "int8_t" when /^uint8_t/ # {aka unsigned char}" @@ -878,12 +993,6 @@ class Generator dputs "#{member["name"]} type is #{typ}" member["type"] = typ end - # Make sure all types have been resolved - foreach_enabled_member do |group, member| - if !member["type"] - raise "Could not resolve type for member #{member["name"]} in group #{group["name"]}" - end - end end def initialize_name_encoder @@ -925,6 +1034,11 @@ class Generator @value_encoder = ValueEncoder.new(values, constantValues) end + def check_member_default_values_presence + missing_default_value_names = foreach_member.inject([]) { |names, (_, member)| member.has_key?("default_value") ? names : names << member["name"] } + raise "Missing default value for #{missing_default_value_names.count} member#{"s" unless missing_default_value_names.one?}: #{missing_default_value_names * ", "}" unless missing_default_value_names.empty? + end + def resolve_constants(constants) return nil unless constants.length > 0 s = Set.new @@ -936,7 +1050,9 @@ class Generator # warnings to find these constants, the compiler # might reach the maximum number of errors and stop # compilation, so we might need multiple passes. - re = /required from 'class expr_(.*?)<(.*)>'/ + gcc_re = /required from ['‘]class expr_(.*?)<(.*?)>['’]/ # gcc 6-9 + clang_re = / template class 'expr_(.*?)<(.*?)>'/ # clang + res = [gcc_re, clang_re] values = Hash.new while s.length > 0 buf = StringIO.new @@ -956,7 +1072,12 @@ class Generator ii += 1 end stderr = compile_test_file(buf) - matches = stderr.scan(re) + matches = [] + res.each do |re| + if matches.length == 0 + matches = stderr.scan(re) + end + end if matches.length == 0 puts stderr raise "No more matches looking for constants" diff --git a/src/utils/update_cli_docs.py b/src/utils/update_cli_docs.py index 7ca8bdf711..f7f7d8a24b 100755 --- a/src/utils/update_cli_docs.py +++ b/src/utils/update_cli_docs.py @@ -1,10 +1,26 @@ #!/usr/bin/env python3 import optparse +import os +import re import yaml # pyyaml / python-yaml SETTINGS_MD_PATH = "docs/Settings.md" SETTINGS_YAML_PATH = "src/main/fc/settings.yaml" +CODE_DEFAULTS_PATH = "src/main" + +DEFAULTS_BLACKLIST = [ + 'baro_hardware', + 'dterm_lpf_type', + 'dterm_lpf2_type', + 'failsafe_procedure', + 'flaperon_throw_offset', + 'heading_hold_rate_limit', + 'mag_hardware', + 'pitot_hardware', + 'rx_min_usec', + 'serialrx_provider', +] def parse_settings_yaml(): """Parse the YAML settings specs""" @@ -21,6 +37,23 @@ def generate_md_table_from_yaml(settings_yaml): for member in group['members']: if not any(key in member for key in ["description", "default_value"]) and not options.quiet: print("Setting \"{}\" has no description or default value specified".format(member['name'])) + # Handle edge cases of YAML autogeneration + if "default_value" in member: + # Replace booleans with "ON"/"OFF" + if type(member["default_value"]) == bool: + member["default_value"] = "ON" if member["default_value"] else "OFF" + # Replace zero placeholder with actual zero + elif member["default_value"] == ":zero": + member["default_value"] = 0 + # Replace target-default placeholder with extended definition + elif member["default_value"] == ":target": + member["default_value"] = "_target default_" + # Replace empty strings with more evident marker + elif member["default_value"] == "": + member["default_value"] = "_empty_" + # Reformat direct code references + elif str(member["default_value"])[0] == ":": + member["default_value"] = f'`{member["default_value"][1:]}`' params[member['name']] = { "description": member["description"] if "description" in member else "", "default": member["default_value"] if "default_value" in member else "" @@ -45,13 +78,86 @@ def write_settings_md(lines): with open(SETTINGS_MD_PATH, "w") as settings_md: settings_md.writelines(lines) +# Return all matches of a compiled regex in a list of files +def regex_search(regex, files): + for f in files: + with open(f, 'r') as _f: + for _, line in enumerate(_f.readlines()): + matches = regex.search(line) + if matches: + yield matches + +# Return plausible default values for a given setting found by scraping the relative source files +def find_default(setting_name, headers): + regex = re.compile(rf'^\s*\.{setting_name}\s=\s([A-Za-z0-9_\-]+)(?:,)?(?:\s+//.+$)?') + files_to_search_in = [] + for header in headers: + header = f'{CODE_DEFAULTS_PATH}/{header}' + files_to_search_in.append(header) + if header.endswith('.h'): + header_c = re.sub(r'\.h$', '.c', header) + if os.path.isfile(header_c): + files_to_search_in.append(header_c) + defaults = [] + for matches in regex_search(regex, files_to_search_in): + defaults.append(matches.group(1)) + return defaults + +# Try to map default values in the YAML spec back to the actual C code and check for mismatches (defaults updated in the code but not in the YAML) +# Done by scraping the source files, prone to false negatives. Settings in `DEFAULTS_BLACKLIST` are ignored for this +# reason (values that refer to other source files are too complex to parse this way). +def check_defaults(settings_yaml): + retval = True + for group in settings_yaml['groups']: + if 'headers' in group: + headers = group['headers'] + for member in group['members']: + # Ignore blacklisted settings + if member['name'] in DEFAULTS_BLACKLIST: + continue + + default_from_code = find_default(member['name'], headers) + if len(default_from_code) == 0: # No default value found (no direct code mapping) + continue + elif len(default_from_code) > 1: # Duplicate default values found (regexes are a quick but poor solution) + if not options.quiet: + print(f"Duplicate default values found for {member['name']}: {default_from_code}, consider adding to blacklist") + continue + + # Extract the only matched value, guarded by the previous checks + default_from_code = default_from_code[0] + # Map C values to their equivalents used in the YAML spec + code_values_map = { 'true': 'ON', 'false': 'OFF' } + if default_from_code in code_values_map: + default_from_code = code_values_map[default_from_code] + + default_from_yaml = member["default_value"] if "default_value" in member else "" + # Remove eventual Markdown formatting + default_from_yaml = default_from_yaml.replace('`', '').replace('*', '').replace('__', '') + # Allow specific C-YAML matches that coudln't be replaced in the previous steps + extra_allowed_matches = { '1': 'ON', '0': 'OFF' } + + if default_from_yaml not in default_from_code: # Equal or substring + if default_from_code in extra_allowed_matches and default_from_yaml in extra_allowed_matches[default_from_code]: + continue + if not options.quiet: + print(f"{member['name']} has mismatched default values. Code reports '{default_from_code}' and YAML reports '{default_from_yaml}'") + retval = False + return retval + if __name__ == "__main__": global options, args parser = optparse.OptionParser() parser.add_option('-q', '--quiet', action="store_true", default=False, help="do not write anything to stdout") + parser.add_option('-d', '--defaults', action="store_true", default=False, help="check for mismatched default values") options, args = parser.parse_args() settings_yaml = parse_settings_yaml() + + if options.defaults: + defaults_match = check_defaults(settings_yaml) + quit(0 if defaults_match else 1) + md_table_lines = generate_md_table_from_yaml(settings_yaml) settings_md_lines = \ ["# CLI Variable Reference\n", "\n" ] + \