diff --git a/.gitignore b/.gitignore index cc71acb9c4..a27fcbeb0a 100644 --- a/.gitignore +++ b/.gitignore @@ -8,6 +8,7 @@ .project .settings .cproject +.cache/ __pycache__ startup_stm32f10x_md_gcc.s .vagrant/ diff --git a/AUTHORS b/AUTHORS index 839a8a5d8e..a254cd3364 100644 --- a/AUTHORS +++ b/AUTHORS @@ -35,6 +35,7 @@ Gaël James Gregor Ottmann Google LLC Hyon Lim +Hubert Jozwiak James Harrison Jan Staal Jeremy Waters diff --git a/CMakeLists.txt b/CMakeLists.txt index 400e890b7f..9e7de58298 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,7 +37,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 3.0.0) +project(INAV VERSION 3.1.0) enable_language(ASM) diff --git a/cmake/stm32h7.cmake b/cmake/stm32h7.cmake index 3642b23d0d..758f9dca25 100644 --- a/cmake/stm32h7.cmake +++ b/cmake/stm32h7.cmake @@ -143,9 +143,9 @@ main_sources(STM32H7_SRC config/config_streamer_stm32h7.c -# drivers/adc_stm32h7xx.c + drivers/adc_stm32h7xx.c drivers/bus_i2c_hal.c -# drivers/dma_stm32h7xx.c + drivers/dma_stm32h7xx.c drivers/bus_spi_hal.c drivers/memprot.h drivers/memprot_hal.c diff --git a/docs/Board - FrSky Pilot.md b/docs/Board - FrSky Pilot.md new file mode 100644 index 0000000000..2a4c68f969 --- /dev/null +++ b/docs/Board - FrSky Pilot.md @@ -0,0 +1,13 @@ +# Board - FRSKYPILOT + +## Vendor Information / specification +https://www.frsky-rc.com/product/r9rxsr-pilot/ + +## Firmware + +Two firmware variants are available. + +* `inav_x.y.z_FRSKYPILOT.hex` +* `inav_x.y.z_FRSKYPILOT_LED.hex` + +The only difference is that the `_LED` variant provides WS2812 LED support on S10. S11 & S12 are used for UART3 only. diff --git a/docs/Board - Matek F411-WSE.md b/docs/Board - Matek F411-WSE.md new file mode 100644 index 0000000000..f7e4bc88f5 --- /dev/null +++ b/docs/Board - Matek F411-WSE.md @@ -0,0 +1,26 @@ +# Board - [MATEKSYS F411-WSE](http://www.mateksys.com/?portfolio=f411-wse) + +![Matek F411 Wing](http://www.mateksys.com/wp-content/uploads/2019/06/F411-WSE_1-1500x600.jpg) + +## Specification: + +* STM32F411 CPU +* OSD +* BMP280 barometer +* Integrated PDB for 2 motors +* 2 UART ports +* 6 servos +* no SD card slot +* 3 BECs + +## Details + +* [Full specification](http://www.mateksys.com/?portfolio=f411-wse) +* SBUS pad has a built-in inverter and is connected to UART2 RX + +## Available TARGETS + +* `MATEKF411SE` Stock target. LED control and have SS1 on ST1 pad, SS2 on TX2 pad. +* `MATEKF411SE_PINIO` Adds USER 2 PINIO support on the LED pad. +* `MATEKF411SE_FD_SFTSRL1` Adds full duplex SS1 by putting the RX on the LED pad. +* `MATEKF411SE_SS2_CH6` SS2 moved to Ch6 pad. This keeps UART 2 as a full UART (for example, for use with Crossfire) and SS2 support. diff --git a/docs/Cli.md b/docs/Cli.md index 04332e2fbf..44d4c53c89 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -147,4 +147,4 @@ For targets that have a flash data chip, typically used for blackbox logs, the f | `flash_write
` | Writes `data` to `address` | ## CLI Variable Reference -See [Settings.md](Settings.md). +See [Settings.md](Settings.md). \ No newline at end of file diff --git a/docs/Controls.md b/docs/Controls.md index 3757088db3..d245d92b04 100644 --- a/docs/Controls.md +++ b/docs/Controls.md @@ -34,7 +34,7 @@ The stick positions are combined to activate different functions: | Trim Acc Forwards | HIGH | CENTER | HIGH | CENTER | | Trim Acc Backwards | HIGH | CENTER | LOW | CENTER | | Save current waypoint mission | LOW | CENTER | HIGH | LOW | -| Load current waypoint mission | LOW | CENTER | HIGH | HIGH | +| Load/unload current waypoint mission | LOW | CENTER | HIGH | HIGH | | Save setting | LOW | LOW | LOW | HIGH | | Enter OSD Menu (CMS) | CENTER | LOW | HIGH | CENTER | diff --git a/docs/Navigation.md b/docs/Navigation.md index 0dc2513b30..8bfac31994 100755 --- a/docs/Navigation.md +++ b/docs/Navigation.md @@ -25,7 +25,7 @@ Throttle tilt compensation attempts to maintain constant vertical thrust when co Position hold requires GPS, accelerometer and compass sensors. Flight modes that require a compass (POSHOLD, RTH) are locked until compass is properly calibrated. When activated, this mode will attempt to keep copter where it is (based on GPS coordinates). From inav 2.0, POSHOLD is a full 3D position hold. Heading hold in this mode is assumed and activated automatically. -### CLI parameters affecting ALTHOLD mode: +### CLI parameters affecting POSHOLD mode: * *nav_user_control_mode* - can be set to "0" (GPS_ATTI) or "1" (GPS_CRUISE), controls how firmware will respond to roll/pitch stick movement. When in GPS_ATTI mode, right stick controls attitude, when it is released, new position is recorded and held. When in GPS_CRUISE mode right stick controls velocity and firmware calculates required attitude on its own. ### Related PIDs diff --git a/docs/Safehomes.md b/docs/Safehomes.md index b3ed76403c..e805659518 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -12,14 +12,20 @@ One potential risk when landing is that there might be buildings, trees and othe ## Safehome -Safehomes are a list of GPS coordinates that identify safe landing points. When the flight controller is armed, it checks the list of safehomes. The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position will be selected. Otherwise, it reverts to the old behaviour of using your current GPS position as home. +Safehomes are a list of GPS coordinates that identify safe landing points. You can define up to 8 safehomes for different locations you fly at. When the flight controller is armed, it checks the list of safehomes. The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position is identified. The arming home location remains as home. -Be aware that the safehome replaces your arming position as home. When flying, RTH will return to the safehome and OSD elements such as distance to home and direction to home will refer to the selected safehome. +When RTH is activated, whether by radio failsafe, or using the RTH radio control mode, the safehome identified during arming will replace the home location. If RTH is turned off, either by regaining radio control or turning off the RTH radio control mode, the home location will return back to arming point. -You can define up to 8 safehomes for different locations you fly at. +The safehome operating mode is set using ```safehome_usage_mode```. If ```OFF```, safehomes will not be used. If ```RTH```, the safehome will replace the arming location when RTH is activated, either manually or because of RX failsafe. If ```RTH_FS```, the safehome will only be used for RX failsafe. This option can be changed using the OSD menu. + +If you frequently use RTH to return back to the arming point, you may not want the aircraft to fly to the safehome. Let it do this at least once to confirm safehomes is working as expected. Afterward, `set safehome_usage_mode = RTH_FS` and the safehome will only be used for failsafe. + +When using mode `RTH_FS`, you should confirm that your radio's failsafe configuration triggers the iNav failsafe mode. With many receivers, you have the ability to specify what signal to output during failsafe conditions. When you are choosing safehome locations, ensure that the location is clear of obstructions for a radius more than 50m (`nav_fw_loiter_radius`). As the plane descends, the circles aren't always symmetrical, as wind direction could result in some wider or tighter turns. Also, the direction and length of the final landing stage is also unknown. You want to choose a point that has provides a margin for variation and the final landing. +If your safehome is not visible from your current location, use extra caution. A visual check of the safehome is recommend prior to flying. If the safehome is in use, you can use the OSD menu to disable safehome usage prior to your flight. + ## OSD Message when Armed When the aircraft is armed, the OSD briefly shows `ARMED` and the current GPS position and current date and time. @@ -36,8 +42,14 @@ If a safehome is selected, an additional message appears: CURRENT DATE CURRENT TIME ``` -The GPS details are those of the selected safehome. -To draw your attention to "HOME" being replaced, the message flashes and stays visible longer. +The GPS details are those of the arming location, not the safehome. +To draw your attention to a safehome being selected, the message flashes and stays visible longer. + +If a safehome was found, but ``safehome_usage_mode``` is ```OFF```, the message ```SAFEHOME FOUND; MODE OFF``` will appear. + +## OSD Message during RTH + +If RTH is in progress to a safehome, the message "DIVERTING TO SAFEHOME" will be displayed. ## CLI command `safehome` to manage safehomes diff --git a/docs/Settings.md b/docs/Settings.md index 9a70f0144c..6caf91d62e 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1,5 +1,6 @@ # CLI Variable Reference +<<<<<<< HEAD | Variable Name | Default Value | Description | | ------------- | ------------- | ----------- | | 3d_deadband_high | 1514 | High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead) | @@ -538,5 +539,569 @@ | 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 | 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. | +======= +| Variable Name | Default Value | Min | Max | Description | +| ------------- | ------------- | --- | --- | ----------- | +| 3d_deadband_high | 1514 | PWM_RANGE_MIN | PWM_RANGE_MAX | High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead) | +| 3d_deadband_low | 1406 | PWM_RANGE_MIN | PWM_RANGE_MAX | 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) | +| 3d_deadband_throttle | 50 | 0 | 200 | Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter. | +| 3d_neutral | 1460 | PWM_RANGE_MIN | PWM_RANGE_MAX | Neutral (stop) throttle value for 3D mode | +| acc_event_threshold_high | 0 | 0 | 65535 | 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. | +| acc_event_threshold_low | 0 | 0 | 900 | 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. | +| acc_event_threshold_neg_x | 0 | 0 | 65535 | 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. | +| acc_hardware | AUTO | | | Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info | +| acc_lpf_hz | 15 | 0 | 200 | 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 | 1 | 1 | 255 | | +| acc_notch_hz | 0 | 0 | 255 | | +| accgain_x | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | +| accgain_y | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | +| accgain_z | 4096 | 1 | 8192 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | +| acczero_x | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. | +| acczero_y | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. | +| acczero_z | 0 | -32768 | 32767 | Calculated value after '6 position avanced calibration'. See Wiki page. | +| airmode_throttle_threshold | 1300 | 1000 | 2000 | 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 | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | 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 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_board_roll | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_board_yaw | 0 | -1800 | 3600 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_gyro | 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_mag | 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_mag_pitch | 0 | -1800 | 3600 | Same as align_mag_roll, but for the pitch axis. | +| align_mag_roll | 0 | -1800 | 3600 | 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 | -1800 | 3600 | Same as align_mag_roll, but for the yaw axis. | +| align_opflow | CW0FLIP | | | Optical flow module alignment (default CW0_DEG_FLIP) | +| alt_hold_deadband | 50 | 10 | 250 | Defines the deadband of throttle during alt_hold [r/c points] | +| antigravity_accelerator | 1 | 1 | 20 | | +| antigravity_cutoff_lpf_hz | 15 | 1 | 30 | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain | +| antigravity_gain | 1 | 1 | 20 | Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allowed to double during rapid throttle movements | +| applied_defaults | 0 | 0 | 3 | Internal (configurator) hint. Should not be changed manually | +| baro_cal_tolerance | 150 | 0 | 1000 | 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 | 0 | 12 | 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 | 0 | 4294967295 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. | +| battery_capacity_critical | 0 | 0 | 4294967295 | 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 | 0 | 4294967295 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. | +| blackbox_device | _target default_ | | | Selection of where to write blackbox data | +| blackbox_rate_denom | 1 | 1 | 65535 | Blackbox logging rate denominator. See blackbox_rate_num. | +| blackbox_rate_num | 1 | 1 | 65535 | 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 | 0 | 4294967295 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit | +| current_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled | +| current_meter_offset | _target default_ | -32768 | 32767 | This sets the output offset voltage of the current sensor in millivolts. | +| current_meter_scale | _target default_ | -10000 | 10000 | 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 | 1.25 | 1 | 3 | | +| d_boost_gyro_delta_lpf_hz | 80 | 10 | 250 | | +| d_boost_max_at_acceleration | 7500 | 1000 | 16000 | | +| deadband | 5 | 0 | 32 | 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_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 | 1 | 0 | 255 | Enables workarounds for different versions of MSP protocol used | +| dshot_beeper_enabled | ON | | | Whether using DShot motors as beepers is enabled | +| dshot_beeper_tone | 1 | 1 | 5 | Sets the DShot beeper tone | +| dterm_lpf2_hz | 0 | 0 | 500 | 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_lpf_hz | 40 | 0 | 500 | 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 | +| dynamic_gyro_notch_min_hz | 150 | 30 | 1000 | 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 | 1 | 1000 | 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 | 435 | 415 | 450 | | +| eleres_loc_delay | 240 | 30 | 1800 | | +| eleres_loc_en | OFF | | | | +| eleres_loc_power | 7 | 0 | 7 | | +| eleres_signature | 0 | | 4294967295 | | +| eleres_telemetry_en | OFF | | | | +| eleres_telemetry_power | 7 | 0 | 7 | | +| esc_sensor_listen_only | OFF | | | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case | +| failsafe_delay | 5 | 0 | 200 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | +| failsafe_fw_pitch_angle | 100 | -800 | 800 | 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 | -800 | 800 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll | +| failsafe_fw_yaw_rate | -45 | -1000 | 1000 | Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn | +| failsafe_lights | ON | | | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. | +| failsafe_lights_flash_on_time | 100 | 20 | 65535 | Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. | +| failsafe_lights_flash_period | 1000 | 40 | 65535 | Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. | +| failsafe_min_distance | 0 | 0 | 65000 | 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. | +| failsafe_min_distance_procedure | DROP | | | What failsafe procedure to initiate in Stage 2 when craft is closer to home than failsafe_min_distance. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | +| failsafe_mission | ON | | | 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 | +| failsafe_off_delay | 200 | 0 | 200 | Time in deciseconds to wait before turning off motors when failsafe is activated. 0 = No timeout. See [Failsafe documentation](Failsafe.md#failsafe_off_delay). | +| failsafe_procedure | SET-THR | | | What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | +| failsafe_recovery_delay | 5 | 0 | 200 | Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). | +| failsafe_stick_threshold | 50 | 0 | 500 | 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 | PWM_RANGE_MIN | PWM_RANGE_MAX | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | +| failsafe_throttle_low_delay | 0 | 0 | 300 | 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 | FLAPERON_THROW_MIN | FLAPERON_THROW_MAX | 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 | 0 | 0 | 50 | | +| frsky_coordinates_format | 0 | 0 | FRSKY_FORMAT_NMEA | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA | +| frsky_default_latitude | 0 | -90 | 90 | 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 | -180 | 180 | 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 | FRSKY_VFAS_PRECISION_LOW | FRSKY_VFAS_PRECISION_HIGH | 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 | +| fw_autotune_ff_to_i_tc | 600 | 100 | 5000 | FF to I time (defines time for I to reach the same level of response as FF) [ms] | +| fw_autotune_ff_to_p_gain | 10 | 0 | 100 | FF to P gain (strength relationship) [%] | +| fw_autotune_max_rate_deflection | 80 | 50 | 100 | The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. | +| fw_autotune_min_stick | 50 | 0 | 100 | Minimum stick input [%] to consider overshoot/undershoot detection | +| fw_autotune_p_to_d_gain | 0 | 0 | 200 | P to D gain (strength relationship) [%] | +| fw_autotune_rate_adjustment | AUTO | | | `AUTO` and `LIMIT` adjust the rates to match the capabilities of the airplane, with `LIMIT` they are never increased above the starting rates setting. `FIXED` does not adjust the rates. Rates are not changed when tuning in `ANGLE` mode. | +| fw_d_level | 75 | 0 | 200 | Fixed-wing attitude stabilisation HORIZON transition point | +| fw_d_pitch | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for PITCH | +| fw_d_roll | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for ROLL | +| fw_d_yaw | 0 | 0 | 200 | Fixed wing rate stabilisation D-gain for YAW | +| fw_ff_pitch | 50 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for PITCH | +| fw_ff_roll | 50 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for ROLL | +| fw_ff_yaw | 60 | 0 | 200 | Fixed-wing rate stabilisation FF-gain for YAW | +| fw_i_level | 5 | 0 | 200 | Fixed-wing attitude stabilisation low-pass filter cutoff | +| fw_i_pitch | 7 | 0 | 200 | Fixed-wing rate stabilisation I-gain for PITCH | +| fw_i_roll | 7 | 0 | 200 | Fixed-wing rate stabilisation I-gain for ROLL | +| fw_i_yaw | 10 | 0 | 200 | Fixed-wing rate stabilisation I-gain for YAW | +| fw_iterm_limit_stick_position | 0.5 | 0 | 1 | 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 | +| fw_iterm_throw_limit | 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX | 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. | +| fw_level_pitch_deadband | 5 | 0 | 20 | Deadband for automatic leveling when AUTOLEVEL mode is used. | +| fw_level_pitch_gain | 5 | 0 | 20 | I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations | +| fw_level_pitch_trim | 0 | -10 | 10 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level | +| fw_loiter_direction | RIGHT | | | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. | +| fw_min_throttle_down_pitch | 0 | 0 | 450 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) | +| fw_p_level | 20 | 0 | 200 | Fixed-wing attitude stabilisation P-gain | +| fw_p_pitch | 5 | 0 | 200 | Fixed-wing rate stabilisation P-gain for PITCH | +| fw_p_roll | 5 | 0 | 200 | Fixed-wing rate stabilisation P-gain for ROLL | +| fw_p_yaw | 6 | 0 | 200 | Fixed-wing rate stabilisation P-gain for YAW | +| fw_reference_airspeed | 1500 | 300 | 6000 | 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. | +| fw_tpa_time_constant | 0 | 0 | 5000 | 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 | 0 | 2 | 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 | 0 | 2 | 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 | 0 | 90 | 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. | +| gps_min_sats | 6 | 5 | 10 | 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. | +| gps_provider | UBLOX | | | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). | +| gps_sbas_mode | NONE | | | Which SBAS mode to be used | +| gps_ublox_use_galileo | OFF | | | 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]. | +| gyro_abg_alpha | 0 | 0 | 1 | Alpha factor for Gyro Alpha-Beta-Gamma filter | +| gyro_abg_boost | 0.35 | 0 | 2 | Boost factor for Gyro Alpha-Beta-Gamma filter | +| gyro_abg_half_life | 0.5 | 0 | 10 | Sample half-life for Gyro Alpha-Beta-Gamma filter | +| gyro_anti_aliasing_lpf_hz | 250 | | 255 | Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz | +| gyro_anti_aliasing_lpf_type | PT1 | | | Specifies the type of the software LPF of the gyro signals. | +| gyro_dyn_lpf_curve_expo | 5 | 1 | 10 | Expo value for the throttle-to-frequency mapping for Dynamic LPF | +| gyro_dyn_lpf_max_hz | 500 | 40 | 1000 | Maximum frequency of the gyro Dynamic LPF | +| gyro_dyn_lpf_min_hz | 200 | 40 | 400 | Minimum frequency of the gyro Dynamic LPF | +| 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_main_lpf_hz | 60 | 0 | 500 | Software based gyro main lowpass filter. Value is cutoff frequency (Hz) | +| gyro_main_lpf_type | BIQUAD | | | Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | +| gyro_notch_cutoff | 1 | 1 | 500 | | +| gyro_notch_hz | 0 | | 500 | | +| gyro_to_use | 0 | 0 | 1 | | +| 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 | HEADING_HOLD_RATE_LIMIT_MIN | HEADING_HOLD_RATE_LIMIT_MAX | 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. | +| hott_alarm_sound_interval | 5 | 0 | 120 | Battery alarm delay in seconds for Hott telemetry | +| 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 | 0 | 255 | Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. | +| idle_power | 0 | 0 | 65535 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit | +| imu2_align_pitch | 0 | -1800 | 3600 | Pitch alignment for Secondary IMU. 1/10 of a degree | +| imu2_align_roll | 0 | -1800 | 3600 | Roll alignment for Secondary IMU. 1/10 of a degree | +| imu2_align_yaw | 0 | -1800 | 3600 | Yaw alignment for Secondary IMU. 1/10 of a degree | +| imu2_gain_acc_x | 0 | -32768 | 32767 | Secondary IMU ACC calibration data | +| imu2_gain_acc_y | 0 | -32768 | 32767 | Secondary IMU ACC calibration data | +| imu2_gain_acc_z | 0 | -32768 | 32767 | Secondary IMU ACC calibration data | +| imu2_gain_mag_x | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | +| imu2_gain_mag_y | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | +| imu2_gain_mag_z | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | +| imu2_hardware | NONE | | | Selection of a Secondary IMU hardware type. NONE disables this functionality | +| imu2_radius_acc | 0 | -32768 | 32767 | Secondary IMU MAG calibration data | +| imu2_radius_mag | 0 | -32768 | 32767 | 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 | 0 | 20 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes | +| imu_acc_ignore_slope | 0 | 0 | 5 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) | +| imu_dcm_ki | 50 | | 65535 | Inertial Measurement Unit KI Gain for accelerometer measurements | +| imu_dcm_ki_mag | 0 | | 65535 | Inertial Measurement Unit KI Gain for compass measurements | +| imu_dcm_kp | 2500 | | 65535 | Inertial Measurement Unit KP Gain for accelerometer measurements | +| imu_dcm_kp_mag | 10000 | | 65535 | 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 | 0 | 9999 | Uncertainty value for barometric sensor [cm] | +| inav_gravity_cal_tolerance | 5 | 0 | 255 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. | +| inav_max_eph_epv | 1000 | 0 | 9999 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] | +| inav_max_surface_altitude | 200 | 0 | 1000 | 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 | 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.01 | 0 | 1 | Weight for accelerometer drift estimation | +| inav_w_xy_flow_p | 1.0 | 0 | 100 | | +| inav_w_xy_flow_v | 2.0 | 0 | 100 | | +| inav_w_xy_gps_p | 1.0 | 0 | 10 | Weight of GPS coordinates in estimated UAV position and speed. | +| inav_w_xy_gps_v | 2.0 | 0 | 10 | Weight of GPS velocity data in estimated UAV speed | +| inav_w_xy_res_v | 0.5 | 0 | 10 | Decay coefficient for estimated velocity when GPS reference for position is lost | +| inav_w_xyz_acc_p | 1.0 | 0 | 1 | | +| inav_w_z_baro_p | 0.35 | 0 | 10 | Weight of barometer measurements in estimated altitude and climb rate | +| inav_w_z_gps_p | 0.2 | 0 | 10 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes | +| inav_w_z_gps_v | 0.1 | 0 | 10 | 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 | 0 | 10 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost | +| inav_w_z_surface_p | 3.5 | 0 | 100 | | +| inav_w_z_surface_v | 6.1 | 0 | 100 | | +| iterm_windup | 50 | 0 | 90 | 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 | | | | +| limit_attn_filter_cutoff | 1.2 | | 100 | Throttle attenuation PI control output filter cutoff frequency | +| limit_burst_current | 0 | | 4000 | Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable | +| limit_burst_current_falldown_time | 0 | | 3000 | Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current` | +| limit_burst_current_time | 0 | | 3000 | Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced | +| limit_burst_power | 0 | | 40000 | Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable | +| limit_burst_power_falldown_time | 0 | | 3000 | Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power` | +| limit_burst_power_time | 0 | | 3000 | Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced | +| limit_cont_current | 0 | | 4000 | Continous current limit (dA), set to 0 to disable | +| limit_cont_power | 0 | | 40000 | Continous power limit (dW), set to 0 to disable | +| limit_pi_i | 100 | | 10000 | Throttle attenuation PI control I term | +| limit_pi_p | 100 | | 10000 | Throttle attenuation PI control P term | +| log_level | ERROR | | | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. | +| log_topics | 0 | 0 | 4294967295 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. | +| looptime | 1000 | | 9000 | 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 | 20 | 120 | Adjust how long time the Calibration of mag will last. | +| mag_declination | 0 | -18000 | 18000 | 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 | 0 | 0 | 1 | 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 | -32768 | 32767 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed | +| maggain_y | 1024 | -32768 | 32767 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed | +| maggain_z | 1024 | -32768 | 32767 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed | +| magzero_x | 0 | -32768 | 32767 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | +| magzero_y | 0 | -32768 | 32767 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | +| magzero_z | 0 | -32768 | 32767 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | +| manual_pitch_rate | 100 | 0 | 100 | Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]% | +| manual_rc_expo | 70 | 0 | 100 | Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] | +| manual_rc_yaw_expo | 20 | 0 | 100 | Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] | +| manual_roll_rate | 100 | 0 | 100 | Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% | +| manual_yaw_rate | 100 | 0 | 100 | Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% | +| mavlink_ext_status_rate | 2 | 0 | 255 | | +| mavlink_extra1_rate | 10 | 0 | 255 | | +| mavlink_extra2_rate | 2 | 0 | 255 | | +| mavlink_extra3_rate | 1 | 0 | 255 | | +| mavlink_pos_rate | 2 | 0 | 255 | | +| mavlink_rc_chan_rate | 5 | 0 | 255 | | +| mavlink_version | 2 | 1 | 2 | Version of MAVLink to use | +| max_angle_inclination_pit | 300 | 100 | 900 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | +| max_angle_inclination_rll | 300 | 100 | 900 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | +| max_check | 1900 | PWM_RANGE_MIN | PWM_RANGE_MAX | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | +| max_throttle | 1850 | PWM_RANGE_MIN | PWM_RANGE_MAX | 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. | +| mc_cd_lpf_hz | 30 | 0 | 200 | Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky | +| mc_cd_pitch | 60 | 0 | 200 | Multicopter Control Derivative gain for PITCH | +| mc_cd_roll | 60 | 0 | 200 | Multicopter Control Derivative gain for ROLL | +| mc_cd_yaw | 60 | 0 | 200 | Multicopter Control Derivative gain for YAW | +| mc_d_level | 75 | 0 | 200 | Multicopter attitude stabilisation HORIZON transition point | +| mc_d_pitch | 23 | 0 | 200 | Multicopter rate stabilisation D-gain for PITCH | +| mc_d_roll | 23 | 0 | 200 | Multicopter rate stabilisation D-gain for ROLL | +| mc_d_yaw | 0 | 0 | 200 | Multicopter rate stabilisation D-gain for YAW | +| mc_i_level | 15 | 0 | 200 | Multicopter attitude stabilisation low-pass filter cutoff | +| mc_i_pitch | 30 | 0 | 200 | Multicopter rate stabilisation I-gain for PITCH | +| mc_i_roll | 30 | 0 | 200 | Multicopter rate stabilisation I-gain for ROLL | +| mc_i_yaw | 45 | 0 | 200 | Multicopter rate stabilisation I-gain for YAW | +| mc_iterm_relax | RP | | | | +| mc_iterm_relax_cutoff | 15 | 1 | 100 | | +| mc_p_level | 20 | 0 | 200 | Multicopter attitude stabilisation P-gain | +| mc_p_pitch | 40 | 0 | 200 | Multicopter rate stabilisation P-gain for PITCH | +| mc_p_roll | 40 | 0 | 200 | Multicopter rate stabilisation P-gain for ROLL | +| mc_p_yaw | 85 | 0 | 200 | Multicopter rate stabilisation P-gain for YAW | +| min_check | 1100 | PWM_RANGE_MIN | PWM_RANGE_MAX | 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. | +| min_command | 1000 | 0 | PWM_RANGE_MAX | 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. | +| mode_range_logic_operator | OR | | | Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. | +| model_preview_type | -1 | -1 | 32767 | ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons. | +| moron_threshold | 32 | | 128 | 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. | +| motor_accel_time | 0 | 0 | 1000 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | +| motor_decel_time | 0 | 0 | 1000 | 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 | 14 | 4 | 255 | 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 | 50 | 32000 | 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 | 0 | 65535 | 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_ | | | Craft name | +| nav_auto_climb_rate | 500 | 10 | 2000 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] | +| nav_auto_speed | 300 | 10 | 2000 | 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 | 100 | 2000 | 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 | 35 | 5 | 80 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll | +| nav_fw_climb_angle | 20 | 5 | 80 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit | +| nav_fw_control_smoothness | 0 | 0 | 9 | How smoothly the autopilot controls the airplane to correct the navigation error | +| nav_fw_cruise_speed | 0 | 0 | 65535 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s | +| nav_fw_cruise_thr | 1400 | 1000 | 2000 | 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 ) | +| nav_fw_cruise_yaw_rate | 20 | 0 | 60 | Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps] | +| nav_fw_dive_angle | 15 | 5 | 80 | Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit | +| nav_fw_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Fixedwing) | +| nav_fw_land_dive_angle | 2 | -20 | 20 | 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 | +| nav_fw_launch_accel | 1863 | 1000 | 20000 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | +| nav_fw_launch_climb_angle | 18 | 5 | 45 | Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | +| nav_fw_launch_detect_time | 40 | 10 | 1000 | Time for which thresholds have to breached to consider launch happened [ms] | +| nav_fw_launch_end_time | 3000 | 0 | 5000 | 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 | 1000 | 2000 | 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 | 0 | 60000 | 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 | 5 | 180 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | +| nav_fw_launch_min_time | 0 | 0 | 60000 | Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]. | +| nav_fw_launch_motor_delay | 500 | 0 | 5000 | Delay between detected launch and launch sequence start and throttling up (ms) | +| nav_fw_launch_spinup_time | 100 | 0 | 1000 | Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller | +| nav_fw_launch_thr | 1700 | 1000 | 2000 | Launch throttle - throttle to be set during launch sequence (pwm units) | +| nav_fw_launch_timeout | 5000 | | 60000 | 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) | +| nav_fw_launch_velocity | 300 | 100 | 10000 | Forward velocity threshold for swing-launch detection [cm/s] | +| nav_fw_loiter_radius | 7500 | 0 | 30000 | PosHold radius. 3000 to 7500 is a good value (30-75m) [cm] | +| nav_fw_max_thr | 1700 | 1000 | 2000 | Maximum throttle for flying wing in GPS assisted modes | +| nav_fw_min_thr | 1200 | 1000 | 2000 | Minimum throttle for flying wing in GPS assisted modes | +| nav_fw_pitch2thr | 10 | 0 | 100 | 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) | +| nav_fw_pitch2thr_smoothing | 6 | 0 | 9 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | +| nav_fw_pitch2thr_threshold | 50 | 0 | 900 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | +| nav_fw_pos_hdg_d | 0 | 0 | 255 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | +| nav_fw_pos_hdg_i | 2 | 0 | 255 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | +| nav_fw_pos_hdg_p | 30 | 0 | 255 | P gain of heading PID controller. (Fixedwing, rovers, boats) | +| nav_fw_pos_hdg_pidsum_limit | 350 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats) | +| nav_fw_pos_xy_d | 8 | 0 | 255 | 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 | 0 | 255 | 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 | 0 | 255 | 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 | 10 | 0 | 255 | D gain of altitude PID controller (Fixedwing) | +| nav_fw_pos_z_i | 5 | 0 | 255 | I gain of altitude PID controller (Fixedwing) | +| nav_fw_pos_z_p | 40 | 0 | 255 | P gain of altitude PID controller (Fixedwing) | +| nav_fw_yaw_deadband | 0 | 0 | 90 | Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course | +| nav_land_maxalt_vspd | 200 | 100 | 2000 | Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s] | +| nav_land_minalt_vspd | 50 | 50 | 500 | Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s] | +| nav_land_slowdown_maxalt | 2000 | 500 | 4000 | Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm] | +| nav_land_slowdown_minalt | 500 | 50 | 1000 | Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm] | +| nav_manual_climb_rate | 200 | 10 | 2000 | Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s] | +| nav_manual_speed | 500 | 10 | 2000 | Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only] | +| nav_max_altitude | 0 | 0 | 65000 | Max allowed altitude (above Home Point) that applies to all NAV modes (including Altitude Hold). 0 means limit is disabled | +| nav_max_terrain_follow_alt | 100 | | 1000 | Max allowed above the ground altitude for terrain following mode | +| nav_mc_auto_disarm_delay | 2000 | 100 | 10000 | Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s) | +| nav_mc_bank_angle | 30 | 15 | 45 | 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 | 15 | 60 | max angle that MR is allowed to bank in BOOST mode | +| nav_mc_braking_boost_disengage_speed | 100 | 0 | 1000 | BOOST will be disabled when speed goes below this value | +| nav_mc_braking_boost_factor | 100 | 0 | 200 | acceleration factor for BOOST phase | +| nav_mc_braking_boost_speed_threshold | 150 | 100 | 1000 | BOOST can be enabled when speed is above this value | +| nav_mc_braking_boost_timeout | 750 | 0 | 5000 | how long in ms BOOST phase can happen | +| nav_mc_braking_disengage_speed | 75 | 0 | 1000 | braking is disabled when speed goes below this value | +| nav_mc_braking_speed_threshold | 100 | 0 | 1000 | min speed in cm/s above which braking can happen | +| nav_mc_braking_timeout | 2000 | 100 | 5000 | timeout in ms for braking | +| nav_mc_heading_p | 60 | 0 | 255 | P gain of Heading Hold controller (Multirotor) | +| nav_mc_hover_thr | 1500 | 1000 | 2000 | Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering. | +| nav_mc_pos_deceleration_time | 120 | 0 | 255 | 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 | +| nav_mc_pos_expo | 10 | 0 | 255 | Expo for PosHold control | +| nav_mc_pos_xy_p | 65 | 0 | 255 | Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity | +| nav_mc_pos_z_p | 50 | 0 | 255 | P gain of altitude PID controller (Multirotor) | +| nav_mc_vel_xy_d | 100 | 0 | 255 | 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 | 0 | 100 | 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 | 0 | 100 | 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 | 0 | 100 | 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 | 0 | 100 | | +| nav_mc_vel_xy_ff | 40 | 0 | 255 | | +| nav_mc_vel_xy_i | 15 | 0 | 255 | 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 | 0 | 255 | 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 | 0 | 255 | D gain of velocity PID controller | +| nav_mc_vel_z_i | 50 | 0 | 255 | I gain of velocity PID controller | +| nav_mc_vel_z_p | 100 | 0 | 255 | 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 | 0 | 5000 | 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 | 0 | 10 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | +| nav_rth_abort_threshold | 50000 | | 65000 | 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] | +| nav_rth_allow_landing | ALWAYS | | | If set to ON drone will land as a last phase of RTH. | +| nav_rth_alt_control_override | OFF | | | If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 1 second. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home (CLIMB FIRST override only works for fixed wing) | +| nav_rth_alt_mode | AT_LEAST | | | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | +| nav_rth_altitude | 1000 | | 65000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | +| nav_rth_climb_first | ON | | | If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor) | +| nav_rth_climb_ignore_emerg | OFF | | | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | +| nav_rth_home_altitude | 0 | | 65000 | 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] | +| nav_rth_tail_first | OFF | | | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | +| nav_use_fw_yaw_control | OFF | | | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | +| nav_use_midthr_for_althold | OFF | | | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude | +| 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_load_on_boot | OFF | | | If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup. | +| nav_wp_radius | 100 | 10 | 10000 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | +| nav_wp_safe_distance | 10000 | | 65000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. | +| opflow_hardware | NONE | | | Selection of OPFLOW hardware. | +| opflow_scale | 10.5 | 0 | 10000 | | +| osd_ahi_bordered | OFF | | | Shows a border/corners around the AHI region (pixel OSD only) | +| osd_ahi_height | 162 | | 255 | AHI height in pixels (pixel OSD only) | +| osd_ahi_max_pitch | 20 | 10 | 90 | 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 | -18 | -128 | 127 | AHI vertical offset from center (pixel OSD only) | +| osd_ahi_width | 132 | | 255 | AHI width in pixels (pixel OSD only) | +| osd_alt_alarm | 100 | 0 | 10000 | Value above which to make the OSD relative altitude indicator blink (meters) | +| osd_baro_temp_alarm_max | 600 | -550 | 1250 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) | +| osd_baro_temp_alarm_min | -200 | -550 | 1250 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) | +| osd_camera_fov_h | 135 | 60 | 150 | Horizontal field of view for the camera in degres | +| osd_camera_fov_v | 85 | 30 | 120 | Vertical field of view for the camera in degres | +| osd_camera_uptilt | 0 | -40 | 80 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal | +| osd_coordinate_digits | 9 | 8 | 11 | | +| 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 | 0 | 255 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. | +| osd_dist_alarm | 1000 | 0 | 50000 | Value above which to make the OSD distance from home indicator blink (meters) | +| osd_esc_temp_alarm_max | 900 | -550 | 1500 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | +| osd_esc_temp_alarm_min | -200 | -550 | 1500 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | +| osd_estimations_wind_compensation | ON | | | Use wind estimation for remaining flight time/distance estimation | +| osd_failsafe_switch_layout | OFF | | | If enabled the OSD automatically switches to the first layout during failsafe | +| osd_force_grid | OFF | | | Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) | +| osd_gforce_alarm | 5 | 0 | 20 | Value above which the OSD g force indicator will blink (g) | +| osd_gforce_axis_alarm_max | 5 | -20 | 20 | Value above which the OSD axis g force indicators will blink (g) | +| osd_gforce_axis_alarm_min | -5 | -20 | 20 | 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 | -2 | 2 | To vertically adjust the whole OSD and AHI and scrolling bars | +| 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 | 0 | 4 | Left and right margins for the hud area | +| osd_hud_margin_v | 3 | 1 | 3 | Top and bottom margins for the hud area | +| osd_hud_radar_disp | 0 | 0 | 4 | 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 | 0 | 4000 | 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 | 100 | 9990 | In meters, radar aircrafts further away than this will not be displayed in the hud | +| osd_hud_radar_range_min | 3 | 1 | 30 | In meters, radar aircrafts closer than this will not be displayed in the hud | +| osd_hud_wp_disp | 0 | 0 | 3 | 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 | -550 | 1250 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | +| osd_imu_temp_alarm_min | -200 | -550 | 1250 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | +| osd_left_sidebar_scroll | NONE | | | | +| osd_left_sidebar_scroll_step | 0 | | 255 | How many units each sidebar step represents. 0 means the default value for the scroll type. | +| osd_link_quality_alarm | 70 | 0 | 100 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | +| osd_main_voltage_decimals | 1 | 1 | 2 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | +| osd_neg_alt_alarm | 5 | 0 | 10000 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | +| osd_pan_servo_index | 0 | 0 | 10 | 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 | -36 | 36 | 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 | 10 | 13 | 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 | NONE | | | | +| osd_right_sidebar_scroll_step | 0 | | 255 | Same as left_sidebar_scroll_step, but for the right sidebar | +| osd_row_shiftdown | 0 | 0 | 1 | Number of rows to shift the OSD display (increase if top rows are cut off) | +| osd_rssi_alarm | 20 | 0 | 100 | Value below which to make the OSD RSSI indicator blink | +| osd_sidebar_height | 3 | 0 | 5 | Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) | +| osd_sidebar_horizontal_offset | 0 | -128 | 127 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. | +| osd_sidebar_scroll_arrows | OFF | | | | +| osd_snr_alarm | 4 | -20 | 10 | Value below which Crossfire SNR Alarm pops-up. (dB) | +| osd_speed_source | GROUND | | | Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR | +| osd_stats_energy_unit | MAH | | | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | +| osd_stats_min_voltage_unit | BATTERY | | | Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats. | +| osd_telemetry | OFF | | | To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST` | +| 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 | 0 | 600 | 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 | 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 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | 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 | 350 | PID_SUM_LIMIT_MIN | PID_SUM_LIMIT_MAX | 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` | 0 | 255 | Mode assignment for PINIO#1 | +| pinio_box2 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | +| pinio_box3 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | +| pinio_box4 | `BOX_PERMANENT_ID_NONE` | 0 | 255 | Mode assignment for PINIO#1 | +| pitch_rate | 20 | 6 | 180 | 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 | 350 | 0 | 10000 | | +| pitot_scale | 1.0 | 0 | 100 | | +| 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 | 2 | 250 | Stick deadband in [r/c points], applied after r/c deadband and expo | +| prearm_timeout | 10000 | 0 | 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 | | 500000 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. | +| rate_accel_limit_yaw | 10000 | | 500000 | 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. | +| rc_expo | 70 | 0 | 100 | Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) | +| rc_filter_frequency | 50 | 0 | 100 | 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 | 0 | 100 | Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) | +| reboot_character | 82 | 48 | 126 | Special character used to trigger reboot | +| 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 | 6 | 180 | 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_harmonics | 1 | 1 | 3 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine | +| rpm_gyro_min_hz | 100 | 30 | 200 | 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 | 1 | 3000 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting | +| rssi_adc_channel | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled | +| rssi_channel | 0 | 0 | MAX_SUPPORTED_RC_CHANNEL_COUNT | RX channel containing the RSSI signal | +| rssi_max | 100 | RSSI_VISIBLE_VALUE_MIN | RSSI_VISIBLE_VALUE_MAX | 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 | RSSI_VISIBLE_VALUE_MIN | RSSI_VISIBLE_VALUE_MAX | 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` | +| rth_energy_margin | 5 | 0 | 100 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation | +| rx_max_usec | 2115 | PWM_PULSE_MIN | PWM_PULSE_MAX | 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 | PWM_PULSE_MIN | PWM_PULSE_MAX | 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 | 0 | 0 | 0 | | +| rx_spi_protocol | _target default_ | | | | +| rx_spi_rf_channel_count | 0 | 0 | 8 | | +| safehome_max_distance | 20000 | 0 | 65000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. | +| safehome_usage_mode | RTH | | | Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`. See [Safehome documentation](Safehomes.md#Safehome) for more information. | +| sbus_sync_interval | 3000 | 500 | 10000 | | +| 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 | _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_autotrim_rotation_limit | 15 | 1 | 60 | Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. | +| servo_center_pulse | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | Servo midpoint | +| servo_lpf_hz | 20 | 0 | 400 | Selects the servo PWM output cutoff frequency. Value is in [Hz] | +| servo_protocol | PWM | | | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) | +| servo_pwm_rate | 50 | 50 | 498 | 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. | +| setpoint_kalman_enabled | OFF | | | Enable Kalman filter on the PID controller setpoint | +| setpoint_kalman_q | 100 | 1 | 16000 | 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 | 1 | 16000 | Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets | +| setpoint_kalman_w | 4 | 1 | 40 | 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_ | | | 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 | -32768 | 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` | | 63 | 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 | SIM_MIN_TRANSMIT_INTERVAL | 65535 | Text message transmission interval in seconds for SIM module. Minimum value: 10 | +| small_angle | 25 | 0 | 180 | 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 | ON | | | | +| smartport_master_inverted | OFF | | | | +| smith_predictor_delay | 0 | 0 | 8 | Expected delay of the gyro signal. In milliseconds | +| smith_predictor_lpf_hz | 50 | 1 | 500 | Cutoff frequency for the Smith Predictor Low Pass Filter | +| smith_predictor_strength | 0.5 | 0 | 1 | The strength factor of a Smith Predictor of PID measurement. In percents | +| spektrum_sat_bind | `SPEKTRUM_SAT_BIND_DISABLED` | SPEKTRUM_SAT_BIND_DISABLED | SPEKTRUM_SAT_BIND_MAX | 0 = disabled. Used to bind the spektrum satellite to RX | +| srxl2_baud_fast | ON | | | | +| srxl2_unit_id | 1 | 0 | 15 | | +| stats | OFF | | | General switch of the statistics recording feature (a.k.a. odometer) | +| stats_total_dist | 0 | | 2147483647 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. | +| stats_total_energy | 0 | | 2147483647 | | +| stats_total_time | 0 | | 2147483647 | Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. | +| switch_disarm_delay | 250 | 0 | 1000 | Delay before disarming when requested by switch (ms) [0-1000] | +| 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 | 1 | 0 | 2 | 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 | 0 | 100 | Throttle exposition value | +| thr_mid | 50 | 0 | 100 | Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. | +| throttle_idle | 15 | 0 | 30 | The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. | +| throttle_scale | 1.0 | 0 | 1 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% | +| throttle_tilt_comp_str | 0 | 0 | 100 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. | +| tpa_breakpoint | 1500 | PWM_RANGE_MIN | PWM_RANGE_MAX | See tpa_rate. | +| tpa_rate | 0 | 0 | 100 | 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. | +| turtle_mode_power_factor | 55 | 0 | 100 | Turtle mode power factor | +| 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 | -1440 | 1440 | 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 | _target default_ | ADC_CHN_NONE | ADC_CHN_MAX | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | +| vbat_cell_detect_voltage | 425 | 100 | 500 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units. | +| vbat_max_cell_voltage | 420 | 100 | 500 | 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_min_cell_voltage | 330 | 100 | 500 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | +| vbat_scale | _target default_ | VBAT_SCALE_MIN | VBAT_SCALE_MAX | 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 | 100 | 500 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | +| vtx_band | 4 | VTX_SETTINGS_NO_BAND | VTX_SETTINGS_MAX_BAND | 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 | VTX_SETTINGS_MIN_CHANNEL | VTX_SETTINGS_MAX_CHANNEL | 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 | 0 | 10000 | 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 | 1 | VTX_SETTINGS_MIN_CHANNEL | VTX_SETTINGS_MAX_CHANNEL | | +| vtx_power | 1 | VTX_SETTINGS_MIN_POWER | VTX_SETTINGS_MAX_POWER | 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 | 0 | 100 | 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 | 0 | 0 | 200 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | +| yaw_rate | 20 | 2 | 180 | 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. | +>>>>>>> master > Note: this table is autogenerated. Do not edit it manually. \ No newline at end of file diff --git a/docs/assets/images/StickPositions.png b/docs/assets/images/StickPositions.png index 2d45981c2a..98281caeef 100644 Binary files a/docs/assets/images/StickPositions.png and b/docs/assets/images/StickPositions.png differ diff --git a/docs/assets/images/StickPositions_trans.png b/docs/assets/images/StickPositions_trans.png new file mode 100644 index 0000000000..2d45981c2a Binary files /dev/null and b/docs/assets/images/StickPositions_trans.png differ diff --git a/docs/development/Building in Windows 10 with MSYS2.md b/docs/development/Building in Windows 10 with MSYS2.md new file mode 100644 index 0000000000..2593e25c61 --- /dev/null +++ b/docs/development/Building in Windows 10 with MSYS2.md @@ -0,0 +1,80 @@ +# General Info + +This is a guide on how to use Windows MSYS2 distribution and building platform to build iNav firmware. This environment is very simple to manage and does not require installing docker for Windows which may get in the way of VMWare or any other virtualization software you already have running for other reasons. Another benefit of this approach is that the compiler runs natively on Windows, so performance is much better than compiling in a virtual environment or a container. You can also integrate with whatever IDE you are using to make code edits and work with github, which makes the entire development and testing workflow a lot more efficient. In addition to MSYS2, this build environment also uses Arm Embedded GCC tolkit from The xPack Project, which provides many benefits over the toolkits maintained by arm.com + +Some of those benefits are described here: + +https://xpack.github.io/arm-none-eabi-gcc/ + +## Setting up build environment + +Download MSYS2 for your architecture (most likely 64-bit) + +https://www.msys2.org/wiki/MSYS2-installation/ + +Click on 64-bit, scroll all the way down for the latest release + +pacman is the package manager which makes it a lot easier to install and maintain all the dependencies + +## Installing dependencies + +Once MSYS2 is installed, you can open up a new terminal window by running: + +"C:\msys64\mingw64.exe" + +You can also make a shortcut of this somewhere on your taskbar, desktop, etc, or even setup a shortcut key to open it up every time you need to get a terminal window. If you right click on the window you can customize the font and layout to make it more comfortable to work with. This is very similar to cygwin or any other terminal program you might have used before + +This is the best part: +``` +pacman -S git ruby make cmake gcc mingw-w64-x86_64-libwinpthread-git unzip wget +``` + +Now, each release needs a different version of arm toolchain. To setup the xPack ARM toolchain, use the following process: + +First, setup your work path, get the release you want to build or master if you want the latest/greatest +``` +mkdir /c/Workspace +cd /c/Workspace +# you can also check out your own fork here which makes contributing easier +git clone https://github.com/iNavFlight/inav +cd inav +# switch to release you want or skip next 2 lines if you want latest +git fetch origin +git checkout -b release_2.6.1 origin/release_2.6.1 +``` +Now create the build and xpack directories and get the toolkit version you need for your inav version +``` +mkdir build +cd build +mkdir /c/Workspace/xpack +cd /c/Workspace/xpack +cat /c/Workspace/inav/cmake/arm-none-eabi-checks.cmake | grep "set(arm_none_eabi_gcc_version" | cut -d\" -f2 +``` +This will give you the version you need for any given release or master branch. You can get to all the releases here and find the version you need + +https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/ +``` +# for version 2.6.1, version needed is 9.2.1 +wget https://github.com/xpack-dev-tools/arm-none-eabi-gcc-xpack/releases/download/v9.2.1-1.1/xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip +unzip xpack-arm-none-eabi-gcc-9.2.1-1.1-win32-x64.zip +``` +This is important, put the toolkit first before your path so that it is picked up ahead of any other versions that may be present on your system +``` +export PATH=/c/Workspace/xpack/xpack-arm-none-eabi-gcc-9.2.1-1.1/bin:$PATH +cd /c/Workspace/inav/build +``` +You may need to run rm -rf * in build directory if you had any failed previous runs now run cmake +``` +# while inside the build directory +cmake .. +``` +Once that's done you can compile the firmware for your controller +``` +make DALRCF405 +``` +To get a list of available targets in iNav, see the target src folder +https://github.com/tednv/inav/tree/master/src/main/target + +The generated hex file will be in /c/Workspace/inav/build folder + +At the time of writting this document, I believe this is the fastest, easiest, and most efficient Windows build environment that is available. I have used this approach several years ago and was very happy with it building iNav 2.1 and 2.2, and now I'm getting back into it so figured I would share my method diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index c154691542..e8376a29df 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -105,6 +105,8 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro_mpu9250.h drivers/accgyro/accgyro_bno055.c drivers/accgyro/accgyro_bno055.h + drivers/accgyro/accgyro_bno055_serial.c + drivers/accgyro/accgyro_bno055_serial.h drivers/adc.c drivers/adc.h @@ -318,6 +320,8 @@ main_sources(COMMON_SRC flight/imu.h flight/kalman.c flight/kalman.h + flight/smith_predictor.c + flight/smith_predictor.h flight/rate_dynamics.c flight/rate_dynamics.h flight/mixer.c @@ -325,6 +329,8 @@ main_sources(COMMON_SRC flight/pid.c flight/pid.h flight/pid_autotune.c + flight/power_limits.c + flight/power_limits.h flight/rth_estimator.c flight/rth_estimator.h flight/servos.c diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index a39aaed510..77225d905f 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1723,9 +1723,8 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("deadband", "%d", rcControlsConfig()->deadband); BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d", rcControlsConfig()->yaw_deadband); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf", "%d", gyroConfig()->gyro_lpf); - 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); + BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_main_lpf_hz); + BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_main_lpf_type); #ifdef USE_DYNAMIC_FILTERS BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ); @@ -1755,7 +1754,6 @@ static bool blackboxWriteSysinfo(void) #endif BLACKBOX_PRINT_HEADER_LINE("acc_notch_hz", "%d", accelerometerConfig()->acc_notch_hz); BLACKBOX_PRINT_HEADER_LINE("acc_notch_cutoff", "%d", accelerometerConfig()->acc_notch_cutoff); - BLACKBOX_PRINT_HEADER_LINE("gyro_stage2_lowpass_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz); BLACKBOX_PRINT_HEADER_LINE("pidSumLimit", "%d", pidProfile()->pidSumLimit); BLACKBOX_PRINT_HEADER_LINE("pidSumLimitYaw", "%d", pidProfile()->pidSumLimitYaw); BLACKBOX_PRINT_HEADER_LINE("axisAccelerationLimitYaw", "%d", pidProfile()->axisAccelerationLimitYaw); diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 1cd0d19f47..fd3b4daaf6 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -42,6 +42,7 @@ #include "msp/msp_serial.h" #include "sensors/gyro.h" +#include "fc/config.h" #define BLACKBOX_SERIAL_PORT_MODE MODE_TX @@ -239,7 +240,7 @@ bool blackboxDeviceOpen(void) * = floor((looptime_ns * 3) / 500.0) * = (looptime_ns * 3) / 500 */ - blackboxMaxHeaderBytesPerIteration = constrain((gyro.targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION); + blackboxMaxHeaderBytesPerIteration = constrain((getLooptime() * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION); return blackboxPort != NULL; } diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 8defa95967..197532337d 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -80,9 +80,14 @@ typedef enum { DEBUG_SPM_VARIO, // Smartport master variometer DEBUG_PCF8574, DEBUG_DYNAMIC_GYRO_LPF, + DEBUG_AUTOLEVEL, DEBUG_FW_D, DEBUG_IMU2, DEBUG_ALTITUDE, + DEBUG_GYRO_ALPHA_BETA_GAMMA, + DEBUG_SMITH_PREDICTOR, + DEBUG_AUTOTRIM, + DEBUG_AUTOTUNE, DEBUG_RATE_DYNAMICS, DEBUG_COUNT } debugType_e; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index eaf5770162..a065c900a4 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -402,8 +402,7 @@ static const OSD_Entry cmsx_menuFilterPerProfileEntries[] = { OSD_LABEL_DATA_ENTRY("-- FILTERING --", profileIndexString), OSD_SETTING_ENTRY("HARDWARE LPF", SETTING_GYRO_HARDWARE_LPF), - OSD_SETTING_ENTRY("GYRO LPF", SETTING_GYRO_LPF_HZ), - OSD_SETTING_ENTRY("GYRO LPF2", SETTING_GYRO_STAGE2_LOWPASS_HZ), + OSD_SETTING_ENTRY("GYRO MAIN", SETTING_GYRO_MAIN_LPF_HZ), OSD_SETTING_ENTRY("DTERM LPF", SETTING_DTERM_LPF_HZ), OSD_SETTING_ENTRY("DTERM LPF2", SETTING_DTERM_LPF2_HZ), #ifdef USE_DYNAMIC_FILTERS diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 9274f3315a..ff9d246ee2 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -75,12 +75,14 @@ static const CMS_Menu cmsx_menuNavSettings = { OSD_SETTING_ENTRY("CLIMB FIRST", SETTING_NAV_RTH_CLIMB_FIRST), OSD_SETTING_ENTRY("TAIL FIRST", SETTING_NAV_RTH_TAIL_FIRST), OSD_SETTING_ENTRY("LAND AFTER RTH", SETTING_NAV_RTH_ALLOW_LANDING), - OSD_SETTING_ENTRY("LAND VERT SPEED", SETTING_NAV_LANDING_SPEED), + OSD_SETTING_ENTRY("LAND MINALT VSPD", SETTING_NAV_LAND_MINALT_VSPD), + OSD_SETTING_ENTRY("LAND MAXALT VSPD", SETTING_NAV_LAND_MAXALT_VSPD), OSD_SETTING_ENTRY("LAND SPEED MIN AT", SETTING_NAV_LAND_SLOWDOWN_MINALT), OSD_SETTING_ENTRY("LAND SPEED SLOW AT", SETTING_NAV_LAND_SLOWDOWN_MAXALT), OSD_SETTING_ENTRY("MIN RTH DISTANCE", SETTING_NAV_MIN_RTH_DISTANCE), OSD_SETTING_ENTRY("RTH ABORT THRES", SETTING_NAV_RTH_ABORT_THRESHOLD), OSD_SETTING_ENTRY("EMERG LANDING SPEED", SETTING_NAV_EMERG_LANDING_SPEED), + OSD_SETTING_ENTRY("SAFEHOME USAGE MODE", SETTING_SAFEHOME_USAGE_MODE), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c old mode 100755 new mode 100644 index 5b36b61300..f95b7bef2a --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -275,9 +275,11 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("WIND VERT", OSD_WIND_SPEED_VERTICAL), OSD_ELEMENT_ENTRY("G-FORCE", OSD_GFORCE), - OSD_ELEMENT_ENTRY("G-FORCE X", OSD_GFORCE), - OSD_ELEMENT_ENTRY("G-FORCE Y", OSD_GFORCE), - OSD_ELEMENT_ENTRY("G-FORCE Z", OSD_GFORCE), + OSD_ELEMENT_ENTRY("G-FORCE X", OSD_GFORCE_X), + OSD_ELEMENT_ENTRY("G-FORCE Y", OSD_GFORCE_Y), + OSD_ELEMENT_ENTRY("G-FORCE Z", OSD_GFORCE_Z), + + OSD_ELEMENT_ENTRY("VTX POWER", OSD_VTX_POWER), #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) OSD_ELEMENT_ENTRY("RC SOURCE", OSD_RC_SOURCE), @@ -288,8 +290,6 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("BARO TEMP", OSD_BARO_TEMPERATURE), #endif - OSD_ELEMENT_ENTRY("VTX POWER", OSD_VTX_POWER), - #ifdef USE_TEMPERATURE_SENSOR OSD_ELEMENT_ENTRY("SENSOR 0 TEMP", OSD_TEMP_SENSOR_0_TEMPERATURE), OSD_ELEMENT_ENTRY("SENSOR 1 TEMP", OSD_TEMP_SENSOR_1_TEMPERATURE), @@ -301,11 +301,29 @@ static const OSD_Entry menuOsdElemsEntries[] = OSD_ELEMENT_ENTRY("SENSOR 7 TEMP", OSD_TEMP_SENSOR_7_TEMPERATURE), #endif + OSD_ELEMENT_ENTRY("GVAR 0", OSD_GVAR_0), + OSD_ELEMENT_ENTRY("GVAR 1", OSD_GVAR_1), + OSD_ELEMENT_ENTRY("GVAR 2", OSD_GVAR_2), + OSD_ELEMENT_ENTRY("GVAR 3", OSD_GVAR_3), + + OSD_ELEMENT_ENTRY("TPA", OSD_TPA), + OSD_ELEMENT_ENTRY("FW CTRL SMOOTH", OSD_NAV_FW_CONTROL_SMOOTHNESS), + OSD_ELEMENT_ENTRY("VERSION", OSD_VERSION), + OSD_ELEMENT_ENTRY("RANGEFINDER", OSD_RANGEFINDER), + #ifdef USE_ESC_SENSOR OSD_ELEMENT_ENTRY("ESC RPM", OSD_ESC_RPM), OSD_ELEMENT_ENTRY("ESC TEMPERATURE", OSD_ESC_TEMPERATURE), #endif +#ifdef USE_POWER_LIMITS + OSD_ELEMENT_ENTRY("PLIM BURST TIME", OSD_PLIMIT_REMAINING_BURST_TIME), + OSD_ELEMENT_ENTRY("PLIM CURR LIMIT", OSD_PLIMIT_ACTIVE_CURRENT_LIMIT), +#ifdef USE_ADC + OSD_ELEMENT_ENTRY("PLIM POWER LIMIT", OSD_PLIMIT_ACTIVE_POWER_LIMIT), +#endif // USE_ADC +#endif // USE_POWER_LIMITS + OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 3a4c9797ef..0656a5dba7 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -250,3 +250,69 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint filter->y1 = y1; filter->y2 = y2; } + +#ifdef USE_ALPHA_BETA_GAMMA_FILTER +void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT) { + // beta, gamma, and eta gains all derived from + // http://yadda.icm.edu.pl/yadda/element/bwmeta1.element.baztech-922ff6cb-e991-417f-93f0-77448f1ef4ec/c/A_Study_Jeong_1_2017.pdf + + const float xi = powf(-alpha + 1.0f, 0.25); // fourth rool of -a + 1 + filter->xk = 0.0f; + filter->vk = 0.0f; + filter->ak = 0.0f; + filter->jk = 0.0f; + filter->a = alpha; + filter->b = (1.0f / 6.0f) * powf(1.0f - xi, 2) * (11.0f + 14.0f * xi + 11 * xi * xi); + filter->g = 2 * powf(1.0f - xi, 3) * (1 + xi); + filter->e = (1.0f / 6.0f) * powf(1 - xi, 4); + filter->dT = dT; + filter->dT2 = dT * dT; + filter->dT3 = dT * dT * dT; + pt1FilterInit(&filter->boostFilter, 100, dT); + + const float boost = boostGain * 100; + + filter->boost = (boost * boost / 10000) * 0.003; + filter->halfLife = halfLife != 0 ? powf(0.5f, dT / halfLife): 1.0f; +} + +FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input) { + //xk - current system state (ie: position) + //vk - derivative of system state (ie: velocity) + //ak - derivative of system velociy (ie: acceleration) + //jk - derivative of system acceleration (ie: jerk) + float rk; // residual error + + // give the filter limited history + filter->xk *= filter->halfLife; + filter->vk *= filter->halfLife; + filter->ak *= filter->halfLife; + filter->jk *= filter->halfLife; + + // update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dT) + filter->xk += filter->dT * filter->vk + (1.0f / 2.0f) * filter->dT2 * filter->ak + (1.0f / 6.0f) * filter->dT3 * filter->jk; + + // update (estimated) velocity (also estimated dterm from measurement) + filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk; + filter->ak += filter->dT * filter->jk; + + // what is our residual error (measured - estimated) + rk = input - filter->xk; + + // artificially boost the error to increase the response of the filter + rk += pt1FilterApply(&filter->boostFilter, fabsf(rk) * rk * filter->boost); + if ((fabsf(rk * filter->a) > fabsf(input - filter->xk))) { + rk = (input - filter->xk) / filter->a; + } + filter->rk = rk; // for logging + + // update our estimates given the residual error. + filter->xk += filter->a * rk; + filter->vk += filter->b / filter->dT * rk; + filter->ak += filter->g / (2.0f * filter->dT2) * rk; + filter->jk += filter->e / (6.0f * filter->dT3) * rk; + + return filter->xk; +} + +#endif \ No newline at end of file diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 81c76cd0dc..a6406ee91c 100644 --- a/src/main/common/filter.h +++ b/src/main/common/filter.h @@ -56,6 +56,18 @@ typedef struct firFilter_s { uint8_t coeffsLength; } firFilter_t; +typedef struct alphaBetaGammaFilter_s { + float a, b, g, e; + float ak; // derivative of system velociy (ie: acceleration) + float vk; // derivative of system state (ie: velocity) + float xk; // current system state (ie: position) + float jk; // derivative of system acceleration (ie: jerk) + float rk; // residual error + float dT, dT2, dT3; + float halfLife, boost; + pt1Filter_t boostFilter; +} alphaBetaGammaFilter_t; + typedef float (*filterApplyFnPtr)(void *filter, float input); typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt); @@ -86,3 +98,6 @@ float biquadFilterReset(biquadFilter_t *filter, float value); float biquadFilterApplyDF1(biquadFilter_t *filter, float input); float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz); void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType); + +void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT); +float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input); \ No newline at end of file diff --git a/src/main/common/fp_pid.c b/src/main/common/fp_pid.c index 753789c9dc..346c6ace3f 100644 --- a/src/main/common/fp_pid.c +++ b/src/main/common/fp_pid.c @@ -96,7 +96,10 @@ float navPidApply3( pid->output_constrained = outValConstrained; /* Update I-term */ - if (!(pidFlags & PID_ZERO_INTEGRATOR)) { + if ( + !(pidFlags & PID_ZERO_INTEGRATOR) && + !(pidFlags & PID_FREEZE_INTEGRATOR) + ) { const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt); if (pidFlags & PID_SHRINK_INTEGRATOR) { @@ -147,6 +150,10 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF float Td = _kD / _kP; pid->param.kT = 2.0f / (Ti + Td); } + else if (_kI > 1e-6f) { + pid->param.kI = _kI; + pid->param.kT = 0.0f; + } else { pid->param.kI = 0.0; pid->param.kT = 0.0; diff --git a/src/main/common/fp_pid.h b/src/main/common/fp_pid.h index 0698c94ddb..17a43bd920 100644 --- a/src/main/common/fp_pid.h +++ b/src/main/common/fp_pid.h @@ -42,6 +42,7 @@ typedef enum { PID_ZERO_INTEGRATOR = 1 << 1, PID_SHRINK_INTEGRATOR = 1 << 2, PID_LIMIT_INTEGRATOR = 1 << 3, + PID_FREEZE_INTEGRATOR = 1 << 4, } pidControllerFlags_e; typedef struct { diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 6d973980cf..26b3fe21ba 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -142,6 +142,18 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } +int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max) +{ + if (ABS(value) < deadband) { + value = 0; + } else if (value > 0) { + value = scaleRange(value - deadband, 0, max - deadband, 0, max); + } else if (value < 0) { + value = scaleRange(value + deadband, min + deadband, 0, min, 0); + } + return value; +} + int32_t constrain(int32_t amt, int32_t low, int32_t high) { if (amt < low) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 19c906b8c1..6f2d9b1245 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -132,6 +132,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu int gcd(int num, int denom); int32_t applyDeadband(int32_t value, int32_t deadband); +int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max); int32_t constrain(int32_t amt, int32_t low, int32_t high); float constrainf(float amt, float low, float high); diff --git a/src/main/common/time.h b/src/main/common/time.h index bef2f6b6a2..ca8f4b6107 100644 --- a/src/main/common/time.h +++ b/src/main/common/time.h @@ -24,10 +24,19 @@ #include "config/parameter_group.h" -// time difference, 32 bits always sufficient +// time difference, signed 32 bits of microseconds overflows at ~35 minutes +// this is worth leaving as int32_t for performance reasons, use timeDeltaLarge_t for deltas that can be big typedef int32_t timeDelta_t; +#define TIMEDELTA_MAX INT32_MAX + +// time difference large, signed 64 bits of microseconds overflows at ~300000 years +typedef int64_t timeDeltaLarge_t; +#define TIMEDELTALARGE_MAX INT64_MAX + // millisecond time -typedef uint32_t timeMs_t ; +typedef uint32_t timeMs_t; +#define TIMEMS_MAX UINT32_MAX + // microsecond time #ifdef USE_64BIT_TIME typedef uint64_t timeUs_t; @@ -48,6 +57,7 @@ typedef uint32_t timeUs_t; #define MS2S(ms) ((ms) * 1e-3f) #define HZ2S(hz) US2S(HZ2US(hz)) +// Use this function only to get small deltas (difference overflows at ~35 minutes) static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); } typedef enum { diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index bb7dfa1bcc..2ac7733651 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -117,7 +117,9 @@ #define PG_DJI_OSD_CONFIG 1027 #define PG_PROGRAMMING_PID 1028 #define PG_SECONDARY_IMU 1029 -#define PG_INAV_END 1029 +#define PG_POWER_LIMITS_CONFIG 1030 +#define PG_OSD_COMMON_CONFIG 1031 +#define PG_INAV_END 1031 // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 @@ -132,4 +134,3 @@ #define PG_ID_INVALID 0 #define PG_ID_FIRST PG_CF_START #define PG_ID_LAST PG_INAV_END - diff --git a/src/main/drivers/accgyro/accgyro.c b/src/main/drivers/accgyro/accgyro.c index 23bd5c0b16..99c69a5c80 100644 --- a/src/main/drivers/accgyro/accgyro.c +++ b/src/main/drivers/accgyro/accgyro.c @@ -98,7 +98,7 @@ void gyroIntExtiInit(gyroDev_t *gyro) } #endif -#if defined (STM32F7) +#if defined (STM32F7) || defined (STM32H7) IOInit(gyro->busDev->irqPin, OWNER_MPU, RESOURCE_EXTI, 0); EXTIHandlerInit(&gyro->exti, gyroIntExtiHandler); diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index fdb73b7250..90dc96c02e 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -32,47 +32,6 @@ #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) @@ -99,18 +58,45 @@ static void bno055SetMode(uint8_t mode) delay(25); } +static 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); +} + 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; } @@ -192,33 +178,4 @@ bno055CalibrationData_t bno055GetCalibrationData(void) 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 index d0c24f6dc4..de0d751d7a 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -25,6 +25,47 @@ #include "common/vector.h" +#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 + typedef struct { uint8_t sys; uint8_t gyr; @@ -47,5 +88,4 @@ 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 +bno055CalibrationData_t bno055GetCalibrationData(void); \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.c b/src/main/drivers/accgyro/accgyro_bno055_serial.c new file mode 100644 index 0000000000..9bf2a0725a --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.c @@ -0,0 +1,271 @@ +/* + * 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 "platform.h" +#ifdef USE_IMU_BNO055 + +#define BNO055_BAUD_RATE 115200 +#define BNO055_FRAME_MAX_TIME_MS 10 + +#include +#include +#include "io/serial.h" +#include "drivers/accgyro/accgyro_bno055.h" +#include "build/debug.h" +#include "drivers/time.h" +#include "flight/secondary_imu.h" + +static serialPort_t * bno055SerialPort = NULL; +static uint8_t receiveBuffer[22]; + +typedef enum { + BNO055_RECEIVE_IDLE, + BNO055_RECEIVE_HEADER, + BNO055_RECEIVE_LENGTH, + BNO055_RECEIVE_PAYLOAD, + BNO055_RECEIVE_ACK, +} bno055ReceiveState_e; + +typedef enum { + BNO055_FRAME_ACK, + BNO055_FRAME_DATA, +} bno055FrameType_e; + +typedef enum { + BNO055_DATA_TYPE_NONE, + BNO055_DATA_TYPE_EULER, + BNO055_DATA_TYPE_CALIBRATION_STATS, +} bno055DataType_e; + +static uint8_t bno055ProtocolState = BNO055_RECEIVE_IDLE; +static uint8_t bno055FrameType; +static uint8_t bno055FrameLength; +static uint8_t bno055FrameIndex; +static timeMs_t bno055FrameStartAtMs = 0; +static uint8_t bno055DataType = BNO055_DATA_TYPE_NONE; + + +static void bno055SerialWriteBuffer(const uint8_t reg, const uint8_t *data, const uint8_t count) { + + bno055ProtocolState = BNO055_RECEIVE_IDLE; + serialWrite(bno055SerialPort, 0xAA); // Start Byte + serialWrite(bno055SerialPort, 0x00); // Write command + serialWrite(bno055SerialPort, reg); + serialWrite(bno055SerialPort, count); + for (uint8_t i = 0; i < count; i++) { + serialWrite(bno055SerialPort, data[i]); + } +} + +static void bno055SerialWrite(const uint8_t reg, const uint8_t data) { + uint8_t buff[1]; + buff[0] = data; + + bno055SerialWriteBuffer(reg, buff, 1); +} + +static void bno055SerialRead(const uint8_t reg, const uint8_t len) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; + serialWrite(bno055SerialPort, 0xAA); // Start Byte + serialWrite(bno055SerialPort, 0x01); // Read command + serialWrite(bno055SerialPort, reg); + serialWrite(bno055SerialPort, len); +} + +void bno055SerialDataReceive(uint16_t c, void *data) { + + UNUSED(data); + + const uint8_t incoming = (uint8_t) c; + + //Failsafe for stuck frames + if (bno055ProtocolState != BNO055_RECEIVE_IDLE && millis() - bno055FrameStartAtMs > BNO055_FRAME_MAX_TIME_MS) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; + } + + if (bno055ProtocolState == BNO055_RECEIVE_IDLE && incoming == 0xEE) { + bno055FrameStartAtMs = millis(); + bno055ProtocolState = BNO055_RECEIVE_HEADER; + bno055FrameType = BNO055_FRAME_ACK; + } else if (bno055ProtocolState == BNO055_RECEIVE_IDLE && incoming == 0xBB) { + bno055FrameStartAtMs = millis(); + bno055ProtocolState = BNO055_RECEIVE_HEADER; + bno055FrameType = BNO055_FRAME_DATA; + } else if (bno055ProtocolState == BNO055_RECEIVE_HEADER && bno055FrameType == BNO055_FRAME_ACK) { + receiveBuffer[0] = incoming; + bno055ProtocolState = BNO055_RECEIVE_IDLE; + } else if (bno055ProtocolState == BNO055_RECEIVE_HEADER && bno055FrameType == BNO055_FRAME_DATA) { + bno055FrameLength = incoming; + bno055FrameIndex = 0; + bno055ProtocolState = BNO055_RECEIVE_LENGTH; + } else if (bno055ProtocolState == BNO055_RECEIVE_LENGTH) { + receiveBuffer[bno055FrameIndex] = incoming; + bno055FrameIndex++; + + if (bno055FrameIndex == bno055FrameLength) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; + + if (bno055DataType == BNO055_DATA_TYPE_EULER) { + secondaryImuState.eulerAngles.raw[0] = ((int16_t)((receiveBuffer[3] << 8) | receiveBuffer[2])) / 1.6f; + secondaryImuState.eulerAngles.raw[1] = ((int16_t)((receiveBuffer[5] << 8) | receiveBuffer[4])) / -1.6f; //Pitch has to be reversed to match INAV notation + secondaryImuState.eulerAngles.raw[2] = ((int16_t)((receiveBuffer[1] << 8) | receiveBuffer[0])) / 1.6f; + secondaryImuProcess(); + } else if (bno055DataType == BNO055_DATA_TYPE_CALIBRATION_STATS) { + secondaryImuState.calibrationStatus.mag = receiveBuffer[0] & 0b00000011; + secondaryImuState.calibrationStatus.acc = (receiveBuffer[0] >> 2) & 0b00000011; + secondaryImuState.calibrationStatus.gyr = (receiveBuffer[0] >> 4) & 0b00000011; + secondaryImuState.calibrationStatus.sys = (receiveBuffer[0] >> 6) & 0b00000011; + } + + bno055DataType = BNO055_DATA_TYPE_NONE; + } + } + +} + +static void bno055SerialSetCalibrationData(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; + } + } + + bno055SerialWriteBuffer(BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12); + delay(25); + + //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 + bno055SerialWriteBuffer(BNO055_ADDR_ACC_RADIUS_LSB, buf, 4); + delay(25); +} + +bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration) { + bno055SerialPort = NULL; + + serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_IMU2); + if (!portConfig) { + return false; + } + + bno055SerialPort = openSerialPort( + portConfig->identifier, + FUNCTION_IMU2, + bno055SerialDataReceive, + NULL, + BNO055_BAUD_RATE, + MODE_RXTX, + SERIAL_NOT_INVERTED | SERIAL_UNIDIR | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO + ); + + if (!bno055SerialPort) { + return false; + } + + bno055SerialRead(0x00, 1); // Read ChipID + delay(5); + + //Check ident + if (bno055FrameType != BNO055_FRAME_DATA || receiveBuffer[0] != 0xA0 || bno055ProtocolState != BNO055_RECEIVE_IDLE) { + return false; // Ident does not match, leave + } + + bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL + delay(25); + + if (setCalibration) { + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG); + delay(25); + + bno055SerialSetCalibrationData(calibrationData); + } + + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); + delay(25); + + return true; +} + +/* + * This function is non-blocking and response will be processed by bno055SerialDataReceive + */ +void bno055SerialFetchEulerAngles() { + bno055DataType = BNO055_DATA_TYPE_EULER; + bno055SerialRead(BNO055_ADDR_EUL_YAW_LSB, 6); +} + +/* + * This function is non-blocking and response will be processed by bno055SerialDataReceive + */ +void bno055SerialGetCalibStat(void) { + bno055DataType = BNO055_DATA_TYPE_CALIBRATION_STATS; + bno055SerialRead(BNO055_ADDR_CALIB_STAT, 1); +} + +/* + * This function is blocking and should not be used during flight conditions! + */ +bno055CalibrationData_t bno055SerialGetCalibrationData(void) { + + bno055CalibrationData_t data; + + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG); + delay(25); + + bno055SerialRead(BNO055_ADDR_ACC_OFFSET_X_LSB, 22); + delay(50); + + 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)((receiveBuffer[bufferBit + 1] << 8) | receiveBuffer[bufferBit]); + bufferBit += 2; + } + } + + data.radius[ACC] = (int16_t)((receiveBuffer[19] << 8) | receiveBuffer[18]); + data.radius[MAG] = (int16_t)((receiveBuffer[21] << 8) | receiveBuffer[20]); + + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); + delay(25); + + return data; +} + +#endif \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.h b/src/main/drivers/accgyro/accgyro_bno055_serial.h new file mode 100644 index 0000000000..1ffd55c646 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.h @@ -0,0 +1,33 @@ +/* + * 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" +#include "drivers/accgyro/accgyro_bno055.h" + +bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration); +void bno055SerialFetchEulerAngles(void); +void bno055SerialGetCalibStat(void); +bno055CalibrationData_t bno055SerialGetCalibrationData(void); \ No newline at end of file diff --git a/src/main/drivers/adc.c b/src/main/drivers/adc.c index e45ffce32f..573c1e69b7 100644 --- a/src/main/drivers/adc.c +++ b/src/main/drivers/adc.c @@ -55,9 +55,9 @@ static uint8_t activeChannelCount[ADCDEV_COUNT] = {0}; static int adcFunctionMap[ADC_FUNCTION_COUNT]; adc_config_t adcConfig[ADC_CHN_COUNT]; // index 0 is dummy for ADC_CHN_NONE -volatile uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES]; +volatile ADC_VALUES_ALIGNMENT(uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES]); -uint8_t adcChannelByTag(ioTag_t ioTag) +uint32_t adcChannelByTag(ioTag_t ioTag) { for (uint8_t i = 0; i < ARRAYLEN(adcTagMap); i++) { if (ioTag == adcTagMap[i].tag) diff --git a/src/main/drivers/adc_impl.h b/src/main/drivers/adc_impl.h index 901122daa9..290a9ef583 100644 --- a/src/main/drivers/adc_impl.h +++ b/src/main/drivers/adc_impl.h @@ -22,19 +22,27 @@ #if defined(STM32F4) || defined(STM32F7) #define ADC_TAG_MAP_COUNT 16 +#elif defined(STM32H7) +#define ADC_TAG_MAP_COUNT 28 #elif defined(STM32F3) #define ADC_TAG_MAP_COUNT 39 #else #define ADC_TAG_MAP_COUNT 10 #endif +#if defined(STM32H7) +#define ADC_VALUES_ALIGNMENT(def) __attribute__ ((section(".DMA_RAM"))) def __attribute__ ((aligned (32))) +#else +#define ADC_VALUES_ALIGNMENT(def) def +#endif + typedef enum ADCDevice { ADCINVALID = -1, ADCDEV_1 = 0, #if defined(STM32F3) ADCDEV_2, ADCDEV_MAX = ADCDEV_2, -#elif defined(STM32F4) || defined(STM32F7) +#elif defined(STM32F4) || defined(STM32F7) || defined(STM32H7) ADCDEV_2, ADCDEV_3, ADCDEV_MAX = ADCDEV_3, @@ -46,20 +54,20 @@ typedef enum ADCDevice { typedef struct adcTagMap_s { ioTag_t tag; - uint8_t channel; + uint32_t channel; } adcTagMap_t; typedef struct adcDevice_s { ADC_TypeDef* ADCx; rccPeriphTag_t rccADC; rccPeriphTag_t rccDMA; -#if defined(STM32F4) || defined(STM32F7) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) DMA_Stream_TypeDef* DMAy_Streamx; uint32_t channel; #else DMA_Channel_TypeDef* DMAy_Channelx; #endif -#if defined(STM32F7) +#if defined(STM32F7) || defined(STM32H7) ADC_HandleTypeDef ADCHandle; DMA_HandleTypeDef DmaHandle; #endif @@ -70,7 +78,7 @@ typedef struct adcDevice_s { typedef struct adc_config_s { ioTag_t tag; ADCDevice adcDevice; - uint8_t adcChannel; // ADC1_INxx channel number + uint32_t adcChannel; // ADC1_INxx channel number uint8_t dmaIndex; // index into DMA buffer in case of sparse channels bool enabled; uint8_t sampleTime; @@ -78,8 +86,8 @@ typedef struct adc_config_s { extern const adcTagMap_t adcTagMap[ADC_TAG_MAP_COUNT]; extern adc_config_t adcConfig[ADC_CHN_COUNT]; -extern volatile uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES]; +extern volatile ADC_VALUES_ALIGNMENT(uint16_t adcValues[ADCDEV_COUNT][ADC_CHN_COUNT * ADC_AVERAGE_N_SAMPLES]); void adcHardwareInit(drv_adc_config_t *init); ADCDevice adcDeviceByInstance(ADC_TypeDef *instance); -uint8_t adcChannelByTag(ioTag_t ioTag); +uint32_t adcChannelByTag(ioTag_t ioTag); diff --git a/src/main/drivers/adc_stm32h7xx.c b/src/main/drivers/adc_stm32h7xx.c new file mode 100644 index 0000000000..be9f302ff1 --- /dev/null +++ b/src/main/drivers/adc_stm32h7xx.c @@ -0,0 +1,230 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/debug.h" + +#include "platform.h" +#include "drivers/time.h" + +#include "drivers/io.h" +#include "io_impl.h" +#include "rcc.h" +#include "dma.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro/accgyro.h" + +#include "adc.h" +#include "adc_impl.h" + + +static adcDevice_t adcHardware[ADCDEV_COUNT] = { + { + .ADCx = ADC1, + .rccADC = RCC_AHB1(ADC12), + .rccDMA = RCC_AHB1(DMA2), + .DMAy_Streamx = DMA2_Stream0, + .channel = DMA_REQUEST_ADC1, + .enabled = false, + .usedChannelCount = 0 + }, + /* currently not used + { + .ADCx = ADC2, + .rccADC = RCC_AHB1(ADC12), + .rccDMA = RCC_AHB1(DMA2), + .DMAy_Streamx = DMA2_Stream1, + .channel = DMA_REQUEST_ADC2, + .enabled = false, + .usedChannelCount = 0 + } + */ +}; + +adcDevice_t adcDevice[ADCDEV_COUNT]; + +/* note these could be packed up for saving space */ +const adcTagMap_t adcTagMap[] = { + { DEFIO_TAG_E__PC0, ADC_CHANNEL_10 }, + { DEFIO_TAG_E__PC1, ADC_CHANNEL_11 }, + { DEFIO_TAG_E__PC2, ADC_CHANNEL_0 }, + { DEFIO_TAG_E__PC3, ADC_CHANNEL_1 }, + { DEFIO_TAG_E__PC4, ADC_CHANNEL_4 }, + { DEFIO_TAG_E__PC5, ADC_CHANNEL_8 }, + { DEFIO_TAG_E__PB0, ADC_CHANNEL_9 }, + { DEFIO_TAG_E__PB1, ADC_CHANNEL_5 }, + { DEFIO_TAG_E__PA0, ADC_CHANNEL_16 }, + { DEFIO_TAG_E__PA1, ADC_CHANNEL_17 }, + { DEFIO_TAG_E__PA2, ADC_CHANNEL_14 }, + { DEFIO_TAG_E__PA3, ADC_CHANNEL_15 }, + { DEFIO_TAG_E__PA4, ADC_CHANNEL_18 }, + { DEFIO_TAG_E__PA5, ADC_CHANNEL_19 }, + { DEFIO_TAG_E__PA6, ADC_CHANNEL_3 }, + { DEFIO_TAG_E__PA7, ADC_CHANNEL_7 }, +}; + +// Translate rank number x to ADC_REGULAR_RANK_x (Note that array index is 0-origin) +static const uint32_t adcRegularRankMap[] = { + ADC_REGULAR_RANK_1, + ADC_REGULAR_RANK_2, + ADC_REGULAR_RANK_3, + ADC_REGULAR_RANK_4, + ADC_REGULAR_RANK_5, + ADC_REGULAR_RANK_6, + ADC_REGULAR_RANK_7, + ADC_REGULAR_RANK_8, + ADC_REGULAR_RANK_9, + ADC_REGULAR_RANK_10, + ADC_REGULAR_RANK_11, + ADC_REGULAR_RANK_12, + ADC_REGULAR_RANK_13, + ADC_REGULAR_RANK_14, + ADC_REGULAR_RANK_15, + ADC_REGULAR_RANK_16, +}; + +ADCDevice adcDeviceByInstance(ADC_TypeDef *instance) +{ + if (instance == ADC1) + return ADCDEV_1; + + if (instance == ADC2) + return ADCDEV_2; + + return ADCINVALID; +} + +static void adcInstanceInit(ADCDevice adcDevice) +{ + adcDevice_t * adc = &adcHardware[adcDevice]; + + RCC_ClockCmd(adc->rccDMA, ENABLE); + RCC_ClockCmd(adc->rccADC, ENABLE); + + adc->ADCHandle.Instance = adc->ADCx; + + adc->ADCHandle.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4; + adc->ADCHandle.Init.Resolution = ADC_RESOLUTION_12B; + adc->ADCHandle.Init.ContinuousConvMode = ENABLE; + adc->ADCHandle.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_CC1; + adc->ADCHandle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; + adc->ADCHandle.Init.NbrOfConversion = adc->usedChannelCount; + adc->ADCHandle.Init.ScanConvMode = adc->usedChannelCount > 1 ? ENABLE : DISABLE; // 1=scan more that one channel in group + adc->ADCHandle.Init.DiscontinuousConvMode = DISABLE; + adc->ADCHandle.Init.NbrOfDiscConversion = 0; + adc->ADCHandle.Init.EOCSelection = DISABLE; + adc->ADCHandle.Init.LowPowerAutoWait = DISABLE; + + // Enable circular DMA. + adc->ADCHandle.Init.ConversionDataManagement = ADC_CONVERSIONDATA_DMA_CIRCULAR; + + adc->ADCHandle.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN; + adc->ADCHandle.Init.OversamplingMode = DISABLE; + + if (HAL_ADC_Init(&adc->ADCHandle) != HAL_OK) { + return; + } + + if (HAL_ADCEx_Calibration_Start(&adc->ADCHandle, ADC_CALIB_OFFSET, ADC_SINGLE_ENDED) != HAL_OK) { + return; + } + + adc->DmaHandle.Init.Request = adc->channel; + adc->DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY; + adc->DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE; + adc->DmaHandle.Init.MemInc = ((adc->usedChannelCount > 1) || (ADC_AVERAGE_N_SAMPLES > 1)) ? DMA_MINC_ENABLE : DMA_MINC_DISABLE; + adc->DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD; + adc->DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD; + adc->DmaHandle.Init.Mode = DMA_CIRCULAR; + adc->DmaHandle.Init.Priority = DMA_PRIORITY_HIGH; + adc->DmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + adc->DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL; + adc->DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE; + adc->DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE; + adc->DmaHandle.Instance = adc->DMAy_Streamx; + + if (HAL_DMA_Init(&adc->DmaHandle) != HAL_OK) { + return; + } + + __HAL_LINKDMA(&adc->ADCHandle, DMA_Handle, adc->DmaHandle); + + uint8_t rank = 0; + for (int i = ADC_CHN_1; i < ADC_CHN_COUNT; i++) { + if (!adcConfig[i].enabled || adcConfig[i].adcDevice != adcDevice) { + continue; + } + + ADC_ChannelConfTypeDef sConfig; + sConfig.Channel = adcConfig[i].adcChannel; + sConfig.Rank = adcRegularRankMap[rank++]; + sConfig.SamplingTime = adcConfig[i].sampleTime; + sConfig.SingleDiff = ADC_SINGLE_ENDED; + sConfig.OffsetNumber = ADC_OFFSET_NONE; + sConfig.Offset = 0; + + if (HAL_ADC_ConfigChannel(&adc->ADCHandle, &sConfig) != HAL_OK) { + return; + } + } + + if (HAL_ADC_Start_DMA(&adc->ADCHandle, (uint32_t*)&adcValues[adcDevice], adc->usedChannelCount * ADC_AVERAGE_N_SAMPLES) != HAL_OK) { + return; + } +} + +void adcHardwareInit(drv_adc_config_t *init) +{ + UNUSED(init); + int configuredAdcChannels = 0; + + for (int i = ADC_CHN_1; i < ADC_CHN_COUNT; i++) { + if (!adcConfig[i].tag) + continue; + + adcDevice_t * adc = &adcHardware[adcConfig[i].adcDevice]; + + IOInit(IOGetByTag(adcConfig[i].tag), OWNER_ADC, RESOURCE_ADC_CH1 + (i - ADC_CHN_1), 0); + IOConfigGPIO(IOGetByTag(adcConfig[i].tag), IO_CONFIG(GPIO_MODE_ANALOG, 0, GPIO_NOPULL)); + + adcConfig[i].adcChannel = adcChannelByTag(adcConfig[i].tag); + adcConfig[i].dmaIndex = adc->usedChannelCount++; + adcConfig[i].sampleTime = ADC_SAMPLETIME_387CYCLES_5; + adcConfig[i].enabled = true; + + adc->enabled = true; + configuredAdcChannels++; + } + + if (configuredAdcChannels == 0) + return; + + for (int i = 0; i < ADCDEV_COUNT; i++) { + if (adcHardware[i].enabled) { + adcInstanceInit(i); + } + } +} diff --git a/src/main/drivers/display_font_metadata.h b/src/main/drivers/display_font_metadata.h index be3fb94a40..ebef4b635a 100644 --- a/src/main/drivers/display_font_metadata.h +++ b/src/main/drivers/display_font_metadata.h @@ -10,12 +10,11 @@ typedef struct displayFontMetadata_s { uint16_t charCount; } displayFontMetadata_t; -// 'I', 'N', 'A', 'V', 1 +// 'I', 'N', 'A', 'V' #define FONT_CHR_IS_METADATA(chr) ((chr)->data[0] == 'I' && \ (chr)->data[1] == 'N' && \ (chr)->data[2] == 'A' && \ - (chr)->data[3] == 'V' && \ - (chr)->data[4] == 1) + (chr)->data[3] == 'V') #define FONT_METADATA_CHR_INDEX 255 // Used for runtime detection of display drivers that might diff --git a/src/main/drivers/dma_stm32h7xx.c b/src/main/drivers/dma_stm32h7xx.c new file mode 100644 index 0000000000..eef3d0c28b --- /dev/null +++ b/src/main/drivers/dma_stm32h7xx.c @@ -0,0 +1,128 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 software. + * + * If not, see . + */ + +#include +#include +#include + +#include + +#include "common/utils.h" +#include "drivers/nvic.h" +#include "drivers/dma.h" +#include "drivers/rcc.h" + +/* + * DMA descriptors. + */ +static dmaChannelDescriptor_t dmaDescriptors[] = { + [0] = DEFINE_DMA_CHANNEL(1, 0, 0), // DMA1_ST0 + [1] = DEFINE_DMA_CHANNEL(1, 1, 6), // DMA1_ST1 + [2] = DEFINE_DMA_CHANNEL(1, 2, 16), // DMA1_ST2 + [3] = DEFINE_DMA_CHANNEL(1, 3, 22), // DMA1_ST3 + [4] = DEFINE_DMA_CHANNEL(1, 4, 32), // DMA1_ST4 + [5] = DEFINE_DMA_CHANNEL(1, 5, 38), // DMA1_ST5 + [6] = DEFINE_DMA_CHANNEL(1, 6, 48), // DMA1_ST6 + [7] = DEFINE_DMA_CHANNEL(1, 7, 54), // DMA1_ST7 + + [8] = DEFINE_DMA_CHANNEL(2, 0, 0), // DMA2_ST0 + [9] = DEFINE_DMA_CHANNEL(2, 1, 6), // DMA2_ST1 + [10] = DEFINE_DMA_CHANNEL(2, 2, 16), // DMA2_ST2 + [11] = DEFINE_DMA_CHANNEL(2, 3, 22), // DMA2_ST3 + [12] = DEFINE_DMA_CHANNEL(2, 4, 32), // DMA2_ST4 + [13] = DEFINE_DMA_CHANNEL(2, 5, 38), // DMA2_ST5 + [14] = DEFINE_DMA_CHANNEL(2, 6, 48), // DMA2_ST6 + [15] = DEFINE_DMA_CHANNEL(2, 7, 54), // DMA2_ST7 +}; + +/* + * DMA IRQ Handlers + */ +DEFINE_DMA_IRQ_HANDLER(1, 0, 0) // DMA1_ST0 +DEFINE_DMA_IRQ_HANDLER(1, 1, 1) // DMA1_ST1 +DEFINE_DMA_IRQ_HANDLER(1, 2, 2) // DMA1_ST2 +DEFINE_DMA_IRQ_HANDLER(1, 3, 3) // DMA1_ST3 +DEFINE_DMA_IRQ_HANDLER(1, 4, 4) // DMA1_ST4 +DEFINE_DMA_IRQ_HANDLER(1, 5, 5) // DMA1_ST5 +DEFINE_DMA_IRQ_HANDLER(1, 6, 6) // DMA1_ST6 +DEFINE_DMA_IRQ_HANDLER(1, 7, 7) // DMA1_ST7 +DEFINE_DMA_IRQ_HANDLER(2, 0, 8) // DMA2_ST0 +DEFINE_DMA_IRQ_HANDLER(2, 1, 9) // DMA2_ST1 +DEFINE_DMA_IRQ_HANDLER(2, 2, 10) // DMA2_ST2 +DEFINE_DMA_IRQ_HANDLER(2, 3, 11) // DMA2_ST3 +DEFINE_DMA_IRQ_HANDLER(2, 4, 12) // DMA2_ST4 +DEFINE_DMA_IRQ_HANDLER(2, 5, 13) // DMA2_ST5 +DEFINE_DMA_IRQ_HANDLER(2, 6, 14) // DMA2_ST6 +DEFINE_DMA_IRQ_HANDLER(2, 7, 15) // DMA2_ST7 + +DMA_t dmaGetByTag(dmaTag_t tag) +{ + for (unsigned i = 0; i < ARRAYLEN(dmaDescriptors); i++) { + if (DMATAG_GET_DMA(dmaDescriptors[i].tag) == DMATAG_GET_DMA(tag) && DMATAG_GET_STREAM(dmaDescriptors[i].tag) == DMATAG_GET_STREAM(tag)) { + return (DMA_t)&dmaDescriptors[i]; + } + } + + return (DMA_t) NULL; +} + +void dmaEnableClock(DMA_t dma) +{ + if (dma->dma == DMA1) { + RCC_ClockCmd(RCC_AHB1(DMA1), ENABLE); + } + else { + RCC_ClockCmd(RCC_AHB1(DMA2), ENABLE); + } +} + +resourceOwner_e dmaGetOwner(DMA_t dma) +{ + return dma->owner; +} + +void dmaInit(DMA_t dma, resourceOwner_e owner, uint8_t resourceIndex) +{ + dmaEnableClock(dma); + dma->owner = owner; + dma->resourceIndex = resourceIndex; +} + +void dmaSetHandler(DMA_t dma, dmaCallbackHandlerFuncPtr callback, uint32_t priority, uint32_t userParam) +{ + dmaEnableClock(dma); + + dma->irqHandlerCallback = callback; + dma->userParam = userParam; + + HAL_NVIC_SetPriority(dma->irqNumber, priority, 0); + HAL_NVIC_EnableIRQ(dma->irqNumber); +} + +DMA_t dmaGetByRef(const DMA_Stream_TypeDef* ref) +{ + for (unsigned i = 0; i < (sizeof(dmaDescriptors) / sizeof(dmaDescriptors[0])); i++) { + if (ref == dmaDescriptors[i].ref) { + return &dmaDescriptors[i]; + } + } + + return NULL; +} diff --git a/src/main/drivers/exti.c b/src/main/drivers/exti.c index 9185257e95..6d3a36e5ed 100644 --- a/src/main/drivers/exti.c +++ b/src/main/drivers/exti.c @@ -24,7 +24,7 @@ extiChannelRec_t extiChannelRecs[16]; static const uint8_t extiGroups[16] = { 0, 1, 2, 3, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 6, 6 }; static uint8_t extiGroupPriority[EXTI_IRQ_GROUPS]; -#if defined(STM32F4) || defined(STM32F7) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { EXTI0_IRQn, EXTI1_IRQn, @@ -48,6 +48,15 @@ static const uint8_t extiGroupIRQn[EXTI_IRQ_GROUPS] = { # warning "Unknown CPU" #endif +// Absorb the difference in IMR and PR assignments to registers +#if defined(STM32H7) +#define EXTI_REG_IMR (EXTI_D1->IMR1) +#define EXTI_REG_PR (EXTI_D1->PR1) +#else +#define EXTI_REG_IMR (EXTI->IMR) +#define EXTI_REG_PR (EXTI->PR) +#endif + void EXTIInit(void) { @@ -64,7 +73,7 @@ void EXTIHandlerInit(extiCallbackRec_t *self, extiHandlerCallback *fn) self->fn = fn; } -#if defined(STM32F7) +#if defined(STM32F7) || defined(STM32H7) void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config) { (void)config; @@ -156,23 +165,23 @@ void EXTIRelease(IO_t io) void EXTIEnable(IO_t io, bool enable) { -#if defined(STM32F4) || defined(STM32F7) +#if defined(STM32F4) || defined(STM32F7) || defined(STM32H7) uint32_t extiLine = IO_EXTI_Line(io); if (!extiLine) return; if (enable) - EXTI->IMR |= extiLine; + EXTI_REG_IMR |= extiLine; else - EXTI->IMR &= ~extiLine; + EXTI_REG_IMR &= ~extiLine; #elif defined(STM32F303xC) int extiLine = IO_EXTI_Line(io); if (extiLine < 0) return; // assume extiLine < 32 (valid for all EXTI pins) if (enable) - EXTI->IMR |= 1 << extiLine; + EXTI_REG_IMR |= 1 << extiLine; else - EXTI->IMR &= ~(1 << extiLine); + EXTI_REG_IMR &= ~(1 << extiLine); #else # error "Unsupported target" #endif @@ -180,13 +189,13 @@ void EXTIEnable(IO_t io, bool enable) void EXTI_IRQHandler(void) { - uint32_t exti_active = EXTI->IMR & EXTI->PR; + uint32_t exti_active = EXTI_REG_IMR & EXTI_REG_PR; while (exti_active) { unsigned idx = 31 - __builtin_clz(exti_active); uint32_t mask = 1 << idx; extiChannelRecs[idx].handler->fn(extiChannelRecs[idx].handler); - EXTI->PR = mask; // clear pending mask (by writing 1) + EXTI_REG_PR = mask; // clear pending mask (by writing 1) exti_active &= ~mask; } } @@ -201,7 +210,7 @@ void EXTI_IRQHandler(void) _EXTI_IRQ_HANDLER(EXTI0_IRQHandler); _EXTI_IRQ_HANDLER(EXTI1_IRQHandler); -#if defined(STM32F7) +#if defined(STM32F7) || defined(STM32H7) _EXTI_IRQ_HANDLER(EXTI2_IRQHandler); #elif defined(STM32F3) || defined(STM32F4) _EXTI_IRQ_HANDLER(EXTI2_TS_IRQHandler); diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index d8accf4c31..62c6db4313 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -31,11 +31,11 @@ #define SYM_VOLT 0x06 // 006 V #define SYM_MAH 0x07 // 007 MAH -// 0x08 // 008 - -// 0x09 // 009 - -// 0x0A // 010 - -// 0x0B // 011 - -// 0x0C // 012 - +#define SYM_AH_KM 0x08 // 008 Ah/km +#define SYM_AH_MI 0x09 // 009 Ah/mi +#define SYM_MAH_MI_0 0x0A // 010 mAh/mi left +#define SYM_MAH_MI_1 0x0B // 010 mAh/mi left +#define SYM_LQ 0x0C // 012 LQ #define SYM_TEMP_F 0x0D // 013 °F #define SYM_TEMP_C 0x0E // 014 °C @@ -119,8 +119,9 @@ #define SYM_RPM 0x8B // 139 RPM #define SYM_WAYPOINT 0x8C // 140 Waypoint #define SYM_AZIMUTH 0x8D // 141 Azimuth -// 0x8E // 142 - -// 0x8F // 143 - + +#define SYM_TELEMETRY_0 0x8E // 142 Antenna tracking telemetry +#define SYM_TELEMETRY_1 0x8F // 143 Antenna tracking telemetry #define SYM_BATT_FULL 0x90 // 144 Battery full #define SYM_BATT_5 0x91 // 145 Battery @@ -153,8 +154,8 @@ #define SYM_HEADING 0xA9 // 169 Compass Heading symbol #define SYM_ALT 0xAA // 170 ALT #define SYM_WH 0xAB // 171 WH -#define SYM_WH_KM_0 0xAC // 172 WH/KM left -#define SYM_WH_KM_1 0xAD // 173 WH/KM right +#define SYM_WH_KM 0xAC // 172 WH/KM +#define SYM_WH_MI 0xAD // 173 WH/MI #define SYM_WATT 0xAE // 174 W #define SYM_SCALE 0xAF // 175 Map scale #define SYM_MPH 0xB0 // 176 MPH @@ -246,6 +247,9 @@ #define SYM_SNR 0xEE // SNR #define SYM_MW 0xED // mW +#define SYM_KILOWATT 0xEF // 239 kW + + #else #define TEMP_SENSOR_SYM_COUNT 0 diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index f1f2c1f646..b78d5b5e17 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -54,5 +54,5 @@ bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn void pwmWriteBeeper(bool onoffBeep); void beeperPwmInit(ioTag_t tag, uint16_t frequency); -void sendDShotCommand(dshotCommands_e directionSpin); +void sendDShotCommand(dshotCommands_e cmd); void initDShotCommands(void); \ No newline at end of file diff --git a/src/main/drivers/serial_softserial.c b/src/main/drivers/serial_softserial.c index 4a382536df..aaed828375 100644 --- a/src/main/drivers/serial_softserial.c +++ b/src/main/drivers/serial_softserial.c @@ -169,7 +169,7 @@ static bool isTimerPeriodTooLarge(uint32_t timerPeriod) static void serialTimerConfigureTimebase(TCH_t * tch, uint32_t baud) { - uint32_t baseClock = SystemCoreClock / timerClockDivisor(tch->timHw->tim); + uint32_t baseClock = timerClock(tch->timHw->tim); uint32_t clock = baseClock; uint32_t timerPeriod; diff --git a/src/main/drivers/serial_uart_hal.c b/src/main/drivers/serial_uart_hal.c index 4454cde61a..b1df6ed754 100644 --- a/src/main/drivers/serial_uart_hal.c +++ b/src/main/drivers/serial_uart_hal.c @@ -71,7 +71,7 @@ static void uartReconfigure(uartPort_t *uartPort) HAL_UART_DeInit(&uartPort->Handle); uartPort->Handle.Init.BaudRate = uartPort->port.baudRate; - uartPort->Handle.Init.WordLength = UART_WORDLENGTH_8B; + uartPort->Handle.Init.WordLength = (uartPort->port.options & SERIAL_PARITY_EVEN) ? UART_WORDLENGTH_9B : UART_WORDLENGTH_8B; uartPort->Handle.Init.StopBits = (uartPort->port.options & SERIAL_STOPBITS_2) ? USART_STOPBITS_2 : USART_STOPBITS_1; uartPort->Handle.Init.Parity = (uartPort->port.options & SERIAL_PARITY_EVEN) ? USART_PARITY_EVEN : USART_PARITY_NONE; uartPort->Handle.Init.HwFlowCtl = UART_HWCONTROL_NONE; diff --git a/src/main/drivers/serial_uart_stm32h7xx.c b/src/main/drivers/serial_uart_stm32h7xx.c index 2ad67b583a..0e22206bed 100644 --- a/src/main/drivers/serial_uart_stm32h7xx.c +++ b/src/main/drivers/serial_uart_stm32h7xx.c @@ -331,6 +331,10 @@ uartPort_t *serialUART(UARTDevice_e device, uint32_t baudRate, portMode_t mode, s->Handle.Instance = uart->dev; + if (uart->rcc) { + RCC_ClockCmd(uart->rcc, ENABLE); + } + IO_t tx = IOGetByTag(uart->tx); IO_t rx = IOGetByTag(uart->rx); diff --git a/src/main/drivers/serial_usb_vcp.c b/src/main/drivers/serial_usb_vcp.c index f4713af6d7..a90633d6cf 100644 --- a/src/main/drivers/serial_usb_vcp.c +++ b/src/main/drivers/serial_usb_vcp.c @@ -216,6 +216,12 @@ void usbVcpInitHardware(void) /* Start Device Process */ USBD_Start(&USBD_Device); + +#ifdef STM32H7 + HAL_PWREx_EnableUSBVoltageDetector(); + delay(100); // Cold boot failures observed without this, even when USB cable is not connected +#endif + #else Set_System(); Set_USBClock(); diff --git a/src/main/drivers/system_stm32h7xx.c b/src/main/drivers/system_stm32h7xx.c index 72826b1fae..a1de36f2c2 100644 --- a/src/main/drivers/system_stm32h7xx.c +++ b/src/main/drivers/system_stm32h7xx.c @@ -51,10 +51,13 @@ bool isMPUSoftReset(void) return false; } -#define SYSMEMBOOT_VECTOR_TABLE ((uint32_t *)0x1ff09800) uint32_t systemBootloaderAddress(void) { - return SYSMEMBOOT_VECTOR_TABLE[1]; +#if defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx) + return 0x1ff09800; +#else +#error Unknown MCU +#endif } void systemClockSetup(uint8_t cpuUnderclock) diff --git a/src/main/drivers/timer.c b/src/main/drivers/timer.c index 4b7e15b551..4418e9ad2c 100755 --- a/src/main/drivers/timer.c +++ b/src/main/drivers/timer.c @@ -240,7 +240,7 @@ uint32_t timerGetBaseClock(TCH_t * tch) uint32_t timerGetBaseClockHW(const timerHardware_t * timHw) { - return SystemCoreClock / timerClockDivisor(timHw->tim); + return timerClock(timHw->tim); } bool timerPWMConfigChannelDMA(TCH_t * tch, void * dmaBuffer, uint8_t dmaBufferElementSize, uint32_t dmaBufferElementCount) diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 661f705584..699d38e953 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -157,7 +157,7 @@ typedef enum { TYPE_TIMER } channelType_t; -uint8_t timerClockDivisor(TIM_TypeDef *tim); +uint32_t timerClock(TIM_TypeDef *tim); uint32_t timerGetBaseClockHW(const timerHardware_t * timHw); const timerHardware_t * timerGetByUsageFlag(timerUsageFlag_e flag); diff --git a/src/main/drivers/timer_stm32f30x.c b/src/main/drivers/timer_stm32f30x.c index 5aaa4df205..545dab0c8f 100644 --- a/src/main/drivers/timer_stm32f30x.c +++ b/src/main/drivers/timer_stm32f30x.c @@ -48,10 +48,10 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { [16] = { .tim = TIM17, .rcc = RCC_APB2(TIM17), .irq = TIM1_TRG_COM_TIM17_IRQn }, }; -uint8_t timerClockDivisor(TIM_TypeDef *tim) +uint32_t timerClock(TIM_TypeDef *tim) { UNUSED(tim); - return 1; + return SystemCoreClock; } _TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1); diff --git a/src/main/drivers/timer_stm32f4xx.c b/src/main/drivers/timer_stm32f4xx.c index 75b46f1cf8..bc5ec353ad 100644 --- a/src/main/drivers/timer_stm32f4xx.c +++ b/src/main/drivers/timer_stm32f4xx.c @@ -95,16 +95,16 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { #endif }; -uint8_t timerClockDivisor(TIM_TypeDef *tim) +uint32_t timerClock(TIM_TypeDef *tim) { #if defined (STM32F411xE) UNUSED(tim); - return 1; + return SystemCoreClock; #elif defined (STM32F40_41xxx) || defined (STM32F427_437xx) || defined (STM32F446xx) if (tim == TIM1 || tim == TIM8 || tim == TIM9 || tim == TIM10 || tim == TIM11) { - return 1; + return SystemCoreClock; } else { - return 2; + return SystemCoreClock / 2; } #else #error "No timer clock defined correctly for the MCU" diff --git a/src/main/drivers/timer_stm32f7xx.c b/src/main/drivers/timer_stm32f7xx.c index 4f8057baf1..ca4f4cb8b3 100644 --- a/src/main/drivers/timer_stm32f7xx.c +++ b/src/main/drivers/timer_stm32f7xx.c @@ -48,12 +48,6 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { [13] = { .tim = TIM14, .rcc = RCC_APB1(TIM14), .irq = TIM8_TRG_COM_TIM14_IRQn}, }; -uint8_t timerClockDivisor(TIM_TypeDef *tim) -{ - UNUSED(tim); - return 1; -} - uint32_t timerClock(TIM_TypeDef *tim) { UNUSED(tim); diff --git a/src/main/drivers/timer_stm32h7xx.c b/src/main/drivers/timer_stm32h7xx.c index 20b7f62902..b01c8647e4 100644 --- a/src/main/drivers/timer_stm32h7xx.c +++ b/src/main/drivers/timer_stm32h7xx.c @@ -48,16 +48,48 @@ const timerDef_t timerDefinitions[HARDWARE_TIMER_DEFINITION_COUNT] = { [13] = { .tim = TIM17, .rcc = RCC_APB2(TIM17), .irq = TIM17_IRQn}, }; -uint8_t timerClockDivisor(TIM_TypeDef *tim) -{ - UNUSED(tim); - return 1; -} - uint32_t timerClock(TIM_TypeDef *tim) { - UNUSED(tim); - return SystemCoreClock; + int timpre; + uint32_t pclk; + uint32_t ppre; + + // Implement the table: + // RM0433 (Rev 6) Table 52. + // RM0455 (Rev 3) Table 55. + // "Ratio between clock timer and pclk" + // (Tables are the same, just D2 or CD difference) + +#if defined(STM32H743xx) || defined(STM32H750xx) || defined(STM32H723xx) || defined(STM32H725xx) +#define PERIPH_PRESCALER(bus) ((RCC->D2CFGR & RCC_D2CFGR_D2PPRE ## bus) >> RCC_D2CFGR_D2PPRE ## bus ## _Pos) +#elif defined(STM32H7A3xx) || defined(STM32H7A3xxQ) +#define PERIPH_PRESCALER(bus) ((RCC->CDCFGR2 & RCC_CDCFGR2_CDPPRE ## bus) >> RCC_CDCFGR2_CDPPRE ## bus ## _Pos) +#else +#error Unknown MCU type +#endif + + if (tim == TIM1 || tim == TIM8 || tim == TIM15 || tim == TIM16 || tim == TIM17) { + // Timers on APB2 + pclk = HAL_RCC_GetPCLK2Freq(); + ppre = PERIPH_PRESCALER(2); + } else { + // Timers on APB1 + pclk = HAL_RCC_GetPCLK1Freq(); + ppre = PERIPH_PRESCALER(1); + } + + timpre = (RCC->CFGR & RCC_CFGR_TIMPRE) ? 1 : 0; + + int index = (timpre << 3) | ppre; + + static uint8_t periphToKernel[16] = { // The mutiplier table + 1, 1, 1, 1, 2, 2, 2, 2, // TIMPRE = 0 + 1, 1, 1, 1, 2, 4, 4, 4 // TIMPRE = 1 + }; + + return pclk * periphToKernel[index]; + +#undef PERIPH_PRESCALER } _TIM_IRQ_HANDLER(TIM1_CC_IRQHandler, 1); @@ -66,8 +98,6 @@ _TIM_IRQ_HANDLER(TIM3_IRQHandler, 3); _TIM_IRQ_HANDLER(TIM4_IRQHandler, 4); _TIM_IRQ_HANDLER(TIM5_IRQHandler, 5); _TIM_IRQ_HANDLER(TIM8_CC_IRQHandler, 8); -//_TIM_IRQ_HANDLER(TIM1_BRK_TIM9_IRQHandler, 9); -//_TIM_IRQ_HANDLER(TIM1_TRG_COM_TIM11_IRQHandler, 11); _TIM_IRQ_HANDLER(TIM8_BRK_TIM12_IRQHandler, 12); _TIM_IRQ_HANDLER(TIM15_IRQHandler, 15); _TIM_IRQ_HANDLER(TIM16_IRQHandler, 16); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 68f951f687..e74e1b9813 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -156,7 +156,7 @@ static const char * const featureNames[] = { "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", "SUPEREXPO", "VTX", "", "", "", "PWM_OUTPUT_ENABLE", - "OSD", "FW_LAUNCH", NULL + "OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL }; /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */ @@ -2501,7 +2501,7 @@ static void cliFeature(char *cmdline) } } -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) static void printBeeper(uint8_t dumpMask, const beeperConfig_t *beeperConfig, const beeperConfig_t *beeperConfigDefault) { const uint8_t beeperCount = beeperTableEntryCount(); @@ -3234,7 +3234,7 @@ static void cliStatus(char *cmdline) #endif cliPrintf("System load: %d", averageSystemLoadPercent); - const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_GYROPID); + const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_PID); const int pidRate = pidTaskDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)pidTaskDeltaTime)); const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX))); const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM))); @@ -3484,7 +3484,7 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("feature"); printFeature(dumpMask, &featureConfig_Copy, featureConfig()); -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) cliPrintHashLine("beeper"); printBeeper(dumpMask, &beeperConfig_Copy, beeperConfig()); #endif @@ -3655,7 +3655,7 @@ const clicmd_t cmdTable[] = { #ifdef USE_CLI_BATCH CLI_COMMAND_DEF("batch", "start or end a batch of commands", "start | end", cliBatch), #endif -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper), #endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d38554bd21..8937097a7b 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -123,7 +123,14 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .name = SETTING_NAME_DEFAULT ); -PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1); + +PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, + .beeper_off_flags = 0, + .preferred_beeper_off_flags = 0, + .dshot_beeper_enabled = SETTING_DSHOT_BEEPER_ENABLED_DEFAULT, + .dshot_beeper_tone = SETTING_DSHOT_BEEPER_TONE_DEFAULT, +); PG_REGISTER_WITH_RESET_TEMPLATE(adcChannelConfig_t, adcChannelConfig, PG_ADC_CHANNEL_CONFIG, 0); @@ -166,6 +173,10 @@ __attribute__((weak)) void targetConfiguration(void) #endif uint32_t getLooptime(void) { + return gyroConfig()->looptime; +} + +uint32_t getGyroLooptime(void) { return gyro.targetLooptime; } @@ -287,13 +298,15 @@ void validateAndFixConfig(void) } #endif -#if !defined(USE_MPU_DATA_READY_SIGNAL) - gyroConfigMutable()->gyroSync = false; -#endif - // Call target-specific validation function validateAndFixTargetConfig(); +#ifdef USE_MAG + if (compassConfig()->mag_align == ALIGN_DEFAULT) { + compassConfigMutable()->mag_align = CW270_DEG_FLIP; + } +#endif + if (settingsValidate(NULL)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_INVALID_SETTING); } else { diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 48dc455e71..4db38b6d6a 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -29,13 +29,8 @@ #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 #define MAX_NAME_LENGTH 16 -typedef enum { - ASYNC_MODE_NONE, - ASYNC_MODE_GYRO, - ASYNC_MODE_ALL -} asyncMode_e; - -typedef enum { +#define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz + typedef enum { FEATURE_THR_VBAT_COMP = 1 << 0, FEATURE_VBAT = 1 << 1, FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command @@ -67,6 +62,7 @@ typedef enum { FEATURE_PWM_OUTPUT_ENABLE = 1 << 28, FEATURE_OSD = 1 << 29, FEATURE_FW_LAUNCH = 1 << 30, + FEATURE_FW_AUTOTRIM = 1 << 31, } features_e; typedef struct systemConfig_s { @@ -88,6 +84,8 @@ PG_DECLARE(systemConfig_t, systemConfig); typedef struct beeperConfig_s { uint32_t beeper_off_flags; uint32_t preferred_beeper_off_flags; + bool dshot_beeper_enabled; + uint8_t dshot_beeper_tone; } beeperConfig_t; PG_DECLARE(beeperConfig_t, beeperConfig); @@ -140,3 +138,4 @@ void resetConfigs(void); void targetConfiguration(void); uint32_t getLooptime(void); +uint32_t getGyroLooptime(void); diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index 82bda8bff3..1ba828c884 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -22,21 +22,6 @@ #include "config/parameter_group.h" #define MAX_CONTROL_RATE_PROFILE_COUNT 3 -/* -Max and min available values for rates are now stored as absolute -tenths of degrees-per-second [dsp/10] -That means, max. rotation rate 180 equals 1800dps - -New defaults of 200dps for pitch,roll and yaw are more less -equivalent of rates 0 from previous versions of iNav, Cleanflight, Baseflight -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_YAW_RATE_MAX 180 -#define CONTROL_RATE_CONFIG_YAW_RATE_MIN 2 - -#define CONTROL_RATE_CONFIG_TPA_MAX 100 typedef struct controlRateConfig_s { diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 9acc36eb6f..95e9734117 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -45,6 +45,7 @@ FILE_COMPILE_FOR_SPEED #include "sensors/boardalignment.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" +#include "sensors/compass.h" #include "sensors/pitotmeter.h" #include "sensors/gyro.h" #include "sensors/battery.h" @@ -90,6 +91,7 @@ FILE_COMPILE_FOR_SPEED #include "flight/rate_dynamics.h" #include "flight/failsafe.h" +#include "flight/power_limits.h" #include "config/feature.h" #include "common/vector.h" @@ -103,9 +105,6 @@ enum { ALIGN_MAG = 2 }; -#define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro -#define GYRO_SYNC_MAX_CONSECUTIVE_FAILURES 100 // After this many consecutive missed interrupts disable gyro sync and fall back to scheduled updates - #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000 #define EMERGENCY_ARMING_COUNTER_STEP_MS 100 @@ -123,6 +122,7 @@ typedef struct emergencyArmingState_s { timeDelta_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop static timeUs_t flightTime = 0; +static timeUs_t armTime = 0; EXTENDED_FASTRAM float dT; @@ -131,14 +131,14 @@ int16_t headFreeModeHold; uint8_t motorControlEnable = false; static bool isRXDataNew; -static uint32_t gyroSyncFailureCount; static disarmReason_t lastDisarmReason = DISARM_NONE; +timeUs_t lastDisarmTimeUs = 0; static emergencyArmingState_t emergencyArming; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible static timeMs_t prearmActivationTime = 0; -bool isCalibrating(void) +bool areSensorsCalibrating(void) { #ifdef USE_BARO if (sensors(SENSOR_BARO) && !baroIsCalibrationComplete()) { @@ -146,6 +146,12 @@ bool isCalibrating(void) } #endif +#ifdef USE_MAG + if (sensors(SENSOR_MAG) && !compassIsCalibrationComplete()) { + return true; + } +#endif + #ifdef USE_PITOT if (sensors(SENSOR_PITOT) && !pitotIsCalibrationComplete()) { return true; @@ -174,7 +180,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) int16_t stickDeflection; stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500); - stickDeflection = applyDeadband(stickDeflection, deadband); + stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500); return rcLookup(stickDeflection, rate); } @@ -186,7 +192,7 @@ static void updateArmingStatus(void) } else { /* CHECK: Run-time calibration */ static bool calibratingFinishedBeep = false; - if (isCalibrating()) { + if (areSensorsCalibrating()) { ENABLE_ARMING_FLAG(ARMING_DISABLED_SENSORS_CALIBRATING); calibratingFinishedBeep = false; } @@ -302,6 +308,17 @@ static void updateArmingStatus(void) DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); } +#ifdef USE_DSHOT + /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */ + if (micros() - getLastDshotBeeperCommandTimeUs() < getDShotBeaconGuardDelayUs()) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); + } +#else + DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); +#endif + if (isModeActivationConditionPresent(BOXPREARM)) { if (IS_RC_MODE_ACTIVE(BOXPREARM)) { if (prearmWasReset && (armingConfig()->prearmTimeoutMs == 0 || millis() - prearmActivationTime < armingConfig()->prearmTimeoutMs)) { @@ -417,6 +434,7 @@ void disarm(disarmReason_t disarmReason) { if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; + lastDisarmTimeUs = micros(); DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -425,9 +443,9 @@ void disarm(disarmReason_t disarmReason) } #endif #ifdef USE_DSHOT - if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) { + if (FLIGHT_MODE(TURTLE_MODE)) { sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_NORMAL); - DISABLE_FLIGHT_MODE(FLIP_OVER_AFTER_CRASH); + DISABLE_FLIGHT_MODE(TURTLE_MODE); } #endif statsOnDisarm(); @@ -443,6 +461,10 @@ void disarm(disarmReason_t disarmReason) } } +timeUs_t getLastDisarmTimeUs(void) { + return lastDisarmTimeUs; +} + disarmReason_t getDisarmReason(void) { return lastDisarmReason; @@ -498,26 +520,28 @@ void tryArm(void) #ifdef USE_DSHOT if ( STATE(MULTIROTOR) && - IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) && + IS_RC_MODE_ACTIVE(BOXTURTLE) && emergencyArmingCanOverrideArmingDisabled() && isMotorProtocolDshot() && - !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH) + !ARMING_FLAG(ARMED) && + !FLIGHT_MODE(TURTLE_MODE) ) { sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED); ENABLE_ARMING_FLAG(ARMED); - enableFlightMode(FLIP_OVER_AFTER_CRASH); + enableFlightMode(TURTLE_MODE); return; } #endif + #ifdef USE_PROGRAMMING_FRAMEWORK if ( - !isArmingDisabled() || - emergencyArmingIsEnabled() || + !isArmingDisabled() || + emergencyArmingIsEnabled() || LOGIC_CONDITION_GLOBAL_FLAG(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_ARMING_SAFETY) ) { -#else +#else if ( - !isArmingDisabled() || + !isArmingDisabled() || emergencyArmingIsEnabled() ) { #endif @@ -709,7 +733,7 @@ void processRx(timeUs_t currentTimeUs) // Handle passthrough mode if (STATE(FIXED_WING_LEGACY)) { if ((IS_RC_MODE_ACTIVE(BOXMANUAL) && !navigationRequiresAngleMode() && !failsafeRequiresAngleMode()) || // Normal activation of passthrough - (!ARMING_FLAG(ARMED) && isCalibrating())){ // Backup - if we are not armed - enforce passthrough while calibrating + (!ARMING_FLAG(ARMED) && areSensorsCalibrating())){ // Backup - if we are not armed - enforce passthrough while calibrating ENABLE_FLIGHT_MODE(MANUAL_MODE); } else { DISABLE_FLIGHT_MODE(MANUAL_MODE); @@ -806,36 +830,17 @@ void processRx(timeUs_t currentTimeUs) // Function for loop trigger void FAST_CODE taskGyro(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point. // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); - timeUs_t gyroUpdateUs = currentTimeUs; - - if (gyroConfig()->gyroSync) { - while (true) { - gyroUpdateUs = micros(); - if (gyroSyncCheckUpdate()) { - gyroSyncFailureCount = 0; - break; - } - else if ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (timeDelta_t)(getLooptime() + GYRO_WATCHDOG_DELAY)) { - gyroSyncFailureCount++; - break; - } - } - - // If we detect gyro sync failure - disable gyro sync - if (gyroSyncFailureCount > GYRO_SYNC_MAX_CONSECUTIVE_FAILURES) { - gyroConfigMutable()->gyroSync = false; - } - } /* Update actual hardware readings */ gyroUpdate(); #ifdef USE_OPFLOW if (sensors(SENSOR_OPFLOW)) { - opflowGyroUpdateCallback((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs)); + opflowGyroUpdateCallback(currentDeltaTime); } #endif } @@ -857,10 +862,15 @@ void taskMainPidLoop(timeUs_t currentTimeUs) if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) { flightTime += cycleTime; + armTime += cycleTime; updateAccExtremes(); } + if (!ARMING_FLAG(ARMED)) { + armTime = 0; + } + + gyroFilter(); - taskGyro(currentTimeUs); imuUpdateAccelerometer(); imuUpdateAttitude(currentTimeUs); @@ -905,6 +915,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs) // FIXME: throttle pitch comp for FW } +#ifdef USE_POWER_LIMITS + powerLimiterApply(&rcCommand[THROTTLE]); +#endif + // Calculate stabilisation pidController(dT); @@ -919,7 +933,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) if (isMixerUsingServos()) { servoMixer(dT); - processServoAutotrim(); + processServoAutotrim(dT); } //Servos should be filtered or written only when mixer is using servos or special feaures are enabled @@ -976,6 +990,11 @@ float getFlightTime() return (float)(flightTime / 1000) / 1000; } +float getArmTime() +{ + return (float)(armTime / 1000) / 1000; +} + void fcReboot(bool bootLoader) { // stop motor/servo outputs diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 34eb6fe68b..e948dec836 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -37,12 +37,14 @@ typedef enum disarmReason_e { void handleInflightCalibrationStickPosition(void); void disarm(disarmReason_t disarmReason); +timeUs_t getLastDisarmTimeUs(void); void tryArm(void); disarmReason_t getDisarmReason(void); void emergencyArmingUpdate(bool armingSwitchIsOn); -bool isCalibrating(void); +bool areSensorsCalibrating(void); float getFlightTime(void); +float getArmTime(void); 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 ce6fecb053..20b754d626 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -97,8 +97,9 @@ #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" -#include "flight/servos.h" +#include "flight/power_limits.h" #include "flight/rpm_filter.h" +#include "flight/servos.h" #include "flight/secondary_imu.h" #include "io/asyncfatfs/asyncfatfs.h" @@ -709,6 +710,10 @@ void init(void) ioPortExpanderInit(); #endif +#ifdef USE_POWER_LIMITS + powerLimiterInit(); +#endif + // Considering that the persistent reset reason is only used during init persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5fdcebc9e1..b80c33cfe0 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1229,11 +1229,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, motorConfig()->motorPwmProtocol); sbufWriteU16(dst, motorConfig()->motorPwmRate); sbufWriteU16(dst, servoConfig()->servoPwmRate); - sbufWriteU8(dst, gyroConfig()->gyroSync); + sbufWriteU8(dst, 0); break; case MSP_FILTER_CONFIG : - sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz); + sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); sbufWriteU16(dst, gyroConfig()->gyro_notch_hz); @@ -1247,7 +1247,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, accelerometerConfig()->acc_notch_hz); sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff); - sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz); + sbufWriteU16(dst, 0); //Was gyroConfig()->gyro_stage2_lowpass_hz break; case MSP_PID_ADVANCED: @@ -1334,7 +1334,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, navConfig()->general.flags.rth_alt_control_mode); sbufWriteU16(dst, navConfig()->general.rth_abort_threshold); sbufWriteU16(dst, navConfig()->general.rth_altitude); - sbufWriteU16(dst, navConfig()->general.land_descent_rate); + sbufWriteU16(dst, navConfig()->general.land_minalt_vspd); + sbufWriteU16(dst, navConfig()->general.land_maxalt_vspd); sbufWriteU16(dst, navConfig()->general.land_slowdown_minalt); sbufWriteU16(dst, navConfig()->general.land_slowdown_maxalt); sbufWriteU16(dst, navConfig()->general.emerg_descent_rate); @@ -1798,14 +1799,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) for (int i = 0; i < 3; i++) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); } else { - ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); } } tmp_u8 = sbufReadU8(src); - ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(tmp_u8, CONTROL_RATE_CONFIG_TPA_MAX); + ((controlRateConfig_t*)currentControlRateProfile)->throttle.dynPID = MIN(tmp_u8, SETTING_TPA_RATE_MAX); ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcMid8 = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->throttle.rcExpo8 = sbufReadU8(src); ((controlRateConfig_t*)currentControlRateProfile)->throttle.pa_breakpoint = sbufReadU16(src); @@ -1835,9 +1836,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) for (uint8_t i = 0; i < 3; ++i) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { - currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); } else { - currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + currentControlRateProfile_p->stabilized.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); } } @@ -1847,9 +1848,9 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) for (uint8_t i = 0; i < 3; ++i) { tmp_u8 = sbufReadU8(src); if (i == FD_YAW) { - currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); } else { - currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + currentControlRateProfile_p->manual.rates[i] = constrain(tmp_u8, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); } } @@ -2163,14 +2164,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) motorConfigMutable()->motorPwmProtocol = sbufReadU8(src); motorConfigMutable()->motorPwmRate = sbufReadU16(src); servoConfigMutable()->servoPwmRate = sbufReadU16(src); - gyroConfigMutable()->gyroSync = sbufReadU8(src); + sbufReadU8(src); //Was gyroSync } else return MSP_RESULT_ERROR; break; case MSP_SET_FILTER_CONFIG : if (dataSize >= 5) { - gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src); + gyroConfigMutable()->gyro_main_lpf_hz = sbufReadU8(src); pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 500); pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255); if (dataSize >= 9) { @@ -2201,7 +2202,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } if (dataSize >= 22) { - gyroConfigMutable()->gyro_stage2_lowpass_hz = constrain(sbufReadU16(src), 0, 500); + sbufReadU16(src); //Was gyro_stage2_lowpass_hz } else { return MSP_RESULT_ERROR; } @@ -2307,7 +2308,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) navConfigMutable()->general.flags.rth_alt_control_mode = sbufReadU8(src); navConfigMutable()->general.rth_abort_threshold = sbufReadU16(src); navConfigMutable()->general.rth_altitude = sbufReadU16(src); - navConfigMutable()->general.land_descent_rate = sbufReadU16(src); + navConfigMutable()->general.land_minalt_vspd = sbufReadU16(src); + navConfigMutable()->general.land_maxalt_vspd = sbufReadU16(src); navConfigMutable()->general.land_slowdown_minalt = sbufReadU16(src); navConfigMutable()->general.land_slowdown_maxalt = sbufReadU16(src); navConfigMutable()->general.emerg_descent_rate = sbufReadU16(src); diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 65bf1de1cc..5ec8491e52 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -89,7 +89,9 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXLOITERDIRCHN, "LOITER CHANGE", 49 }, { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 }, { BOXPREARM, "PREARM", 51 }, - { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 }, + { BOXTURTLE, "TURTLE", 52 }, + { BOXNAVCRUISE, "NAV CRUISE", 53 }, + { BOXAUTOLEVEL, "AUTO LEVEL", 54 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -220,6 +222,7 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; if (STATE(AIRPLANE)) { activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; + activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; } } } @@ -240,10 +243,17 @@ void initActiveBoxIds(void) if (!feature(FEATURE_FW_LAUNCH)) { activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH; } - activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM; + + if (!feature(FEATURE_FW_AUTOTRIM)) { + activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM; + } + #if defined(USE_AUTOTUNE_FIXED_WING) activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE; #endif + if (sensors(SENSOR_BARO)) { + activeBoxIds[activeBoxIdCount++] = BOXAUTOLEVEL; + } } /* @@ -312,7 +322,7 @@ void initActiveBoxIds(void) #ifdef USE_DSHOT if(STATE(MULTIROTOR) && isMotorProtocolDshot()) - activeBoxIds[activeBoxIdCount++] = BOXFLIPOVERAFTERCRASH; + activeBoxIds[activeBoxIdCount++] = BOXTURTLE; #endif } @@ -346,6 +356,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) 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_COURSE_HOLD_MODE)), BOXNAVCOURSEHOLD); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) && IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVCRUISE); 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); @@ -373,6 +384,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE); #endif + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)), BOXAUTOLEVEL); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 33d417a1d6..e4ef2a2132 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -43,14 +43,15 @@ #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "flight/dynamic_lpf.h" #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" -#include "flight/wind_estimator.h" -#include "flight/secondary_imu.h" +#include "flight/power_limits.h" #include "flight/rpm_filter.h" +#include "flight/secondary_imu.h" #include "flight/servos.h" -#include "flight/dynamic_lpf.h" +#include "flight/wind_estimator.h" #include "navigation/navigation.h" @@ -112,18 +113,29 @@ void taskHandleSerial(timeUs_t currentTimeUs) void taskUpdateBattery(timeUs_t currentTimeUs) { static timeUs_t batMonitoringLastServiced = 0; - timeUs_t BatMonitoringTimeSinceLastServiced = cmpTimeUs(currentTimeUs, batMonitoringLastServiced); + timeDelta_t BatMonitoringTimeSinceLastServiced = cmpTimeUs(currentTimeUs, batMonitoringLastServiced); - if (isAmperageConfigured()) + if (isAmperageConfigured()) { currentMeterUpdate(BatMonitoringTimeSinceLastServiced); +#ifdef USE_POWER_LIMITS + currentLimiterUpdate(BatMonitoringTimeSinceLastServiced); +#endif + } + #ifdef USE_ADC - if (feature(FEATURE_VBAT)) + if (feature(FEATURE_VBAT)) { batteryUpdate(BatMonitoringTimeSinceLastServiced); + } + if (feature(FEATURE_VBAT) && isAmperageConfigured()) { powerMeterUpdate(BatMonitoringTimeSinceLastServiced); sagCompensatedVBatUpdate(currentTimeUs, BatMonitoringTimeSinceLastServiced); +#if defined(USE_POWER_LIMITS) && defined(USE_ADC) + powerLimiterUpdate(BatMonitoringTimeSinceLastServiced); +#endif } #endif + batMonitoringLastServiced = currentTimeUs; } @@ -292,21 +304,25 @@ void taskUpdateOsd(timeUs_t currentTimeUs) void taskUpdateAux(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); updatePIDCoefficients(); dynamicLpfGyroTask(); + updateFixedWingLevelTrim(currentTimeUs); } void fcTasksInit(void) { schedulerInit(); - rescheduleTask(TASK_GYROPID, getLooptime()); - setTaskEnabled(TASK_GYROPID, true); + rescheduleTask(TASK_PID, getLooptime()); + setTaskEnabled(TASK_PID, true); + + rescheduleTask(TASK_GYRO, getGyroLooptime()); + setTaskEnabled(TASK_GYRO, true); + setTaskEnabled(TASK_AUX, true); setTaskEnabled(TASK_SERIAL, true); -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) setTaskEnabled(TASK_BEEPER, true); #endif #ifdef USE_LIGHTS @@ -388,12 +404,18 @@ cfTask_t cfTasks[TASK_COUNT] = { .desiredPeriod = TASK_PERIOD_HZ(10), // run every 100 ms, 10Hz .staticPriority = TASK_PRIORITY_HIGH, }, - [TASK_GYROPID] = { - .taskName = "GYRO/PID", + [TASK_PID] = { + .taskName = "PID", .taskFunc = taskMainPidLoop, .desiredPeriod = TASK_PERIOD_US(1000), .staticPriority = TASK_PRIORITY_REALTIME, }, + [TASK_GYRO] = { + .taskName = "GYRO", + .taskFunc = taskGyro, + .desiredPeriod = TASK_PERIOD_US(TASK_GYRO_LOOPTIME), + .staticPriority = TASK_PRIORITY_REALTIME, + }, [TASK_SERIAL] = { .taskName = "SERIAL", .taskFunc = taskHandleSerial, @@ -401,7 +423,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_LOW, }, -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) [TASK_BEEPER] = { .taskName = "BEEPER", .taskFunc = beeperUpdate, diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index c05a390ea4..c5c67436a1 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -39,6 +39,7 @@ #include "fc/controlrate_profile.h" #include "fc/rc_adjustments.h" #include "fc/rc_curves.h" +#include "fc/settings.h" #include "navigation/navigation.h" @@ -61,15 +62,6 @@ static uint8_t adjustmentStateMask = 0; #define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex) (adjustmentStateMask & (1 << adjustmentIndex)) -#define RC_ADJUSTMENT_EXPO_MIN 0 -#define RC_ADJUSTMENT_EXPO_MAX 100 - -#define RC_ADJUSTMENT_MANUAL_RATE_MIN 0 -#define RC_ADJUSTMENT_MANUAL_RATE_MAX 100 - -#define RC_ADJUSTMENT_PID_MIN 0 -#define RC_ADJUSTMENT_PID_MAX 200 - // sync with adjustmentFunction_e static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COUNT - 1] = { { @@ -368,17 +360,17 @@ static void applyAdjustmentU16(adjustmentFunction_e adjustmentFunction, uint16_t static void applyAdjustmentExpo(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta) { - applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_EXPO_MIN, RC_ADJUSTMENT_EXPO_MAX); + applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_RC_EXPO_MIN, SETTING_RC_EXPO_MAX); } static void applyAdjustmentManualRate(adjustmentFunction_e adjustmentFunction, uint8_t *val, int delta) { - return applyAdjustmentU8(adjustmentFunction, val, delta, RC_ADJUSTMENT_MANUAL_RATE_MIN, RC_ADJUSTMENT_MANUAL_RATE_MAX); + return applyAdjustmentU8(adjustmentFunction, val, delta, SETTING_CONSTANT_MANUAL_RATE_MIN, SETTING_CONSTANT_MANUAL_RATE_MAX); } static void applyAdjustmentPID(adjustmentFunction_e adjustmentFunction, uint16_t *val, int delta) { - applyAdjustmentU16(adjustmentFunction, val, delta, RC_ADJUSTMENT_PID_MIN, RC_ADJUSTMENT_PID_MAX); + applyAdjustmentU16(adjustmentFunction, val, delta, SETTING_CONSTANT_RPYL_PID_MIN, SETTING_CONSTANT_RPYL_PID_MAX); } static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int delta) @@ -406,7 +398,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t break; case ADJUSTMENT_PITCH_ROLL_RATE: case ADJUSTMENT_PITCH_RATE: - applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + applyAdjustmentU8(ADJUSTMENT_PITCH_RATE, &controlRateConfig->stabilized.rates[FD_PITCH], delta, SETTING_PITCH_RATE_MIN, SETTING_PITCH_RATE_MAX); if (adjustmentFunction == ADJUSTMENT_PITCH_RATE) { schedulePidGainsUpdate(); break; @@ -415,7 +407,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t FALLTHROUGH; case ADJUSTMENT_ROLL_RATE: - applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN, CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX); + applyAdjustmentU8(ADJUSTMENT_ROLL_RATE, &controlRateConfig->stabilized.rates[FD_ROLL], delta, SETTING_CONSTANT_ROLL_PITCH_RATE_MIN, SETTING_CONSTANT_ROLL_PITCH_RATE_MAX); schedulePidGainsUpdate(); break; case ADJUSTMENT_MANUAL_PITCH_ROLL_RATE: @@ -429,7 +421,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t applyAdjustmentManualRate(ADJUSTMENT_MANUAL_PITCH_RATE, &controlRateConfig->manual.rates[FD_PITCH], delta); break; case ADJUSTMENT_YAW_RATE: - applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, CONTROL_RATE_CONFIG_YAW_RATE_MIN, CONTROL_RATE_CONFIG_YAW_RATE_MAX); + applyAdjustmentU8(ADJUSTMENT_YAW_RATE, &controlRateConfig->stabilized.rates[FD_YAW], delta, SETTING_YAW_RATE_MIN, SETTING_YAW_RATE_MAX); schedulePidGainsUpdate(); break; case ADJUSTMENT_MANUAL_YAW_RATE: @@ -508,10 +500,10 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t schedulePidGainsUpdate(); break; case ADJUSTMENT_NAV_FW_CRUISE_THR: - applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, 1000, 2000); + applyAdjustmentU16(ADJUSTMENT_NAV_FW_CRUISE_THR, &navConfigMutable()->fw.cruise_throttle, delta, SETTING_NAV_FW_CRUISE_THR_MIN, SETTING_NAV_FW_CRUISE_THR_MAX); break; case ADJUSTMENT_NAV_FW_PITCH2THR: - applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, 0, 100); + applyAdjustmentU8(ADJUSTMENT_NAV_FW_PITCH2THR, &navConfigMutable()->fw.pitch_to_throttle, delta, SETTING_NAV_FW_PITCH2THR_MIN, SETTING_NAV_FW_PITCH2THR_MAX); break; case ADJUSTMENT_ROLL_BOARD_ALIGNMENT: updateBoardAlignment(delta, 0); @@ -586,7 +578,7 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t navigationUsePIDs(); break; case ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE: - applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, 0, FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX); + applyAdjustmentU16(ADJUSTMENT_FW_MIN_THROTTLE_DOWN_PITCH_ANGLE, &mixerConfigMutable()->fwMinThrottleDownPitchAngle, delta, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MIN, SETTING_FW_MIN_THROTTLE_DOWN_PITCH_MAX); break; #if defined(USE_VTX_SMARTAUDIO) || defined(USE_VTX_TRAMP) case ADJUSTMENT_VTX_POWER_LEVEL: @@ -599,13 +591,13 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t break; #endif case ADJUSTMENT_TPA: - applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, CONTROL_RATE_CONFIG_TPA_MAX); + applyAdjustmentU8(ADJUSTMENT_TPA, &controlRateConfig->throttle.dynPID, delta, 0, SETTING_TPA_RATE_MAX); break; case ADJUSTMENT_TPA_BREAKPOINT: applyAdjustmentU16(ADJUSTMENT_TPA_BREAKPOINT, &controlRateConfig->throttle.pa_breakpoint, delta, PWM_RANGE_MIN, PWM_RANGE_MAX); break; case ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS: - applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, 0, 9); + applyAdjustmentU8(ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS, &navConfigMutable()->fw.control_smoothness, delta, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MIN, SETTING_NAV_FW_CONTROL_SMOOTHNESS_MAX); break; default: break; diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 48dface10e..b832f88b34 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -212,6 +212,7 @@ void updateUsedModeActivationConditionFlags(void) isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) || isModeActivationConditionPresent(BOXNAVRTH) || isModeActivationConditionPresent(BOXNAVCOURSEHOLD) || + isModeActivationConditionPresent(BOXNAVCRUISE) || isModeActivationConditionPresent(BOXNAVWP); #endif } diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 6b3f877197..9c47866cab 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -69,7 +69,9 @@ typedef enum { BOXLOITERDIRCHN = 40, BOXMSPRCOVERRIDE = 41, BOXPREARM = 42, - BOXFLIPOVERAFTERCRASH = 43, + BOXTURTLE = 43, + BOXNAVCRUISE = 44, + BOXAUTOLEVEL = 45, CHECKBOX_ITEM_COUNT } boxId_e; @@ -129,4 +131,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); \ No newline at end of file +void configureModeActivationCondition(int macIndex, boxId_e modeId, uint8_t auxChannelIndex, uint16_t startPwm, uint16_t endPwm); diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 47d6267cbd..57cc63f8bc 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", "NOPREARM" + "SETTINGFAIL", "PWMOUT", "NOPREARM", "DSHOTBEEPER" }; #endif @@ -58,7 +58,8 @@ const armingFlag_e armDisableReasonsChecklist[] = { ARMING_DISABLED_ROLLPITCH_NOT_CENTERED, ARMING_DISABLED_SERVO_AUTOTRIM, ARMING_DISABLED_OOM, - ARMING_DISABLED_NO_PREARM + ARMING_DISABLED_NO_PREARM, + ARMING_DISABLED_DSHOT_BEEPER }; armingFlag_e isArmingDisabledReason(void) @@ -154,9 +155,12 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) if (FLIGHT_MODE(NAV_POSHOLD_MODE)) return FLM_POSITION_HOLD; - if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) return FLM_CRUISE; + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) + return FLM_COURSE_HOLD; + if (FLIGHT_MODE(NAV_WP_MODE)) return FLM_MISSION; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 9f6ec9efa3..e77e7c18b4 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -44,6 +44,7 @@ typedef enum { ARMING_DISABLED_INVALID_SETTING = (1 << 26), ARMING_DISABLED_PWM_OUTPUT_ERROR = (1 << 27), ARMING_DISABLED_NO_PREARM = (1 << 28), + ARMING_DISABLED_DSHOT_BEEPER = (1 << 29), 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 +53,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_NO_PREARM), + ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER), } armingFlag_e; // Arming blockers that can be overriden by emergency arming. @@ -97,7 +98,7 @@ typedef enum { NAV_COURSE_HOLD_MODE = (1 << 12), FLAPERON = (1 << 13), TURN_ASSISTANT = (1 << 14), - FLIP_OVER_AFTER_CRASH = (1 << 15), + TURTLE_MODE = (1 << 15), } flightModeFlags_e; extern uint32_t flightModeFlags; @@ -151,6 +152,7 @@ typedef enum { FLM_POSITION_HOLD, FLM_RTH, FLM_MISSION, + FLM_COURSE_HOLD, FLM_CRUISE, FLM_LAUNCH, FLM_FAILSAFE, diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b5e29c29ae..50226d8d1e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -10,7 +10,7 @@ tables: values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X", "US42"] enum: rangefinderType_e - name: secondary_imu_hardware - values: ["NONE", "BNO055"] + values: ["NONE", "BNO055", "BNO055_SERIAL"] enum: secondaryImuType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"] @@ -69,9 +69,15 @@ tables: - name: osd_stats_energy_unit values: ["MAH", "WH"] enum: osd_stats_energy_unit_e + - name: osd_stats_min_voltage_unit + values: ["BATTERY", "CELL"] + enum: osd_stats_min_voltage_unit_e - name: osd_video_system values: ["AUTO", "PAL", "NTSC"] enum: videoSystem_e + - name: osd_telemetry + values: ["OFF", "ON","TEST"] + enum: osd_telemetry_e - name: osd_alignment values: ["LEFT", "RIGHT"] enum: osd_alignment_e @@ -87,8 +93,8 @@ 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", "FW_D", "IMU2", "ALTITUDE", - "RATE_DYNAMICS"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "FW_D", "IMU2", "ALTITUDE", + "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -98,7 +104,7 @@ tables: values: ["DEFAULT", "AIRCRAFT", "TYPE3", "TYPE4", "TYPE5", "TYPE6", "TYPE7"] enum: osd_crosshairs_style_e - name: osd_sidebar_scroll - values: ["NONE", "ALTITUDE", "GROUND_SPEED", "HOME_DISTANCE"] + values: ["NONE", "ALTITUDE", "SPEED", "HOME_DISTANCE"] enum: osd_sidebar_scroll_e - name: nav_rth_allow_landing values: ["NEVER", "ALWAYS", "FS_ONLY"] @@ -150,24 +156,40 @@ tables: values: ["AUTO", "ON", "OFF"] - name: osd_crsf_lq_format enum: osd_crsf_lq_format_e - values: ["TYPE1", "TYPE2"] + values: ["TYPE1", "TYPE2", "TYPE3"] - name: off_on values: ["OFF", "ON"] - name: djiOsdTempSource values: ["ESC", "IMU", "BARO"] enum: djiOsdTempSource_e - - name: djiOsdSpeedSource + - name: osdSpeedSource values: ["GROUND", "3D", "AIR"] - enum: djiOsdSpeedSource_e + enum: osdSpeedSource_e - name: nav_overrides_motor_stop enum: navOverridesMotorStop_e values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] - name: osd_plus_code_short values: ["0", "2", "4", "6"] + - name: autotune_rate_adjustment + enum: autotune_rate_adjustment_e + values: ["FIXED", "LIMIT", "AUTO"] + - name: safehome_usage_mode + values: ["OFF", "RTH", "RTH_FS"] + enum: safehomeUsageMode_e - name: nav_rth_climb_first enum: navRTHClimbFirst_e values: ["OFF", "ON", "ON_FW_SPIRAL"] +constants: + RPYL_PID_MIN: 0 + RPYL_PID_MAX: 200 + + MANUAL_RATE_MIN: 0 + MANUAL_RATE_MAX: 100 + + ROLL_PITCH_RATE_MIN: 6 + ROLL_PITCH_RATE_MAX: 180 + groups: - name: PG_GYRO_CONFIG type: gyroConfig_t @@ -177,11 +199,6 @@ groups: 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 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: ON - field: gyroSync - type: bool - name: align_gyro 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" @@ -193,15 +210,15 @@ groups: 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 - field: gyro_soft_lpf_hz - max: 200 - - name: gyro_lpf_type - description: "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." - default_value: "BIQUAD" - field: gyro_soft_lpf_type + - name: gyro_anti_aliasing_lpf_hz + description: "Gyro processing anti-aliasing filter cutoff frequency. In normal operation this filter setting should never be changed. In Hz" + default_value: 250 + field: gyro_anti_aliasing_lpf_hz + max: 255 + - name: gyro_anti_aliasing_lpf_type + description: "Specifies the type of the software LPF of the gyro signals." + default_value: "PT1" + field: gyro_anti_aliasing_lpf_type 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." @@ -217,16 +234,16 @@ groups: 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 - field: gyro_stage2_lowpass_hz + - name: gyro_main_lpf_hz + description: "Software based gyro main lowpass filter. Value is cutoff frequency (Hz)" + default_value: 60 + field: gyro_main_lpf_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." + - name: gyro_main_lpf_type + description: "Defines the type of the main gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." default_value: BIQUAD - field: gyro_stage2_lowpass_type + field: gyro_main_lpf_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." @@ -282,6 +299,27 @@ groups: min: 0 max: 1 default_value: 0 + - name: gyro_abg_alpha + description: "Alpha factor for Gyro Alpha-Beta-Gamma filter" + default_value: 0 + field: alphaBetaGammaAlpha + condition: USE_ALPHA_BETA_GAMMA_FILTER + min: 0 + max: 1 + - name: gyro_abg_boost + description: "Boost factor for Gyro Alpha-Beta-Gamma filter" + default_value: 0.35 + field: alphaBetaGammaBoost + condition: USE_ALPHA_BETA_GAMMA_FILTER + min: 0 + max: 2 + - name: gyro_abg_half_life + description: "Sample half-life for Gyro Alpha-Beta-Gamma filter" + default_value: 0.5 + field: alphaBetaGammaHalfLife + condition: USE_ALPHA_BETA_GAMMA_FILTER + min: 0 + max: 10 - name: PG_ADC_CHANNEL_CONFIG type: adcChannelConfig_t @@ -837,10 +875,10 @@ groups: 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" + - name: turtle_mode_power_factor + field: turtleModePowerFactor + default_value: 55 + description: "Turtle mode power factor" condition: "USE_DSHOT" min: 0 max: 100 @@ -1180,6 +1218,11 @@ groups: 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 type: bool + - name: servo_autotrim_rotation_limit + description: "Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`." + default_value: 15 + min: 1 + max: 60 - name: PG_CONTROL_RATE_PROFILES type: controlRateConfig_t @@ -1203,7 +1246,7 @@ groups: default_value: 0 field: throttle.dynPID min: 0 - max: CONTROL_RATE_CONFIG_TPA_MAX + max: 100 - name: tpa_breakpoint description: "See tpa_rate." default_value: 1500 @@ -1234,20 +1277,20 @@ groups: 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 field: stabilized.rates[FD_ROLL] - min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN - max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX + min: ROLL_PITCH_RATE_MIN + max: 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 field: stabilized.rates[FD_PITCH] - min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN - max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX + min: ROLL_PITCH_RATE_MIN + max: 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 field: stabilized.rates[FD_YAW] - min: CONTROL_RATE_CONFIG_YAW_RATE_MIN - max: CONTROL_RATE_CONFIG_YAW_RATE_MAX + min: 2 + max: 180 - name: manual_rc_expo description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]" default_value: 70 @@ -1264,20 +1307,20 @@ groups: description: "Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]%" default_value: 100 field: manual.rates[FD_ROLL] - min: 0 - max: 100 + min: MANUAL_RATE_MIN + max: MANUAL_RATE_MAX - name: manual_pitch_rate description: "Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]%" default_value: 100 field: manual.rates[FD_PITCH] - min: 0 - max: 100 + min: MANUAL_RATE_MIN + max: MANUAL_RATE_MAX - name: manual_yaw_rate description: "Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]%" default_value: 100 field: manual.rates[FD_YAW] - min: 0 - max: 100 + min: MANUAL_RATE_MIN + max: MANUAL_RATE_MAX - name: fpv_mix_degrees field: misc.fpvCamAngleDegrees min: 0 @@ -1540,182 +1583,182 @@ groups: description: "Multicopter rate stabilisation P-gain for PITCH" default_value: 40 field: bank_mc.pid[PID_PITCH].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_i_pitch description: "Multicopter rate stabilisation I-gain for PITCH" default_value: 30 field: bank_mc.pid[PID_PITCH].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_d_pitch description: "Multicopter rate stabilisation D-gain for PITCH" default_value: 23 field: bank_mc.pid[PID_PITCH].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_cd_pitch description: "Multicopter Control Derivative gain for PITCH" default_value: 60 field: bank_mc.pid[PID_PITCH].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_p_roll description: "Multicopter rate stabilisation P-gain for ROLL" default_value: 40 field: bank_mc.pid[PID_ROLL].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_i_roll description: "Multicopter rate stabilisation I-gain for ROLL" default_value: 30 field: bank_mc.pid[PID_ROLL].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_d_roll description: "Multicopter rate stabilisation D-gain for ROLL" default_value: 23 field: bank_mc.pid[PID_ROLL].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_cd_roll description: "Multicopter Control Derivative gain for ROLL" default_value: 60 field: bank_mc.pid[PID_ROLL].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_p_yaw description: "Multicopter rate stabilisation P-gain for YAW" default_value: 85 field: bank_mc.pid[PID_YAW].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_i_yaw description: "Multicopter rate stabilisation I-gain for YAW" default_value: 45 field: bank_mc.pid[PID_YAW].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_d_yaw description: "Multicopter rate stabilisation D-gain for YAW" default_value: 0 field: bank_mc.pid[PID_YAW].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_cd_yaw description: "Multicopter Control Derivative gain for YAW" default_value: 60 field: bank_mc.pid[PID_YAW].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_p_level description: "Multicopter attitude stabilisation P-gain" default_value: 20 field: bank_mc.pid[PID_LEVEL].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_i_level description: "Multicopter attitude stabilisation low-pass filter cutoff" default_value: 15 field: bank_mc.pid[PID_LEVEL].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: mc_d_level description: "Multicopter attitude stabilisation HORIZON transition point" default_value: 75 field: bank_mc.pid[PID_LEVEL].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_p_pitch description: "Fixed-wing rate stabilisation P-gain for PITCH" default_value: 5 field: bank_fw.pid[PID_PITCH].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_i_pitch description: "Fixed-wing rate stabilisation I-gain for PITCH" default_value: 7 field: bank_fw.pid[PID_PITCH].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - 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 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_ff_pitch description: "Fixed-wing rate stabilisation FF-gain for PITCH" default_value: 50 field: bank_fw.pid[PID_PITCH].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_p_roll description: "Fixed-wing rate stabilisation P-gain for ROLL" default_value: 5 field: bank_fw.pid[PID_ROLL].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_i_roll description: "Fixed-wing rate stabilisation I-gain for ROLL" default_value: 7 field: bank_fw.pid[PID_ROLL].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - 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 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_ff_roll description: "Fixed-wing rate stabilisation FF-gain for ROLL" default_value: 50 field: bank_fw.pid[PID_ROLL].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_p_yaw description: "Fixed-wing rate stabilisation P-gain for YAW" default_value: 6 field: bank_fw.pid[PID_YAW].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_i_yaw description: "Fixed-wing rate stabilisation I-gain for YAW" default_value: 10 field: bank_fw.pid[PID_YAW].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - 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 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_ff_yaw description: "Fixed-wing rate stabilisation FF-gain for YAW" default_value: 60 field: bank_fw.pid[PID_YAW].FF - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_p_level description: "Fixed-wing attitude stabilisation P-gain" default_value: 20 field: bank_fw.pid[PID_LEVEL].P - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_i_level description: "Fixed-wing attitude stabilisation low-pass filter cutoff" default_value: 5 field: bank_fw.pid[PID_LEVEL].I - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: fw_d_level description: "Fixed-wing attitude stabilisation HORIZON transition point" default_value: 75 field: bank_fw.pid[PID_LEVEL].D - min: 0 - max: 200 + min: RPYL_PID_MIN + max: RPYL_PID_MAX - name: max_angle_inclination_rll description: "Maximum inclination in level (angle) mode (ROLL axis). 100=10°" default_value: 300 @@ -1767,10 +1810,10 @@ 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: 1500 field: fixedWingReferenceAirspeed - min: 1 - max: 5000 + min: 300 + max: 6000 - 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 @@ -2090,27 +2133,48 @@ groups: field: fixedWingLevelTrim min: -10 max: 10 + - name: smith_predictor_strength + description: "The strength factor of a Smith Predictor of PID measurement. In percents" + default_value: 0.5 + field: smithPredictorStrength + condition: USE_SMITH_PREDICTOR + min: 0 + max: 1 + - name: smith_predictor_delay + description: "Expected delay of the gyro signal. In milliseconds" + default_value: 0 + field: smithPredictorDelay + condition: USE_SMITH_PREDICTOR + min: 0 + max: 8 + - name: smith_predictor_lpf_hz + description: "Cutoff frequency for the Smith Predictor Low Pass Filter" + default_value: 50 + field: smithPredictorFilterHz + condition: USE_SMITH_PREDICTOR + min: 1 + max: 500 + - name: fw_level_pitch_gain + description: "I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations" + default_value: 5 + field: fixedWingLevelTrimGain + min: 0 + max: 20 + - name: fw_level_pitch_deadband + description: "Deadband for automatic leveling when AUTOLEVEL mode is used." + default_value: 5 + field: fixedWingLevelTrimDeadband + min: 0 + max: 20 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t condition: USE_AUTOTUNE_FIXED_WING members: - - name: fw_autotune_overshoot_time - description: "Time [ms] to detect sustained overshoot" - 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 - field: fw_undershoot_time - min: 50 - max: 500 - - name: fw_autotune_threshold - description: "Threshold [%] of max rate to consider overshoot/undershoot detection" + - name: fw_autotune_min_stick + description: "Minimum stick input [%] to consider overshoot/undershoot detection" default_value: 50 - field: fw_max_rate_threshold + field: fw_min_stick min: 0 max: 100 - name: fw_autotune_ff_to_p_gain @@ -2119,12 +2183,30 @@ groups: field: fw_ff_to_p_gain min: 0 max: 100 + - name: fw_autotune_p_to_d_gain + description: "P to D gain (strength relationship) [%]" + default_value: 0 + field: fw_p_to_d_gain + min: 0 + max: 200 - 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 field: fw_ff_to_i_time_constant min: 100 max: 5000 + - name: fw_autotune_rate_adjustment + description: "`AUTO` and `LIMIT` adjust the rates to match the capabilities of the airplane, with `LIMIT` they are never increased above the starting rates setting. `FIXED` does not adjust the rates. Rates are not changed when tuning in `ANGLE` mode." + default_value: "AUTO" + field: fw_rate_adjustment + table: autotune_rate_adjustment + type: uint8_t + - name: fw_autotune_max_rate_deflection + description: "The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`." + default_value: 80 + field: fw_max_rate_deflection + min: 50 + max: 100 - name: PG_POSITION_ESTIMATION_CONFIG type: positionEstimationConfig_t @@ -2288,6 +2370,11 @@ groups: field: general.pos_failure_timeout min: 0 max: 10 + - name: nav_wp_load_on_boot + description: "If set to ON, waypoints will be automatically loaded from EEPROM to the FC during startup." + default_value: OFF + field: general.waypoint_load_on_boot + type: bool - name: nav_wp_radius description: "Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius" default_value: 100 @@ -2323,20 +2410,26 @@ groups: 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]" + - name: nav_land_minalt_vspd + description: "Vertical descent velocity under nav_land_slowdown_minalt during the RTH landing phase. [cm/s]" + default_value: 50 + field: general.land_minalt_vspd + min: 50 + max: 500 + - name: nav_land_maxalt_vspd + description: "Vertical descent velocity above nav_land_slowdown_maxalt during the RTH landing phase. [cm/s]" default_value: 200 - field: general.land_descent_rate + field: general.land_maxalt_vspd 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]" + description: "Defines at what altitude the descent velocity should start to be `nav_land_minalt_vspd` [cm]" 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]" + description: "Defines at what altitude the descent velocity should start to ramp down from `nav_land_maxalt_vspd` to `nav_land_minalt_vspd` during the RTH landing phase [cm]" default_value: 2000 field: general.land_slowdown_maxalt min: 500 @@ -2383,6 +2476,11 @@ groups: default_value: "AT_LEAST" field: general.flags.rth_alt_control_mode table: nav_rth_alt_mode + - name: nav_rth_alt_control_override + description: "If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 1 second. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home (CLIMB FIRST override only works for fixed wing)" + default_value: OFF + field: general.flags.rth_alt_control_override + 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 @@ -2394,6 +2492,12 @@ groups: description: "Max allowed above the ground altitude for terrain following mode" max: 1000 default_value: 100 + - name: nav_max_altitude + field: general.max_altitude + description: "Max allowed altitude (above Home Point) that applies to all NAV modes (including Altitude Hold). 0 means limit is disabled" + default_value: 0 + max: 65000 + min: 0 - name: nav_rth_altitude description: "Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters)" default_value: 1000 @@ -2410,6 +2514,11 @@ groups: field: general.safehome_max_distance min: 0 max: 65000 + - name: safehome_usage_mode + description: "Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`. See [Safehome documentation](Safehomes.md#Safehome) for more information." + default_value: "RTH" + field: general.flags.safehome_usage_mode + table: safehome_usage_mode - 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 @@ -2656,7 +2765,7 @@ groups: 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" + description: "Climb angle (attitude of model, not climb slope) for launch sequence (degrees), is also restrained by global max_angle_inclination_pit" default_value: 18 field: fw.launch_climb_angle min: 5 @@ -2916,6 +3025,12 @@ groups: headers: ["io/osd.h", "drivers/osd.h"] condition: USE_OSD members: + - name: osd_telemetry + description: "To enable OSD telemetry for antenna tracker. Possible values are `OFF`, `ON` and `TEST`" + table: osd_telemetry + field: telemetry + type: uint8_t + default_value: "OFF" - name: osd_video_system description: "Video system used. Possible values are `AUTO`, `PAL` and `NTSC`" default_value: "AUTO" @@ -2940,6 +3055,12 @@ groups: field: stats_energy_unit table: osd_stats_energy_unit type: uint8_t + - name: osd_stats_min_voltage_unit + description: "Display minimum voltage of the `BATTERY` or the average per `CELL` in the OSD stats." + default_value: "BATTERY" + field: stats_min_voltage_unit + table: osd_stats_min_voltage_unit + type: uint8_t - name: osd_rssi_alarm description: "Value below which to make the OSD RSSI indicator blink" @@ -3262,6 +3383,13 @@ groups: default_value: 0 description: Same as left_sidebar_scroll_step, but for the right sidebar + - name: osd_sidebar_height + field: sidebar_height + min: 0 + max: 5 + default_value: 3 + description: Height of sidebars in rows. 0 leaves only the level indicator arrows (Not for pixel OSD) + - name: osd_home_position_arm_screen type: bool default_value: ON @@ -3281,6 +3409,17 @@ groups: min: -36 max: 36 + - name: PG_OSD_COMMON_CONFIG + type: osdCommonConfig_t + headers: ["io/osd_common.h"] + condition: USE_OSD || USE_DJI_HD_OSD + members: + - name: osd_speed_source + description: "Sets the speed type displayed by the DJI OSD and OSD canvas (FrSky Pixel): GROUND, 3D, AIR" + default_value: "GROUND" + field: speedSource + table: osdSpeedSource + type: uint8_t - name: PG_SYSTEM_CONFIG type: systemConfig_t @@ -3530,9 +3669,85 @@ 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 + + - name: PG_BEEPER_CONFIG + type: beeperConfig_t + headers: [ "fc/config.h" ] + members: + - name: dshot_beeper_enabled + description: "Whether using DShot motors as beepers is enabled" + default_value: ON + field: dshot_beeper_enabled + type: bool + - name: dshot_beeper_tone + description: "Sets the DShot beeper tone" + min: 1 + max: 5 + default_value: 1 + field: dshot_beeper_tone + type: uint8_t + + - name: PG_POWER_LIMITS_CONFIG + type: powerLimitsConfig_t + headers: ["flight/power_limits.h"] + condition: USE_POWER_LIMITS + members: + - name: limit_cont_current + description: "Continous current limit (dA), set to 0 to disable" + default_value: 0 + field: continuousCurrent + max: 4000 + - name: limit_burst_current + description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable" + default_value: 0 + field: burstCurrent + max: 4000 + - name: limit_burst_current_time + description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced" + default_value: 0 + field: burstCurrentTime + max: 3000 + - name: limit_burst_current_falldown_time + description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`" + default_value: 0 + field: burstCurrentFalldownTime + max: 3000 + - name: limit_cont_power + condition: USE_ADC + description: "Continous power limit (dW), set to 0 to disable" + default_value: 0 + field: continuousPower + max: 40000 + - name: limit_burst_power + condition: USE_ADC + description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable" + default_value: 0 + field: burstPower + max: 40000 + - name: limit_burst_power_time + condition: USE_ADC + description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced" + default_value: 0 + field: burstPowerTime + max: 3000 + - name: limit_burst_power_falldown_time + condition: USE_ADC + description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`" + default_value: 0 + field: burstPowerFalldownTime + max: 3000 + - name: limit_pi_p + description: "Throttle attenuation PI control P term" + default_value: 100 + field: piP + max: 10000 + - name: limit_pi_i + description: "Throttle attenuation PI control I term" + default_value: 100 + field: piI + max: 10000 + - name: limit_attn_filter_cutoff + description: "Throttle attenuation PI control output filter cutoff frequency" + default_value: 1.2 + field: attnFilterCutoff + max: 100 diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 7a0c3612d7..90173dc473 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -47,6 +47,7 @@ #include "flight/pid.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "rx/rx.h" @@ -362,12 +363,15 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) // Craft is closer than minimum failsafe procedure distance (if set to non-zero) // GPS must also be working, and home position set - if ((failsafeConfig()->failsafe_min_distance > 0) && - ((GPS_distanceToHome * 100) < failsafeConfig()->failsafe_min_distance) && - sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { + if (failsafeConfig()->failsafe_min_distance > 0 && + sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { - // Use the alternate, minimum distance failsafe procedure instead - return failsafeConfig()->failsafe_min_distance_procedure; + // get the distance to the original arming point + uint32_t distance = calculateDistanceToDestination(&original_rth_home); + if (distance < failsafeConfig()->failsafe_min_distance) { + // Use the alternate, minimum distance failsafe procedure instead + return failsafeConfig()->failsafe_min_distance_procedure; + } } return failsafeConfig()->failsafe_procedure; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index af1ca40a20..2ee79621f0 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -73,6 +73,8 @@ static EXTENDED_FASTRAM int throttleRangeMin = 0; static EXTENDED_FASTRAM int throttleRangeMax = 0; static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1; +int motorZeroCommand = 0; + PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0); PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, @@ -114,13 +116,12 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .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, + .turtleModePowerFactor = SETTING_TURTLE_MODE_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); @@ -273,7 +274,6 @@ void mixerInit(void) void mixerResetDisarmedMotors(void) { - int motorZeroCommand; if (feature(FEATURE_REVERSIBLE_MOTORS)) { motorZeroCommand = reversibleMotorsConfig()->neutral; @@ -327,10 +327,10 @@ static uint16_t handleOutputScaling( } return value; } -static void applyFlipOverAfterCrashModeToMotors(void) { +static void applyTurtleModeToMotors(void) { if (ARMING_FLAG(ARMED)) { - const float flipPowerFactor = ((float)motorConfig()->flipOverAfterPowerFactor)/100.0f; + const float flipPowerFactor = ((float)motorConfig()->turtleModePowerFactor)/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); @@ -393,13 +393,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) { 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; + motor[i] = (int16_t)scaleRangef(motorOutputNormalised, 0, 1, motorConfig()->mincommand, motorConfig()->maxthrottle); } } else { // Disarmed mode @@ -531,8 +525,8 @@ static int getReversibleMotorsThrottleDeadband(void) void FAST_CODE mixTable(const float dT) { #ifdef USE_DSHOT - if (FLIGHT_MODE(FLIP_OVER_AFTER_CRASH)) { - applyFlipOverAfterCrashModeToMotors(); + if (FLIGHT_MODE(TURTLE_MODE)) { + applyTurtleModeToMotors(); return; } #endif @@ -720,3 +714,18 @@ void loadPrimaryMotorMixer(void) { currentMixer[i] = *primaryMotorMixer(i); } } + +bool areMotorsRunning(void) +{ + if (ARMING_FLAG(ARMED)) { + return true; + } else { + for (int i = 0; i < motorCount; i++) { + if (motor_disarmed[i] != motorZeroCommand) { + return true; + } + } + } + + return false; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 7f5088fafc..195df58c30 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -25,11 +25,6 @@ #define MAX_SUPPORTED_MOTORS 12 #endif -#define YAW_JUMP_PREVENTION_LIMIT_LOW 80 -#define YAW_JUMP_PREVENTION_LIMIT_HIGH 500 - -#define FW_MIN_THROTTLE_DOWN_PITCH_ANGLE_MAX 450 - // Digital protocol has fixed values #define DSHOT_DISARM_COMMAND 0 #define DSHOT_MIN_THROTTLE 48 @@ -92,7 +87,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 + uint8_t turtleModePowerFactor; // Power factor from 0 to 100% of flip over after crash } motorConfig_t; PG_DECLARE(motorConfig_t, motorConfig); @@ -126,8 +121,11 @@ void mixerUpdateStateFlags(void); void mixerResetDisarmedMotors(void); void mixTable(const float dT); void writeMotors(void); -void processServoAutotrim(void); +void processServoAutotrim(const float dT); +void processServoAutotrimMode(void); +void processContinuousServoAutotrim(const float dT); void stopMotors(void); void stopPwmAllMotors(void); -void loadPrimaryMotorMixer(void); \ No newline at end of file +void loadPrimaryMotorMixer(void); +bool areMotorsRunning(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b8c4b94e24..6f6222d901 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -48,6 +48,7 @@ FILE_COMPILE_FOR_SPEED #include "flight/rpm_filter.h" #include "flight/secondary_imu.h" #include "flight/kalman.h" +#include "flight/smith_predictor.h" #include "io/gps.h" @@ -109,6 +110,8 @@ typedef struct { bool itermFreezeActive; biquadFilter_t rateTargetFilter; + + smithPredictor_t smithPredictor; } pidState_t; STATIC_FASTRAM bool pidFiltersConfigured = false; @@ -157,9 +160,16 @@ static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; -static EXTENDED_FASTRAM float fixedWingLevelTrim; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); +#define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand +#define FIXED_WING_LEVEL_TRIM_DIVIDER 500.0f +#define FIXED_WING_LEVEL_TRIM_MULTIPLIER 1.0f / FIXED_WING_LEVEL_TRIM_DIVIDER +#define FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT FIXED_WING_LEVEL_TRIM_DIVIDER * FIXED_WING_LEVEL_TRIM_MAX_ANGLE + +static EXTENDED_FASTRAM float fixedWingLevelTrim; +static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController; + +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -297,6 +307,14 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, #endif .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT, + .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT, + .fixedWingLevelTrimDeadband = SETTING_FW_LEVEL_PITCH_DEADBAND_DEFAULT, + +#ifdef USE_SMITH_PREDICTOR + .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT, + .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT, + .smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT, +#endif ); bool pidInitFilters(void) @@ -349,6 +367,30 @@ bool pidInitFilters(void) } } +#ifdef USE_SMITH_PREDICTOR + smithPredictorInit( + &pidState[FD_ROLL].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); + smithPredictorInit( + &pidState[FD_PITCH].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); + smithPredictorInit( + &pidState[FD_YAW].smithPredictor, + pidProfile()->smithPredictorDelay, + pidProfile()->smithPredictorStrength, + pidProfile()->smithPredictorFilterHz, + getLooptime() + ); +#endif + pidFiltersConfigured = true; return true; @@ -371,6 +413,22 @@ void pidResetErrorAccumulators(void) } } +void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) +{ + pidState[axis].errorGyroIf -= delta; + pidState[axis].errorGyroIfLimit -= delta; +} + +float getTotalRateTarget(void) +{ + return sqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget)); +} + +float getAxisIterm(uint8_t axis) +{ + return pidState[axis].errorGyroIf; +} + static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination) { stick = constrain(stick, -500, 500); @@ -406,7 +464,7 @@ static float calculateFixedWingTPAFactor(uint16_t throttle) // tpa_rate is amount of curve TPA applied to PIDs // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned) - if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue()) { + if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) { if (throttle > getThrottleIdleValue()) { // Calculate TPA according to throttle tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); @@ -548,8 +606,28 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]); // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle - 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); + 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 + if (axis == FD_PITCH && STATE(AIRPLANE)) { + DEBUG_SET(DEBUG_AUTOLEVEL, 0, angleTarget * 10); + DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10); + DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z)); + + /* + * fixedWingLevelTrim has opposite sign to rcCommand. + * Positive rcCommand means nose should point downwards + * Negative rcCommand mean nose should point upwards + * This is counter intuitive and a natural way suggests that + should mean UP + * This is why fixedWingLevelTrim has opposite sign to rcCommand + * Positive fixedWingLevelTrim means nose should point upwards + * Negative fixedWingLevelTrim means nose should point downwards + */ + angleTarget -= fixedWingLevelTrim; + DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10); + } //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming if (axis == FD_PITCH && STATE(AIRPLANE)) { @@ -717,14 +795,14 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); } + axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit); + #ifdef USE_AUTOTUNE_FIXED_WING if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) { - autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm); + autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit)); } #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; @@ -1052,6 +1130,13 @@ void FAST_CODE pidController(float dT) pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget); } #endif + +#ifdef USE_SMITH_PREDICTOR + DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis, pidState[axis].gyroRate); + pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate); + DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis + 3, pidState[axis].gyroRate); +#endif + DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate); } @@ -1194,6 +1279,16 @@ void pidInit(void) #endif fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim; + + navPidInit( + &fixedWingLevelTrimController, + 0.0f, + (float)pidProfile()->fixedWingLevelTrimGain / 100000.0f, + 0.0f, + 0.0f, + 2.0f + ); + } const pidBank_t * pidBank(void) { @@ -1202,3 +1297,61 @@ const pidBank_t * pidBank(void) { pidBank_t * pidBankMutable(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } + +void updateFixedWingLevelTrim(timeUs_t currentTimeUs) +{ + if (!STATE(AIRPLANE)) { + return; + } + + static timeUs_t previousUpdateTimeUs; + static bool previousArmingState; + const float dT = US2S(currentTimeUs - previousUpdateTimeUs); + + /* + * On every ARM reset the controller + */ + if (ARMING_FLAG(ARMED) && !previousArmingState) { + navPidReset(&fixedWingLevelTrimController); + } + + /* + * On disarm update the default value + */ + if (!ARMING_FLAG(ARMED) && previousArmingState) { + pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE); + } + + /* + * Prepare flags for the PID controller + */ + pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR; + + //Iterm should freeze when pitch stick is deflected + if ( + !IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || + rxGetChannelValue(PITCH) > (PWM_RANGE_MIDDLE + pidProfile()->fixedWingLevelTrimDeadband) || + rxGetChannelValue(PITCH) < (PWM_RANGE_MIDDLE - pidProfile()->fixedWingLevelTrimDeadband) || + (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) || + navigationIsControllingAltitude() + ) { + flags |= PID_FREEZE_INTEGRATOR; + } + + const float output = navPidApply3( + &fixedWingLevelTrimController, + 0, //Setpoint is always 0 as we try to keep level flight + getEstimatedActualVelocity(Z), + dT, + -FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT, + FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT, + flags, + 1.0f, + 1.0f + ); + + DEBUG_SET(DEBUG_AUTOLEVEL, 4, output); + fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER); + + previousArmingState = !!ARMING_FLAG(ARMED); +} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3f3d5a3bcc..0d4719acb8 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -19,12 +19,13 @@ #include "config/parameter_group.h" #include "fc/runtime_config.h" +#include "common/time.h" #define GYRO_SATURATION_LIMIT 1800 // 1800dps #define PID_SUM_LIMIT_MIN 100 #define PID_SUM_LIMIT_MAX 1000 #define PID_SUM_LIMIT_DEFAULT 500 -#define PID_SUM_LIMIT_YAW_DEFAULT 350 +#define PID_SUM_LIMIT_YAW_DEFAULT 400 #define HEADING_HOLD_RATE_LIMIT_MIN 10 #define HEADING_HOLD_RATE_LIMIT_MAX 250 @@ -54,6 +55,8 @@ FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13 #define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff +#define FIXED_WING_LEVEL_TRIM_DEADBAND_DEFAULT 5 + #define TASK_AUX_RATE_HZ 100 //In Hz typedef enum { @@ -161,16 +164,31 @@ typedef struct pidProfile_s { #endif float fixedWingLevelTrim; + float fixedWingLevelTrimGain; + float fixedWingLevelTrimDeadband; +#ifdef USE_SMITH_PREDICTOR + float smithPredictorStrength; + float smithPredictorDelay; + uint16_t smithPredictorFilterHz; +#endif } pidProfile_t; typedef struct pidAutotuneConfig_s { - uint16_t fw_overshoot_time; // Time [ms] to detect sustained overshoot - uint16_t fw_undershoot_time; // Time [ms] to detect sustained undershoot - uint8_t fw_max_rate_threshold; // Threshold [%] of max rate to consider autotune detection + uint16_t fw_detect_time; // Time [ms] to detect sustained undershoot or overshoot + uint8_t fw_min_stick; // Minimum stick input required to update rates and gains uint8_t fw_ff_to_p_gain; // FF to P gain (strength relationship) [%] + uint8_t fw_p_to_d_gain; // P to D gain (strength relationship) [%] uint16_t fw_ff_to_i_time_constant; // FF to I time (defines time for I to reach the same level of response as FF) [ms] + uint8_t fw_rate_adjustment; // Adjust rate settings during autotune? + uint8_t fw_max_rate_deflection; // Percentage of max mixer output used for calculating the rates } pidAutotuneConfig_t; +typedef enum { + FIXED, + LIMIT, + AUTO, +} fw_autotune_rate_adjustment_e; + PG_DECLARE_PROFILE(pidProfile_t, pidProfile); PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig); @@ -183,6 +201,9 @@ extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[]; void pidInit(void); bool pidInitFilters(void); void pidResetErrorAccumulators(void); +void pidReduceErrorAccumulators(int8_t delta, uint8_t axis); +float getAxisIterm(uint8_t axis); +float getTotalRateTarget(void); void pidResetTPAFilter(void); struct controlRateConfig_s; @@ -210,3 +231,5 @@ void autotuneUpdateState(void); void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput); pidType_e pidIndexGetType(pidIndex_e pidIndex); + +void updateFixedWingLevelTrim(timeUs_t currentTimeUs); diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 684f42d1b7..9679e736e2 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -25,6 +25,8 @@ #include "blackbox/blackbox.h" #include "blackbox/blackbox_fielddefs.h" +#include "build/debug.h" + #include "common/axis.h" #include "common/maths.h" #include "common/utils.h" @@ -44,36 +46,45 @@ #include "flight/pid.h" -#define AUTOTUNE_FIXED_WING_INTEGRATOR_TC 600 -#define AUTOTUNE_FIXED_WING_DECREASE_STEP 8 // 8% -#define AUTOTUNE_FIXED_WING_INCREASE_STEP 5 // 5% #define AUTOTUNE_FIXED_WING_MIN_FF 10 #define AUTOTUNE_FIXED_WING_MAX_FF 200 +#define AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE 40 +#define AUTOTUNE_FIXED_WING_MIN_YAW_RATE 10 +#define AUTOTUNE_FIXED_WING_MAX_RATE 720 +#define AUTOTUNE_FIXED_WING_CONVERGENCE_RATE 10 +#define AUTOTUNE_FIXED_WING_SAMPLE_INTERVAL 20 // ms +#define AUTOTUNE_FIXED_WING_SAMPLES 1000 // Use averagea over the last 20 seconds +#define AUTOTUNE_FIXED_WING_MIN_SAMPLES 250 // Start updating tune after 5 seconds -PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 2); PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, - .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_min_stick = SETTING_FW_AUTOTUNE_MIN_STICK_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, + .fw_p_to_d_gain = SETTING_FW_AUTOTUNE_P_TO_D_GAIN_DEFAULT, + .fw_rate_adjustment = SETTING_FW_AUTOTUNE_RATE_ADJUSTMENT_DEFAULT, + .fw_max_rate_deflection = SETTING_FW_AUTOTUNE_MAX_RATE_DEFLECTION_DEFAULT, ); typedef enum { DEMAND_TOO_LOW, DEMAND_UNDERSHOOT, DEMAND_OVERSHOOT, + TUNE_UPDATED, } pidAutotuneState_e; typedef struct { - pidAutotuneState_e state; - timeMs_t stateEnterTime; - - bool pidSaturated; float gainP; float gainI; + float gainD; float gainFF; + float rate; + float initialRate; + float absDesiredRateAccum; + float absReachedRateAccum; + float absPidOutputAccum; + uint32_t updateCount; } pidAutotuneData_t; #define AUTOTUNE_SAVE_PERIOD 5000 // Save interval is 5 seconds - when we turn off autotune we'll restore values from previous update at most 5 sec ago @@ -89,8 +100,9 @@ void autotuneUpdateGains(pidAutotuneData_t * data) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pidBankMutable()->pid[axis].P = lrintf(data[axis].gainP); pidBankMutable()->pid[axis].I = lrintf(data[axis].gainI); - pidBankMutable()->pid[axis].D = 0; + pidBankMutable()->pid[axis].D = lrintf(data[axis].gainD); pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF); + ((controlRateConfig_t *)currentControlRateProfile)->stabilized.rates[axis] = lrintf(data[axis].rate/10.0f); } schedulePidGainsUpdate(); } @@ -114,10 +126,14 @@ void autotuneStart(void) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { tuneCurrent[axis].gainP = pidBank()->pid[axis].P; tuneCurrent[axis].gainI = pidBank()->pid[axis].I; + tuneCurrent[axis].gainD = pidBank()->pid[axis].D; tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF; - tuneCurrent[axis].pidSaturated = false; - tuneCurrent[axis].stateEnterTime = millis(); - tuneCurrent[axis].state = DEMAND_TOO_LOW; + tuneCurrent[axis].rate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; + tuneCurrent[axis].initialRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; + tuneCurrent[axis].absDesiredRateAccum = 0; + tuneCurrent[axis].absReachedRateAccum = 0; + tuneCurrent[axis].absPidOutputAccum = 0; + tuneCurrent[axis].updateCount = 0; } memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT); @@ -161,73 +177,87 @@ static void blackboxLogAutotuneEvent(adjustmentFunction_e adjustmentFunction, in #if defined(USE_AUTOTUNE_FIXED_WING) -void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput) +void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRate, float reachedRate, float pidOutput) { + float maxRateSetting = tuneCurrent[axis].rate; + float gainFF = tuneCurrent[axis].gainFF; + float maxDesiredRate = maxRateSetting; + + const float pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit; + const float absDesiredRate = fabsf(desiredRate); + const float absReachedRate = fabsf(reachedRate); + const float absPidOutput = fabsf(pidOutput); + const bool correctDirection = (desiredRate>0) == (reachedRate>0); + float rateFullStick; + + bool gainsUpdated = false; + bool ratesUpdated = false; + const timeMs_t currentTimeMs = millis(); - const float absDesiredRateDps = fabsf(desiredRateDps); - float maxDesiredRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; - pidAutotuneState_e newState; + static timeMs_t previousSampleTimeMs = 0; + const timeDelta_t timeSincePreviousSample = currentTimeMs - previousSampleTimeMs; - // Use different max desired rate in ANGLE for pitch and roll - // Maximum reasonable error in ANGLE mode is 200% of angle inclination (control dublet), but we are conservative and tune for control singlet. - if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { + // Use different max rate in ANLGE mode + if (FLIGHT_MODE(ANGLE_MODE)) { float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER; - maxDesiredRate = MIN(maxDesiredRate, maxDesiredRateInAngleMode); + maxDesiredRate = MIN(maxRateSetting, maxDesiredRateInAngleMode); } - if (fabsf(pidOutput) >= pidProfile()->pidSumLimit) { - // If we have saturated the pid output by P+FF don't increase the gain - tuneCurrent[axis].pidSaturated = true; - } + const float stickInput = absDesiredRate / maxDesiredRate; - if (absDesiredRateDps < (pidAutotuneConfig()->fw_max_rate_threshold / 100.0f) * maxDesiredRate) { - // We can make decisions only when we are demanding at least 50% of max configured rate - newState = DEMAND_TOO_LOW; - } - else if (fabsf(reachedRateDps) > absDesiredRateDps) { - newState = DEMAND_OVERSHOOT; - } - else { - newState = DEMAND_UNDERSHOOT; - } + if ((stickInput > (pidAutotuneConfig()->fw_min_stick / 100.0f)) && correctDirection && (timeSincePreviousSample >= AUTOTUNE_FIXED_WING_SAMPLE_INTERVAL)) { + // Record values every 20ms and calculate moving average over samples + tuneCurrent[axis].updateCount++; + tuneCurrent[axis].absDesiredRateAccum += (absDesiredRate - tuneCurrent[axis].absDesiredRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES); + tuneCurrent[axis].absReachedRateAccum += (absReachedRate - tuneCurrent[axis].absReachedRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES); + tuneCurrent[axis].absPidOutputAccum += (absPidOutput - tuneCurrent[axis].absPidOutputAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);; - if (newState != tuneCurrent[axis].state) { - const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime; - bool gainsUpdated = false; + if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) { + if (pidAutotuneConfig()->fw_rate_adjustment != FIXED && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode + + // Target 80% control surface deflection to leave some room for P and I to work + float pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit; - switch (tuneCurrent[axis].state) { - case DEMAND_TOO_LOW: - break; - case DEMAND_OVERSHOOT: - if (stateTimeMs >= pidAutotuneConfig()->fw_overshoot_time) { - tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f; - if (tuneCurrent[axis].gainFF < AUTOTUNE_FIXED_WING_MIN_FF) { - tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MIN_FF; - } - gainsUpdated = true; + // Theoretically achievable rate with target deflection + rateFullStick = pidSumTarget / tuneCurrent[axis].absPidOutputAccum * tuneCurrent[axis].absReachedRateAccum; + + // Rate update + if (rateFullStick > (maxRateSetting + 10.0f)) { + maxRateSetting += 10.0f; + } else if (rateFullStick < (maxRateSetting - 10.0f)) { + maxRateSetting -= 10.0f; } - break; - case DEMAND_UNDERSHOOT: - if (stateTimeMs >= pidAutotuneConfig()->fw_undershoot_time && !tuneCurrent[axis].pidSaturated) { - tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f; - if (tuneCurrent[axis].gainFF > AUTOTUNE_FIXED_WING_MAX_FF) { - tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MAX_FF; - } - gainsUpdated = true; - } - break; + + // Constrain to safe values + uint16_t minRate = (axis == FD_YAW) ? AUTOTUNE_FIXED_WING_MIN_YAW_RATE : AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE; + uint16_t maxRate = (pidAutotuneConfig()->fw_rate_adjustment == AUTO) ? AUTOTUNE_FIXED_WING_MAX_RATE : MAX(tuneCurrent[axis].initialRate, minRate); + tuneCurrent[axis].rate = constrainf(maxRateSetting, minRate, maxRate); + ratesUpdated = true; + } + + // Update FF towards value needed to achieve current rate target + gainFF += (tuneCurrent[axis].absPidOutputAccum / tuneCurrent[axis].absReachedRateAccum * FP_PID_RATE_FF_MULTIPLIER - gainFF) * (AUTOTUNE_FIXED_WING_CONVERGENCE_RATE / 100.0f); + tuneCurrent[axis].gainFF = constrainf(gainFF, AUTOTUNE_FIXED_WING_MIN_FF, AUTOTUNE_FIXED_WING_MAX_FF); + gainsUpdated = true; } - if (gainsUpdated) { - // Set P-gain to 10% of FF gain (quite agressive - FIXME) - tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f); + // Reset timer + previousSampleTimeMs = currentTimeMs; + } - // Set integrator gain to reach the same response as FF gain in 0.667 second - tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER; - tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f); - autotuneUpdateGains(tuneCurrent); + if (gainsUpdated) { + // Set P-gain to 10% of FF gain (quite agressive - FIXME) + tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f); - switch (axis) { + // Set D-gain relative to P-gain (0 for now) + tuneCurrent[axis].gainD = tuneCurrent[axis].gainP * (pidAutotuneConfig()->fw_p_to_d_gain / 100.0f); + + // Set integrator gain to reach the same response as FF gain in 0.667 second + tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER; + tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f); + autotuneUpdateGains(tuneCurrent); + + switch (axis) { case FD_ROLL: blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF); break; @@ -239,13 +269,30 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa case FD_YAW: blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF); break; - } + } + gainsUpdated = false; + } + + if (ratesUpdated) { + autotuneUpdateGains(tuneCurrent); + + switch (axis) { + case FD_ROLL: + blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_RATE, tuneCurrent[axis].rate); + break; + + case FD_PITCH: + blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_RATE, tuneCurrent[axis].rate); + break; + + case FD_YAW: + blackboxLogAutotuneEvent(ADJUSTMENT_YAW_RATE, tuneCurrent[axis].rate); + break; } - // Change state and reset saturation flag - tuneCurrent[axis].state = newState; - tuneCurrent[axis].stateEnterTime = currentTimeMs; - tuneCurrent[axis].pidSaturated = false; + // Debug + DEBUG_SET(DEBUG_AUTOTUNE, axis, rateFullStick); + ratesUpdated = false; } } #endif diff --git a/src/main/flight/power_limits.c b/src/main/flight/power_limits.c new file mode 100644 index 0000000000..1ce770aca4 --- /dev/null +++ b/src/main/flight/power_limits.c @@ -0,0 +1,292 @@ +/* + * 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 + +#include "platform.h" +FILE_COMPILE_FOR_SPEED + +#if defined(USE_POWER_LIMITS) + +#include "flight/power_limits.h" + +#include "build/debug.h" + +#include "common/filter.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + +#include "drivers/time.h" + +#include "fc/settings.h" + +#include "rx/rx.h" + +#include "sensors/battery.h" + +#define LIMITING_THR_FILTER_TCONST 50 + +PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 0); + +PG_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, + .continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT, // dA + .burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT, // dA + .burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT, // dS + .burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT, // dS +#ifdef USE_ADC + .continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT, // dW + .burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT, // dW + .burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT, // dS + .burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT, // dS +#endif + .piP = SETTING_LIMIT_PI_P_DEFAULT, + .piI = SETTING_LIMIT_PI_I_DEFAULT, + .attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT, // Hz +); + +static float burstCurrentReserve; // cA.µs +static float burstCurrentReserveMax; // cA.µs +static float burstCurrentReserveFalldown; // cA.µs +static int32_t activeCurrentLimit; // cA +static float currentThrAttnIntegrator = 0; +static pt1Filter_t currentThrAttnFilter; +static pt1Filter_t currentThrLimitingBaseFilter; +static bool wasLimitingCurrent = false; + +#ifdef USE_ADC +static float burstPowerReserve; // cW.µs +static float burstPowerReserveMax; // cW.µs +static float burstPowerReserveFalldown; // cW.µs +static int32_t activePowerLimit; // cW +static float powerThrAttnIntegrator = 0; +static pt1Filter_t powerThrAttnFilter; +static pt1Filter_t powerThrLimitingBaseFilter; +static bool wasLimitingPower = false; +#endif + +void powerLimiterInit(void) { + if (powerLimitsConfig()->burstCurrent < powerLimitsConfig()->continuousCurrent) { + powerLimitsConfigMutable()->burstCurrent = powerLimitsConfig()->continuousCurrent; + } + + activeCurrentLimit = powerLimitsConfig()->burstCurrent; + + uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent; + burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentTime * 1e6; + burstCurrentReserveFalldown = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentFalldownTime * 1e6; + + pt1FilterInit(¤tThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0); + pt1FilterInitRC(¤tThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); + +#ifdef USE_ADC + if (powerLimitsConfig()->burstPower < powerLimitsConfig()->continuousPower) { + powerLimitsConfigMutable()->burstPower = powerLimitsConfig()->continuousPower; + } + + activePowerLimit = powerLimitsConfig()->burstPower; + + uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower; + burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * powerLimitsConfig()->burstPowerTime * 1e6; + burstPowerReserveFalldown = powerBurstOverContinuous * powerLimitsConfig()->burstPowerFalldownTime * 1e6; + + pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0); + pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0); +#endif +} + +static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, uint32_t burstLimit, float *burstReserve, float burstReserveFalldown, float burstReserveMax, timeDelta_t timeDelta) { + int32_t continuousDiff = value - continuousLimit * 10; + float spentReserveChunk = continuousDiff * timeDelta; + *burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax); + + if (powerLimitsConfig()->burstCurrentFalldownTime) { + return scaleRangef(MIN(*burstReserve, burstReserveFalldown), 0, burstReserveFalldown, continuousLimit, burstLimit) * 10; + } + + return (*burstReserve ? burstLimit : continuousLimit) * 10; +} + +void currentLimiterUpdate(timeDelta_t timeDelta) { + activeCurrentLimit = calculateActiveLimit(getAmperage(), + powerLimitsConfig()->continuousCurrent, powerLimitsConfig()->burstCurrent, + &burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax, + timeDelta); +} + +#ifdef USE_ADC +void powerLimiterUpdate(timeDelta_t timeDelta) { + activePowerLimit = calculateActiveLimit(getPower(), + powerLimitsConfig()->continuousPower, powerLimitsConfig()->burstPower, + &burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax, + timeDelta); +} +#endif + +void powerLimiterApply(int16_t *throttleCommand) { + +#ifdef USE_ADC + if (!activeCurrentLimit && !activePowerLimit) { + return; + } +#else + if (!activeCurrentLimit) { + return; + } +#endif + + static timeUs_t lastCallTimestamp = 0; + timeUs_t currentTimeUs = micros(); + timeDelta_t callTimeDelta = cmpTimeUs(currentTimeUs, lastCallTimestamp); + + int16_t throttleBase; + int16_t currentThrottleCommand; +#ifdef USE_ADC + int16_t powerThrottleCommand; +#endif + + int16_t current = getAmperageSample(); +#ifdef USE_ADC + uint16_t voltage = getVBatSample(); + int32_t power = (int32_t)voltage * current / 100; +#endif + + // Current limiting + int32_t overCurrent = current - activeCurrentLimit; + + if (lastCallTimestamp) { + currentThrAttnIntegrator = constrainf(currentThrAttnIntegrator + overCurrent * powerLimitsConfig()->piI * callTimeDelta * 2e-7, 0, PWM_RANGE_MAX - PWM_RANGE_MIN); + } + + float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3; + + uint16_t currentThrAttn = lrintf(pt1FilterApply3(¤tThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6)); + + throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(¤tThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand; + uint16_t currentThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - currentThrAttn); + + if (activeCurrentLimit && currentThrAttned < *throttleCommand) { + if (!wasLimitingCurrent && getAmperage() >= activeCurrentLimit) { + pt1FilterReset(¤tThrLimitingBaseFilter, *throttleCommand); + wasLimitingCurrent = true; + } + + currentThrottleCommand = currentThrAttned; + } else { + wasLimitingCurrent = false; + currentThrAttnIntegrator = 0; + pt1FilterReset(¤tThrAttnFilter, 0); + + currentThrottleCommand = *throttleCommand; + } + +#ifdef USE_ADC + // Power limiting + int32_t overPower = power - activePowerLimit; + + if (lastCallTimestamp) { + powerThrAttnIntegrator = constrainf(powerThrAttnIntegrator + overPower * powerLimitsConfig()->piI * callTimeDelta / voltage * 2e-5, 0, PWM_RANGE_MAX - PWM_RANGE_MIN); + } + + float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1; + + uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6)); + + throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand; + uint16_t powerThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - powerThrAttn); + + if (activePowerLimit && powerThrAttned < *throttleCommand) { + if (!wasLimitingPower && getPower() >= activePowerLimit) { + pt1FilterReset(&powerThrLimitingBaseFilter, *throttleCommand); + wasLimitingPower = true; + } + + powerThrottleCommand = powerThrAttned; + } else { + wasLimitingPower = false; + powerThrAttnIntegrator = 0; + pt1FilterReset(&powerThrAttnFilter, 0); + + powerThrottleCommand = *throttleCommand; + } + + *throttleCommand = MIN(currentThrottleCommand, powerThrottleCommand); +#else + *throttleCommand = currentThrottleCommand; +#endif + + lastCallTimestamp = currentTimeUs; +} + +bool powerLimiterIsLimiting(void) { +#ifdef USE_ADC + return wasLimitingPower || wasLimitingCurrent; +#else + return wasLimitingCurrent; +#endif +} + +bool powerLimiterIsLimitingCurrent(void) { + return wasLimitingCurrent; +} + +#ifdef USE_ADC +bool powerLimiterIsLimitingPower(void) { + return wasLimitingPower; +} +#endif + +// returns seconds +float powerLimiterGetRemainingBurstTime(void) { + uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent; + float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7; + +#ifdef USE_ADC + uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower; + float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7; + + if (!powerLimitsConfig()->continuousCurrent) { + return remainingPowerBurstTime; + } + + if (!powerLimitsConfig()->continuousPower) { + return remainingCurrentBurstTime; + } + + return MIN(remainingCurrentBurstTime, remainingPowerBurstTime); +#else + return remainingCurrentBurstTime; +#endif +} + +// returns cA +uint16_t powerLimiterGetActiveCurrentLimit(void) { + return activeCurrentLimit; +} + +#ifdef USE_ADC +// returns cW +uint16_t powerLimiterGetActivePowerLimit(void) { + return activePowerLimit; +} +#endif + +#endif diff --git a/src/main/flight/power_limits.h b/src/main/flight/power_limits.h new file mode 100644 index 0000000000..b82b938c58 --- /dev/null +++ b/src/main/flight/power_limits.h @@ -0,0 +1,66 @@ +/* + * This file is part of iNav + * + * iNav free software. You can redistribute + * this software and/or modify this software 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 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 software. + * + * If not, see . + */ + +#pragma once + +#include +#include + +#include + +#include "config/parameter_group.h" + +#if defined(USE_POWER_LIMITS) + +typedef struct { + uint16_t continuousCurrent; // dA + uint16_t burstCurrent; // dA + uint16_t burstCurrentTime; // ds + uint16_t burstCurrentFalldownTime; // ds + +#ifdef USE_ADC + uint16_t continuousPower; // dW + uint16_t burstPower; // dW + uint16_t burstPowerTime; // ds + uint16_t burstPowerFalldownTime; // ds +#endif + + uint16_t piP; + uint16_t piI; + + float attnFilterCutoff; // Hz +} powerLimitsConfig_t; + +PG_DECLARE(powerLimitsConfig_t, powerLimitsConfig); + +void powerLimiterInit(void); +void currentLimiterUpdate(timeDelta_t timeDelta); +void powerLimiterUpdate(timeDelta_t timeDelta); +void powerLimiterApply(int16_t *throttleCommand); +bool powerLimiterIsLimiting(void); +bool powerLimiterIsLimitingCurrent(void); +float powerLimiterGetRemainingBurstTime(void); // returns seconds +uint16_t powerLimiterGetActiveCurrentLimit(void); // returns cA +#ifdef USE_ADC +uint16_t powerLimiterGetActivePowerLimit(void); // returns cW +bool powerLimiterIsLimitingPower(void); +#endif + +#endif diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 93cd11b9d1..b8e5a20160 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -29,10 +29,13 @@ #include "common/utils.h" #include "common/axis.h" +#include "build/debug.h" +#include "scheduler/scheduler.h" #include "config/parameter_group_ids.h" #include "drivers/sensor.h" #include "drivers/accgyro/accgyro_bno055.h" +#include "drivers/accgyro/accgyro_bno055_serial.h" #include "fc/settings.h" @@ -94,6 +97,17 @@ void secondaryImuInit(void) if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); + + if (secondaryImuState.active) { + rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(10)); + } + + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + secondaryImuState.active = bno055SerialInit(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); + + if (secondaryImuState.active) { + rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(50)); + } } if (!secondaryImuState.active) { @@ -102,19 +116,7 @@ void secondaryImuInit(void) } -void taskSecondaryImu(timeUs_t currentTimeUs) -{ - if (!secondaryImuState.active) - { - return; - } - /* - * Secondary IMU works in decidegrees - */ - UNUSED(currentTimeUs); - - bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); - +void secondaryImuProcess(void) { //Apply mag declination secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination; @@ -141,17 +143,6 @@ void taskSecondaryImu(timeUs_t currentTimeUs) 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); @@ -162,8 +153,56 @@ void taskSecondaryImu(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination); } +void taskSecondaryImu(timeUs_t currentTimeUs) +{ + static uint8_t tick = 0; + tick++; + + if (!secondaryImuState.active) + { + return; + } + /* + * Secondary IMU works in decidegrees + */ + UNUSED(currentTimeUs); + + if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { + bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); + secondaryImuProcess(); + + /* + * Every 2 seconds fetch current calibration state + */ + if (tick == 20) + { + secondaryImuState.calibrationStatus = bno055GetCalibStat(); + tick = 0; + } + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + /* + * Every 2 seconds fetch current calibration state + */ + if (tick == 100) + { + bno055SerialGetCalibStat(); + tick = 0; + } else { + bno055SerialFetchEulerAngles(); + } + } +} + void secondaryImuFetchCalibration(void) { - bno055CalibrationData_t calibrationData = bno055GetCalibrationData(); + bno055CalibrationData_t calibrationData; + + if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { + calibrationData = bno055GetCalibrationData(); + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + calibrationData = bno055SerialGetCalibrationData(); + } else { + return; + } secondaryImuConfigMutable()->calibrationOffsetAcc[X] = calibrationData.offset[ACC][X]; secondaryImuConfigMutable()->calibrationOffsetAcc[Y] = calibrationData.offset[ACC][Y]; diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index b674bc9c93..e7414a48c7 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -28,10 +28,12 @@ #include "common/time.h" #include "sensors/sensors.h" #include "drivers/accgyro/accgyro_bno055.h" +#include "drivers/accgyro/accgyro_bno055_serial.h" typedef enum { - SECONDARY_IMU_NONE = 0, - SECONDARY_IMU_BNO055 = 1, + SECONDARY_IMU_NONE = 0, + SECONDARY_IMU_BNO055 = 1, + SECONDARY_IMU_BNO055_SERIAL = 2, } secondaryImuType_e; typedef struct secondaryImuConfig_s { @@ -60,6 +62,7 @@ extern secondaryImuState_t secondaryImuState; PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig); +void secondaryImuProcess(void); void secondaryImuInit(void); void taskSecondaryImu(timeUs_t currentTimeUs); void secondaryImuFetchCalibration(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index df58e6f649..c3a242497c 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -40,6 +40,7 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -51,11 +52,13 @@ #include "flight/pid.h" #include "flight/servos.h" +#include "io/gps.h" + #include "rx/rx.h" #include "sensors/gyro.h" -PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 2); PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .servoCenterPulse = SETTING_SERVO_CENTER_PULSE_DEFAULT, @@ -63,7 +66,8 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .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 + .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT, + .servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT ); PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); @@ -83,7 +87,7 @@ void pgResetFn_customServoMixers(servoMixer_t *instance) } } -PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 3); void pgResetFn_servoParams(servoParam_t *instance) { @@ -93,7 +97,7 @@ void pgResetFn_servoParams(servoParam_t *instance) .max = DEFAULT_SERVO_MAX, .middle = DEFAULT_SERVO_MIDDLE, .rate = 100 - ); + ); } } @@ -113,6 +117,9 @@ static bool servoFilterIsSet; static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS]; static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES]; +STATIC_FASTRAM pt1Filter_t rotRateFilter; +STATIC_FASTRAM pt1Filter_t targetRateFilter; + int16_t getFlaperonDirection(uint8_t servoPin) { if (servoPin == SERVO_FLAPPERON_2) { @@ -196,7 +203,7 @@ static void filterServos(void) // Initialize servo lowpass filter (servos are calculated at looptime rate) if (!servoFilterIsSet) { for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - biquadFilterInitLPF(&servoFilter[i], servoConfig()->servo_lowpass_freq, gyro.targetLooptime); + biquadFilterInitLPF(&servoFilter[i], servoConfig()->servo_lowpass_freq, getLooptime()); biquadFilterReset(&servoFilter[i], servo[i]); } servoFilterIsSet = true; @@ -385,27 +392,31 @@ typedef enum { AUTOTRIM_DONE, } servoAutotrimState_e; -void processServoAutotrim(void) +void processServoAutotrimMode(void) { static servoAutotrimState_e trimState = AUTOTRIM_IDLE; static timeMs_t trimStartedAt; static int16_t servoMiddleBackup[MAX_SUPPORTED_SERVOS]; static int32_t servoMiddleAccum[MAX_SUPPORTED_SERVOS]; - static int32_t servoMiddleAccumCount; + static int32_t servoMiddleAccumCount[MAX_SUPPORTED_SERVOS]; if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { switch (trimState) { case AUTOTRIM_IDLE: if (ARMING_FLAG(ARMED)) { - // We are activating servo trim - backup current middles and prepare to average the data - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoMiddleBackup[servoIndex] = servoParams(servoIndex)->middle; - servoMiddleAccum[servoIndex] = 0; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { + servoMiddleBackup[target] = servoParams(target)->middle; + servoMiddleAccum[target] = 0; + servoMiddleAccumCount[target] = 0; + } + } } - trimStartedAt = millis(); - servoMiddleAccumCount = 0; trimState = AUTOTRIM_COLLECTING; } else { @@ -415,15 +426,26 @@ void processServoAutotrim(void) case AUTOTRIM_COLLECTING: if (ARMING_FLAG(ARMED)) { - servoMiddleAccumCount++; - - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoMiddleAccum[servoIndex] += servo[servoIndex]; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { + servoMiddleAccum[target] += servo[target]; + servoMiddleAccumCount[target]++; + } + } } if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) { - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoParamsMutable(servoIndex)->middle = servoMiddleAccum[servoIndex] / servoMiddleAccumCount; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { + servoParamsMutable(target)->middle = servoMiddleAccum[target] / servoMiddleAccumCount[target]; + } + } } trimState = AUTOTRIM_SAVE_PENDING; pidResetErrorAccumulators(); //Reset Iterm since new midpoints override previously acumulated errors @@ -449,8 +471,14 @@ void processServoAutotrim(void) else { // We are deactivating servo trim - restore servo midpoints if (trimState == AUTOTRIM_SAVE_PENDING) { - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoParamsMutable(servoIndex)->middle = servoMiddleBackup[servoIndex]; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { + servoParamsMutable(target)->middle = servoMiddleBackup[target]; + } + } } } @@ -458,6 +486,90 @@ void processServoAutotrim(void) } } +#define SERVO_AUTOTRIM_FILTER_CUTOFF 1 // LPF cutoff frequency +#define SERVO_AUTOTRIM_CENTER_MIN 1300 +#define SERVO_AUTOTRIM_CENTER_MAX 1700 +#define SERVO_AUTOTRIM_UPDATE_SIZE 5 + +void processContinuousServoAutotrim(const float dT) +{ + static timeMs_t lastUpdateTimeMs; + static servoAutotrimState_e trimState = AUTOTRIM_IDLE; + static uint32_t servoMiddleUpdateCount; + + const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF)); + const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT); + const float targetRateMagnitude = getTotalRateTarget(); + const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, targetRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT); + + if (ARMING_FLAG(ARMED)) { + trimState = AUTOTRIM_COLLECTING; + if ((millis() - lastUpdateTimeMs) > 500) { + const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit); + const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit; + const bool planeIsFlyingLevel = calculateCosTiltAngle() >= 0.878153032f; + if ( + planeIsFlyingStraight && + noRotationCommanded && + planeIsFlyingLevel && + !FLIGHT_MODE(MANUAL_MODE) && + isGPSHeadingValid() // TODO: proper flying detection + ) { + // Plane is flying straight and level: trim servos + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + // For each stabilized axis, add 5 units of I-term to all associated servo midpoints + const float axisIterm = getAxisIterm(axis); + if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE) { + const int8_t ItermUpdate = axisIterm > 0.0f ? SERVO_AUTOTRIM_UPDATE_SIZE : -SERVO_AUTOTRIM_UPDATE_SIZE; + for (int i = 0; i < servoRuleCount; i++) { +#ifdef USE_PROGRAMMING_FRAMEWORK + if (!logicConditionGetValue(currentServoMixer[i].conditionId)) { + continue; + } +#endif + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { + // Convert axis I-term to servo PWM and add to midpoint + const float mixerRate = currentServoMixer[i].rate / 100.0f; + const float servoRate = servoParams(target)->rate / 100.0f; + servoParamsMutable(target)->middle += ItermUpdate * mixerRate * servoRate; + servoParamsMutable(target)->middle = constrain(servoParamsMutable(target)->middle, SERVO_AUTOTRIM_CENTER_MIN, SERVO_AUTOTRIM_CENTER_MAX); + } + } + pidReduceErrorAccumulators(ItermUpdate, axis); + } + } + servoMiddleUpdateCount++; + } + // Reset timer + lastUpdateTimeMs = millis(); + } + } else if (trimState == AUTOTRIM_COLLECTING) { + // We have disarmed, save midpoints to EEPROM + writeEEPROM(); + trimState = AUTOTRIM_IDLE; + } + + // Debug + DEBUG_SET(DEBUG_AUTOTRIM, 0, servoParams(2)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 2, servoParams(3)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 4, servoParams(4)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 6, servoParams(5)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount); + DEBUG_SET(DEBUG_AUTOTRIM, 3, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), targetRateMagnitudeFiltered)); + DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]); + DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]); +} + +void processServoAutotrim(const float dT) { + if (feature(FEATURE_FW_AUTOTRIM)) { + processContinuousServoAutotrim(dT); + } else { + processServoAutotrimMode(); + } +} + bool isServoOutputEnabled(void) { return servoOutputEnabled; diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 3b536ad3d6..841415ea23 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -138,6 +138,8 @@ typedef struct servoConfig_s { uint16_t flaperon_throw_offset; uint8_t servo_protocol; // See servoProtocolType_e uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed + uint8_t servo_autotrim_rotation_limit; // Max rotation for servo midpoints to be updated + uint8_t servo_autotrim_iterm_threshold; // How much of the Iterm is carried over to the servo midpoints on each update } servoConfig_t; PG_DECLARE(servoConfig_t, servoConfig); diff --git a/src/main/flight/smith_predictor.c b/src/main/flight/smith_predictor.c new file mode 100644 index 0000000000..271c088add --- /dev/null +++ b/src/main/flight/smith_predictor.c @@ -0,0 +1,70 @@ +/* + * 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/. + * + * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight + * + */ + +#include "platform.h" +#ifdef USE_SMITH_PREDICTOR + +#include +#include "common/axis.h" +#include "common/utils.h" +#include "flight/smith_predictor.h" +#include "build/debug.h" + +FUNCTION_COMPILE_FOR_SPEED +float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample) { + UNUSED(axis); + if (predictor->enabled) { + predictor->data[predictor->idx] = sample; + + predictor->idx++; + if (predictor->idx > predictor->samples) { + predictor->idx = 0; + } + + // filter the delayed data to help reduce the overall noise this prediction adds + float delayed = pt1FilterApply(&predictor->smithPredictorFilter, predictor->data[predictor->idx]); + float delayCompensatedSample = predictor->smithPredictorStrength * (sample - delayed); + + sample += delayCompensatedSample; + } + return sample; +} + +FUNCTION_COMPILE_FOR_SIZE +void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime) { + if (delay > 0.1) { + predictor->enabled = true; + predictor->samples = (delay * 1000) / looptime; + predictor->idx = 0; + predictor->smithPredictorStrength = strength; + pt1FilterInit(&predictor->smithPredictorFilter, filterLpfHz, looptime * 1e-6f); + } else { + predictor->enabled = false; + } +} + +#endif \ No newline at end of file diff --git a/src/main/flight/smith_predictor.h b/src/main/flight/smith_predictor.h new file mode 100644 index 0000000000..1d8ebcb13b --- /dev/null +++ b/src/main/flight/smith_predictor.h @@ -0,0 +1,45 @@ +/* + * 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/. + * + * This code is a derivative of work done in EmuFlight Project https://github.com/emuflight/EmuFlight + * + */ + +#pragma once + +#include +#include "common/filter.h" + +#define MAX_SMITH_SAMPLES 64 + +typedef struct smithPredictor_s { + bool enabled; + uint8_t samples; + uint8_t idx; + float data[MAX_SMITH_SAMPLES + 1]; + pt1Filter_t smithPredictorFilter; + float smithPredictorStrength; +} smithPredictor_t; + +float applySmithPredictor(uint8_t axis, smithPredictor_t *predictor, float sample); +void smithPredictorInit(smithPredictor_t *predictor, float delay, float strength, uint16_t filterLpfHz, uint32_t looptime); \ No newline at end of file diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index e24216cda0..f19797afc4 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -45,6 +45,13 @@ #include "config/feature.h" +#ifdef USE_DSHOT +#include "drivers/pwm_output.h" +#include "fc/fc_core.h" +#include "flight/mixer.h" +static timeUs_t lastDshotBeeperCommandTimeUs; +#endif + #include "io/beeper.h" #define MAX_MULTI_BEEPS 20 //size limit for 'beep_multiBeeps[]' @@ -331,6 +338,15 @@ void beeperUpdate(timeUs_t currentTimeUs) } if (!beeperIsOn) { +#ifdef USE_DSHOT + if (isMotorProtocolDshot() && !areMotorsRunning() && beeperConfig()->dshot_beeper_enabled + && currentTimeUs - lastDshotBeeperCommandTimeUs > getDShotBeaconGuardDelayUs()) + { + lastDshotBeeperCommandTimeUs = currentTimeUs; + sendDShotCommand(beeperConfig()->dshot_beeper_tone); + } +#endif + beeperIsOn = 1; if (currentBeeperEntry->sequence[beeperPos] != 0) { if (!(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1)))) @@ -407,3 +423,25 @@ int beeperTableEntryCount(void) { return (int)BEEPER_TABLE_ENTRY_COUNT; } + +#ifdef USE_DSHOT +timeUs_t getDShotBeaconGuardDelayUs(void) { + // Based on Digital_Cmd_Spec.txt - all delays have 100ms added to ensure that the minimum time has passed. + switch (beeperConfig()->dshot_beeper_tone) { + case 1: + case 2: + return 260000 + 100000; + case 3: + case 4: + return 280000 + 100000; + case 5: + default: + return 1020000 + 100000; + } +} + +timeUs_t getLastDshotBeeperCommandTimeUs(void) +{ + return lastDshotBeeperCommandTimeUs; +} +#endif diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 0fcc8249dd..38f38b236e 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -60,3 +60,7 @@ uint32_t getArmingBeepTimeMicros(void); beeperMode_e beeperModeForTableIndex(int idx); const char *beeperNameForTableIndex(int idx); int beeperTableEntryCount(void); +#ifdef USE_DSHOT +timeUs_t getDShotBeaconGuardDelayUs(void); +timeUs_t getLastDshotBeeperCommandTimeUs(void); +#endif diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 8fd74fc8b6..022d85fa36 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -935,7 +935,7 @@ void ledStripUpdate(timeUs_t currentTimeUs) for (timId_e timId = 0; timId < timTimerCount; timId++) { // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. // max delay is limited to 5s - int32_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]); + timeDelta_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]); if (delta < 0 && delta > -LED_STRIP_MS(5000)) continue; // not ready yet timActive |= 1 << timId; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ab7c286d3f..f0c4af8c24 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -85,10 +85,11 @@ FILE_COMPILE_FOR_SPEED #include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" +#include "flight/power_limits.h" #include "flight/rth_estimator.h" -#include "flight/wind_estimator.h" #include "flight/secondary_imu.h" #include "flight/servos.h" +#include "flight/wind_estimator.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -144,7 +145,7 @@ FILE_COMPILE_FOR_SPEED #define OSD_CENTER_LEN(x) ((osdDisplayPort->cols - x) / 2) #define OSD_CENTER_S(s) OSD_CENTER_LEN(strlen(s)) -#define OSD_MIN_FONT_VERSION 1 +#define OSD_MIN_FONT_VERSION 2 static unsigned currentLayout = 0; static int layoutOverride = -1; @@ -156,8 +157,8 @@ static float GForce, GForceAxis[XYZ_AXIS_COUNT]; typedef struct statistic_s { uint16_t max_speed; uint16_t min_voltage; // /100 - int16_t max_current; // /100 - int16_t max_power; // /100 + int16_t max_current; + int32_t max_power; int16_t min_rssi; int16_t min_lq; // for CRSF int16_t min_rssi_dbm; // for CRSF @@ -193,7 +194,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, 15); +PG_REGISTER_WITH_RESET_TEMPLATE(osdConfig_t, osdConfig, PG_OSD_CONFIG, 2); PG_REGISTER_WITH_RESET_FN(osdLayoutsConfig_t, osdLayoutsConfig, PG_OSD_LAYOUTS_CONFIG, 0); static int digitCount(int32_t value) @@ -534,18 +535,24 @@ static uint16_t osdConvertRSSI(void) return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); } -static uint16_t osdGetLQ(void) +static uint16_t osdGetCrsfLQ(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; + int16_t displayedLQ; + switch (osdConfig()->crsf_lq_format) { + case OSD_CRSF_LQ_TYPE1: + case OSD_CRSF_LQ_TYPE2: + displayedLQ = statsLQ; + break; + case OSD_CRSF_LQ_TYPE3: + displayedLQ = rxLinkStatistics.rfMode >= 2 ? scaledLQ : statsLQ; + break; } + return displayedLQ; } -static uint16_t osdGetdBm(void) +static int16_t osdGetCrsfdBm(void) { return rxLinkStatistics.uplinkRSSI; } @@ -737,6 +744,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR); case ARMING_DISABLED_NO_PREARM: return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); + case ARMING_DISABLED_DSHOT_BEEPER: + return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; @@ -802,6 +811,15 @@ static const char * osdFailsafeInfoMessage(void) } return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); } +#if defined(USE_SAFE_HOME) +static const char * divertingToSafehomeMessage(void) +{ + if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) { + return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); + } + return NULL; +} +#endif static const char * navigationStateMessage(void) { @@ -837,6 +855,11 @@ static const char * navigationStateMessage(void) return OSD_MESSAGE_STR(OSD_MSG_LANDING); case MW_NAV_STATE_HOVER_ABOVE_HOME: if (STATE(FIXED_WING_LEGACY)) { +#if defined(USE_SAFE_HOME) + if (safehome_applied) { + return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME); + } +#endif return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME); } return OSD_MESSAGE_STR(OSD_MSG_HOVERING); @@ -899,9 +922,15 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t if (autoThr && navigationIsControllingThrottle()) { buff[0] = SYM_AUTO_THR0; buff[1] = SYM_AUTO_THR1; - if (isFixedWingAutoThrottleManuallyIncreased()) + if (isFixedWingAutoThrottleManuallyIncreased()) { TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); + } } +#ifdef USE_POWER_LIMITS + if (powerLimiterIsLimiting()) { + TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr); + } +#endif tfp_sprintf(buff + 2, "%3d", getThrottlePercent()); } @@ -968,7 +997,7 @@ static bool osdIsHeadingValid(void) } else { return isImuHeadingValid(); } -#else +#else return isImuHeadingValid(); #endif } @@ -981,7 +1010,7 @@ int16_t osdGetHeading(void) } else { return attitude.values.yaw; } -#else +#else return attitude.values.yaw; #endif } @@ -1167,6 +1196,67 @@ static void osdDrawRadar(uint16_t *drawn, uint32_t *usedScale) osdDrawMap(reference, 0, SYM_ARROW_UP, GPS_distanceToHome, poiDirection, SYM_HOME, drawn, usedScale); } +static uint16_t crc_accumulate(uint8_t data, uint16_t crcAccum) +{ + uint8_t tmp; + tmp = data ^ (uint8_t)(crcAccum & 0xff); + tmp ^= (tmp << 4); + crcAccum = (crcAccum >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4); + return crcAccum; +} + + +static void osdDisplayTelemetry(void) +{ + uint32_t trk_data; + uint16_t trk_crc = 0; + char trk_buffer[31]; + static int16_t trk_elevation = 127; + static uint16_t trk_bearing = 0; + + if (ARMING_FLAG(ARMED)) { + if (STATE(GPS_FIX)){ + if (GPS_distanceToHome > 5) { + trk_bearing = GPS_directionToHome; + trk_bearing += 360 + 180; + trk_bearing %= 360; + int32_t alt = CENTIMETERS_TO_METERS(osdGetAltitude()); + float at = atan2(alt, GPS_distanceToHome); + trk_elevation = (float)at * 57.2957795; // 57.2957795 = 1 rad + trk_elevation += 37; // because elevation in telemetry should be from -37 to 90 + if (trk_elevation < 0) { + trk_elevation = 0; + } + } + } + } + else{ + trk_elevation = 127; + trk_bearing = 0; + } + + trk_data = 0; // bit 0 - packet type 0 = bearing/elevation, 1 = 2 byte data packet + trk_data = trk_data | (uint32_t)(0x7F & trk_elevation) << 1; // bits 1-7 - elevation angle to target. NOTE number is abused. constrained value of -37 to 90 sent as 0 to 127. + trk_data = trk_data | (uint32_t)trk_bearing << 8; // bits 8-17 - bearing angle to target. 0 = true north. 0 to 360 + trk_crc = crc_accumulate(0xFF & trk_data, trk_crc); // CRC First Byte bits 0-7 + trk_crc = crc_accumulate(0xFF & trk_bearing, trk_crc); // CRC Second Byte bits 8-15 + trk_crc = crc_accumulate(trk_bearing >> 8, trk_crc); // CRC Third Byte bits 16-17 + trk_data = trk_data | (uint32_t)trk_crc << 17; // bits 18-29 CRC & 0x3FFFF + + for (uint8_t t_ctr = 0; t_ctr < 30; t_ctr++) { // Prepare screen buffer and write data line. + if (trk_data & (uint32_t)1 << t_ctr){ + trk_buffer[29 - t_ctr] = SYM_TELEMETRY_0; + } + else{ + trk_buffer[29 - t_ctr] = SYM_TELEMETRY_1; + } + } + trk_buffer[30] = 0; + displayWrite(osdDisplayPort, 0, 0, trk_buffer); + if (osdConfig()->telemetry>1){ + displayWrite(osdDisplayPort, 0, 3, trk_buffer); // Test display because normal telemetry line is not visible + } +} #endif static void osdFormatPidControllerOutput(char *buff, const char *label, const pidController_t *pidController, uint8_t scale, bool showDecimal) { @@ -1628,7 +1718,7 @@ static bool osdDrawSingleElement(uint8_t item) /*static int32_t updatedTimeSeconds = 0;*/ timeUs_t currentTimeUs = micros(); static int32_t timeSeconds = -1; - if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); #else @@ -1659,7 +1749,7 @@ static bool osdDrawSingleElement(uint8_t item) static timeUs_t updatedTimestamp = 0; timeUs_t currentTimeUs = micros(); static int32_t distanceMeters = -1; - if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); #else @@ -1693,8 +1783,10 @@ static bool osdDrawSingleElement(uint8_t item) p = "!FS!"; else if (FLIGHT_MODE(MANUAL_MODE)) p = "MANU"; + else if (FLIGHT_MODE(TURTLE_MODE)) + p = "TURT"; else if (FLIGHT_MODE(NAV_RTH_MODE)) - p = "RTH "; + p = isWaypointMissionRTHActive() ? "WRTH" : "RTH "; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) p = "HOLD"; else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) @@ -1713,8 +1805,6 @@ 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; @@ -1759,38 +1849,33 @@ static bool osdDrawSingleElement(uint8_t item) #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); - if (!failsafeIsReceivingRxData()){ - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } + int16_t rssi = rxLinkStatistics.uplinkRSSI; + buff[0] = (rxLinkStatistics.activeAnt == 0) ? SYM_RSSI : SYM_2RSS; // Separate symbols for each antenna + if (rssi <= -100) { + tfp_sprintf(buff + 1, "%4d%c", rssi, SYM_DBM); } else { - buff[0] = SYM_2RSS; - tfp_sprintf(buff + 1, "%4d%c", rxLinkStatistics.uplinkRSSI, SYM_DBM); - if (!failsafeIsReceivingRxData()){ - TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); - } + tfp_sprintf(buff + 1, "%3d%c%c", rssi, SYM_DBM, ' '); + } + if (!failsafeIsReceivingRxData()){ + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } break; } case OSD_CRSF_LQ: { - buff[0] = SYM_BLANK; + buff[0] = SYM_LQ; 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, "%"); - } else { - tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); - } - } else { - if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) { - tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%"); - } else { - tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); - } + switch (osdConfig()->crsf_lq_format) { + case OSD_CRSF_LQ_TYPE1: + tfp_sprintf(buff+1, "%3d", rxLinkStatistics.uplinkLQ); + break; + case OSD_CRSF_LQ_TYPE2: + tfp_sprintf(buff+1, "%d:%3d", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ); + break; + case OSD_CRSF_LQ_TYPE3: + tfp_sprintf(buff+1, "%3d", rxLinkStatistics.rfMode >= 2 ? scaledLQ : rxLinkStatistics.uplinkLQ); + break; } if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1802,10 +1887,14 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_CRSF_SNR_DB: { + static pt1Filter_t snrFilterState; + static timeMs_t snrUpdated = 0; + int8_t snrFiltered = pt1FilterApply4(&snrFilterState, rxLinkStatistics.uplinkSNR, 0.5f, MS2S(millis() - snrUpdated)); + snrUpdated = millis(); + const char* showsnr = "-20"; const char* hidesnr = " "; - int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR; - if (osdSNR_Alarm > osdConfig()->snr_alarm) { + if (snrFiltered > osdConfig()->snr_alarm) { if (cmsInMenu) { buff[0] = SYM_SNR; tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); @@ -1813,9 +1902,13 @@ static bool osdDrawSingleElement(uint8_t item) buff[0] = SYM_BLANK; tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); } - } else if (osdSNR_Alarm <= osdConfig()->snr_alarm) { + } else if (snrFiltered <= osdConfig()->snr_alarm) { buff[0] = SYM_SNR; - tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB); + if (snrFiltered <= -10) { + tfp_sprintf(buff + 1, "%3d%c", snrFiltered, SYM_DB); + } else { + tfp_sprintf(buff + 1, "%2d%c%c", snrFiltered, SYM_DB, ' '); + } } break; } @@ -1881,14 +1974,18 @@ static bool osdDrawSingleElement(uint8_t item) 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) { + if (j > posControl.waypointCount - 1) { // limit to max WP index for mission + break; + } + if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0) { wp2.lat = posControl.waypointList[j].lat; wp2.lon = posControl.waypointList[j].lon; wp2.alt = posControl.waypointList[j].alt; fpVector3_t poi; - geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, GEO_ALT_RELATIVE); + geoConvertGeodeticToLocal(&poi, &posControl.gpsOrigin, &wp2, waypointMissionAltConvMode(posControl.waypointList[j].p3)); + int32_t altConvModeAltitude = waypointMissionAltConvMode(posControl.waypointList[j].p3) == GEO_ALT_ABSOLUTE ? osdGetAltitudeMsl() : osdGetAltitude(); while (j > 9) j -= 10; // Only the last digit displayed if WP>=10, no room for more - osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - osdGetAltitude())/ 100, 2, SYM_WAYPOINT, 49 + j, i); + osdHudDrawPoi(calculateDistanceToDestination(&poi) / 100, osdGetHeadingAngle(calculateBearingToDestination(&poi) / 100), (posControl.waypointList[j].alt - altConvModeAltitude)/ 100, 2, SYM_WAYPOINT, 49 + j, i); } } } @@ -1925,7 +2022,7 @@ static bool osdDrawSingleElement(uint8_t item) pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch); } else { rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); } #else rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); @@ -2158,8 +2255,8 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_POWER: { - osdFormatCentiNumber(buff, getPower(), 0, 2, 0, 3); - buff[3] = SYM_WATT; + bool kiloWatt = osdFormatCentiNumber(buff, getPower(), 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; buff[4] = '\0'; uint8_t current_alarm = osdConfig()->current_alarm; @@ -2186,7 +2283,7 @@ static bool osdDrawSingleElement(uint8_t item) dateTime_t dateTime; rtcGetDateTimeLocal(&dateTime); buff[0] = SYM_CLOCK; - tfp_sprintf(buff + 1, "%02u:%02u", dateTime.hours, dateTime.minutes); + tfp_sprintf(buff + 1, "%02u:%02u:%02u", dateTime.hours, dateTime.minutes, dateTime.seconds); break; } @@ -2241,26 +2338,52 @@ static bool osdDrawSingleElement(uint8_t item) static pt1Filter_t eFilterState; static timeUs_t efficiencyUpdated = 0; int32_t value = 0; + bool moreThanAh = false; timeUs_t currentTimeUs = micros(); timeDelta_t efficiencyTimeDelta = cmpTimeUs(currentTimeUs, efficiencyUpdated); if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, efficiencyTimeDelta * 1e-6f); + 1, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } else { value = eFilterState.state; } } - if (value > 0 && gpsSol.groundSpeed > 100) { - tfp_sprintf(buff, "%3d", (int)value); - } else { - buff[0] = buff[1] = buff[2] = '-'; + bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + moreThanAh = osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10, 1000, 0, 2, 3); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_MI_0; + buff[4] = SYM_MAH_MI_1; + buff[5] = '\0'; + } + break; + case OSD_UNIT_METRIC: + moreThanAh = osdFormatCentiNumber(buff, value * 100, 1000, 0, 2, 3); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_KM_0; + buff[4] = SYM_MAH_KM_1; + buff[5] = '\0'; + } + break; } - buff[3] = SYM_MAH_KM_0; - buff[4] = SYM_MAH_KM_1; - buff[5] = '\0'; break; } @@ -2276,21 +2399,30 @@ static bool osdDrawSingleElement(uint8_t item) if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, - 1, efficiencyTimeDelta * 1e-6f); + 1, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } else { value = eFilterState.state; } } - if (value > 0 && gpsSol.groundSpeed > 100) { - osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); - } else { + bool efficiencyValid = (value > 0) && (gpsSol.groundSpeed > 100); + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + osdFormatCentiNumber(buff, value * METERS_PER_MILE / 10000, 0, 2, 0, 3); + buff[3] = SYM_WH_MI; + break; + case OSD_UNIT_METRIC: + osdFormatCentiNumber(buff, value / 10, 0, 2, 0, 3); + buff[3] = SYM_WH_KM; + break; + } + buff[4] = '\0'; + if (!efficiencyValid) { buff[0] = buff[1] = buff[2] = '-'; } - buff[3] = SYM_WH_KM_0; - buff[4] = SYM_WH_KM_1; - buff[5] = '\0'; break; } @@ -2584,9 +2716,47 @@ static bool osdDrawSingleElement(uint8_t item) return true; } + case OSD_NAV_FW_CONTROL_SMOOTHNESS: osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS); return true; + +#ifdef USE_POWER_LIMITS + case OSD_PLIMIT_REMAINING_BURST_TIME: + osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3); + buff[3] = 'S'; + buff[4] = '\0'; + break; + + case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT: + if (powerLimitsConfig()->continuousCurrent) { + osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3); + buff[3] = SYM_AMP; + buff[4] = '\0'; + + if (powerLimiterIsLimitingCurrent()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; + +#ifdef USE_ADC + case OSD_PLIMIT_ACTIVE_POWER_LIMIT: + { + if (powerLimitsConfig()->continuousPower) { + bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; + + if (powerLimiterIsLimitingPower()) { + TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); + } + } + break; + } +#endif // USE_ADC +#endif // USE_POWER_LIMITS + default: return false; } @@ -2661,6 +2831,12 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex) } } +#ifndef USE_POWER_LIMITS + if (elementIndex == OSD_NAV_FW_CONTROL_SMOOTHNESS) { + elementIndex = OSD_ITEM_COUNT; + } +#endif + if (elementIndex == OSD_ITEM_COUNT) { elementIndex = 0; } @@ -2676,8 +2852,11 @@ void osdDrawNextElement(void) elementIndex = osdIncElementIndex(elementIndex); } while(!osdDrawSingleElement(elementIndex) && index != elementIndex); - // Draw artificial horizon last + // Draw artificial horizon + tracking telemtry last osdDrawSingleElement(OSD_ARTIFICIAL_HORIZON); + if (osdConfig()->telemetry>0){ + osdDisplayTelemetry(); + } } PG_RESET_TEMPLATE(osdConfig_t, osdConfig, @@ -2732,6 +2911,7 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .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, + .sidebar_height = SETTING_OSD_SIDEBAR_HEIGHT_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, @@ -2758,7 +2938,8 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig, .force_grid = SETTING_OSD_FORCE_GRID_DEFAULT, - .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT + .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT, + .stats_min_voltage_unit = SETTING_OSD_STATS_MIN_VOLTAGE_UNIT_DEFAULT ); void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) @@ -2902,6 +3083,12 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4); #endif +#ifdef USE_POWER_LIMITS + osdLayoutsConfig->item_pos[0][OSD_PLIMIT_REMAINING_BURST_TIME] = OSD_POS(3, 4); + osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_CURRENT_LIMIT] = OSD_POS(3, 5); + osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_POWER_LIMIT] = OSD_POS(3, 6); +#endif + // Under OSD_FLYMODE. TODO: Might not be visible on NTSC? osdLayoutsConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG; @@ -3004,12 +3191,17 @@ static void osdCompleteAsyncInitialization(void) displayWrite(osdDisplayPort, STATS_LABEL_X_POS, ++y, "AVG EFFICIENCY:"); if (statsConfig()->stats_total_dist) { uint32_t avg_efficiency = statsConfig()->stats_total_energy / (statsConfig()->stats_total_dist / METERS_PER_KILOMETER); // mWh/km - osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); - } else - strcpy(string_buffer, "---"); - string_buffer[3] = SYM_WH_KM_0; - string_buffer[4] = SYM_WH_KM_1; - string_buffer[5] = '\0'; + if (osdConfig()->units == OSD_UNIT_IMPERIAL) { + osdFormatCentiNumber(string_buffer, avg_efficiency / 10, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_MI; + } else { + osdFormatCentiNumber(string_buffer, avg_efficiency / 10000 * METERS_PER_MILE, 0, 2, 0, 3); + string_buffer[3] = SYM_WH_KM; + } + } else { + string_buffer[0] = string_buffer[1] = string_buffer[2] = '-'; + } + string_buffer[4] = '\0'; displayWrite(osdDisplayPort, STATS_VALUE_X_POS-3, y, string_buffer); } #endif // USE_ADC @@ -3052,7 +3244,7 @@ static void osdResetStats(void) static void osdUpdateStats(void) { - int16_t value; + int32_t value; if (feature(FEATURE_GPS)) { value = osdGet3DSpeed(); @@ -3067,11 +3259,11 @@ static void osdUpdateStats(void) if (stats.min_voltage > value) stats.min_voltage = value; - value = abs(getAmperage() / 100); + value = abs(getAmperage()); if (stats.max_current < value) stats.max_current = value; - value = abs(getPower() / 100); + value = labs(getPower()); if (stats.max_power < value) stats.max_power = value; @@ -3079,11 +3271,11 @@ static void osdUpdateStats(void) if (stats.min_rssi > value) stats.min_rssi = value; - value = osdGetLQ(); + value = osdGetCrsfLQ(); if (stats.min_lq > value) stats.min_lq = value; - value = osdGetdBm(); + value = osdGetCrsfdBm(); if (stats.min_rssi_dbm > value) stats.min_rssi_dbm = value; @@ -3102,7 +3294,7 @@ static void osdShowStatsPage1(void) displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayClearScreen(osdDisplayPort); - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2"); + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); if (feature(FEATURE_GPS)) { displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); @@ -3123,22 +3315,24 @@ static void osdShowStatsPage1(void) osdFormatAltitudeStr(buff, stats.max_altitude); displayWrite(osdDisplayPort, statValuesX, top++, 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); + switch (rxConfig()->serialrx_provider) { + case 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, "MIN RSSI :"); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + break; + default: + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); uint16_t flySeconds = getFlightTime(); @@ -3165,59 +3359,93 @@ static void osdShowStatsPage2(void) displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayClearScreen(osdDisplayPort); - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 2/2"); + 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); + if (osdConfig()->stats_min_voltage_unit == OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY) { + displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); + osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); + } else { + displayWrite(osdDisplayPort, statNameX, top, "MIN CELL VOLTAGE :"); + osdFormatCentiNumber(buff, stats.min_voltage/getBatteryCellCount(), 0, 2, 0, 3); + } 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); + osdFormatCentiNumber(buff, stats.max_current, 0, 2, 0, 3); 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); - tfp_sprintf(buff, "%s%c", buff, SYM_WATT); + bool kiloWatt = osdFormatCentiNumber(buff, stats.max_power, 1000, 2, 2, 3); + buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT; + buff[4] = '\0'; displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "USED CAPACITY :"); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { - displayWrite(osdDisplayPort, statNameX, top, "USED MAH :"); tfp_sprintf(buff, "%d%c", (int)getMAhDrawn(), SYM_MAH); } else { - displayWrite(osdDisplayPort, statNameX, top, "USED WH :"); osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); tfp_sprintf(buff, "%s%c", buff, SYM_WH); } displayWrite(osdDisplayPort, statValuesX, top++, buff); int32_t totalDistance = getTotalTravelDistance(); + bool moreThanAh = false; + bool efficiencyValid = totalDistance >= 10000; if (feature(FEATURE_GPS)) { displayWrite(osdDisplayPort, statNameX, top, "AVG EFFICIENCY :"); - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) - tfp_sprintf(buff, "%d%c%c", (int)(getMAhDrawn() * 100000 / totalDistance), - SYM_MAH_KM_0, SYM_MAH_KM_1); - else { - osdFormatCentiNumber(buff, getMWhDrawn() * 10000 / totalDistance, 0, 2, 0, 3); - buff[3] = SYM_WH_KM_0; - buff[4] = SYM_WH_KM_1; - buff[5] = '\0'; - } - // If traveled distance is less than 100 meters efficiency numbers are useless and unreliable so display --- instead - if (totalDistance < 10000) { - buff[0] = buff[1] = buff[2] = '-'; - if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH){ - buff[3] = SYM_MAH_KM_0; - buff[4] = SYM_MAH_KM_1; - buff[5] = '\0'; - } else { - buff[3] = SYM_WH_KM_0; - buff[4] = SYM_WH_KM_1; - buff[5] = '\0'; - } + switch (osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000.0f * METERS_PER_MILE / totalDistance), 1000, 0, 2, 3); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_MI_0, SYM_MAH_MI_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_MI); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_MI_0; + buff[4] = SYM_MAH_MI_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10.0f * METERS_PER_MILE / totalDistance), 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_MI); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; + case OSD_UNIT_METRIC: + if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { + moreThanAh = osdFormatCentiNumber(buff, (int32_t)(getMAhDrawn() * 10000000.0f / totalDistance), 1000, 0, 2, 3); + if (!moreThanAh) { + tfp_sprintf(buff, "%s%c%c", buff, SYM_MAH_KM_0, SYM_MAH_KM_1); + } else { + tfp_sprintf(buff, "%s%c", buff, SYM_AH_KM); + } + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + buff[3] = SYM_MAH_KM_0; + buff[4] = SYM_MAH_KM_1; + buff[5] = '\0'; + } + } else { + osdFormatCentiNumber(buff, (int32_t)(getMWhDrawn() * 10000.0f / totalDistance), 0, 2, 0, 3); + tfp_sprintf(buff, "%s%c", buff, SYM_WH_KM); + if (!efficiencyValid) { + buff[0] = buff[1] = buff[2] = '-'; + } + } + break; } + osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX, top++, buff); } } @@ -3278,13 +3506,17 @@ static void osdShowArmed(void) } y += 4; #if defined (USE_SAFE_HOME) - if (isSafeHomeInUse()) { - textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; - char buf2[12]; // format the distance first - osdFormatDistanceStr(buf2, safehome_distance); - tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_used); - // write this message above the ARMED message to make it obvious - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); + if (safehome_distance) { // safehome found during arming + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + strcpy(buf, "SAFEHOME FOUND; MODE OFF"); + } else { + char buf2[12]; // format the distance first + osdFormatDistanceStr(buf2, safehome_distance); + tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index); + } + textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; + // write this message above the ARMED message to make it obvious + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); } #endif } else { @@ -3310,7 +3542,7 @@ static void osdShowArmed(void) static void osdFilterData(timeUs_t currentTimeUs) { static timeUs_t lastRefresh = 0; - float refresh_dT = cmpTimeUs(currentTimeUs, lastRefresh) * 1e-6; + float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; @@ -3353,7 +3585,7 @@ static void osdRefresh(timeUs_t currentTimeUs) uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; statsPagesCheck = 0; #if defined(USE_SAFE_HOME) - if (isSafeHomeInUse()) + if (safehome_distance) delay *= 3; #endif osdSetNextRefreshIn(delay); @@ -3555,6 +3787,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter const char *failsafePhaseMessage = osdFailsafePhaseMessage(); const char *failsafeInfoMessage = osdFailsafeInfoMessage(); const char *navStateFSMessage = navigationStateMessage(); + if (failsafePhaseMessage) { messages[messageCount++] = failsafePhaseMessage; } @@ -3564,6 +3797,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (navStateFSMessage) { messages[messageCount++] = navStateFSMessage; } +#if defined(USE_SAFE_HOME) + const char *safehomeMessage = divertingToSafehomeMessage(); + if (safehomeMessage) { + messages[messageCount++] = safehomeMessage; + } +#endif if (messageCount > 0) { message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; if (message == failsafeInfoMessage) { @@ -3580,9 +3819,16 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter } } else { if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { + if (isWaypointMissionRTHActive()) { + // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH + messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL); + } + 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); + char buf[6]; + osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); + tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", posControl.activeWaypointIndex + 1, posControl.waypointCount, buf); messages[messageCount++] = messageBuf; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { // WP hold time countdown in seconds @@ -3598,6 +3844,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = navStateMessage; } } +#if defined(USE_SAFE_HOME) + const char *safehomeMessage = divertingToSafehomeMessage(); + if (safehomeMessage) { + messages[messageCount++] = safehomeMessage; + } +#endif } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); const char *launchStateMessage = fixedWingLaunchStateMessage(); diff --git a/src/main/io/osd.h b/src/main/io/osd.h old mode 100755 new mode 100644 index ae0f3ef4b3..8f4e4d4bf9 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -77,6 +77,7 @@ #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_DSHOT_BEEPER "MOTOR BEEPER ACTIVE" #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!" @@ -86,6 +87,7 @@ #define OSD_MSG_HOLDING_WAYPOINT "HOLDING WAYPOINT" #define OSD_MSG_TO_WP "TO WP" #define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT" +#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH" #define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING" #define OSD_MSG_LANDING "LANDING" #define OSD_MSG_LOITERING_HOME "LOITERING AROUND HOME" @@ -99,6 +101,11 @@ #define OSD_MSG_HEADFREE "(HEADFREE)" #define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" +#if defined(USE_SAFE_HOME) +#define OSD_MSG_DIVERT_SAFEHOME "DIVERTING TO SAFEHOME" +#define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" +#endif + typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -221,6 +228,9 @@ typedef enum { OSD_NAV_FW_CONTROL_SMOOTHNESS, OSD_VERSION, OSD_RANGEFINDER, + OSD_PLIMIT_REMAINING_BURST_TIME, + OSD_PLIMIT_ACTIVE_CURRENT_LIMIT, + OSD_PLIMIT_ACTIVE_POWER_LIMIT, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -237,6 +247,11 @@ typedef enum { OSD_STATS_ENERGY_UNIT_WH, } osd_stats_energy_unit_e; +typedef enum { + OSD_STATS_MIN_VOLTAGE_UNIT_BATTERY, + OSD_STATS_MIN_VOLTAGE_UNIT_CELL, +} osd_stats_min_voltage_unit_e; + typedef enum { OSD_CROSSHAIRS_STYLE_DEFAULT, OSD_CROSSHAIRS_STYLE_AIRCRAFT, @@ -250,7 +265,7 @@ typedef enum { typedef enum { OSD_SIDEBAR_SCROLL_NONE, OSD_SIDEBAR_SCROLL_ALTITUDE, - OSD_SIDEBAR_SCROLL_GROUND_SPEED, + OSD_SIDEBAR_SCROLL_SPEED, OSD_SIDEBAR_SCROLL_HOME_DISTANCE, OSD_SIDEBAR_SCROLL_MAX = OSD_SIDEBAR_SCROLL_HOME_DISTANCE, @@ -269,6 +284,7 @@ typedef enum { typedef enum { OSD_CRSF_LQ_TYPE1, OSD_CRSF_LQ_TYPE2, + OSD_CRSF_LQ_TYPE3 } osd_crsf_lq_format_e; typedef struct osdLayoutsConfig_s { @@ -333,6 +349,7 @@ typedef struct osdConfig_s { uint8_t units; // from osd_unit_e uint8_t stats_energy_unit; // from osd_stats_energy_unit_e + uint8_t stats_min_voltage_unit; // from osd_stats_min_voltage_unit_e #ifdef USE_WIND_ESTIMATOR bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance @@ -356,6 +373,8 @@ typedef struct osdConfig_s { uint8_t pan_servo_index; // Index of the pan servo used for home direction offset int8_t pan_servo_pwm2centideg; // Centidegrees of servo rotation per us pwm uint8_t crsf_lq_format; + uint8_t sidebar_height; // sidebar height in rows, 0 turns off sidebars leaving only level indicator arrows + uint8_t telemetry; // use telemetry on displayed pixel line 0 } osdConfig_t; diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index 5044e93c26..9d7a965bde 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -510,20 +510,23 @@ static int32_t osdCanvasSidebarGetValue(osd_sidebar_scroll_e scroll) return osdGetAltitude(); } break; - case OSD_SIDEBAR_SCROLL_GROUND_SPEED: + case OSD_SIDEBAR_SCROLL_SPEED: + { #if defined(USE_GPS) - switch ((osd_unit_e)osdConfig()->units) { - case OSD_UNIT_UK: - FALLTHROUGH; - case OSD_UNIT_IMPERIAL: - // cms/s to (mi/h) * 100 - return gpsSol.groundSpeed * 224 / 100; - case OSD_UNIT_METRIC: - // cm/s to (km/h) * 100 - return gpsSol.groundSpeed * 36 / 10; - } + int speed = osdGetSpeedFromSelectedSource(); + switch ((osd_unit_e)osdConfig()->units) { + case OSD_UNIT_UK: + FALLTHROUGH; + case OSD_UNIT_IMPERIAL: + // cms/s to (mi/h) * 100 + return speed * 224 / 100; + case OSD_UNIT_METRIC: + // cm/s to (km/h) * 100 + return speed * 36 / 10; + } #endif - break; + break; + } case OSD_SIDEBAR_SCROLL_HOME_DISTANCE: #if defined(USE_GPS) switch ((osd_unit_e)osdConfig()->units) { @@ -547,7 +550,7 @@ static uint8_t osdCanvasSidebarGetOptions(int *width, osd_sidebar_scroll_e scrol break; case OSD_SIDEBAR_SCROLL_ALTITUDE: FALLTHROUGH; - case OSD_SIDEBAR_SCROLL_GROUND_SPEED: + case OSD_SIDEBAR_SCROLL_SPEED: FALLTHROUGH; case OSD_SIDEBAR_SCROLL_HOME_DISTANCE: *width = OSD_CHAR_WIDTH * 5; // 4 numbers + unit @@ -588,7 +591,7 @@ static void osdCanvasSidebarGetUnit(osdUnit_t *unit, uint16_t *countsPerStep, os break; } break; - case OSD_SIDEBAR_SCROLL_GROUND_SPEED: + case OSD_SIDEBAR_SCROLL_SPEED: switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_UK: FALLTHROUGH; diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index d785eac979..adad66c6e2 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -28,15 +28,50 @@ #include "common/utils.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" + #include "drivers/display.h" #include "drivers/display_canvas.h" #include "drivers/osd.h" +#include "fc/settings.h" + #include "io/osd_canvas.h" #include "io/osd_common.h" #include "io/osd_grid.h" #include "navigation/navigation.h" +#include "sensors/pitotmeter.h" + + +#if defined(USE_OSD) || defined(USE_DJI_HD_OSD) + +PG_REGISTER_WITH_RESET_TEMPLATE(osdCommonConfig_t, osdCommonConfig, PG_OSD_COMMON_CONFIG, 0); + +PG_RESET_TEMPLATE(osdCommonConfig_t, osdCommonConfig, + .speedSource = SETTING_OSD_SPEED_SOURCE_DEFAULT +); + +int osdGetSpeedFromSelectedSource(void) { + int speed = 0; + switch (osdCommonConfig()->speedSource) { + case OSD_SPEED_SOURCE_GROUND: + speed = gpsSol.groundSpeed; + break; + case OSD_SPEED_SOURCE_3D: + speed = osdGet3DSpeed(); + break; + case OSD_SPEED_SOURCE_AIR: + #ifdef USE_PITOT + speed = pitot.airSpeed; + #endif + break; + } + return speed; +} + +#endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD) #if defined(USE_OSD) @@ -162,4 +197,4 @@ int16_t osdGet3DSpeed(void) int16_t hor_speed = gpsSol.groundSpeed; return (int16_t)sqrtf(sq(hor_speed) + sq(vert_speed)); } -#endif \ No newline at end of file +#endif diff --git a/src/main/io/osd_common.h b/src/main/io/osd_common.h index 2bed67d569..ff34948182 100644 --- a/src/main/io/osd_common.h +++ b/src/main/io/osd_common.h @@ -29,6 +29,26 @@ #include #include +#include "config/parameter_group.h" + +#if defined(USE_OSD) || defined(USE_DJI_HD_OSD) + +typedef enum { + OSD_SPEED_SOURCE_GROUND = 0, + OSD_SPEED_SOURCE_3D = 1, + OSD_SPEED_SOURCE_AIR = 2 +} osdSpeedSource_e; + +typedef struct { + osdSpeedSource_e speedSource; +} osdCommonConfig_t; + +PG_DECLARE(osdCommonConfig_t, osdCommonConfig); + +int osdGetSpeedFromSelectedSource(void); + +#endif // defined(USE_OSD) || defined(USE_DJI_HD_OSD) + #define OSD_VARIO_CM_S_PER_ARROW 50 // 1 arrow = 50cm/s #define OSD_VARIO_HEIGHT_ROWS 5 @@ -86,4 +106,3 @@ 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 ed72a973d3..0c9916d33a 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -112,7 +112,7 @@ }) -/* +/* * DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0 * DJI uses a subset of messages and assume fixed bit positions for flight modes * @@ -125,7 +125,6 @@ PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, .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 @@ -505,6 +504,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR("PWM ERR"); case ARMING_DISABLED_NO_PREARM: return OSD_MESSAGE_STR("NO PREARM"); + case ARMING_DISABLED_DSHOT_BEEPER: + return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE"); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; @@ -685,7 +686,7 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { // Show feet when dist < 0.5mi tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); - } + } else { // Show miles when dist >= 0.5mi tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)), @@ -722,7 +723,7 @@ static void osdDJIEfficiencyMahPerKM(char *buff) if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, efficiencyTimeDelta * 1e-6f); + 1, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } @@ -755,7 +756,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) if (enabledElements[0] == 'W') { enabledElements += 1; } - + int elemLen = strlen(enabledElements); if (elemLen > 0) { @@ -825,14 +826,14 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) // during a lost aircraft recovery and blinking // will cause it to be missing from some frames. } - } + } else { if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { messages[messageCount++] = navStateMessage; } - } + } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = "AUTOLAUNCH"; } @@ -994,7 +995,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms for (int i = 0; i < STICK_CHANNEL_COUNT; i++) { sbufWriteU16(dst, rxGetChannelValue(i)); } - break; + break; case DJI_MSP_RAW_GPS: sbufWriteU8(dst, gpsSol.fixType); @@ -1002,21 +1003,7 @@ 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); - - 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, osdGetSpeedFromSelectedSource()); sbufWriteU16(dst, gpsSol.groundCourse); break; @@ -1186,7 +1173,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms break; case DJI_MSP_FILTER_CONFIG: - sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz + sbufWriteU8(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz sbufWriteU16(dst, pidProfile()->dterm_lpf_hz); // BF: currentPidProfile->dterm_lowpass_hz sbufWriteU16(dst, pidProfile()->yaw_lpf_hz); // BF: currentPidProfile->yaw_lowpass_hz sbufWriteU16(dst, gyroConfig()->gyro_notch_hz); // BF: gyroConfig()->gyro_soft_notch_hz_1 @@ -1198,8 +1185,8 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms sbufWriteU8(dst, 0); // BF: currentPidProfile->dterm_filter_type sbufWriteU8(dst, gyroConfig()->gyro_lpf); // BF: gyroConfig()->gyro_hardware_lpf); sbufWriteU8(dst, 0); // BF: DEPRECATED: gyro_32khz_hardware_lpf - sbufWriteU16(dst, gyroConfig()->gyro_soft_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz); - sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz); // BF: gyroConfig()->gyro_lowpass2_hz); + sbufWriteU16(dst, gyroConfig()->gyro_main_lpf_hz); // BF: gyroConfig()->gyro_lowpass_hz); + sbufWriteU16(dst, 0); // BF: gyroConfig()->gyro_lowpass2_hz); sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass_type); sbufWriteU8(dst, 0); // BF: gyroConfig()->gyro_lowpass2_type); sbufWriteU16(dst, 0); // BF: currentPidProfile->dterm_lowpass2_hz); diff --git a/src/main/io/osd_dji_hd.h b/src/main/io/osd_dji_hd.h index 8405323167..d105b275b6 100644 --- a/src/main/io/osd_dji_hd.h +++ b/src/main/io/osd_dji_hd.h @@ -68,12 +68,6 @@ 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, }; @@ -82,7 +76,6 @@ 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/osd_grid.c b/src/main/io/osd_grid.c index 971fd1f106..2a1ee09d0c 100644 --- a/src/main/io/osd_grid.c +++ b/src/main/io/osd_grid.c @@ -237,7 +237,7 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side offset = osdGetAltitude(); steps = offset / 20; break; - case OSD_SIDEBAR_SCROLL_GROUND_SPEED: + case OSD_SIDEBAR_SCROLL_SPEED: #if defined(USE_GPS) offset = gpsSol.groundSpeed; #endif @@ -295,7 +295,7 @@ void osdGridDrawSidebars(displayPort_t *display) uint8_t rightDecoration = osdUpdateSidebar(osdConfig()->right_sidebar_scroll, &right, currentTimeMs); const int hudwidth = OSD_AH_SIDEBAR_WIDTH_POS; - const int hudheight = OSD_AH_SIDEBAR_HEIGHT_POS; + const int hudheight = osdConfig()->sidebar_height; // Arrows if (osdConfig()->sidebar_scroll_arrows) { @@ -315,11 +315,12 @@ void osdGridDrawSidebars(displayPort_t *display) // Draw AH sides int leftX = MAX(elemPosX - hudwidth - osdConfig()->sidebar_horizontal_offset, 0); int rightX = MIN(elemPosX + hudwidth + osdConfig()->sidebar_horizontal_offset, display->cols - 1); - for (int y = -hudheight; y <= hudheight; y++) { - displayWriteChar(display, leftX, elemPosY + y, leftDecoration); - displayWriteChar(display, rightX, elemPosY + y, rightDecoration); + if (osdConfig()->sidebar_height) { + for (int y = -hudheight; y <= hudheight; y++) { + displayWriteChar(display, leftX, elemPosY + y, leftDecoration); + displayWriteChar(display, rightX, elemPosY + y, rightDecoration); + } } - // AH level indicators displayWriteChar(display, leftX + 1, elemPosY, SYM_AH_RIGHT); displayWriteChar(display, rightX - 1, elemPosY, SYM_AH_LEFT); diff --git a/src/main/io/serial.c b/src/main/io/serial.c index 7c6360c43c..7e4906d521 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -543,6 +543,8 @@ void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer if (serialRxBytesWaiting(left)) { LED0_ON; uint8_t c = serialRead(left); + // Make sure there is space in the tx buffer + while (!serialTxBytesFree(right)); serialWrite(right, c); leftC(c); LED0_OFF; @@ -550,6 +552,8 @@ void serialPassthrough(serialPort_t *left, serialPort_t *right, serialConsumer if (serialRxBytesWaiting(right)) { LED0_ON; uint8_t c = serialRead(right); + // Make sure there is space in the tx buffer + while (!serialTxBytesFree(left)); serialWrite(left, c); rightC(c); LED0_OFF; diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 7b4f05c76b..3c76e160b3 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -44,7 +44,7 @@ typedef enum { FUNCTION_RCDEVICE = (1 << 10), // 1024 FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048 FUNCTION_VTX_TRAMP = (1 << 12), // 4096 - FUNCTION_UNUSED_1 = (1 << 13), // 8192: former UAV_INTERCONNECT + FUNCTION_UNUSED_1 = (1 << 13), // 8192: former\ UAV_INTERCONNECT FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 FUNCTION_LOG = (1 << 15), // 32768 FUNCTION_RANGEFINDER = (1 << 16), // 65536 @@ -55,6 +55,7 @@ typedef enum { FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152 FUNCTION_SERVO_SERIAL = (1 << 22), // 4194304 FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608 + FUNCTION_IMU2 = (1 << 24), // 16777216 } serialPortFunction_e; typedef enum { diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index a45f41c8bc..0119e61e75 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -427,11 +427,11 @@ void esc4wayProcess(serialPort_t *mspPort) port = mspPort; // Start here with UART Main loop - #ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper silent here beeperSilence(); - #endif +#endif bool isExitScheduled = false; while (1) { diff --git a/src/main/io/smartport_master.c b/src/main/io/smartport_master.c index ade2b00acc..60b9a93e26 100644 --- a/src/main/io/smartport_master.c +++ b/src/main/io/smartport_master.c @@ -550,7 +550,7 @@ void smartportMasterHandle(timeUs_t currentTimeUs) return; } - if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > SMARTPORT_POLLING_INTERVAL * 1000)) { + if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > MS2US(SMARTPORT_POLLING_INTERVAL))) { if (forwardRequestCount() && (forcedPolledPhyID == -1)) { // forward next payload if there is one in queue and we are not waiting from the response of the previous one smartportMasterForwardNextPayload(); } else { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c old mode 100755 new mode 100644 index 2ef4f5b6a6..37a6c3c000 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -74,10 +74,14 @@ gpsLocation_t GPS_home; uint32_t GPS_distanceToHome; // distance to home point in meters int16_t GPS_directionToHome; // direction to home point in degrees +fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET + radar_pois_t radar_pois[RADAR_MAX_POIS]; #if defined(USE_SAFE_HOME) -int8_t safehome_used; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise -uint32_t safehome_distance; // distance to the selected safehome +int8_t safehome_index = -1; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise +uint32_t safehome_distance = 0; // distance to the nearest safehome +fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming +bool safehome_applied = false; // whether the safehome has been applied to home. PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0); @@ -92,7 +96,7 @@ 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, 10); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 12); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -107,20 +111,24 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .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, + .safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT, }, // General navigation parameters .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 + .waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot .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 + .land_minalt_vspd = SETTING_NAV_LAND_MINALT_VSPD_DEFAULT, // centimeters/s + .land_maxalt_vspd = SETTING_NAV_LAND_MAXALT_VSPD_DEFAULT, // centimeters/s .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 @@ -128,7 +136,8 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .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 - }, + .max_altitude = SETTING_NAV_MAX_ALTITUDE_DEFAULT, + }, // MC-specific .mc = { @@ -229,11 +238,15 @@ void calculateInitialHoldPosition(fpVector3_t * pos); void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance); void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance); static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome); -static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint); +static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv); static navigationFSMEvent_t nextForNonGeoStates(void); +static bool isWaypointMissionValid(void); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); +void updateHomePosition(void); + +static bool rthAltControlStickOverrideCheck(unsigned axis); /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); @@ -263,7 +276,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINISHED(navigationFSMState_t previousState); @@ -655,11 +667,12 @@ 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_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, } }, @@ -693,19 +706,18 @@ 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_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_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [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_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, } }, @@ -784,27 +796,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { } }, - [NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME] = { - .persistentId = NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME, - .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, - .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP, - .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE, - .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_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - } - }, - /** EMERGENCY LANDING ************************************************/ [NAV_STATE_EMERGENCY_LANDING_INITIALIZE] = { .persistentId = NAV_PERSISTENT_ID_EMERGENCY_LANDING_INITIALIZE, @@ -1191,6 +1182,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n { UNUSED(previousState); + rthAltControlStickOverrideCheck(PITCH); + if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1206,7 +1199,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // If we reached desired initial RTH altitude or we don't want to climb first - if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF)) { + if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF) || rthAltControlStickOverrideCheck(ROLL)) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance if (STATE(FIXED_WING_LEGACY)) { @@ -1270,6 +1263,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio { UNUSED(previousState); + rthAltControlStickOverrideCheck(PITCH); + if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1317,30 +1312,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND // If position ok OR within valid timeout - continue if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) { - - // Wait until target heading is reached (with 15 deg margin for error) - if (STATE(FIXED_WING_LEGACY)) { + // Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing + if ((ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) { resetLandingDetector(); updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); - return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; + return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land + } + else if (!validateRTHSanityChecker()) { + // Continue to check for RTH sanity during pre-landing hover + return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } else { - if (ABS(wrap_18000(posControl.rthState.homePosition.yaw - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) { - resetLandingDetector(); - updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET); - return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; - } - else if (!validateRTHSanityChecker()) { - // Continue to check for RTH sanity during pre-landing hover - return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; - } - else { - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); - setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); - return NAV_FSM_EVENT_NONE; - } + fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_FINAL); + setDesiredPosition(tmpHomePos, posControl.rthState.homePosition.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); + return NAV_FSM_EVENT_NONE; } - } else { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } @@ -1400,22 +1386,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // Do not allow descent velocity slower than -30cm/s so the landing detector works. - descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f); + descentVelLimited = navConfig()->general.land_minalt_vspd; } else { - fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_LAND); // Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm. - float descentVelScaling = (navGetCurrentActualPositionAndVelocity()->pos.z - tmpHomePos->z - navConfig()->general.land_slowdown_minalt) - / (navConfig()->general.land_slowdown_maxalt - navConfig()->general.land_slowdown_minalt) * 0.75f + 0.25f; // Yield 1.0 at 2000 alt and 0.25 at 500 alt + float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z, + navConfig()->general.land_slowdown_minalt, navConfig()->general.land_slowdown_maxalt, + navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); - descentVelScaling = constrainf(descentVelScaling, 0.25f, 1.0f); + descentVelLimited = constrainf(descentVelScaled, navConfig()->general.land_minalt_vspd, navConfig()->general.land_maxalt_vspd); - // Do not allow descent velocity slower than -50cm/s so the landing detector works. - descentVelLimited = MIN(-descentVelScaling * navConfig()->general.land_descent_rate, -50.0f); } - updateClimbRateToAltitudeController(descentVelLimited, ROC_TO_ALT_NORMAL); + updateClimbRateToAltitudeController(-descentVelLimited, ROC_TO_ALT_NORMAL); return NAV_FSM_EVENT_NONE; } @@ -1439,7 +1423,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHED(navigation UNUSED(previousState); if (STATE(ALTITUDE_CONTROL)) { - updateClimbRateToAltitudeController(-0.3f * navConfig()->general.land_descent_rate, ROC_TO_ALT_NORMAL); // FIXME + updateClimbRateToAltitudeController(-1.1f * navConfig()->general.land_minalt_vspd, ROC_TO_ALT_NORMAL); // FIXME } // Prevent I-terms growing when already landed @@ -1467,6 +1451,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav */ setupJumpCounters(); posControl.activeWaypointIndex = 0; + wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE; return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION } } @@ -1519,7 +1504,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav if (STATE(MULTIROTOR)) { wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI; mapWaypointToLocalPosition(&wpHeadingControl.poi_pos, - &posControl.waypointList[posControl.activeWaypointIndex]); + &posControl.waypointList[posControl.activeWaypointIndex], GEO_ALT_RELATIVE); } return nextForNonGeoStates(); @@ -1536,10 +1521,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav return nextForNonGeoStates(); case NAV_WP_ACTION_RTH: - posControl.rthState.rthInitialDistance = posControl.homeDistance; - initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos); - calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL)); - return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS + return NAV_FSM_EVENT_SWITCH_TO_RTH; }; UNREACHABLE(); @@ -1585,21 +1567,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_SET_HEAD: case NAV_WP_ACTION_SET_POI: - UNREACHABLE(); - case NAV_WP_ACTION_RTH: - if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) { - return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED - } - else { - if(navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) - setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING_TAIL_FIRST); - else - setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING); - setDesiredPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL), 0, NAV_POS_UPDATE_Z); - return NAV_FSM_EVENT_NONE; // will re-process state in >10ms - } - break; + UNREACHABLE(); } } /* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */ @@ -1624,15 +1593,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_SET_HEAD: case NAV_WP_ACTION_SET_POI: - UNREACHABLE(); - case NAV_WP_ACTION_RTH: - if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) { - return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND; - } - else { - return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME; - } + UNREACHABLE(); case NAV_WP_ACTION_LAND: return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND; @@ -1712,14 +1674,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig } } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME(navigationFSMState_t previousState) -{ - UNUSED(previousState); - - const navigationFSMEvent_t hoverEvent = navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(previousState); - return hoverEvent; -} - static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState) { // TODO: @@ -1923,6 +1877,13 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode) break; case RTH_HOME_FINAL_LAND: + // if WP mission p2 > 0 use p2 value as landing elevation (in meters !) (otherwise default to takeoff home elevation) + if (FLIGHT_MODE(NAV_WP_MODE) && posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND && posControl.waypointList[posControl.activeWaypointIndex].p2 != 0) { + posControl.rthState.homeTmpWaypoint.z = posControl.waypointList[posControl.activeWaypointIndex].p2 * 100; // 100 -> m to cm + if (waypointMissionAltConvMode(posControl.waypointList[posControl.activeWaypointIndex].p3) == GEO_ALT_ABSOLUTE) { + posControl.rthState.homeTmpWaypoint.z -= posControl.gpsOrigin.alt; // correct to relative if absolute SL altitude datum used + } + } break; } @@ -2343,26 +2304,42 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void) #if defined(USE_SAFE_HOME) -/******************************************************* - * Is a safehome being used instead of the arming point? - *******************************************************/ -bool isSafeHomeInUse(void) +void checkSafeHomeState(bool shouldBeEnabled) { - return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES); + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + shouldBeEnabled = false; + } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) { + // if safehomes are only used with failsafe and we're trying to enable safehome + // then enable the safehome only with failsafe + shouldBeEnabled = posControl.flags.forcedRTHActivated; + } + // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything + if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) { + return; + } + if (shouldBeEnabled) { + // set home to safehome + setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + safehome_applied = true; + } else { + // set home to original arming point + setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + safehome_applied = false; + } + // if we've changed the home position, update the distance and direction + updateHomePosition(); } /*********************************************************** * See if there are any safehomes near where we are arming. - * If so, use it instead of the arming point for home. - * Select the nearest safehome + * If so, save the nearest one in case we need it later for RTH. **********************************************************/ -bool foundNearbySafeHome(void) +bool findNearestSafeHome(void) { - safehome_used = -1; + safehome_index = -1; uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1; uint32_t distance_to_current; fpVector3_t currentSafeHome; - fpVector3_t nearestSafeHome; gpsLocation_t shLLH; shLLH.alt = 0; for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) { @@ -2375,20 +2352,19 @@ bool foundNearbySafeHome(void) distance_to_current = calculateDistanceToDestination(¤tSafeHome); if (distance_to_current < nearest_safehome_distance) { // this safehome is the nearest so far - keep track of it. - safehome_used = i; + safehome_index = i; nearest_safehome_distance = distance_to_current; nearestSafeHome.x = currentSafeHome.x; nearestSafeHome.y = currentSafeHome.y; nearestSafeHome.z = currentSafeHome.z; } } - if (safehome_used >= 0) { + if (safehome_index >= 0) { safehome_distance = nearest_safehome_distance; - setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - return true; + } else { + safehome_distance = 0; } - safehome_distance = 0; - return false; + return safehome_distance > 0; } #endif @@ -2414,9 +2390,13 @@ void updateHomePosition(void) } if (setHome) { #if defined(USE_SAFE_HOME) - if (!foundNearbySafeHome()) + findNearestSafeHome(); #endif - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + // save the current location in case it is replaced by a safehome or HOME_RESET + original_rth_home.x = posControl.rthState.homePosition.pos.x; + original_rth_home.y = posControl.rthState.homePosition.pos.y; + original_rth_home.z = posControl.rthState.homePosition.pos.z; } } } @@ -2445,6 +2425,39 @@ void updateHomePosition(void) } } +/* ----------------------------------------------------------- + * Override RTH preset altitude and Climb First option + * using Pitch/Roll stick held for > 1 seconds + * Climb First override limited to Fixed Wing only + *-----------------------------------------------------------*/ +static bool rthAltControlStickOverrideCheck(unsigned axis) +{ + if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated || (axis == ROLL && STATE(MULTIROTOR))) { + return false; + } + static timeMs_t rthOverrideStickHoldStartTime[2]; + + if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { + timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis]; + + if (!rthOverrideStickHoldStartTime[axis]) { + rthOverrideStickHoldStartTime[axis] = millis(); + } else if (ABS(1500 - holdTime) < 500) { // 1s delay to activate, activation duration limited to 1 sec + if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + return true; + } else if (axis == ROLL) { // ROLL right to override climb first + return true; + } + } + } else { + rthOverrideStickHoldStartTime[axis] = 0; + } + + return false; +} + /*----------------------------------------------------------- * Update flight statistics *-----------------------------------------------------------*/ @@ -2565,6 +2578,18 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti posControl.desiredState.pos.z = altitudeToUse; } else { + + /* + * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 + * In other words, when altitude is reached, allow it only to shrink + */ + if (navConfig()->general.max_altitude > 0 && + altitudeToUse >= navConfig()->general.max_altitude && + desiredClimbRate > 0 + ) { + desiredClimbRate = 0; + } + if (STATE(FIXED_WING_LEGACY)) { // Fixed wing climb rate controller is open-loop. We simply move the known altitude target float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs); @@ -2692,8 +2717,8 @@ static bool adjustPositionFromRCInput(void) } else { - const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband); - const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); + const int16_t rcPitchAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband, -500, 500); + const int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment); } @@ -2844,6 +2869,12 @@ bool loadNonVolatileWaypointList(void) if (ARMING_FLAG(ARMED)) return false; + // if waypoints are already loaded, just unload them. + if (posControl.waypointCount > 0) { + resetWaypointList(); + return false; + } + resetWaypointList(); for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) { @@ -2886,7 +2917,7 @@ void resetSafeHomes(void) } #endif -static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint) +static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv) { gpsLocation_t wpLLH; @@ -2894,7 +2925,7 @@ static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint wpLLH.lon = waypoint->lon; wpLLH.alt = waypoint->alt; - geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, GEO_ALT_RELATIVE); + geoConvertGeodeticToLocal(localPos, &posControl.gpsOrigin, &wpLLH, altConv); } static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos) @@ -2908,10 +2939,15 @@ static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); } +geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag) +{ + return datumFlag == NAV_WP_MSL_DATUM ? GEO_ALT_ABSOLUTE : GEO_ALT_RELATIVE; +} + static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint) { fpVector3_t localPos; - mapWaypointToLocalPosition(&localPos, waypoint); + mapWaypointToLocalPosition(&localPos, waypoint, waypointMissionAltConvMode(waypoint->p3)); calculateAndSetActiveWaypointToLocalPosition(&localPos); } @@ -3125,6 +3161,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) const bool canActivatePosHold = canActivatePosHoldMode(); const bool canActivateNavigation = canActivateNavigationModes(); const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH; + checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated); // Keep canActivateWaypoint flag at FALSE if there is no mission loaded // Also block WP mission if we are executing RTH @@ -3156,15 +3193,16 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } } - // RTH/Failsafe_RTH can override MANUAL + // Failsafe_RTH (can override MANUAL) if (posControl.flags.forcedRTHActivated) { // If we request forced RTH - attempt to activate it no matter what // This might switch to emergency landing controller if GPS is unavailable return NAV_FSM_EVENT_SWITCH_TO_RTH; } - // Pilot-triggered RTH, also fall-back for WP if there is no mission loaded - if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint)) { + // Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded + // Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection) + if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !IS_RC_MODE_ACTIVE(BOXMANUAL))) { // Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss // If don't keep this, loss of any of the canActivatePosHold && canActivateNavigation && canActivateAltHold // will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back @@ -3195,7 +3233,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D; } - // PH has priority over CRUISE + // CRUISE has priority over COURSE_HOLD and AH + if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) { + if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)) + return NAV_FSM_EVENT_SWITCH_TO_CRUISE; + } + + // PH has priority over COURSE_HOLD // CRUISE has priority on AH if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) { @@ -3285,8 +3329,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(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)); + const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE))); + const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)); if (usedBypass) { *usedBypass = false; @@ -3316,7 +3360,7 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) // Don't allow arming if first waypoint is farther than configured safe distance if ((posControl.waypointCount > 0) && (navConfig()->general.waypoint_safe_distance != 0)) { fpVector3_t startingWaypointPos; - mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0]); + mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE); const bool navWpMissionStartTooFar = calculateDistanceToDestination(&startingWaypointPos) > navConfig()->general.waypoint_safe_distance; @@ -3539,6 +3583,11 @@ void navigationInit(void) } else { DISABLE_STATE(FW_HEADING_USE_YAW); } + +#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) + if (navConfig()->general.waypoint_load_on_boot) + loadNonVolatileWaypointList(); +#endif } /*----------------------------------------------------------- @@ -3561,6 +3610,7 @@ void activateForcedRTH(void) { abortFixedWingLaunch(); posControl.flags.forcedRTHActivated = true; + checkSafeHomeState(true); navProcessFSMEvents(selectNavEventFromBoxModeInput()); } @@ -3569,6 +3619,7 @@ void abortForcedRTH(void) // Disable failsafe RTH and make sure we back out of navigation mode to IDLE // If any navigation mode was active prior to RTH it will be re-enabled with next RX update posControl.flags.forcedRTHActivated = false; + checkSafeHomeState(false); navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); } @@ -3588,6 +3639,11 @@ rthState_e getStateOfForcedRTH(void) } } +bool isWaypointMissionRTHActive(void) +{ + return FLIGHT_MODE(NAV_RTH_MODE) && IS_RC_MODE_ACTIVE(BOXNAVWP) && !(IS_RC_MODE_ACTIVE(BOXNAVRTH) || posControl.flags.forcedRTHActivated); +} + bool navigationIsExecutingAnEmergencyLanding(void) { return navGetCurrentStateFlags() & NAV_CTL_EMERG; @@ -3605,6 +3661,11 @@ bool navigationIsControllingThrottle(void) return navigationInAutomaticThrottleMode() && (getMotorStatus() != MOTOR_STOPPED_USER); } +bool navigationIsControllingAltitude(void) { + navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); + return (stateFlags & NAV_CTL_ALT); +} + bool navigationIsFlyingAutonomousMode(void) { navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); @@ -3613,9 +3674,12 @@ bool navigationIsFlyingAutonomousMode(void) bool navigationRTHAllowsLanding(void) { - if (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_LAND) - return true; + // WP mission RTH landing setting + if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { + return posControl.waypointList[posControl.waypointCount - 1].p1 > 0; + } + // normal RTH landing setting navRTHAllowLanding_e allow = navConfig()->general.flags.rth_allow_landing; return allow == NAV_RTH_ALLOW_LANDING_ALWAYS || (allow == NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE)); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 8123a771b0..b71f932f99 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -35,6 +35,7 @@ extern gpsLocation_t GPS_home; extern uint32_t GPS_distanceToHome; // distance to home point in meters extern int16_t GPS_directionToHome; // direction to home point in degrees +extern fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET extern bool autoThrottleManuallyIncreased; @@ -50,14 +51,20 @@ typedef struct { int32_t lon; } navSafeHome_t; +typedef enum { + SAFEHOME_USAGE_OFF = 0, // Don't use safehomes + SAFEHOME_USAGE_RTH = 1, // Default - use safehome for RTH + SAFEHOME_USAGE_RTH_FS = 2, // Use safehomes for RX failsafe only +} safehomeUsageMode_e; + PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig); -extern int8_t safehome_used; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise -extern uint32_t safehome_distance; // distance to the selected safehome +extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise +extern uint32_t safehome_distance; // distance to the nearest safehome +extern bool safehome_applied; // whether the safehome has been applied to home. void resetSafeHomes(void); // remove all safehomes -bool isSafeHomeInUse(void); // Are we using a safehome instead of the arming point? -bool foundNearbySafeHome(void); // Did we find a safehome nearby? +bool findNearestSafeHome(void); // Find nearest safehome #endif // defined(USE_SAFE_HOME) @@ -189,17 +196,21 @@ typedef struct navConfig_s { uint8_t disarm_on_landing; // uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH + uint8_t rth_alt_control_override; // Override RTH Altitude and Climb First settings using Pitch and Roll stick uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor + uint8_t safehome_usage_mode; // Controls when safehomes are used } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm) uint16_t waypoint_safe_distance; // Waypoint mission sanity check distance + bool waypoint_load_on_boot; // load waypoints automatically during boot uint16_t max_auto_speed; // autonomous navigation speed cm/sec uint16_t max_auto_climb_rate; // max vertical speed limitation cm/sec uint16_t max_manual_speed; // manual velocity control max horizontal speed uint16_t max_manual_climb_rate; // manual velocity control max vertical speed - uint16_t land_descent_rate; // normal RTH landing descent rate + uint16_t land_minalt_vspd; // Final RTH landing descent rate under minalt + uint16_t land_maxalt_vspd; // RTH landing descent rate target at maxalt uint16_t land_slowdown_minalt; // Altitude to stop lowering descent rate during RTH descend uint16_t land_slowdown_maxalt; // Altitude to start lowering descent rate during RTH descend uint16_t emerg_descent_rate; // emergency landing descent rate @@ -209,6 +220,7 @@ typedef struct navConfig_s { uint16_t rth_abort_threshold; // Initiate emergency landing if during RTH we get this much [cm] away from home uint16_t max_terrain_follow_altitude; // Max altitude to be used in SURFACE TRACKING mode uint16_t safehome_max_distance; // Max distance that a safehome is from the arming point + uint16_t max_altitude; // Max altitude when in AltHold mode (not Surface Following) } general; struct { @@ -471,6 +483,11 @@ typedef enum { GEO_ORIGIN_RESET_ALTITUDE } geoOriginResetMode_e; +typedef enum { + NAV_WP_TAKEOFF_DATUM, + NAV_WP_MSL_DATUM +} geoAltitudeDatumFlag_e; + // geoSetOrigin stores the location provided in llh as a GPS origin in the // provided origin parameter. resetMode indicates wether all origin coordinates // should be overwritten by llh (GEO_ORIGIN_SET) or just the altitude, leaving @@ -491,6 +508,8 @@ bool geoConvertGeodeticToLocalOrigin(fpVector3_t * pos, const gpsLocation_t *llh // the provided origin is valid and the conversion could be performed. bool geoConvertLocalToGeodetic(gpsLocation_t *llh, const gpsOrigin_t *origin, const fpVector3_t *pos); float geoCalculateMagDeclination(const gpsLocation_t * llh); // degrees units +// Select absolute or relative altitude based on WP mission flag setting +geoAltitudeConversionMode_e waypointMissionAltConvMode(geoAltitudeDatumFlag_e datumFlag); /* Distance/bearing calculation */ bool navCalculatePathToDestination(navDestinationPath_t *result, const fpVector3_t * destinationPos); @@ -506,10 +525,12 @@ bool navigationIsControllingThrottle(void); bool isFixedWingAutoThrottleManuallyIncreased(void); bool navigationIsFlyingAutonomousMode(void); bool navigationIsExecutingAnEmergencyLanding(void); +bool navigationIsControllingAltitude(void); /* Returns true iff navConfig()->general.flags.rth_allow_landing is NAV_RTH_ALLOW_LANDING_ALWAYS * or if it's NAV_RTH_ALLOW_LANDING_FAILSAFE and failsafe mode is active. */ bool navigationRTHAllowsLanding(void); +bool isWaypointMissionRTHActive(void); bool isNavLaunchEnabled(void); bool isFixedWingLaunchDetected(void); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 40f2f49b0b..6501cc7ea2 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -106,7 +106,7 @@ void resetFixedWingAltitudeController(void) bool adjustFixedWingAltitudeFromRCInput(void) { - int16_t rcAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband); + int16_t rcAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband, -500, 500); if (rcAdjustment) { // set velocity proportional to stick movement @@ -171,11 +171,11 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) if ((posControl.flags.estAltStatus >= EST_USABLE)) { if (posControl.flags.verticalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - // Check if last correction was too log ago - ignore this update - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + // Check if last correction was not too long ago + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate); } else { @@ -281,7 +281,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // Shift position according to pilot's ROLL input (up to max_manual_speed velocity) if (posControl.flags.isAdjustingPosition) { - int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); + int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); if (rcRollAdjustment) { float rcShiftY = rcRollAdjustment * navConfig()->general.max_manual_speed / 500.0f * trackingPeriod; @@ -295,7 +295,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) bool adjustFixedWingPositionFromRCInput(void) { - int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); + int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); return (rcRollAdjustment); } @@ -401,10 +401,10 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { // Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ) // Account for pilot's roll input (move position target left/right at max of max_manual_speed) // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs @@ -440,10 +440,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); // If we are in the deadband of 50cm/s - don't update speed boost @@ -473,7 +473,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs) { static timeUs_t previousTimePitchToThrCorr = 0; - const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr; + const timeDeltaLarge_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr; previousTimePitchToThrCorr = currentTimeUs; static pt1Filter_t pitchToThrFilterState; @@ -650,7 +650,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, } if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) - rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); + rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); //if (navStateFlags & NAV_CTL_YAW) if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index a0beb4149c..8af05db4da 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -132,7 +132,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) return true; } else { - const int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband); + const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500); if (rcThrottleAdjustment) { // set velocity proportional to stick movement float rcClimbRate; @@ -209,11 +209,11 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) // If we have an update on vertical position data - update velocity and accel targets if (posControl.flags.verticalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - // Check if last correction was too log ago - ignore this update - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + // Check if last correction was not too long ago + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { // If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump if (prepareForTakeoffOnReset) { const navEstimatedPosVel_t * posToUse = navGetCurrentActualPositionAndVelocity(); @@ -477,8 +477,8 @@ static float computeNormalizedVelocity(const float value, const float maxValue) } static float computeVelocityScale( - const float value, - const float maxValue, + const float value, + const float maxValue, const float attenuationFactor, const float attenuationStart, const float attenuationEnd @@ -491,7 +491,7 @@ static float computeVelocityScale( } static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed) -{ +{ const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x; const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y; @@ -539,15 +539,15 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA * Scale down dTerm with 2D speed */ const float setpointScale = computeVelocityScale( - setpointXY, - maxSpeed, + setpointXY, + maxSpeed, multicopterPosXyCoefficients.dTermAttenuation, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd ); const float measurementScale = computeVelocityScale( - posControl.actualState.velXY, - maxSpeed, + posControl.actualState.velXY, + maxSpeed, multicopterPosXyCoefficients.dTermAttenuation, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd @@ -560,23 +560,23 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit // Thus we don't need to do anything else with calculated acceleration float newAccelX = navPidApply3( - &posControl.pids.vel[X], - setpointX, - measurementX, - US2S(deltaMicros), - accelLimitXMin, - accelLimitXMax, + &posControl.pids.vel[X], + setpointX, + measurementX, + US2S(deltaMicros), + accelLimitXMin, + accelLimitXMax, 0, // Flags 1.0f, // Total gain scale dtermScale // Additional dTerm scale ); float newAccelY = navPidApply3( - &posControl.pids.vel[Y], - setpointY, - measurementY, - US2S(deltaMicros), - accelLimitYMin, - accelLimitYMax, + &posControl.pids.vel[Y], + setpointY, + measurementY, + US2S(deltaMicros), + accelLimitYMin, + accelLimitYMax, 0, // Flags 1.0f, // Total gain scale dtermScale // Additional dTerm scale @@ -638,14 +638,14 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; if (!bypassPositionController) { // Update position controller - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s - const float maxSpeed = getActiveWaypointSpeed(); + const float maxSpeed = getActiveWaypointSpeed(); updatePositionVelocityController_MC(maxSpeed); updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed); } @@ -761,11 +761,11 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) // Normal sensor data if (posControl.flags.verticalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - // Check if last correction was too log ago - ignore this update - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + // Check if last correction was not too long ago + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 9848dfefbe..b26299e707 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -37,6 +37,9 @@ #define INAV_SURFACE_MAX_DISTANCE 40 +#define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro +_Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!"); + typedef enum { NAV_POS_UPDATE_NONE = 0, NAV_POS_UPDATE_Z = 1 << 1, @@ -138,7 +141,6 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, 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_COURSE_HOLD, NAV_FSM_EVENT_SWITCH_TO_CRUISE, @@ -197,7 +199,7 @@ typedef enum { NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35, NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36, - NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME = 37, + NAV_PERSISTENT_ID_UNUSED_4 = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME } navigationPersistentId_e; @@ -229,7 +231,6 @@ typedef enum { NAV_STATE_WAYPOINT_NEXT, NAV_STATE_WAYPOINT_FINISHED, NAV_STATE_WAYPOINT_RTH_LAND, - NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, NAV_STATE_EMERGENCY_LANDING_INITIALIZE, NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c index 1f0a01669a..40c075107b 100644 --- a/src/main/navigation/navigation_rover_boat.c +++ b/src/main/navigation/navigation_rover_boat.c @@ -71,11 +71,11 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs) static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate static timeUs_t previousTimeUpdate; // Occurs @ looptime rate - const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate; + const timeDeltaLarge_t deltaMicros = currentTimeUs - previousTimeUpdate; previousTimeUpdate = currentTimeUs; // If last position update was too long in the past - ignore it (likely restarting altitude controller) - if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicros > MAX_POSITION_UPDATE_INTERVAL_US) { previousTimeUpdate = currentTimeUs; previousTimePositionUpdate = currentTimeUs; resetFixedWingPositionController(); @@ -86,10 +86,10 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate); } else { resetFixedWingPositionController(); @@ -143,4 +143,4 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, } } -#endif \ No newline at end of file +#endif diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 89a1fc6fe6..01e2dada95 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -559,10 +559,14 @@ static int logicConditionGetFlightModeOperandValue(int operand) { return (bool) FLIGHT_MODE(NAV_POSHOLD_MODE); break; - case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE: + case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD: return (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE); break; + case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE: + return (bool)(FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)); + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD: return (bool) FLIGHT_MODE(NAV_ALTHOLD_MODE); break; diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 005c7a85f4..3b795ba43e 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -135,6 +135,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_MODE_AIR, // 8 LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER1, // 9 LOGIC_CONDITION_OPERAND_FLIGHT_MODE_USER2, // 10 + LOGIC_CONDITION_OPERAND_FLIGHT_MODE_COURSE_HOLD, // 11 } logicFlightModeOperands_e; typedef enum { diff --git a/src/main/rx/fport2.c b/src/main/rx/fport2.c index de2b7d7062..55d4f4349e 100644 --- a/src/main/rx/fport2.c +++ b/src/main/rx/fport2.c @@ -531,7 +531,7 @@ static uint8_t frameStatus(rxRuntimeConfig_t *rxRuntimeConfig) } #endif - if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > FPORT2_MAX_TELEMETRY_AGE_MS * 1000)) { + if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > MS2US(FPORT2_MAX_TELEMETRY_AGE_MS))) { lqTrackerSet(rxRuntimeConfig->lqTracker, 0); frameReceivedTimestamp = 0; } @@ -593,7 +593,7 @@ static bool processFrame(const rxRuntimeConfig_t *rxRuntimeConfig) } - timeUs_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp); + timeDelta_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp); if (!firmwareUpdateError && (otaResponseTime <= otaMaxResponseTime)) { // We can answer in time (firmwareUpdateStore can take time because it might need to erase flash) writeUplinkFramePhyID(downlinkPhyID, otaResponsePayload); DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT2_OTA_FRAME_RESPONSE_TIME, otaResponseTime); diff --git a/src/main/rx/ghst.c b/src/main/rx/ghst.c index 54d3b6f0f5..4c620de3e5 100644 --- a/src/main/rx/ghst.c +++ b/src/main/rx/ghst.c @@ -119,7 +119,7 @@ void ghstRxWriteTelemetryData(const void *data, int len) } void ghstRxSendTelemetryData(void) -{ +{ // if there is telemetry data to write if (telemetryBufLen > 0) { serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen); @@ -178,7 +178,7 @@ STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data) static bool shouldSendTelemetryFrame(void) { const timeUs_t now = micros(); - const timeUs_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs); + const timeDelta_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs); return telemetryBufLen > 0 && timeSinceRxFrameEndUs > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEndUs < GHST_RX_TO_TELEMETRY_MAX_US; } @@ -191,7 +191,7 @@ static void ghstIdle(void) static void ghstUpdateFailsafe(unsigned pktIdx) { - // pktIdx is an offset of RC channel packet, + // pktIdx is an offset of RC channel packet, // We'll track arrival time of each of the frame types we ever saw arriving from this receiver if (pktIdx < GHST_UL_RC_CHANS_FRAME_COUNT) { if (ghstFsTracker[pktIdx].onTimePacketCounter < GHST_RC_FRAME_COUNT_THRESHOLD) { @@ -282,7 +282,7 @@ static bool ghstProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) int startIdx = 0; if ( - ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST && + ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST && ghstValidatedFrame.frame.type <= GHST_UL_RC_CHANS_HS4_LAST ) { const ghstPayloadPulses_t* const rcChannels = (ghstPayloadPulses_t*)&ghstValidatedFrame.frame.payload; @@ -300,7 +300,7 @@ static bool ghstProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) case GHST_UL_RC_CHANS_HS4_RSSI: { const ghstPayloadPulsesRSSI_t* const rssiFrame = (ghstPayloadPulsesRSSI_t*)&ghstValidatedFrame.frame.payload; lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rssiFrame->lq, 0, 100), 0, 100, 0, RSSI_MAX_VALUE)); - + break; } diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 1e89705e37..2c27a1e95d 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -79,7 +79,7 @@ static void sumdDataReceive(uint16_t c, void *rxCallbackData) static uint8_t sumdIndex; sumdTime = micros(); - if (cmpTimeUs(sumdTime, sumdTimeLast) > 4000) + if (cmpTimeUs(sumdTime, sumdTimeLast) > MS2US(4)) sumdIndex = 0; sumdTimeLast = sumdTime; diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index 6f9af2ec2b..33ffa0e07d 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -69,7 +69,7 @@ static void sumhDataReceive(uint16_t c, void *rxCallbackData) sumhTime = micros(); sumhTimeInterval = cmpTimeUs(sumhTime, sumhTimeLast); sumhTimeLast = sumhTime; - if (sumhTimeInterval > 5000) { + if (sumhTimeInterval > MS2US(5)) { sumhFramePosition = 0; } diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index f3de93c7b7..b03276aacb 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -219,22 +219,10 @@ void FAST_CODE NOINLINE scheduler(void) // Cache currentTime const timeUs_t currentTimeUs = micros(); - // Check for realtime tasks - timeUs_t timeToNextRealtimeTask = TIMEUS_MAX; - for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) { - const timeUs_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod; - if ((int32_t)(currentTimeUs - nextExecuteAt) >= 0) { - timeToNextRealtimeTask = 0; - } else { - const timeUs_t newTimeInterval = nextExecuteAt - currentTimeUs; - timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval); - } - } - const bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > 0); - // The task to be invoked cfTask_t *selectedTask = NULL; uint16_t selectedTaskDynamicPriority = 0; + bool forcedRealTimeTask = false; // Update task dynamic priorities uint16_t waitingTasks = 0; @@ -263,6 +251,14 @@ void FAST_CODE NOINLINE scheduler(void) } else { task->taskAgeCycles = 0; } + } else if (task->staticPriority == TASK_PRIORITY_REALTIME) { + //realtime tasks take absolute priority. Any RT tasks that is overdue, should be execute immediately + if (((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) > task->desiredPeriod) { + selectedTaskDynamicPriority = task->dynamicPriority; + selectedTask = task; + waitingTasks++; + forcedRealTimeTask = true; + } } else { // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods) // Task age is calculated from last execution @@ -273,15 +269,9 @@ void FAST_CODE NOINLINE scheduler(void) } } - if (task->dynamicPriority > selectedTaskDynamicPriority) { - const bool taskCanBeChosenForScheduling = - (outsideRealtimeGuardInterval) || - (task->taskAgeCycles > 1) || - (task->staticPriority == TASK_PRIORITY_REALTIME); - if (taskCanBeChosenForScheduling) { - selectedTaskDynamicPriority = task->dynamicPriority; - selectedTask = task; - } + if (!forcedRealTimeTask && task->dynamicPriority > selectedTaskDynamicPriority) { + selectedTaskDynamicPriority = task->dynamicPriority; + selectedTask = task; } } diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 0ad456967c..4efa96e17d 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -27,7 +27,7 @@ typedef enum { TASK_PRIORITY_MEDIUM = 3, TASK_PRIORITY_MEDIUM_HIGH = 4, TASK_PRIORITY_HIGH = 5, - TASK_PRIORITY_REALTIME = 6, + TASK_PRIORITY_REALTIME = 18, TASK_PRIORITY_MAX = 255 } cfTaskPriority_e; @@ -51,12 +51,13 @@ typedef struct { typedef enum { /* Actual tasks */ TASK_SYSTEM = 0, - TASK_GYROPID, + TASK_PID, + TASK_GYRO, TASK_RX, TASK_SERIAL, TASK_BATTERY, TASK_TEMPERATURE, -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) TASK_BEEPER, #endif #ifdef USE_LIGHTS diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index fe54342561..feaf9c46ba 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -45,6 +45,7 @@ #include "flight/mixer.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "config/feature.h" @@ -217,9 +218,7 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) switch (batteryMetersConfig()->voltage.type) { case VOLTAGE_SENSOR_ADC: { - // calculate battery voltage based on ADC reading - // result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k) - vbat = (uint64_t)adcGetChannel(ADC_BATTERY) * batteryMetersConfig()->voltage.scale * ADCVREF / (0xFFF * 1000); + vbat = getVBatSample(); break; } #if defined(USE_ESC_SENSOR) @@ -374,6 +373,14 @@ bool isBatteryVoltageConfigured(void) return feature(FEATURE_VBAT); } +#ifdef USE_ADC +uint16_t getVBatSample(void) { + // calculate battery voltage based on ADC reading + // result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k) + return (uint64_t)adcGetChannel(ADC_BATTERY) * batteryMetersConfig()->voltage.scale * ADCVREF / (0xFFF * 1000); +} +#endif + uint16_t getBatteryVoltage(void) { if (batteryMetersConfig()->voltageSource == BAT_VOLTAGE_SAG_COMP) { @@ -447,6 +454,12 @@ int16_t getAmperage(void) return amperage; } +int16_t getAmperageSample(void) +{ + int32_t microvolts = ((uint32_t)adcGetChannel(ADC_CURRENT) * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100; + return microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps +} + int32_t getPower(void) { return power; @@ -462,7 +475,6 @@ int32_t getMWhDrawn(void) return mWhDrawn; } - void currentMeterUpdate(timeUs_t timeDelta) { static pt1Filter_t amperageFilterState; @@ -471,16 +483,23 @@ void currentMeterUpdate(timeUs_t timeDelta) switch (batteryMetersConfig()->current.type) { case CURRENT_SENSOR_ADC: { - int32_t microvolts = ((uint32_t)adcGetChannel(ADC_CURRENT) * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100; - amperage = microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps - amperage = pt1FilterApply4(&erageFilterState, amperage, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f); + amperage = pt1FilterApply4(&erageFilterState, getAmperageSample(), AMPERAGE_LPF_FREQ, timeDelta * 1e-6f); break; } case CURRENT_SENSOR_VIRTUAL: amperage = batteryMetersConfig()->current.offset; if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); - int32_t throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; + navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); + bool allNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_ALL_NAV && posControl.navState != NAV_STATE_IDLE; + bool autoNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_AUTO_ONLY && (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP)); + int32_t throttleOffset; + + if (allNav || autoNav) { // account for motors running in Nav modes with throttle low + motor stop + throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; + } else { + throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; + } int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000; } @@ -560,7 +579,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) } else { - if (cmpTimeUs(currentTime, recordTimestamp) > 500000) + if (cmpTimeUs(currentTime, recordTimestamp) > MS2US(500)) recordTimestamp = 0; if (!recordTimestamp) { @@ -577,7 +596,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) if (impedanceFilterState.state) { pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2 : 0.5); - pt1FilterApply3(&impedanceFilterState, impedanceSample, timeDelta * 1e-6f); + pt1FilterApply3(&impedanceFilterState, impedanceSample, US2S(timeDelta)); } else { pt1FilterReset(&impedanceFilterState, impedanceSample); } @@ -591,7 +610,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000); pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40 : 500); - sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, timeDelta * 1e-6f)); + sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, US2S(timeDelta))); } DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance); diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 94201fb28c..6c4cd1bede 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -151,6 +151,7 @@ uint16_t getPowerSupplyImpedance(void); bool isAmperageConfigured(void); int16_t getAmperage(void); +int16_t getAmperageSample(void); int32_t getPower(void); int32_t getMAhDrawn(void); int32_t getMWhDrawn(void); @@ -159,6 +160,7 @@ int32_t getMWhDrawn(void); void batteryUpdate(timeUs_t timeDelta); void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta); void powerMeterUpdate(timeUs_t timeDelta); +uint16_t getVBatSample(void); #endif void currentMeterUpdate(timeUs_t timeDelta); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 8e1da170b9..92ea43ee6b 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -95,9 +95,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_QMC5883: #ifdef USE_MAG_QMC5883 if (qmc5883Detect(dev)) { -#ifdef MAG_QMC5883_ALIGN - dev->magAlign.onBoard = MAG_QMC5883_ALIGN; -#endif magHardware = MAG_QMC5883; break; } @@ -111,9 +108,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_HMC5883: #ifdef USE_MAG_HMC5883 if (hmc5883lDetect(dev)) { -#ifdef MAG_HMC5883_ALIGN - dev->magAlign.onBoard = MAG_HMC5883_ALIGN; -#endif magHardware = MAG_HMC5883; break; } @@ -127,9 +121,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_AK8975: #ifdef USE_MAG_AK8975 if (ak8975Detect(dev)) { -#ifdef MAG_AK8975_ALIGN - dev->magAlign.onBoard = MAG_AK8975_ALIGN; -#endif magHardware = MAG_AK8975; break; } @@ -143,9 +134,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_AK8963: #ifdef USE_MAG_AK8963 if (ak8963Detect(dev)) { -#ifdef MAG_AK8963_ALIGN - dev->magAlign.onBoard = MAG_AK8963_ALIGN; -#endif magHardware = MAG_AK8963; break; } @@ -159,9 +147,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_GPS: #ifdef USE_GPS if (gpsMagDetect(dev)) { -#ifdef MAG_GPS_ALIGN - dev->magAlign.onBoard = MAG_GPS_ALIGN; -#endif magHardware = MAG_GPS; break; } @@ -175,9 +160,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_MAG3110: #ifdef USE_MAG_MAG3110 if (mag3110detect(dev)) { -#ifdef MAG_MAG3110_ALIGN - dev->magAlign.onBoard = MAG_MAG3110_ALIGN; -#endif magHardware = MAG_MAG3110; break; } @@ -191,9 +173,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_IST8310: #ifdef USE_MAG_IST8310 if (ist8310Detect(dev)) { -#ifdef MAG_IST8310_ALIGN - dev->magAlign.onBoard = MAG_IST8310_ALIGN; -#endif magHardware = MAG_IST8310; break; } @@ -207,9 +186,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_IST8308: #ifdef USE_MAG_IST8308 if (ist8308Detect(dev)) { -#ifdef MAG_IST8308_ALIGN - dev->magAlign.onBoard = MAG_IST8308_ALIGN; -#endif magHardware = MAG_IST8308; break; } @@ -223,9 +199,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_MPU9250: #ifdef USE_MAG_MPU9250 if (mpu9250CompassDetect(dev)) { -#ifdef MAG_MPU9250_ALIGN - dev->magAlign.onBoard = MAG_MPU9250_ALIGN; -#endif magHardware = MAG_MPU9250; break; } @@ -235,9 +208,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) case MAG_LIS3MDL: #ifdef USE_MAG_LIS3MDL if (lis3mdlDetect(dev)) { -#ifdef MAG_LIS3MDL_ALIGN - dev->magAlign = MAG_LIS3MDL_ALIGN; -#endif magHardware = MAG_LIS3MDL; break; } @@ -266,9 +236,6 @@ bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) 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; } @@ -344,6 +311,8 @@ bool compassInit(void) mag.dev.magAlign.useExternal = false; if (compassConfig()->mag_align != ALIGN_DEFAULT) { mag.dev.magAlign.onBoard = compassConfig()->mag_align; + } else { + mag.dev.magAlign.onBoard = CW270_DEG_FLIP; // The most popular default is 270FLIP for external mags } } @@ -360,6 +329,16 @@ bool compassIsReady(void) return magUpdatedAtLeastOnce; } +bool compassIsCalibrationComplete(void) +{ + if (STATE(COMPASS_CALIBRATED)) { + return true; + } + else { + return false; + } +} + void compassUpdate(timeUs_t currentTimeUs) { static sensorCalibrationState_t calState; diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index baba7bf6a1..fb0e75367f 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -78,5 +78,6 @@ bool compassInit(void); void compassUpdate(timeUs_t currentTimeUs); bool compassIsReady(void); bool compassIsHealthy(void); +bool compassIsCalibrationComplete(void); #endif diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6a8710bff2..e3ec90d5d4 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -104,23 +104,29 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11); +#ifdef USE_ALPHA_BETA_GAMMA_FILTER + +STATIC_FASTRAM filterApplyFnPtr abgFilterApplyFn; +STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT]; + +#endif + +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 13); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .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_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT, + .gyro_anti_aliasing_lpf_type = SETTING_GYRO_ANTI_ALIASING_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, + .gyro_main_lpf_hz = SETTING_GYRO_MAIN_LPF_HZ_DEFAULT, + .gyro_main_lpf_type = SETTING_GYRO_MAIN_LPF_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, @@ -131,6 +137,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT, .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT, #endif +#ifdef USE_ALPHA_BETA_GAMMA_FILTER + .alphaBetaGammaAlpha = SETTING_GYRO_ABG_ALPHA_DEFAULT, + .alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT, + .alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT, +#endif ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) @@ -248,7 +259,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard return gyroHardware; } -static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff) +static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff, uint32_t looptime) { *applyFn = nullFilterApply; if (cutoff > 0) { @@ -257,13 +268,13 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t case FILTER_PT1: *applyFn = (filterApplyFnPtr)pt1FilterApply; for (int axis = 0; axis < 3; axis++) { - pt1FilterInit(&state[axis].pt1, cutoff, getLooptime()* 1e-6f); + pt1FilterInit(&state[axis].pt1, cutoff, looptime * 1e-6f); } break; case FILTER_BIQUAD: *applyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { - biquadFilterInitLPF(&state[axis].biquad, cutoff, getLooptime()); + biquadFilterInitLPF(&state[axis].biquad, cutoff, looptime); } break; } @@ -275,9 +286,13 @@ static void gyroInitFilters(void) STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; notchFilter1ApplyFn = nullFilterApply; - initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_stage2_lowpass_type, gyroConfig()->gyro_stage2_lowpass_hz); - initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_soft_lpf_type, gyroConfig()->gyro_soft_lpf_hz); + //First gyro LPF running at full gyro frequency 8kHz + initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_anti_aliasing_lpf_type, gyroConfig()->gyro_anti_aliasing_lpf_hz, getGyroLooptime()); + + //Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON + initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_main_lpf_type, gyroConfig()->gyro_main_lpf_hz, getLooptime()); + //Static Gyro notch running and PID frequency if (gyroConfig()->gyro_notch_hz) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { @@ -285,6 +300,24 @@ static void gyroInitFilters(void) biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff); } } + +#ifdef USE_ALPHA_BETA_GAMMA_FILTER + + abgFilterApplyFn = (filterApplyFnPtr)nullFilterApply; + + if (gyroConfig()->alphaBetaGammaAlpha > 0) { + abgFilterApplyFn = (filterApplyFnPtr)alphaBetaGammaFilterApply; + for (int axis = 0; axis < 3; axis++) { + alphaBetaGammaFilterInit( + &abgFilter[axis], + gyroConfig()->alphaBetaGammaAlpha, + gyroConfig()->alphaBetaGammaBoost, + gyroConfig()->alphaBetaGammaHalfLife, + getLooptime() * 1e-6f + ); + } + } +#endif } bool gyroInit(void) @@ -313,12 +346,12 @@ bool gyroInit(void) // Driver initialisation gyroDev[0].lpf = gyroConfig()->gyro_lpf; - gyroDev[0].requestedSampleIntervalUs = gyroConfig()->looptime; - gyroDev[0].sampleRateIntervalUs = gyroConfig()->looptime; + gyroDev[0].requestedSampleIntervalUs = TASK_GYRO_LOOPTIME; + gyroDev[0].sampleRateIntervalUs = TASK_GYRO_LOOPTIME; gyroDev[0].initFn(&gyroDev[0]); // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value - gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime; + gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs; // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record // If configuration says different - override @@ -327,7 +360,9 @@ bool gyroInit(void) } gyroInitFilters(); + #ifdef USE_DYNAMIC_FILTERS + // Dynamic notch running at PID frequency dynamicGyroNotchFiltersInit(&dynamicGyroNotchState); gyroDataAnalyseStateInit( &gyroAnalyseState, @@ -433,22 +468,15 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC } } -void FAST_CODE NOINLINE gyroUpdate() +void FAST_CODE NOINLINE gyroFilter() { if (!gyro.initialized) { return; } - if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) { - return; - } - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s] float gyroADCf = gyro.gyroADCf[axis]; - DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); - #ifdef USE_RPM_FILTER DEBUG_SET(DEBUG_RPM_FILTER, axis, gyroADCf); gyroADCf = rpmFilterGyroApply(axis, gyroADCf); @@ -456,9 +484,14 @@ void FAST_CODE NOINLINE gyroUpdate() #endif gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf); - gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf); gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); +#ifdef USE_ALPHA_BETA_GAMMA_FILTER + DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis, gyroADCf); + gyroADCf = abgFilterApplyFn(&abgFilter[axis], gyroADCf); + DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis + 3, gyroADCf); +#endif + #ifdef USE_DYNAMIC_FILTERS if (dynamicGyroNotchState.enabled) { gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf); @@ -467,6 +500,7 @@ void FAST_CODE NOINLINE gyroUpdate() DEBUG_SET(DEBUG_DYNAMIC_FILTER, axis + 3, gyroADCf); } #endif + gyro.gyroADCf[axis] = gyroADCf; } @@ -483,7 +517,31 @@ void FAST_CODE NOINLINE gyroUpdate() } } #endif +} +void FAST_CODE NOINLINE gyroUpdate() +{ + if (!gyro.initialized) { + return; + } + + if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) { + return; + } + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s] + float gyroADCf = gyro.gyroADCf[axis]; + + DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); + + /* + * First gyro LPF is the only filter applied with the full gyro sampling speed + */ + gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf); + + gyro.gyroADCf[axis] = gyroADCf; + } } bool gyroReadTemperature(void) @@ -518,27 +576,14 @@ int16_t gyroRateDps(int axis) return lrintf(gyro.gyroADCf[axis]); } -bool gyroSyncCheckUpdate(void) -{ - if (!gyro.initialized) { - return false; - } - - if (!gyroDev[0].intStatusFn) { - return false; - } - - return gyroDev[0].intStatusFn(&gyroDev[0]); -} - void gyroUpdateDynamicLpf(float cutoffFreq) { - if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { + if (gyroConfig()->gyro_main_lpf_type == FILTER_PT1) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq); + pt1FilterUpdateCutoff(&gyroLpf2State[axis].pt1, cutoffFreq); } - } else if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) { + } else if (gyroConfig()->gyro_main_lpf_type == FILTER_BIQUAD) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterUpdate(&gyroLpfState[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF); + biquadFilterUpdate(&gyroLpf2State[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF); } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 6aa3c11414..e018e1a79a 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -61,18 +61,17 @@ 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. - 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; + uint8_t gyro_anti_aliasing_lpf_hz; + uint8_t gyro_anti_aliasing_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; - uint8_t gyro_stage2_lowpass_type; + uint16_t gyro_main_lpf_hz; + uint8_t gyro_main_lpf_type; uint8_t useDynamicLpf; uint16_t gyroDynamicLpfMinHz; uint16_t gyroDynamicLpfMaxHz; @@ -83,6 +82,11 @@ typedef struct gyroConfig_s { uint16_t dynamicGyroNotchMinHz; uint8_t dynamicGyroNotchEnabled; #endif +#ifdef USE_ALPHA_BETA_GAMMA_FILTER + float alphaBetaGammaAlpha; + float alphaBetaGammaBoost; + float alphaBetaGammaHalfLife; +#endif } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); @@ -90,10 +94,10 @@ PG_DECLARE(gyroConfig_t, gyroConfig); bool gyroInit(void); void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF); void gyroUpdate(void); +void gyroFilter(void); void gyroStartCalibration(void); bool gyroIsCalibrationComplete(void); bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); -bool gyroSyncCheckUpdate(void); void gyroUpdateDynamicLpf(float cutoffFreq); diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index cec9b00faf..8b26c586d8 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -227,7 +227,7 @@ timeDelta_t rangefinderUpdate(void) rangefinder.dev.update(&rangefinder.dev); } - return rangefinder.dev.delayMs * 1000; // to microseconds + return MS2US(rangefinder.dev.delayMs); } /** diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h index 596a0b54fd..70279b57b2 100644 --- a/src/main/target/AIRBOTF4/target.h +++ b/src/main/target/AIRBOTF4/target.h @@ -46,7 +46,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define MAG_HMC5883_ALIGN CW90_DEG #define USE_MAG_HMC5883 #define USE_MAG_AK8963 #define USE_MAG_AK8975 @@ -128,8 +127,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define MAG_GPS_ALIGN CW180_DEG_FLIP - #define DEFAULT_RX_TYPE RX_TYPE_PPM #define DISABLE_RX_PWM_FEATURE #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_VBAT) diff --git a/src/main/target/ALIENFLIGHTF3/target.h b/src/main/target/ALIENFLIGHTF3/target.h index 1fe6bae942..d60025155c 100644 --- a/src/main/target/ALIENFLIGHTF3/target.h +++ b/src/main/target/ALIENFLIGHTF3/target.h @@ -60,7 +60,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 #define USE_MAG_MPU9250 -#define MAG_MPU9250_ALIGN CW0_DEG #define USE_MAG_AK8963 #define USE_MAG_AK8975 #define USE_MAG_HMC5883 diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index f57dae6317..aae496c07d 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -45,6 +45,7 @@ #include "sensors/battery.h" #include "sensors/sensors.h" +#include "sensors/compass.h" #include "telemetry/telemetry.h" @@ -56,6 +57,8 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { + compassConfigMutable()->mag_align = CW90_DEG; + serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL; batteryMetersConfigMutable()->current.offset = CURRENTOFFSET; batteryMetersConfigMutable()->current.scale = CURRENTSCALE; diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 9d7718d640..feb65d8dcb 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -61,8 +61,6 @@ #define USE_MAG_IST8308 #define USE_MAG_LIS3MDL -#define MAG_MPU9250_ALIGN CW0_DEG - #define TEMPERATURE_I2C_BUS BUS_I2C1 #define BNO055_I2C_BUS BUS_I2C1 diff --git a/src/main/target/ALIENFLIGHTNGF7/config.c b/src/main/target/ALIENFLIGHTNGF7/config.c index 3fcda3d1b2..0b56c008f5 100644 --- a/src/main/target/ALIENFLIGHTNGF7/config.c +++ b/src/main/target/ALIENFLIGHTNGF7/config.c @@ -47,6 +47,7 @@ #include "config/feature.h" #include "sensors/battery.h" +#include "sensors/compass.h" #include "hardware_revision.h" @@ -58,6 +59,8 @@ // alternative defaults settings for AlienFlight targets void targetConfiguration(void) { + compassConfigMutable()->mag_align = CW90_DEG; + serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL; batteryMetersConfigMutable()->current.offset = CURRENTOFFSET; batteryMetersConfigMutable()->current.scale = CURRENTSCALE; diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 0a4522d312..9f48f87c37 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -59,9 +59,6 @@ #define USE_MAG_IST8308 #define USE_MAG_LIS3MDL -#define MAG_AK9863_ALIGN CW0_DEG -#define MAG_MPU9250_ALIGN CW0_DEG - #define AK8963_CS_PIN PC15 #define AK8963_SPI_BUS BUS_SPI3 diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index ad5fdf17a7..3d86866897 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -47,7 +47,6 @@ #define USE_MAG_IST8310 #define USE_MAG_IST8308 #define USE_MAG_LIS3MDL -#define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define TEMPERATURE_I2C_BUS BUS_I2C2 @@ -104,8 +103,6 @@ //#define HIL -#define MAG_GPS_ALIGN CW180_DEG_FLIP - #define USE_ADC #define ADC_CHANNEL_1_PIN PC0 #define ADC_CHANNEL_2_PIN PC1 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index 1e6d6c9f42..fbd9683c92 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -40,7 +40,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 @@ -142,8 +141,6 @@ #define USE_I2C_DEVICE_4 #define USE_I2C_DEVICE_2 -#define MAG_GPS_ALIGN CW180_DEG_FLIP - #define SENSORS_SET (SENSOR_ACC|SENSOR_MAG|SENSOR_BARO) #define USE_NAV diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index 07e09434a1..0bb51355ec 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -40,7 +40,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 7963cae436..9d86514f9d 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -45,7 +45,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define MAG_HMC5883_ALIGN CW90_DEG #define USE_MAG_HMC5883 #define USE_MAG_AK8963 #define USE_MAG_AK8975 diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index 0b33a58755..872327b41a 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -119,7 +119,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -//#define MAG_HMC5883_ALIGN CW90_DEG #define USE_MAG_AK8963 #define USE_MAG_AK8975 #define USE_MAG_HMC5883 diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index eae3c8ebdd..507936adf5 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -48,7 +48,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define MAG_AK8975_ALIGN CW90_DEG_FLIP #define USE_MAG_AK8975 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 diff --git a/src/main/target/CLRACINGF4AIR/config.c b/src/main/target/CLRACINGF4AIR/config.c new file mode 100644 index 0000000000..fa0d0196d7 --- /dev/null +++ b/src/main/target/CLRACINGF4AIR/config.c @@ -0,0 +1,55 @@ +/* + * 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 "common/axis.h" + +#include "drivers/sensor.h" +#include "drivers/pwm_esc_detect.h" +#include "drivers/pwm_mapping.h" +#include "drivers/pwm_output.h" +#include "drivers/serial.h" + +#include "fc/rc_controls.h" + +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "io/serial.h" + +#include "telemetry/telemetry.h" + +#include "sensors/battery.h" +#include "sensors/sensors.h" + +#include "config/config_master.h" +#include "config/feature.h" + +#include "sensors/battery.h" +#include "sensors/compass.h" + +void targetConfiguration(void) +{ + compassConfigMutable()->mag_align = CW90_DEG; +} diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h index e5f5aa5a4f..56c3918aff 100644 --- a/src/main/target/CLRACINGF4AIR/target.h +++ b/src/main/target/CLRACINGF4AIR/target.h @@ -52,7 +52,6 @@ #define USE_MAG #define USE_MAG_MPU9250 -#define MAG_MPU9250_ALIGN CW90_DEG // MPU6 interrupts #define USE_EXTI diff --git a/src/main/target/COLIBRI/config.c b/src/main/target/COLIBRI/config.c index 02c9785f62..84e9e625ed 100755 --- a/src/main/target/COLIBRI/config.c +++ b/src/main/target/COLIBRI/config.c @@ -20,18 +20,18 @@ #include "platform.h" #include "config/feature.h" - #include "fc/config.h" - #include "flight/mixer.h" - #include "io/serial.h" - #include "rx/rx.h" +#include "sensors/compass.h" void targetConfiguration(void) { rxConfigMutable()->serialrx_provider = SERIALRX_SBUS; rxConfigMutable()->receiverType = RX_TYPE_SERIAL; serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL; +#ifdef QUANTON + compassConfigMutable()->mag_align = CW90_DEG; +#endif } diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index eff71edd24..c111bcb378 100755 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -60,10 +60,8 @@ #ifdef QUANTON #define IMU_MPU6000_ALIGN CW90_DEG -#define MAG_HMC5883_ALIGN CW90_DEG #else #define IMU_MPU6000_ALIGN CW270_DEG_FLIP -#define MAG_HMC5883_ALIGN CW270_DEG_FLIP #endif #define USE_BARO diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h index 0b37dc2c11..c2787f71ef 100644 --- a/src/main/target/DALRCF405/target.h +++ b/src/main/target/DALRCF405/target.h @@ -151,3 +151,5 @@ //TIMER #define MAX_PWM_OUTPUT_PORTS 10 + +#define USE_DSHOT diff --git a/src/main/target/F4BY/config.c b/src/main/target/F4BY/config.c new file mode 100755 index 0000000000..d538e04b50 --- /dev/null +++ b/src/main/target/F4BY/config.c @@ -0,0 +1,32 @@ +/* + * 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 "platform.h" + +#include "config/feature.h" +#include "fc/config.h" +#include "flight/mixer.h" +#include "io/serial.h" +#include "rx/rx.h" +#include "sensors/compass.h" + +void targetConfiguration(void) +{ + compassConfigMutable()->mag_align = CW90_DEG; +} diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 6dbb58442f..e50a360d41 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -47,7 +47,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define MAG_HMC5883_ALIGN CW90_DEG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index d862359d49..e7090f09b7 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -59,9 +59,8 @@ void targetConfiguration(void) gyroConfigMutable()->looptime = 1000; - gyroConfigMutable()->gyroSync = 1; gyroConfigMutable()->gyro_lpf = 0; // 256 Hz - gyroConfigMutable()->gyro_soft_lpf_hz = 90; + gyroConfigMutable()->gyro_main_lpf_hz = 90; gyroConfigMutable()->gyro_notch_hz = 150; gyroConfigMutable()->gyro_notch_cutoff = 80; diff --git a/src/main/target/FF_F35_LIGHTNING/config.c b/src/main/target/FF_F35_LIGHTNING/config.c index 4de3455f24..d0825a06fe 100644 --- a/src/main/target/FF_F35_LIGHTNING/config.c +++ b/src/main/target/FF_F35_LIGHTNING/config.c @@ -18,21 +18,16 @@ #include #include - #include "config/config_master.h" - #include "flight/mixer.h" - #include "rx/rx.h" - #include "io/serial.h" - #include "telemetry/telemetry.h" +#include "sensors/compass.h" -// alternative defaults settings for FF_F35_LIGHTNING targets void targetConfiguration(void) { - motorConfigMutable()->maxthrottle = 2000; + compassConfigMutable()->mag_align = CW90_DEG; serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; serialConfigMutable()->portConfigs[1].msp_baudrateIndex = BAUD_57600; diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h index 73e3e0fee9..a916b186d7 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.h +++ b/src/main/target/FF_F35_LIGHTNING/target.h @@ -42,7 +42,6 @@ #define USE_MAG #define USE_MAG_MPU9250 -#define MAG_MPU9250_ALIGN CW90_DEG_FLIP #define USE_BARO #define USE_BARO_BMP280 diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 40c0fd199e..45b8b5b865 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -60,7 +60,6 @@ #define USE_MAG_IST8308 #define USE_MAG_QMC5883 #define USE_MAG_LIS3MDL -#define MAG_IST8310_ALIGN CW270_DEG // *************** Temperature sensor ***************** #define TEMPERATURE_I2C_BUS BUS_I2C1 diff --git a/src/main/target/FLYWOOF745/CMakeLists.txt b/src/main/target/FLYWOOF745/CMakeLists.txt new file mode 100644 index 0000000000..7839c348b3 --- /dev/null +++ b/src/main/target/FLYWOOF745/CMakeLists.txt @@ -0,0 +1,2 @@ +target_stm32f745xg(FLYWOOF745) +target_stm32f745xg(FLYWOOF745NANO) diff --git a/src/main/target/FLYWOOF745/target.c b/src/main/target/FLYWOOF745/target.c new file mode 100644 index 0000000000..7512bff47b --- /dev/null +++ b/src/main/target/FLYWOOF745/target.c @@ -0,0 +1,45 @@ +/* + * 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 "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" +#include "drivers/bus.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM1, CH3, PE13, TIM_USE_PPM, 0, 1), // PPM, DMA2_ST6 + + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M1 , DMA1_ST7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // M2 , DMA1_ST2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 2), // M3 , DMA2_ST2 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // M4 , DMA2_ST4 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M5 , DMA2_ST7 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M6 , DMA1_ST1 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M7 , DMA1_ST4 + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO | TIM_USE_MC_SERVO, 0, 0), // M8 , DMA1_ST5 + DEF_TIM(TIM4, CH1, PD12, TIM_USE_LED, 0, 0), // LED_STRIP, DMA1_ST0 +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/FLYWOOF745/target.h b/src/main/target/FLYWOOF745/target.h new file mode 100644 index 0000000000..ddf5690b2d --- /dev/null +++ b/src/main/target/FLYWOOF745/target.h @@ -0,0 +1,176 @@ +/* + * This file is part of INAV. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software 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 and Betaflight are distributed in the hope that they + * 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 software. + * + * If not, see . + */ + +#pragma once + +#ifdef FLYWOOF745 +#define TARGET_BOARD_IDENTIFIER "FWF7" +#define USBD_PRODUCT_STRING "FLYWOOF745" +#else +#define TARGET_BOARD_IDENTIFIER "FW7N" +#define USBD_PRODUCT_STRING "FLYWOOF745NANO" +#endif + +#define LED0 PA2// + +#define BEEPER PD15// +#define BEEPER_INVERTED + +#define USE_DSHOT +#define USE_ESC_SENSOR + +#define USE_MPU_DATA_READY_SIGNAL +#define USE_EXTI + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW270_DEG// +#define GYRO_INT_EXTI PE1// +#define MPU6000_CS_PIN SPI4_NSS_PIN// +#define MPU6000_SPI_BUS BUS_SPI4// + + +#define USB_IO +#define USE_VCP +#define VBUS_SENSING_ENABLED +#define VBUS_SENSING_PIN PA8// + +#define USE_UART1 +#define UART1_TX_PIN PA9// +#define UART1_RX_PIN PA10// + +#define USE_UART2 +#define UART2_TX_PIN PD5// +#define UART2_RX_PIN PD6// + +#define USE_UART3 + +#ifdef FLYWOOF745 +#define UART3_TX_PIN PB10// +#define UART3_RX_PIN PB11// +#else +#define UART3_TX_PIN PD8// +#define UART3_RX_PIN PD9// +#endif + +#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_UART6 +#define UART6_TX_PIN PC6// +#define UART6_RX_PIN PC7// + +#define USE_UART7 +#define UART7_TX_PIN PE8// +#define UART7_RX_PIN PE7// + +#define SERIAL_PORT_COUNT 8 //VCP,UART1,UART2,UART3,UART4,UART5,UART6,UART7 + +#define USE_SPI +#define USE_SPI_DEVICE_1 //FLASH +#define USE_SPI_DEVICE_2 //OSD +#define USE_SPI_DEVICE_4 //ICM20689 + +#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 SPI4_NSS_PIN PE4// +#define SPI4_SCK_PIN PE2// +#define SPI4_MISO_PIN PE5// +#define SPI4_MOSI_PIN PE6// + +#define USE_OSD + +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI2 +#define MAX7456_CS_PIN SPI2_NSS_PIN + +#define M25P16_CS_PIN SPI1_NSS_PIN +#define M25P16_SPI_BUS BUS_SPI1 +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB6 +#define I2C1_SDA PB7 + +#ifdef FLYWOOF745NANO +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PB10 +#define I2C2_SDA PB11 +#endif +#define USE_BARO +#define USE_BARO_BMP280 +#define BARO_I2C_BUS BUS_I2C1 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C1 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_MAG3110 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_LIS3MDL + +#define TEMPERATURE_I2C_BUS BUS_I2C1 + +#define RANGEFINDER_I2C_BUS BUS_I2C1 + +#define USE_ADC +#define ADC_CHANNEL_1_PIN PC2// +#define ADC_CHANNEL_2_PIN PC3// +#define ADC_CHANNEL_3_PIN PC5// + +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_1 +#define VBAT_ADC_CHANNEL ADC_CHN_2 +#define RSSI_ADC_CHANNEL ADC_CHN_3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_VBAT | FEATURE_BLACKBOX) +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define USE_LED_STRIP +#define WS2811_PIN PD12 // //TIM4_CH1 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 8 diff --git a/src/main/target/FRSKYPILOT/CMakeLists.txt b/src/main/target/FRSKYPILOT/CMakeLists.txt index 2f22760080..2a044b7cb0 100644 --- a/src/main/target/FRSKYPILOT/CMakeLists.txt +++ b/src/main/target/FRSKYPILOT/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f765xg(FRSKYPILOT) +target_stm32f765xg(FRSKYPILOT_LED) diff --git a/src/main/target/FRSKYPILOT/target.c b/src/main/target/FRSKYPILOT/target.c index 164e4b15cc..003b3759f5 100644 --- a/src/main/target/FRSKYPILOT/target.c +++ b/src/main/target/FRSKYPILOT/target.c @@ -30,18 +30,22 @@ BUSDEV_REGISTER_SPI_TAG(busdev_mpu6500, DEVHW_MPU6500, MPU6500_SPI_BUS, BUSDEV_REGISTER_SPI_TAG(busdev_mpu6000, DEVHW_MPU6000, MPU6000_SPI_BUS, MPU6000_CS_PIN, MPU6000_EXTI_PIN, 1, DEVFLAGS_NONE, IMU_MPU6000_ALIGN); const timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), - DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S2 + DEF_TIM(TIM1, CH1, PE9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 + DEF_TIM(TIM1, CH2, PE11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 + DEF_TIM(TIM1, CH3, PE13, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 + DEF_TIM(TIM1, CH4, PE14, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 + DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S9 +#ifdef FRSKYPILOT_LED + DEF_TIM(TIM2, CH2, PA1, TIM_USE_LED | TIM_USE_LED, 0, 0), // S10 now LED, S11 & S12 UART 3 only +#else + DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S10 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S11 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S12 +#endif DEF_TIM(TIM5, CH1, PA0, TIM_USE_BEEPER, 0, 0), // beeper }; diff --git a/src/main/target/FRSKYPILOT/target.h b/src/main/target/FRSKYPILOT/target.h index 87bd0b072b..d4da84dde0 100644 --- a/src/main/target/FRSKYPILOT/target.h +++ b/src/main/target/FRSKYPILOT/target.h @@ -200,6 +200,13 @@ #define TARGET_IO_PORTD 0xffff #define TARGET_IO_PORTE 0xffff -#define MAX_PWM_OUTPUT_PORTS 12 +#ifdef FRSKYPILOT_LED + #define USE_LED_STRIP + #define WS2811_PIN PA1 // S10 pad for iNav + + #define MAX_PWM_OUTPUT_PORTS 9 +#else + #define MAX_PWM_OUTPUT_PORTS 12 +#endif #define USE_DSHOT diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 134889a0f3..2b0f908a91 100755 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -57,7 +57,6 @@ # define USE_MAG # define MAG_I2C_BUS BUS_I2C1 # define USE_MAG_HMC5883 -# define MAG_HMC5883_ALIGN CW180_DEG # define USE_MAG_QMC5883 # define USE_MAG_MAG3110 # define USE_MAG_IST8310 diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index da8f8c9d1a..0d42c7e2d6 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -140,7 +140,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW180_DEG #define USE_MAG_QMC5883 #define USE_MAG_MAG3110 #define USE_MAG_IST8310 diff --git a/src/main/target/KROOZX/target.h b/src/main/target/KROOZX/target.h index d0b86ce00c..4f5eac5f7c 100755 --- a/src/main/target/KROOZX/target.h +++ b/src/main/target/KROOZX/target.h @@ -41,7 +41,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 diff --git a/src/main/target/MAMBAF405US/CMakeLists.txt b/src/main/target/MAMBAF405US/CMakeLists.txt index d4565822bf..544ae6a123 100644 --- a/src/main/target/MAMBAF405US/CMakeLists.txt +++ b/src/main/target/MAMBAF405US/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f405xg(MAMBAF405US) +target_stm32f405xg(MAMBAF405US_I2C) diff --git a/src/main/target/MAMBAF405US/config.c b/src/main/target/MAMBAF405US/config.c index 636c30ed9b..8c60250dc4 100644 --- a/src/main/target/MAMBAF405US/config.c +++ b/src/main/target/MAMBAF405US/config.c @@ -44,10 +44,21 @@ #include "sensors/sensors.h" #include "telemetry/telemetry.h" +#include "fc/fc_msp_box.h" +#include "io/piniobox.h" void targetConfiguration(void) { +#ifdef MAMBAF405US // MSP @ 19200 on SERIAL4 serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_19200; +#else + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_ESCSERIAL; + pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; + + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; +#endif } diff --git a/src/main/target/MAMBAF405US/target.c b/src/main/target/MAMBAF405US/target.c index ffcee4ee53..7fb13ca119 100644 --- a/src/main/target/MAMBAF405US/target.c +++ b/src/main/target/MAMBAF405US/target.c @@ -27,10 +27,17 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM11, CH1, PB9, TIM_USE_PPM, 0, 0 ), // PPM IN +#ifdef MAMBAF405US_I2C + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S1 pin A9: DMA2 Stream 6 Channel 0 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S2 pin A8: DMA2 Stream 6 Channel 0 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S3 pin C9: DMA2 Stream 7 Channel 7 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1 ), // S4 pin C8: DMA2 Stream 2 Channel 0 +#else DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S4_OUT – D(2, 2, 6) DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1 ), // S3_OUT – D(2, 1, 6) DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S2_OUT – D(2, 7, 7) DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0 ), // S1_OUT – D(2, 4, 7) +#endif DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – D(1, 6, 3) }; diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h index f1ca6b7b00..adb4c09f1e 100644 --- a/src/main/target/MAMBAF405US/target.h +++ b/src/main/target/MAMBAF405US/target.h @@ -20,7 +20,12 @@ #define USE_TARGET_CONFIG #define TARGET_BOARD_IDENTIFIER "MBUS" + +#ifdef MAMBAF405US_I2C +#define USBD_PRODUCT_STRING "MAMBAF405US_I2C" +#else #define USBD_PRODUCT_STRING "MAMBAF405US" +#endif // ******** Board LEDs ********************** #define LED0 PC15 @@ -44,26 +49,40 @@ // *************** Baro ************************** #define USE_I2C -#define USE_I2C_DEVICE_2 +#ifdef MAMBAF405US_I2C + +#define USE_I2C_DEVICE_1 +#define I2C2_SCL PB8 +#define I2C2_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 + +#else + +#define USE_I2C_DEVICE_2 #define I2C2_SCL PB10 // SCL pad TX3 #define I2C2_SDA PB11 // SDA pad RX3 #define DEFAULT_I2C_BUS BUS_I2C2 +#endif + #define USE_BARO #define BARO_I2C_BUS DEFAULT_I2C_BUS #define USE_BARO_BMP280 #define USE_BARO_MS5611 - -#define BNO055_I2C_BUS BUS_I2C2 +#define USE_BARO_DPS310 //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS +#define USE_MAG_AK8975 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL // ******* SERIAL ******** @@ -93,12 +112,6 @@ #define UART6_TX_PIN PC6 #define UART6_RX_PIN PC7 -/* -#define USE_SOFTSERIAL1 -#define SOFTSERIAL_1_TX_PIN PA2 -#define SOFTSERIAL_1_RX_PIN PA2 -*/ - #define SERIAL_PORT_COUNT 7 #define USE_UART_INVERTER @@ -180,5 +193,10 @@ // ESC-related features #define USE_DSHOT #define USE_SERIALSHOT -//#define USE_ESC_SENSOR +#define USE_ESC_SENSOR #define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS +#define BNO055_I2C_BUS DEFAULT_I2C_BUS \ No newline at end of file diff --git a/src/main/target/MAMBAF722/CMakeLists.txt b/src/main/target/MAMBAF722/CMakeLists.txt index e893c24684..7d9092268d 100644 --- a/src/main/target/MAMBAF722/CMakeLists.txt +++ b/src/main/target/MAMBAF722/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f722xe(MAMBAF722) +target_stm32f722xe(MAMBAF722_I2C) diff --git a/src/main/target/MAMBAF722/config.c b/src/main/target/MAMBAF722/config.c index 97de32d85a..cc2e226724 100644 --- a/src/main/target/MAMBAF722/config.c +++ b/src/main/target/MAMBAF722/config.c @@ -50,7 +50,14 @@ void targetConfiguration(void) // RX6 is hard-wired to ESC-telemetry serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART6)].functionMask = FUNCTION_ESCSERIAL; +#ifdef MAMBAF722_I2C // Built-in bluetooth on SERIAL4 + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_RX_SERIAL; serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].functionMask = FUNCTION_MSP; - serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_19200; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART4)].msp_baudrateIndex = BAUD_115200; +#else + // Built-in bluetooth on SERIAL1 + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].functionMask = FUNCTION_MSP; + serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(SERIAL_PORT_USART1)].msp_baudrateIndex = BAUD_19200; +#endif } diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h index 20bb2e4873..2c1cb07cf7 100644 --- a/src/main/target/MAMBAF722/target.h +++ b/src/main/target/MAMBAF722/target.h @@ -20,7 +20,11 @@ #define USE_TARGET_CONFIG #define TARGET_BOARD_IDENTIFIER "MBF7" +#ifdef MAMBAF722_I2C +#define USBD_PRODUCT_STRING "MAMBAF722_I2C" +#else #define USBD_PRODUCT_STRING "MAMBAF722" +#endif // ******** Board LEDs ********************** #define LED0 PC15 @@ -42,28 +46,39 @@ #define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW180_DEG -// *************** Baro ************************** #define USE_I2C + +#ifdef MAMBAF722_I2C + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 +#define DEFAULT_I2C_BUS BUS_I2C1 + +#else + #define USE_I2C_DEVICE_2 #define I2C2_SCL PB10 // SCL pad TX3 #define I2C2_SDA PB11 // SDA pad RX3 #define DEFAULT_I2C_BUS BUS_I2C2 +#endif + +// *************** Baro ************************** #define USE_BARO #define BARO_I2C_BUS DEFAULT_I2C_BUS - #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 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL // ******* SERIAL ******** @@ -74,7 +89,6 @@ #define USE_UART4 #define USE_UART5 #define USE_UART6 -#define USE_SOFTSERIAL1 #define UART1_TX_PIN PB6 #define UART1_RX_PIN PB7 @@ -94,11 +108,20 @@ #define UART6_TX_PIN PC6 #define UART6_RX_PIN PC7 +#ifdef MAMBAF722_I2C + +#define SERIAL_PORT_COUNT 7 + +#else + +#define USE_SOFTSERIAL1 + #define SOFTSERIAL_1_TX_PIN PA2 #define SOFTSERIAL_1_RX_PIN PA2 - #define SERIAL_PORT_COUNT 8 +#endif + // ******* SPI ******** #define USE_SPI @@ -152,10 +175,14 @@ // ******* FEATURES ******** #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define SERIALRX_UART SERIAL_PORT_USART2 +#define SERIALRX_UART SERIAL_PORT_USART1 #define SERIALRX_PROVIDER SERIALRX_SBUS +#ifdef MAMBAF722_I2C +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY) +#else #define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) +#endif #define TARGET_IO_PORTA 0xffff #define TARGET_IO_PORTB 0xffff @@ -170,3 +197,14 @@ #define USE_SERIALSHOT #define USE_ESC_SENSOR #define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#ifdef MAMBAF722_I2C + +#define USE_RANGEFINDER +#define RANGEFINDER_I2C_BUS DEFAULT_I2C_BUS +#define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define PITOT_I2C_BUS DEFAULT_I2C_BUS + +#define BNO055_I2C_BUS DEFAULT_I2C_BUS + +#endif \ No newline at end of file diff --git a/src/main/target/MATEKF405CAN/config.c b/src/main/target/MATEKF405CAN/config.c index f3f22b74a9..34166b5512 100644 --- a/src/main/target/MATEKF405CAN/config.c +++ b/src/main/target/MATEKF405CAN/config.c @@ -20,11 +20,12 @@ #include "config/config_master.h" #include "config/feature.h" #include "io/serial.h" +#include "sensors/compass.h" void targetConfiguration(void) { - + compassConfigMutable()->mag_align = CW0_DEG_FLIP; 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.h b/src/main/target/MATEKF405CAN/target.h index f2c0999ee8..b9dde1e434 100644 --- a/src/main/target/MATEKF405CAN/target.h +++ b/src/main/target/MATEKF405CAN/target.h @@ -85,7 +85,6 @@ #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 diff --git a/src/main/target/MATEKF411SE/CMakeLists.txt b/src/main/target/MATEKF411SE/CMakeLists.txt index 79d7f68c5a..c7b79ee634 100644 --- a/src/main/target/MATEKF411SE/CMakeLists.txt +++ b/src/main/target/MATEKF411SE/CMakeLists.txt @@ -1,3 +1,4 @@ target_stm32f411xe(MATEKF411SE) target_stm32f411xe(MATEKF411SE_PINIO) target_stm32f411xe(MATEKF411SE_FD_SFTSRL1) +target_stm32f411xe(MATEKF411SE_SS2_CH6) diff --git a/src/main/target/MATEKF411SE/target.c b/src/main/target/MATEKF411SE/target.c index 1a2da60c6d..aaac4b55f7 100755 --- a/src/main/target/MATEKF411SE/target.c +++ b/src/main/target/MATEKF411SE/target.c @@ -24,19 +24,24 @@ #include "drivers/pinio.h" const timerHardware_t timerHardware[] = { - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,4,5) - DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,5,5) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(2,1,6) - DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 D(2,2,6) - DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S5 D(2,6,6) - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,7,2) + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(1,4,5) + DEF_TIM(TIM3, CH2, PB5, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(1,5,5) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S3 D(2,1,6) + DEF_TIM(TIM1, CH2, PA9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S4 D(2,2,6) + DEF_TIM(TIM1, CH3, PA10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 1), // S5 D(2,6,6) +#ifndef MATEKF411SE_SS2_CH6 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,7,2) - DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), //ST1 pad -softserial_tx1 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 pad -softserial_tx2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 pad - softserial_tx2 +#else + DEF_TIM(TIM4, CH3, PB8, TIM_USE_ANY, 0, 0), //CH6 pad - softserial 2 +#endif - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM + DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), //ST1 pad -softserial_tx1 + + 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) + 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 59d59e42de..2da0393f54 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -77,8 +77,13 @@ #endif #define USE_SOFTSERIAL2 +#ifdef MATEKF411SE_SS2_CH6 +#define SOFTSERIAL_2_TX_PIN PB8 // CH6 pad +#define SOFTSERIAL_2_RX_PIN PB8 +#else #define SOFTSERIAL_2_TX_PIN PA2 // TX2 pad #define SOFTSERIAL_2_RX_PIN PA2 +#endif #define SERIAL_PORT_COUNT 5 diff --git a/src/main/target/MATEKF722SE/CMakeLists.txt b/src/main/target/MATEKF722SE/CMakeLists.txt index a541da229f..75aa91ec0a 100644 --- a/src/main/target/MATEKF722SE/CMakeLists.txt +++ b/src/main/target/MATEKF722SE/CMakeLists.txt @@ -1,2 +1,3 @@ target_stm32f722xe(MATEKF722SE) target_stm32f722xe(MATEKF722MINI) +target_stm32f722xe(MATEKF722SE_8MOTOR) diff --git a/src/main/target/MATEKF722SE/target.c b/src/main/target/MATEKF722SE/target.c index 2c4170a1ce..36d49b8cfd 100644 --- a/src/main/target/MATEKF722SE/target.c +++ b/src/main/target/MATEKF722SE/target.c @@ -38,8 +38,13 @@ const timerHardware_t timerHardware[] = { 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) +#if (defined(MATEKF722SE_8MOTOR)) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 UP1-6 D(1, 0, 2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 UP1-6 D(1, 3, 2) +#else 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) +#endif DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0) diff --git a/src/main/target/MATEKH743/target.c b/src/main/target/MATEKH743/target.c index 209789cb8a..5a77e56bcf 100644 --- a/src/main/target/MATEKH743/target.c +++ b/src/main/target/MATEKH743/target.c @@ -29,12 +29,30 @@ BUSDEV_REGISTER_SPI_TAG(busdev_imu1_6000, DEVHW_MPU6000, IMU1_SPI_BUS, IMU1_CS_PIN, IMU1_EXTI_PIN, 0, DEVFLAGS_NONE, IMU1_ALIGN); BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6500, DEVHW_MPU6500, IMU2_SPI_BUS, IMU2_CS_PIN, IMU2_EXTI_PIN, 1, DEVFLAGS_NONE, IMU2_ALIGN); -//BUSDEV_REGISTER_SPI_TAG(busdev_imu1_6500, DEVHW_MPU6500, IMU1_SPI_BUS, IMU1_CS_PIN, IMU1_EXTI_PIN, 0, DEVFLAGS_NONE, IMU1_ALIGN); -//BUSDEV_REGISTER_SPI_TAG(busdev_imu2_6000, DEVHW_MPU6000, IMU2_SPI_BUS, IMU2_CS_PIN, IMU2_EXTI_PIN, 1, DEVFLAGS_NONE, IMU2_ALIGN); - const timerHardware_t timerHardware[] = { - DEF_TIM(TIM2, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 + DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // S1 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 2), // S2 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 + DEF_TIM(TIM5, CH4, PA3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 + + DEF_TIM(TIM4, CH1, PD12, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S7 + DEF_TIM(TIM4, CH2, PD13, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S8 + DEF_TIM(TIM4, CH3, PD14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 1), // S9 + DEF_TIM(TIM4, CH4, PD15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA_NONE + + DEF_TIM(TIM15, CH1, PE5, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S11 + DEF_TIM(TIM15, CH2, PE6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S12 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // LED_2812 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + + DEF_TIM(TIM8, CH2, PC7, TIM_USE_PPM, 0, 0), // RX6 PPM + DEF_TIM(TIM8, CH1, PC6, TIM_USE_ANY, 0, 0), // TX6 + DEF_TIM(TIM16, CH1, PB8, TIM_USE_ANY, 0, 0), // RX4 + DEF_TIM(TIM17, CH1, PB9, TIM_USE_ANY, 0, 0), // TX4 }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h index cc3409c8f5..ca99c9ddcc 100644 --- a/src/main/target/MATEKH743/target.h +++ b/src/main/target/MATEKH743/target.h @@ -18,8 +18,8 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "H765" -#define USBD_PRODUCT_STRING "MATEKH765" +#define TARGET_BOARD_IDENTIFIER "H743" +#define USBD_PRODUCT_STRING "MATEKH743" #define LED0 PE3 #define LED1 PE4 @@ -27,9 +27,6 @@ #define BEEPER PA15 #define BEEPER_INVERTED - - - // *************** UART ***************************** #define USE_VCP @@ -67,23 +64,13 @@ #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART6 - - -#define MAX_PWM_OUTPUT_PORTS 15 - - - - -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTE 0xffff - // *************** IMU generic *********************** #define USE_DUAL_GYRO #define USE_TARGET_IMU_HARDWARE_DESCRIPTORS +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL + // *************** SPI1 IMU1 ************************* #define USE_SPI #define USE_SPI_DEVICE_1 @@ -105,12 +92,10 @@ #define SPI2_MISO_PIN PB14 #define SPI2_MOSI_PIN PB15 -/* #define USE_OSD #define USE_MAX7456 #define MAX7456_SPI_BUS BUS_SPI2 #define MAX7456_CS_PIN PB12 -*/ // *************** SPI3 SD BLACKBOX******************* /* @@ -146,6 +131,7 @@ #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_I2C1 @@ -163,20 +149,16 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 - - -#if 0 -#define USE_EXTI -#define USE_MPU_DATA_READY_SIGNAL - // *************** ADC ***************************** #define USE_ADC #define ADC_INSTANCE ADC1 -#define ADC_CHANNEL_1_PIN PC2 -#define ADC_CHANNEL_2_PIN PC3 -#define ADC_CHANNEL_3_PIN PC1 -#define ADC_CHANNEL_4_PIN PC0 +#define ADC_CHANNEL_1_PIN PC0 //ADC123 VBAT1 +#define ADC_CHANNEL_2_PIN PC1 //ADC123 CURR1 +#define ADC_CHANNEL_3_PIN PC5 //ADC12 RSSI +#define ADC_CHANNEL_4_PIN PC4 //ADC12 AirS +#define ADC_CHANNEL_5_PIN PA4 //ADC12 VB2 +#define ADC_CHANNEL_6_PIN PA7 //ADC12 CU2 #define VBAT_ADC_CHANNEL ADC_CHN_1 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 @@ -198,7 +180,13 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define MAX_PWM_OUTPUT_PORTS 15 #define USE_DSHOT #define USE_ESC_SENSOR -#define USE_SERIALSHOT -#endif \ No newline at end of file +#define USE_SERIALSHOT \ No newline at end of file diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 2a4383ab4d..4027144c8d 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -90,7 +90,6 @@ #define USE_MAG #define MAG_I2C_BUS I2C_EXT_BUS -#define MAG_HMC5883_ALIGN CW90_DEG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index d82ba047b1..50e10578b4 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -149,7 +149,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 diff --git a/src/main/target/PIXRACER/config.c b/src/main/target/PIXRACER/config.c new file mode 100755 index 0000000000..d538e04b50 --- /dev/null +++ b/src/main/target/PIXRACER/config.c @@ -0,0 +1,32 @@ +/* + * 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 "platform.h" + +#include "config/feature.h" +#include "fc/config.h" +#include "flight/mixer.h" +#include "io/serial.h" +#include "rx/rx.h" +#include "sensors/compass.h" + +void targetConfiguration(void) +{ + compassConfigMutable()->mag_align = CW90_DEG; +} diff --git a/src/main/target/PIXRACER/target.h b/src/main/target/PIXRACER/target.h index 5d6f71b1c8..96fcd68237 100755 --- a/src/main/target/PIXRACER/target.h +++ b/src/main/target/PIXRACER/target.h @@ -44,7 +44,6 @@ #define USE_IMU_MPU9250 #define IMU_MPU9250_ALIGN CW180_DEG_FLIP -#define MAG_MPU9250_ALIGN CW90_DEG #define USE_DUAL_GYRO diff --git a/src/main/target/RADIX/target.h b/src/main/target/RADIX/target.h index 2f778992b5..dd29db84e6 100644 --- a/src/main/target/RADIX/target.h +++ b/src/main/target/RADIX/target.h @@ -36,7 +36,6 @@ // #define USE_MAG // #define MAG_I2C_BUS BUS_I2C1 -// #define MAG_HMC5883_ALIGN CW90_DEG // #define USE_MAG_HMC5883 // #define USE_MAG_QMC5883 // #define USE_MAG_IST8310 diff --git a/src/main/target/REVO/config.c b/src/main/target/REVO/config.c new file mode 100755 index 0000000000..d538e04b50 --- /dev/null +++ b/src/main/target/REVO/config.c @@ -0,0 +1,32 @@ +/* + * 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 "platform.h" + +#include "config/feature.h" +#include "fc/config.h" +#include "flight/mixer.h" +#include "io/serial.h" +#include "rx/rx.h" +#include "sensors/compass.h" + +void targetConfiguration(void) +{ + compassConfigMutable()->mag_align = CW90_DEG; +} diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index 360d097c69..f8a3c7264c 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -47,7 +47,6 @@ #define USE_DUAL_MAG #define MAG_I2C_BUS_EXT BUS_I2C2 #define MAG_I2C_BUS_INT BUS_I2C1 -#define MAG_HMC5883_ALIGN CW90_DEG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 @@ -131,8 +130,6 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT -#define MAG_GPS_ALIGN CW180_DEG_FLIP - #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) diff --git a/src/main/target/SPARKY/target.h b/src/main/target/SPARKY/target.h index 26baef81c2..5052027dc4 100644 --- a/src/main/target/SPARKY/target.h +++ b/src/main/target/SPARKY/target.h @@ -42,7 +42,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 -#define MAG_AK8975_ALIGN CW0_DEG #define USE_MAG_AK8975 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 diff --git a/src/main/target/SPARKY2/config.c b/src/main/target/SPARKY2/config.c new file mode 100755 index 0000000000..a6dc77f9e0 --- /dev/null +++ b/src/main/target/SPARKY2/config.c @@ -0,0 +1,32 @@ +/* + * 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 "platform.h" + +#include "config/feature.h" +#include "fc/config.h" +#include "flight/mixer.h" +#include "io/serial.h" +#include "rx/rx.h" +#include "sensors/compass.h" + +void targetConfiguration(void) +{ + compassConfigMutable()->mag_align = CW0_DEG; +} diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index c1d18603d5..d5b291b323 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -42,7 +42,6 @@ #define USE_MAG #define USE_MAG_MPU9250 -#define MAG_MPU9250_ALIGN CW0_DEG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h index c51b558832..3920653df7 100644 --- a/src/main/target/SPEEDYBEEF4/target.h +++ b/src/main/target/SPEEDYBEEF4/target.h @@ -140,7 +140,6 @@ #define USE_MAG_IST8310 #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL -#define MAG_HMC5883_ALIGN CW90_DEG /*** ADC ***/ #define USE_ADC @@ -171,3 +170,5 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) + +#define RANGEFINDER_I2C_BUS BUS_I2C1 \ No newline at end of file diff --git a/src/main/target/SPRACINGF3/target.h b/src/main/target/SPRACINGF3/target.h index 05727f5d42..bab9f28842 100644 --- a/src/main/target/SPRACINGF3/target.h +++ b/src/main/target/SPRACINGF3/target.h @@ -42,7 +42,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define MAG_HMC5883_ALIGN CW270_DEG #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 #define USE_MAG_IST8310 diff --git a/src/main/target/SPRACINGF3EVO/target.h b/src/main/target/SPRACINGF3EVO/target.h index c2b139a044..a667188092 100755 --- a/src/main/target/SPRACINGF3EVO/target.h +++ b/src/main/target/SPRACINGF3EVO/target.h @@ -47,7 +47,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define MAG_MPU9250_ALIGN CW270_DEG #define USE_MAG_MPU9250 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 diff --git a/src/main/target/SPRACINGF3MINI/target.h b/src/main/target/SPRACINGF3MINI/target.h index 6a7ec647f1..406ee535f0 100644 --- a/src/main/target/SPRACINGF3MINI/target.h +++ b/src/main/target/SPRACINGF3MINI/target.h @@ -47,7 +47,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 -#define MAG_AK8963_ALIGN CW270_DEG #define USE_MAG_AK8963 #define USE_MAG_HMC5883 #define USE_MAG_QMC5883 diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index 7f05ef7ef4..741f224252 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -64,7 +64,6 @@ #define USE_MAG #define MAG_I2C_BUS BUS_I2C2 #define USE_MAG_HMC5883 -#define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_MAG_QMC5883 #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h index 820431f3e3..a95336a738 100644 --- a/src/main/target/YUPIF7/target.h +++ b/src/main/target/YUPIF7/target.h @@ -44,7 +44,6 @@ #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 diff --git a/src/main/target/ZEEZF7/CMakeLists.txt b/src/main/target/ZEEZF7/CMakeLists.txt index 513e023060..eb14cb8802 100644 --- a/src/main/target/ZEEZF7/CMakeLists.txt +++ b/src/main/target/ZEEZF7/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f722xe(ZEEZF7) +target_stm32f722xe(ZEEZF7V2) diff --git a/src/main/target/ZEEZF7/config.c b/src/main/target/ZEEZF7/config.c index ffbe3639f0..bde6f06f21 100644 --- a/src/main/target/ZEEZF7/config.c +++ b/src/main/target/ZEEZF7/config.c @@ -25,6 +25,8 @@ void targetConfiguration(void) { +#ifndef ZEEZF7V2 pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; // VTX power switcher //pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +#endif } diff --git a/src/main/target/ZEEZF7/target.c b/src/main/target/ZEEZF7/target.c index 86573bcc1c..b0ede6a513 100755 --- a/src/main/target/ZEEZF7/target.c +++ b/src/main/target/ZEEZF7/target.c @@ -25,10 +25,21 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { +#ifdef ZEEZF7V2 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S3 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR, 0, 0), // S4 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // S5 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR, 0, 0), // S6 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR, 0, 0), // S7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S8 +#else DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR, 0, 0), // S1 DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S2 DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S3 DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S4 +#endif DEF_TIM(TIM1, CH2N, PB0, TIM_USE_LED , 0, 0), // LED STRIP }; diff --git a/src/main/target/ZEEZF7/target.h b/src/main/target/ZEEZF7/target.h index 2d6e4ab51f..4558922417 100755 --- a/src/main/target/ZEEZF7/target.h +++ b/src/main/target/ZEEZF7/target.h @@ -17,9 +17,13 @@ #pragma once +#ifdef ZEEZF7V2 +#define TARGET_BOARD_IDENTIFIER "ZEF7V2" +#define USBD_PRODUCT_STRING "ZEEZF7V2" +#else #define TARGET_BOARD_IDENTIFIER "ZEF7" - #define USBD_PRODUCT_STRING "ZEEZF7" +#endif #define LED0 PC14 #define LED1 PC15 @@ -29,6 +33,21 @@ // *************** Gyro & ACC ********************** #define USE_SPI + +#ifdef ZEEZF7V2 + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 +#define GYRO_INT_EXTI PC4 +#define IMU_MPU6000_ALIGN CW0_DEG + +#else + #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 @@ -36,15 +55,44 @@ #define MPU6000_CS_PIN PB12 #define MPU6000_SPI_BUS BUS_SPI2 +#define GYRO_INT_EXTI PC9 -#define USE_EXTI -#define GYRO_INT_EXTI PC9 -#define USE_MPU_DATA_READY_SIGNAL - -#define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW180_DEG +#endif + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL +#define USE_IMU_MPU6000 + // *************** I2C/Baro/Mag ********************* +#ifdef ZEEZF7V2 + +#define USE_I2C +#define USE_BARO +#define USE_BARO_BMP280 +#define BARO_I2C_BUS BUS_I2C1 + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// External I2C Pads -- I2C3 +#define USE_I2C_DEVICE_3 +#define I2C3_SCL PA8 +#define I2C3_SDA PC9 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C3 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_AK8975 + +#else // Target has no I2C but can use MSP devices, such as those provided on Mateksys MQ8-CAN/MSP // Which contains: GPS SAM-M8Q, Compass QMC5883L, Barometer DPS310 // See: http://www.mateksys.com/?portfolio=m8q-can @@ -55,18 +103,34 @@ #define USE_MAG #define USE_MAG_QMC5883 +#endif + // *************** Flash **************************** -#define USE_SPI_DEVICE_1 +#ifdef ZEEZF7V2 +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 + +#else + +#define USE_SPI_DEVICE_1 #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define USE_FLASHFS -#define USE_FLASH_M25P16 #define M25P16_SPI_BUS BUS_SPI1 #define M25P16_CS_PIN PA4 + +#endif + +#define USE_FLASHFS +#define USE_FLASH_M25P16 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT // *************** OSD ***************************** @@ -82,14 +146,16 @@ #define MAX7456_CS_PIN PA15 // *************** PINIO *************************** +#ifdef ZEEZF7 + #define USE_PINIO #define USE_PINIOBOX #define PINIO1_PIN PB11 // VTX power switcher +#endif + // *************** UART ***************************** #define USE_VCP -//#define VBUS_SENSING_PIN PB12 -//#define VBUS_SENSING_ENABLED #define USE_UART1 #define UART1_RX_PIN PA10 @@ -111,12 +177,20 @@ #define UART5_RX_PIN PD2 #define UART5_TX_PIN PC12 +#ifdef ZEEZF7V2 + +#define SERIAL_PORT_COUNT 6 + +#else + #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 #define SERIAL_PORT_COUNT 7 +#endif + #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART4 @@ -125,15 +199,24 @@ #define USE_ADC #define ADC_INSTANCE ADC1 #define ADC1_DMA_STREAM DMA2_Stream0 +#ifdef ZEEZF7V2 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#else #define ADC_CHANNEL_1_PIN PC2 #define ADC_CHANNEL_2_PIN PC3 +#endif #define VBAT_ADC_CHANNEL ADC_CHN_1 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD ) +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_LED_STRIP) + +#ifdef ZEEZF7V2 +#define CURRENT_METER_SCALE 250 +#endif #define USE_LED_STRIP -#define WS2811_PIN PB0 +#define WS2811_PIN PB0 #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -142,7 +225,11 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define MAX_PWM_OUTPUT_PORTS 4 +#ifdef ZEEZF7V2 +#define MAX_PWM_OUTPUT_PORTS 8 +#else +#define MAX_PWM_OUTPUT_PORTS 4 +#endif #define USE_DSHOT #define USE_ESC_SENSOR #define USE_SERIALSHOT diff --git a/src/main/target/common.h b/src/main/target/common.h index 15ae8cfc24..08d79b7809 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -84,9 +84,14 @@ #define USE_PITOT_ADC #define USE_PITOT_VIRTUAL +#define USE_ALPHA_BETA_GAMMA_FILTER #define USE_DYNAMIC_FILTERS #define USE_GYRO_KALMAN +<<<<<<< HEAD #define USE_RATE_DYNAMICS +======= +#define USE_SMITH_PREDICTOR +>>>>>>> master #define USE_EXTENDED_CMS_MENUS #define USE_HOTT_TEXTMODE @@ -166,6 +171,8 @@ #define USE_SECONDARY_IMU #define USE_IMU_BNO055 +#define USE_POWER_LIMITS + #else // MCU_FLASH_SIZE < 256 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR #endif diff --git a/src/main/target/link/stm32_flash_F765xI.ld b/src/main/target/link/stm32_flash_F765xI.ld deleted file mode 100644 index 130169290e..0000000000 --- a/src/main/target/link/stm32_flash_F765xI.ld +++ /dev/null @@ -1,55 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f765.ld -** -** Abstract : Linker script for STM32F765xITx Device with -** 2048KByte FLASH, 512KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x00000000 to 0x00003FFF 16K TCM RAM, - -0x08000000 to 0x081FFFFF 2048K full flash, -0x08000000 to 0x08007FFF 32K isr vector, startup code, -0x08008000 to 0x0800FFFF 32K config, -0x08010000 to 0x081FFFFF 1984K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K - - ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K - ITCM_FLASH_CFG (r) : ORIGIN = 0x00208000, LENGTH = 32K - ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 1984K - - AXIM_FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K - AXIM_FLASH_CFG (r) : ORIGIN = 0x08008000, LENGTH = 32K - AXIM_FLASH1 (rx) : ORIGIN = 0x08010000, LENGTH = 1984K - - DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - SRAM1 (rwx) : ORIGIN = 0x20020000, LENGTH = 368K - SRAM2 (rwx) : ORIGIN = 0x2007C000, LENGTH = 16K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("FLASH", AXIM_FLASH) -REGION_ALIAS("FLASH_CONFIG", AXIM_FLASH_CFG) -REGION_ALIAS("FLASH1", AXIM_FLASH1) - -REGION_ALIAS("STACKRAM", DTCM_RAM) -REGION_ALIAS("FASTRAM", DTCM_RAM) -REGION_ALIAS("RAM", SRAM1) - -INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_F765xI_bl.ld b/src/main/target/link/stm32_flash_F765xI_bl.ld deleted file mode 100644 index 45ced4e7dd..0000000000 --- a/src/main/target/link/stm32_flash_F765xI_bl.ld +++ /dev/null @@ -1,56 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f765.ld -** -** Abstract : Linker script for STM32F765xITx Device with -** 2048KByte FLASH, 512KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x00000000 to 0x00003FFF 16K TCM RAM, - -0x08000000 to 0x081FFFFF 2048K full flash, -0x08000000 to 0x08007FFF 32K isr vector, startup code, -0x08008000 to 0x0800FFFF 32K config, -0x08010000 to 0x081FFFFF 1984K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K - - ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K - ITCM_FLASH_CFG (r) : ORIGIN = 0x00208000, LENGTH = 32K - ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 1984K - - AXIM_FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 32K - AXIM_FIRMWARE (rx) : ORIGIN = 0x08008000, LENGTH = 32K - AXIM_FLASH_CFG (r) : ORIGIN = 0x08010000, LENGTH = 32K - - DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - SRAM1 (rwx) : ORIGIN = 0x20020000, LENGTH = 368K - SRAM2 (rwx) : ORIGIN = 0x2007C000, LENGTH = 16K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("FLASH", AXIM_FLASH) -REGION_ALIAS("FLASH_CONFIG", AXIM_FLASH_CFG) - -REGION_ALIAS("STACKRAM", DTCM_RAM) -REGION_ALIAS("FASTRAM", DTCM_RAM) -REGION_ALIAS("RAM", SRAM1) - -__firmware_start = ORIGIN(AXIM_FIRMWARE); - -INCLUDE "stm32_flash_f7.ld" diff --git a/src/main/target/link/stm32_flash_F765xI_for_bl.ld b/src/main/target/link/stm32_flash_F765xI_for_bl.ld deleted file mode 100644 index 14f2a4bedf..0000000000 --- a/src/main/target/link/stm32_flash_F765xI_for_bl.ld +++ /dev/null @@ -1,57 +0,0 @@ -/* -***************************************************************************** -** -** File : stm32_flash_f765.ld -** -** Abstract : Linker script for STM32F765xITx Device with -** 2048KByte FLASH, 512KByte RAM -** -***************************************************************************** -*/ - -/* Stack & Heap sizes */ -_Min_Heap_Size = 0; -_Min_Stack_Size = 0x1800; - -/* Entry Point */ -ENTRY(Reset_Handler) - -/* -0x00000000 to 0x00003FFF 16K TCM RAM, - -0x08000000 to 0x081FFFFF 2048K full flash, -0x08000000 to 0x08007FFF 32K isr vector, startup code, -0x08008000 to 0x0800FFFF 32K config, -0x08010000 to 0x081FFFFF 1984K firmware, -*/ - -/* Specify the memory areas */ -MEMORY -{ - ITCM_RAM (rx) : ORIGIN = 0x00000000, LENGTH = 16K - - ITCM_FLASH (rx) : ORIGIN = 0x00200000, LENGTH = 32K - ITCM_FLASH_CFG (r) : ORIGIN = 0x00208000, LENGTH = 32K - ITCM_FLASH1 (rx) : ORIGIN = 0x00210000, LENGTH = 1984K - - AXIM_FLASH (rx) : ORIGIN = 0x08008000, LENGTH = 32K - AXIM_FLASH_CFG (r) : ORIGIN = 0x08010000, LENGTH = 32K - AXIM_FLASH1 (rx) : ORIGIN = 0x08018000, LENGTH = 1984K - - DTCM_RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K - SRAM1 (rwx) : ORIGIN = 0x20020000, LENGTH = 368K - SRAM2 (rwx) : ORIGIN = 0x2007C000, LENGTH = 16K - MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K -} - -REGION_ALIAS("FLASH", AXIM_FLASH) -REGION_ALIAS("FLASH_CONFIG", AXIM_FLASH_CFG) -REGION_ALIAS("FLASH1", AXIM_FLASH1) - -REGION_ALIAS("STACKRAM", DTCM_RAM) -REGION_ALIAS("FASTRAM", DTCM_RAM) -REGION_ALIAS("RAM", SRAM1) - -__firmware_start = ORIGIN(AXIM_FLASH); - -INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 6c9ded6869..06332a166c 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -733,7 +733,7 @@ void mavlinkSendHUDAndHeartbeat(void) mavSystemState = MAV_STATE_ACTIVE; } } - else if (isCalibrating()) { + else if (areSensorsCalibrating()) { mavSystemState = MAV_STATE_CALIBRATING; } else { diff --git a/src/test/unit/scheduler_unittest.cc.txt b/src/test/unit/scheduler_unittest.cc.txt index 96191469dd..ff8c4cdb0e 100644 --- a/src/test/unit/scheduler_unittest.cc.txt +++ b/src/test/unit/scheduler_unittest.cc.txt @@ -88,7 +88,7 @@ TEST(SchedulerUnittest, TestPriorites) EXPECT_EQ(14, TASK_COUNT); // if any of these fail then task priorities have changed and ordering in TestQueue needs to be re-checked EXPECT_EQ(TASK_PRIORITY_HIGH, cfTasks[TASK_SYSTEM].staticPriority); - EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority); + EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_PID].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority); EXPECT_EQ(TASK_PRIORITY_LOW, cfTasks[TASK_SERIAL].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY].staticPriority); @@ -117,16 +117,16 @@ TEST(SchedulerUnittest, TestQueue) EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - queueAdd(&cfTasks[TASK_GYROPID]); // TASK_PRIORITY_REALTIME + queueAdd(&cfTasks[TASK_PID]); // TASK_PRIORITY_REALTIME EXPECT_EQ(2, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_PID], queueFirst()); EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); EXPECT_EQ(NULL, queueNext()); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); queueAdd(&cfTasks[TASK_SERIAL]); // TASK_PRIORITY_LOW EXPECT_EQ(3, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_PID], queueFirst()); EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); EXPECT_EQ(NULL, queueNext()); @@ -134,7 +134,7 @@ TEST(SchedulerUnittest, TestQueue) queueAdd(&cfTasks[TASK_BEEPER]); // TASK_PRIORITY_MEDIUM EXPECT_EQ(4, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_PID], queueFirst()); EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext()); EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); @@ -143,7 +143,7 @@ TEST(SchedulerUnittest, TestQueue) queueAdd(&cfTasks[TASK_RX]); // TASK_PRIORITY_HIGH EXPECT_EQ(5, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_PID], queueFirst()); EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); EXPECT_EQ(&cfTasks[TASK_RX], queueNext()); EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext()); @@ -153,7 +153,7 @@ TEST(SchedulerUnittest, TestQueue) queueRemove(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH EXPECT_EQ(4, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_PID], queueFirst()); EXPECT_EQ(&cfTasks[TASK_RX], queueNext()); EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext()); EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); @@ -275,38 +275,38 @@ TEST(SchedulerUnittest, TestScheduleEmptyQueue) TEST(SchedulerUnittest, TestSingleTask) { schedulerInit(); - // disable all tasks except TASK_GYROPID + // disable all tasks except TASK_PID for (int taskId=0; taskId < TASK_COUNT; ++taskId) { setTaskEnabled(static_cast(taskId), false); } - setTaskEnabled(TASK_GYROPID, true); - cfTasks[TASK_GYROPID].lastExecutedAt = 1000; + setTaskEnabled(TASK_PID, true); + cfTasks[TASK_PID].lastExecutedAt = 1000; simulatedTime = 4000; // run the scheduler and check the task has executed scheduler(); EXPECT_NE(static_cast(0), unittest_scheduler_selectedTask); - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); - EXPECT_EQ(3000, cfTasks[TASK_GYROPID].taskLatestDeltaTime); - EXPECT_EQ(4000, cfTasks[TASK_GYROPID].lastExecutedAt); - EXPECT_EQ(pidLoopCheckerTime, cfTasks[TASK_GYROPID].totalExecutionTime); + EXPECT_EQ(&cfTasks[TASK_PID], unittest_scheduler_selectedTask); + EXPECT_EQ(3000, cfTasks[TASK_PID].taskLatestDeltaTime); + EXPECT_EQ(4000, cfTasks[TASK_PID].lastExecutedAt); + EXPECT_EQ(pidLoopCheckerTime, cfTasks[TASK_PID].totalExecutionTime); // task has run, so its dynamic priority should have been set to zero - EXPECT_EQ(0, cfTasks[TASK_GYROPID].dynamicPriority); + EXPECT_EQ(0, cfTasks[TASK_PID].dynamicPriority); } TEST(SchedulerUnittest, TestTwoTasks) { - // disable all tasks except TASK_GYROPID and TASK_ACCEL + // disable all tasks except TASK_PID and TASK_ACCEL for (int taskId=0; taskId < TASK_COUNT; ++taskId) { setTaskEnabled(static_cast(taskId), false); } setTaskEnabled(TASK_ACCEL, true); - setTaskEnabled(TASK_GYROPID, true); + setTaskEnabled(TASK_PID, true); - // set it up so that TASK_ACCEL ran just before TASK_GYROPID + // set it up so that TASK_ACCEL ran just before TASK_PID static const uint32_t startTime = 4000; simulatedTime = startTime; - cfTasks[TASK_GYROPID].lastExecutedAt = simulatedTime; - cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_GYROPID].lastExecutedAt - updateAccelerometerTime; + cfTasks[TASK_PID].lastExecutedAt = simulatedTime; + cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_PID].lastExecutedAt - updateAccelerometerTime; EXPECT_EQ(0, cfTasks[TASK_ACCEL].taskAgeCycles); // run the scheduler scheduler(); @@ -314,7 +314,7 @@ TEST(SchedulerUnittest, TestTwoTasks) EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); // NOTE: - // TASK_GYROPID desiredPeriod is 1000 microseconds + // TASK_PID desiredPeriod is 1000 microseconds // TASK_ACCEL desiredPeriod is 10000 microseconds // 500 microseconds later simulatedTime += 500; @@ -323,27 +323,27 @@ TEST(SchedulerUnittest, TestTwoTasks) EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); EXPECT_EQ(0, unittest_scheduler_waitingTasks); - // 500 microseconds later, TASK_GYROPID desiredPeriod has elapsed + // 500 microseconds later, TASK_PID desiredPeriod has elapsed simulatedTime += 500; - // TASK_GYROPID should now run + // TASK_PID should now run scheduler(); - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + EXPECT_EQ(&cfTasks[TASK_PID], unittest_scheduler_selectedTask); EXPECT_EQ(1, unittest_scheduler_waitingTasks); EXPECT_EQ(5000 + pidLoopCheckerTime, simulatedTime); simulatedTime += 1000 - pidLoopCheckerTime; scheduler(); - // TASK_GYROPID should run again - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + // TASK_PID should run again + EXPECT_EQ(&cfTasks[TASK_PID], unittest_scheduler_selectedTask); scheduler(); EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); EXPECT_EQ(0, unittest_scheduler_waitingTasks); - simulatedTime = startTime + 10500; // TASK_GYROPID and TASK_ACCEL desiredPeriods have elapsed - // of the two TASK_GYROPID should run first + simulatedTime = startTime + 10500; // TASK_PID and TASK_ACCEL desiredPeriods have elapsed + // of the two TASK_PID should run first scheduler(); - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + EXPECT_EQ(&cfTasks[TASK_PID], unittest_scheduler_selectedTask); // and finally TASK_ACCEL should now run scheduler(); EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); @@ -351,12 +351,12 @@ TEST(SchedulerUnittest, TestTwoTasks) TEST(SchedulerUnittest, TestRealTimeGuardInNoTaskRun) { - // disable all tasks except TASK_GYROPID and TASK_SYSTEM + // disable all tasks except TASK_PID and TASK_SYSTEM for (int taskId=0; taskId < TASK_COUNT; ++taskId) { setTaskEnabled(static_cast(taskId), false); } - setTaskEnabled(TASK_GYROPID, true); - cfTasks[TASK_GYROPID].lastExecutedAt = 200000; + setTaskEnabled(TASK_PID, true); + cfTasks[TASK_PID].lastExecutedAt = 200000; simulatedTime = 200700; setTaskEnabled(TASK_SYSTEM, true); @@ -371,17 +371,17 @@ TEST(SchedulerUnittest, TestRealTimeGuardInNoTaskRun) EXPECT_EQ(NULL, unittest_scheduler_selectedTask); EXPECT_EQ(100000, cfTasks[TASK_SYSTEM].lastExecutedAt); - EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt); + EXPECT_EQ(200000, cfTasks[TASK_PID].lastExecutedAt); } TEST(SchedulerUnittest, TestRealTimeGuardOutTaskRun) { - // disable all tasks except TASK_GYROPID and TASK_SYSTEM + // disable all tasks except TASK_PID and TASK_SYSTEM for (int taskId=0; taskId < TASK_COUNT; ++taskId) { setTaskEnabled(static_cast(taskId), false); } - setTaskEnabled(TASK_GYROPID, true); - cfTasks[TASK_GYROPID].lastExecutedAt = 200000; + setTaskEnabled(TASK_PID, true); + cfTasks[TASK_PID].lastExecutedAt = 200000; simulatedTime = 200699; setTaskEnabled(TASK_SYSTEM, true); @@ -396,7 +396,7 @@ TEST(SchedulerUnittest, TestRealTimeGuardOutTaskRun) EXPECT_EQ(&cfTasks[TASK_SYSTEM], unittest_scheduler_selectedTask); EXPECT_EQ(200699, cfTasks[TASK_SYSTEM].lastExecutedAt); - EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt); + EXPECT_EQ(200000, cfTasks[TASK_PID].lastExecutedAt); } // STUBS diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 0433b88939..f80ecf7db3 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -143,6 +143,7 @@ uint32_t micros(void) {return 0;} void beeper(beeperMode_e) {} uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; timeDelta_t getLooptime(void) {return gyro.targetLooptime;} +timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} } diff --git a/src/utils/settings.rb b/src/utils/settings.rb index acae823773..877e325fcd 100755 --- a/src/utils/settings.rb +++ b/src/utils/settings.rb @@ -301,6 +301,7 @@ class Generator check_member_default_values_presence sanitize_fields + resolv_min_max_and_default_values_if_possible initialize_name_encoder initialize_value_encoder validate_default_values @@ -359,6 +360,7 @@ class Generator @data = YAML.load_file(@settings_file) initialize_tables + initialize_constants check_conditions end @@ -415,6 +417,11 @@ class Generator buf << "extern const char * const #{table_variable_name(name)}[];\n" end + # Write setting constants from settings file + @constants.each do |name, value| + buf << "#define SETTING_CONSTANT_#{name.upcase} #{value.inspect}\n" + end + # Write #define'd constants for referencing each setting ii = 0 foreach_enabled_member do |group, member| @@ -715,6 +722,10 @@ class Generator end end + def initialize_constants + @constants = @data["constants"] + end + def ordered_table_names @enabled_tables.to_a().sort() end @@ -877,6 +888,8 @@ class Generator foreach_enabled_member do |_, member| name = member["name"] type = member["type"] + min = member["min"] || 0 + max = member["max"] default_value = member["default_value"] next if %i[ zero target ].include? default_value @@ -894,13 +907,17 @@ class Generator 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' + min = type_range.min if min =~ /\AU?INT\d+_MIN\Z/ + max = type_range.max if max =~ /\AU?INT\d+_MAX\Z/ 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 + raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' + raise "Member #{name} default value is outside of the allowed range" if default_value.is_a? Numeric and min.is_a? Numeric and max.is_a? Numeric and not (min..max) === 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 + raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' + raise "Member #{name} default value is outside of the allowed range" if default_value.is_a? Numeric and min.is_a? Numeric and max.is_a? Numeric and not (min..max) === default_value when type == "string" max = member["max"].to_i @@ -971,6 +988,8 @@ class Generator types.each do |idx, type| member = members[idx] case type + when /^bool/ + typ = "bool" when /^int8_t/ # {aka signed char}" typ = "int8_t" when /^uint8_t/ # {aka unsigned char}" @@ -988,7 +1007,7 @@ class Generator member["max"] = $1.to_i - 1; typ = "string" else - raise "Unknown type #{m[1]} when resolving type for setting #{member["name"]}" + raise "Unknown type #{type} when resolving type for setting #{member["name"]}" end dputs "#{member["name"]} type is #{typ}" member["type"] = typ @@ -1039,6 +1058,18 @@ class Generator 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 resolv_min_max_and_default_values_if_possible + foreach_member do |_, member| + %w[ min max default_value ].each do |value_type| + member_value = member[value_type] + if member_value.is_a? String + constant_value = @constants[member_value] + member[value_type] = constant_value unless constant_value.nil? + end + end + end + end + def resolve_constants(constants) return nil unless constants.length > 0 s = Set.new diff --git a/src/utils/update_cli_docs.py b/src/utils/update_cli_docs.py index f7f7d8a24b..f685d908a8 100755 --- a/src/utils/update_cli_docs.py +++ b/src/utils/update_cli_docs.py @@ -22,6 +22,16 @@ DEFAULTS_BLACKLIST = [ 'serialrx_provider', ] +MIN_MAX_REPLACEMENTS = { + 'INT16_MIN': -32768, + 'INT16_MAX': 32767, + 'INT32_MIN': -2147483648, + 'INT32_MAX': 2147483647, + 'UINT8_MAX': 255, + 'UINT16_MAX': 65535, + 'UINT32_MAX': 4294967295, +} + def parse_settings_yaml(): """Parse the YAML settings specs""" @@ -32,42 +42,59 @@ def generate_md_table_from_yaml(settings_yaml): """Generate a sorted markdown table with description & default value for each setting""" params = {} - # Extract description and default value of each setting from the YAML specs (if present) + # Extract description, default/min/max values of each setting from the YAML specs (if present) for group in settings_yaml['groups']: 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:]}`' + if not any(key in member for key in ["description", "default_value", "min", "max"]) and not options.quiet: + print("Setting \"{}\" has an incomplete specification".format(member['name'])) + + # Handle default/min/max fields for each setting + for key in ["default_value", "min", "max"]: + # Basing on the check above, not all fields may be present + if key in member: + ### Fetch actual values from the `constants` block if present + if ('constants' in settings_yaml) and (member[key] in settings_yaml['constants']): + member[key] = settings_yaml['constants'][member[key]] + ### Fetch actual values from hardcoded min/max replacements + elif member[key] in MIN_MAX_REPLACEMENTS: + member[key] = MIN_MAX_REPLACEMENTS[member[key]] + + ### Handle edge cases of YAML autogeneration and prettify some values + # Replace booleans with "ON"/"OFF" + if type(member[key]) == bool: + member[key] = "ON" if member[key] else "OFF" + # Replace zero placeholder with actual zero + elif member[key] == ":zero": + member[key] = 0 + # Replace target-default placeholder with extended definition + elif member[key] == ":target": + member[key] = "_target default_" + # Replace empty strings with more evident marker + elif member[key] == "": + member[key] = "_empty_" + # Reformat direct code references + elif str(member[key])[0] == ":": + member[key] = f'`{member[key][1:]}`' + + params[member['name']] = { "description": member["description"] if "description" in member else "", - "default": member["default_value"] if "default_value" in member else "" + "default": member["default_value"] if "default_value" in member else "", + "min": member["min"] if "min" in member else "", + "max": member["max"] if "max" in member else "" } # MD table header md_table_lines = [ - "| Variable Name | Default Value | Description |\n", - "| ------------- | ------------- | ----------- |\n", + "| Variable Name | Default Value | Min | Max | Description |\n", + "| ------------- | ------------- | --- | --- | ----------- |\n", ] # Sort the settings by name and build the rows of the table for param in sorted(params.items()): - md_table_lines.append("| {} | {} | {} |\n".format(param[0], param[1]['default'], param[1]['description'])) + md_table_lines.append("| {} | {} | {} | {} | {} |\n".format( + param[0], param[1]['default'], param[1]['min'], param[1]['max'], param[1]['description'] + )) # Return the assembled table return md_table_lines