mpc: add acceleration control filter and gain

* [Doc] Data link loss exceptions

* Tidy

.github/workflows/docs_crowdin_download.yml

@@ -41,7 +41,7 @@ jobs:
           pull_request_base_branch_name: 'main'
           pull_request_title: New PX4 guide translations (Crowdin) - ${{ matrix.lc }}
           pull_request_body: 'New PX4 guide Crowdin translations by [Crowdin GH Action](https://github.com/crowdin/github-action) for ${{ matrix.lc }}'
-          pull_request_labels: Documentation
+          pull_request_labels: 'Documentation 📑'
           pull_request_reviewers: hamishwillee
           download_language: ${{ matrix.lc }}
         env:

.github/workflows/docs_flaw_checker.yml

@@ -21,13 +21,13 @@ jobs:
       - name: Install Node.js
         uses: actions/setup-node@v3
         with:
-          node-version: '16'
+          node-version: '18'
 
       - name: Create logs directory
         run: |
           mkdir logs
 
-      - name: Get changed english files
+      - name: Get changed english doc files
         id: get_changed_markdown_english
         uses: tj-actions/changed-files@v35.9.2
         with:
@@ -48,10 +48,10 @@ jobs:
       - name: Run link checker
         id: link-check
         run: |
-          npm -g install markdown_link_checker_sc@0.0.134
-          markdown_link_checker_sc -r ${{ github.workspace }} -d en -f ./docs/logs/prFiles.json -i assets -u docs.px4.io/main/ > ./docs/logs/errorsFilteredByPrPages.md
+          npm -g install markdown_link_checker_sc@0.0.137
+          markdown_link_checker_sc -r ${{ github.workspace }} -d docs -e en -f ./logs/prFiles.json -i assets -u docs.px4.io/main/ > ./logs/errorsFilteredByPrPages.md
           mkdir -p ./pr
-          cp ./docs/logs/errorsFilteredByPrPages.md ./pr/errorsFilteredByPrPages.md
+          cp ./logs/errorsFilteredByPrPages.md ./pr/errorsFilteredByPrPages.md
 
       - name: Read errorsFilteredByPrPages.md file
         id: read-errors-by-page

boards/ark/fpv/default.px4board

@@ -8,6 +8,8 @@ CONFIG_BOARD_SERIAL_TEL4="/dev/ttyS3"
 CONFIG_BOARD_SERIAL_RC="/dev/ttyS5"
 CONFIG_DRIVERS_ADC_BOARD_ADC=y
 CONFIG_DRIVERS_BAROMETER_BMP388=y
+CONFIG_DRIVERS_CAMERA_CAPTURE=y
+CONFIG_DRIVERS_CAMERA_TRIGGER=y
 CONFIG_DRIVERS_CDCACM_AUTOSTART=y
 CONFIG_COMMON_DIFFERENTIAL_PRESSURE=y
 CONFIG_COMMON_DISTANCE_SENSOR=y

docs/.vitepress/config.mjs

@@ -89,7 +89,11 @@ export default defineConfig({
               : (c) =>
                   `${
                     window.location.origin
-                  }/__open-in-editor?file=${encodeURIComponent(c.filePath)}`,
+                  }/__open-in-editor?file=${encodeURIComponent(
+                    c.frontmatter.newEditLink
+                      ? c.frontmatter.newEditLink
+                      : c.filePath
+                  )}`,
         },
       },
     },

docs/en/advanced/gimbal_control.md

@@ -43,7 +43,9 @@ The gimbal can be connected to _any free serial port_ using the instructions in
 For example, if the `TELEM2` port on the flight controller is unused you can connect it to the gimbal and set the following PX4 parameters:
 
 - [MAV_1_CONFIG](../advanced_config/parameter_reference.md#MAV_1_CONFIG) to **TELEM2** (if `MAV_1_CONFIG` is already used for a companion computer (say), use `MAV_2_CONFIG`).
-- [MAV_1_MODE](../advanced_config/parameter_reference.md#MAV_1_MODE) to **NORMAL**
+- [MAV_1_MODE](../advanced_config/parameter_reference.md#MAV_1_MODE) to **Gimbal**
+- [MAV_1_FLOW_CTRL](../advanced_config/parameter_reference.md#MAV_1_FLOW_CTRL) to **Off (0)** (very few gimbals will have RST/CST wires connected).
+- [MAV_1_FORWARD](../advanced_config/parameter_reference.md#MAV_1_FORWARD) to **Enabled** (Note strictly necessary as forwarding is enabled when `MAV_1_MODE` is set to Gimbal).
 - [SER_TEL2_BAUD](../advanced_config/parameter_reference.md#SER_TEL2_BAUD) to manufacturer recommended baud rate.
 
 ### Multiple Gimbal Support

docs/en/config/safety.md

@@ -146,6 +146,12 @@ The settings and underlying parameters are shown below.
 | Data Link Loss Timeout | [COM_DL_LOSS_T](../advanced_config/parameter_reference.md#COM_DL_LOSS_T) | Amount of time after losing the data connection before the failsafe will trigger. |
 | Failsafe Action        | [NAV_DLL_ACT](../advanced_config/parameter_reference.md#NAV_DLL_ACT)     | Disabled, Hold mode, Return mode, Land mode, Disarm, Terminate.                   |
 
+The following settings also apply, but are not displayed in the QGC UI.
+
+| Setting                                                     | Parameter                                                                  | Description                                          |
+| ----------------------------------------------------------- | -------------------------------------------------------------------------- | ---------------------------------------------------- |
+| <a id="COM_DLL_EXCEPT"></a>Mode exceptions for DLL failsafe | [COM_DLL_EXCEPT](../advanced_config/parameter_reference.md#COM_DLL_EXCEPT) | Set modes where DL loss will not trigger a failsafe. |
+
 ## Geofence Failsafe
 
 The _Geofence Failsafe_ is triggered when the drone breaches a "virtual" perimeter.

docs/en/data_links/index.md

@@ -11,3 +11,7 @@ This section provides information about various radio systems that you can use,
 - [FrSky Telemetry](../peripherals/frsky_telemetry.md) — Telemetry on your (FRSky) RC Receiver
 - [TBS Crossfire (CRSF) Telemetry](../telemetry/crsf_telemetry.md) — Telemetry on your (TBS Crossfire) RC Receiver
 - [Satellite Comms (Iridium/RockBlock)](../advanced_features/satcom_roadblock.md) — High-latency comms via satellite
+
+## See Also
+
+- [Safety Configuration > Data Link Loss Failsafe](../config/safety.html#data-link-loss-failsafe)

docs/en/peripherals/mavlink_peripherals.md

@@ -1,6 +1,6 @@
-# MAVLink Peripherals (GCS/OSD/Companion)
+# MAVLink Peripherals (GCS/OSD/Gimbal/Camera/Companion)
 
-Ground Control Stations (GCS), On-Screen Displays (OSD), Companion Computers, ADS-B receivers, and other MAVLink peripherals interact with PX4 using separate MAVLink streams, sent via different serial ports.
+Ground Control Stations (GCS), On-Screen Displays (OSD), MAVLink Cameras & Gimbals, Remote IDs, Companion Computers, ADS-B receivers, and other MAVLink peripherals interact with PX4 using separate MAVLink streams, sent via different serial ports.
 
 In order to configure that a particular serial port is used for MAVLink traffic with a particular peripheral, we use [Serial Port Configuration](../peripherals/serial_configuration.md), assigning one of the abstract "MAVLink instance" configuration parameters to the desired port.
 We then set other properties of the MAVLink channel using the parameters associated with our selected MAVLink instance, so that they match the requirements of our particular peripheral.
@@ -36,8 +36,10 @@ The parameters for each instance are:
   - _OSD_: Standard set of messages for an OSD system.
   - _Config_: Standard set of messages and rate configuration for a fast link (e.g. USB).
   - _Minimal_: Minimal set of messages for use with a GCS connected on a high latency link.
-  - _ExtVision_ or _ExtVisionMin_: Messages for offboard vision systems (ExtVision needed for VIO).
-  - _Iridium_: Messages for an [Iridium satellite communication system](../advanced_features/satcom_roadblock.md).
+  - _External Vision_: Messages for offboard vision systems.
+  - _Gimbal_: Messages for a gimbal. Note this also enables [message forwarding](#MAV_X_FORWARD)
+  - _Onboard Low Bandwidth_: Standard set of messages for a companion computer connected on a lower speed link.
+  - _uAvionix_: Messages for a uAvionix ADS-B beacon.
 
   ::: info
   If you need to find the specific set of message for each mode search for `MAVLINK_MODE_` in [/src/modules/mavlink/mavlink_main.cpp](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/mavlink/mavlink_main.cpp).
@@ -64,9 +66,7 @@ You will need to reboot PX4 to make the parameter available (i.e. in QGroundCont
 The parameter used will depend on the [assigned serial port](../advanced_config/parameter_reference.md#serial) - for example: `SER_GPS1_BAUD`, `SER_TEL2_BAUD`, etc.
 The value you use will depend on the type of connection and the capabilities of the connected MAVLink peripheral.
 
-<a id="default_ports"></a>
-
-## Default MAVLink Ports
+## Default MAVLink Ports {#default_ports}
 
 ### TELEM1
 
@@ -108,8 +108,16 @@ On this hardware, there is a [default serial port mapping](../peripherals/serial
 
 For more information see: [PX4 Ethernet Setup](../advanced_config/ethernet_setup.md)
 
+## Device Specific Setup
+
+Links to setup instructions for specific MAVLink components:
+
+- [MAVLink Cameras (Camera Protocol v2) > PX4 Configuration](../camera/mavlink_v2_camera.md#px4-configuration)
+- [Gimbal Configuration > MAVLink Gimbal (MNT_MODE_OUT=MAVLINK)](../advanced/gimbal_control.md#mavlink-gimbal-mnt-mode-out-mavlink)
+
 ## See Also
 
 - [Serial Port Configuration](../peripherals/serial_configuration.md)
 - [PX4 Ethernet Setup > PX4 MAVLink Serial Port Configuration](../advanced_config/ethernet_setup.md#px4-mavlink-serial-port-configuration)
 - [Serial Port Mapping](../hardware/serial_port_mapping.md)
+

docs/ko/config/_autotune.md

@@ -158,7 +158,7 @@ Fast oscillations (more than 1 oscillation per second): this is because the gain
 
 </div>
 
-### The auto-tuning sequence fails
+### 자동 튜닝 실패
 
 If the drone was not moving enough during auto-tuning, the system identification algorithm might have issues to find the correct coefficients.
 
@@ -169,7 +169,7 @@ Increase the <div style="display: inline;" v-if="$frontmatter.frame === 'Multico
 Due to effects not included in the mathematical model such as delays, saturation, slew-rate, airframe flexibility, the loop gain can be too high.
 To fix this, follow the same steps described [when the drone oscillates in the pre-tuning test](#drone-oscillates-when-performing-the-pre-tuning-test).
 
-### I still can't get it to work
+### 여전히 정상 작동하지 않는 경우
 
 Attempt manual tuning using the guides listed in [See also](#see-also) below.
 
@@ -191,7 +191,7 @@ This behaviour can be configured using the [FW_AT_APPLY](../advanced_config/para
 </div>
 
 - `0`: the gains are not applied.
-  This is used for testing purposes if the user wants to inspect results of the auto-tuning algorithm without using them directly.
+  자동 튜닝의 결과를 직접적으로 사용하지 않은 체로 검사하는 경우에 사용합니다.
 - `1`: apply the gains after disarm (default for multirotors).
   The operator can then test the new tuning while taking-off carefully.
 - `2`: apply immediately (default for fixed-wings).
@@ -225,7 +225,7 @@ Fixed-wing vehicles (only) can select which axes are tuned using the [FW_AT_AXES
 
 </div>
 
-## Developers/SDKs
+## 개발자 SDK
 
 Autotuning is started using [MAV_CMD_DO_AUTOTUNE_ENABLE](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_AUTOTUNE_ENABLE) MAVLink command.
 
@@ -239,7 +239,7 @@ PX4 should stream progress as the protocol does not allow polling.
 
 The feature is not yet supported by MAVSDK.
 
-## Background/Detail
+## 배경 및 세부 사항
 
 PX4 uses [PID controllers](../flight_stack/controller_diagrams.md) (rate, attitude, velocity, and position) to calculate the outputs required to move a vehicle from its current estimated state to match a desired setpoint.
 The controllers must be well tuned in order to get the best performance out of a vehicle.
@@ -266,20 +266,20 @@ The default behaviour can be configured using [parameters](#optional-configurati
 
 ### 자주 묻는 질문
 
-#### What frames types are supported?
+#### 어떤 기체 유형이 지원됩니까?
 
 Autotuning is enabled for multicopter, fixed-wing, and hybrid VTOL fixed-wing vehicles.
 
 While it is not yet enabled for other frame types, in theory it an be used with any frame that uses a rate controller.
 
-#### Does autotuning work for all supported airframes?
+#### 지원되는 모든 기체에 대해 자동 튜닝이 작동됩니까?
 
 The mathematical model used by autotuning to estimate the dynamics of the drone assumes this it is a linear system with no coupling between the axes (SISO), and with a limited complexity (2 poles and 2 zeros).
 If the real drone is too far from those conditions, the model will not be able to represent the real dynamics of the drone.
 
 In practise, autotuning generally works well for fixed-wing and multicopter, provided the frame is not too flexible.
 
-#### How long does autotuning take?
+#### 자동 튜닝은 얼마나 걸립니까?
 
 Tuning takes 5s-20s per axis (aborted if tuning could not be established in 20s) + 2s pause between each axis + 4s of testing if the new gains are applied in air.
 

docs/ko/config_vtol/vtol_quad_configuration.md

@@ -83,6 +83,22 @@ This is the airspeed which, when reached, will trigger the transition out of mul
 It is critical that you have properly calibrated your airspeed sensor.
 It is also important that you pick an airspeed that is comfortably above your airframes stall speed (check `FW_AIRSPD_MIN`) as this is currently not checked.
 
+#### Openloop Transition Time
+
+Parameter: [VT_F_TR_OL_TM](../advanced_config/parameter_reference.md#VT_F_TR_OL_TM)
+
+This specifies the duration of the front transition in seconds when no airspeed feedback is available (e.g. no airspeed sensor present).
+It should be set to a value which ensures that the vehicle reaches a high enough airspeed to complete the transition, e.g. airspeed should exceed [VT_ARSP_TRANS](../advanced_config/parameter_reference.md#VT_ARSP_TRANS).
+
+#### Transition Timeout
+
+[VT_TRANS_TIMEOUT](../advanced_config/parameter_reference.md#VT_TRANS_TIMEOUT)
+
+This specifies the upper limit for the duration of the front transition. If the vehicle has not reached the transition airspeed after this time, then the transition will be aborted and a [Quadchute](../config/safety.md#quad-chute-failsafe) event will be triggered.
+:::note
+Additionally, if an airspeed sensor is present, the transition will also be aborted if the airspeed has not reached [VT_ARSP_BLEND](../advanced_config/parameter_reference.md#VT_ARSP_BLEND) after the openloop transition time [VT_F_TR_OL_TM](../advanced_config/parameter_reference.md#VT_F_TR_OL_TM) has elapsed. This checks is used to avoid a scenario where the vehicle gains excessive speed when the airspeed sensor is faulty.
+:::
+
 ### 전환 팁
 
 As already mentioned make sure you have a well tuned multirotor mode.

docs/ko/flight_controller/ark_pi6x.md

@@ -1,6 +1,6 @@
 # ARK Pi6X Flow
 
-The [ARK Pi6X Flow](\(https://arkelectron.gitbook.io/ark-documentation/flight-controllers/ark-pi6x-flow\)) integrates a Raspberry Pi Compute Module 4 (CM4) Carrier, [ARKV6X Flight Controller](../flight_controller/ark_v6x.md), [ARK Flow sensors](../dronecan/ark_flow.md) , [ARK PAB Power Module](../power_module/ark_pab_power_module.md), and a 4-in-1 ESC, all mounted onto one compact board.
+The [ARK Pi6X Flow](https://arkelectron.gitbook.io/ark-documentation/flight-controllers/ark-pi6x-flow) integrates a Raspberry Pi Compute Module 4 (CM4) Carrier, [ARKV6X Flight Controller](../flight_controller/ark_v6x.md), [ARK Flow sensors](../dronecan/ark_flow.md) , [ARK PAB Power Module](../power_module/ark_pab_power_module.md), and a 4-in-1 ESC, all mounted onto one compact board.
 
 ![ARK Pi6X Flow Flight Controller](../../assets/flight_controller/ark_pi6x_flow/ark_pi6xflow.jpg)
 

docs/uk/config/_autotune.md

@@ -158,9 +158,9 @@ Fast oscillations (more than 1 oscillation per second): this is because the gain
 
 </div>
 
-### The auto-tuning sequence fails
+### Послідовність автоматичної настройки не вдається
 
-If the drone was not moving enough during auto-tuning, the system identification algorithm might have issues to find the correct coefficients.
+Якщо безпілотник не рухався достатньо під час автоматичного налаштування, алгоритм ідентифікації системи може мати проблеми з визначенням правильних коефіцієнтів.
 
 Increase the <div style="display: inline;" v-if="$frontmatter.frame === 'Multicopter'">[MC_AT_SYSID_AMP](../advanced_config/parameter_reference.md#MC_AT_SYSID_AMP)</div><div style="display: inline;" v-else-if="$frontmatter.frame === 'Plane'">[FW_AT_SYSID_AMP](../advanced_config/parameter_reference.md#FW_AT_SYSID_AMP)</div> parameter by steps of 1 and trigger the auto-tune again.
 
@@ -169,11 +169,11 @@ Increase the <div style="display: inline;" v-if="$frontmatter.frame === 'Multico
 Due to effects not included in the mathematical model such as delays, saturation, slew-rate, airframe flexibility, the loop gain can be too high.
 To fix this, follow the same steps described [when the drone oscillates in the pre-tuning test](#drone-oscillates-when-performing-the-pre-tuning-test).
 
-### I still can't get it to work
+### Я все ще не можу зрозуміти, як це працює
 
 Attempt manual tuning using the guides listed in [See also](#see-also) below.
 
-## Optional Configuration
+## Необов'язкова Конфігурація
 
 ### Apply Tuning when In-Air/Landed
 
@@ -191,12 +191,12 @@ This behaviour can be configured using the [FW_AT_APPLY](../advanced_config/para
 </div>
 
 - `0`: the gains are not applied.
-  This is used for testing purposes if the user wants to inspect results of the auto-tuning algorithm without using them directly.
+  Це використовується для тестування, якщо користувач хоче перевірити результати автоналаштування алгоритму без прямого їх використання.
 - `1`: apply the gains after disarm (default for multirotors).
-  The operator can then test the new tuning while taking-off carefully.
+  Оператор може перевірити нове налаштування під час обережного зльоту.
 - `2`: apply immediately (default for fixed-wings).
-  The new tuning is applied, disturbances are sent to the controller and the stability is monitored during the next 4 seconds.
-  If the control loop is unstable, the control gains are immediately reverted back to their previous value.
+  Нове налаштування застосовується, перешкоди надсилаються контролеру, а стабільність контролюється протягом наступних 4 секунд.
+  Якщо керуюче коло нестійке, керуючі коефіцієнти негайно повертаються до свого попереднього значення.
   If the test passes, the pilot can then use the new tuning.
 
 <div v-if="$frontmatter.frame === 'Plane'">
@@ -225,33 +225,33 @@ Fixed-wing vehicles (only) can select which axes are tuned using the [FW_AT_AXES
 
 </div>
 
-## Developers/SDKs
+## Розробники/SDKs
 
 Autotuning is started using [MAV_CMD_DO_AUTOTUNE_ENABLE](https://mavlink.io/en/messages/common.html#MAV_CMD_DO_AUTOTUNE_ENABLE) MAVLink command.
 
 At time of writing the message is resent at regular intervals to poll PX4 for progress: the `COMMAND_ACK` includes result that the operation is in progress, and also the progress as a percentage.
-The operation completes when the progress is 100% or the vehicle lands and disarms.
+Операція завершується, коли прогрес досягає 100% або транспортний засіб приземляється і роззброюється.
 
 :::info
 This is not a MAVLink-compliant implementation of a [command protocol long running command](https://mavlink.io/en/services/command.html#long_running_commands).
-PX4 should stream progress as the protocol does not allow polling.
+PX4 повинен транслювати прогрес, оскільки протокол не дозволяє опитування.
 :::
 
-The feature is not yet supported by MAVSDK.
+Функція ще не підтримується MAVSDK.
 
 ## Background/Detail
 
 PX4 uses [PID controllers](../flight_stack/controller_diagrams.md) (rate, attitude, velocity, and position) to calculate the outputs required to move a vehicle from its current estimated state to match a desired setpoint.
-The controllers must be well tuned in order to get the best performance out of a vehicle.
-In particular, a poorly tuned rate controller results in less stable flight in all modes, and takes longer to recover from disturbances.
+Контролери повинні бути добре налаштовані, щоб отримати найкращу продуктивність з автомобіля.
+Зокрема, погано налаштований регулятор швидкості призводить до менш стабільного польоту у всіх режимах і потребує більше часу на відновлення після перешкод.
 
 Generally if you use a [frame configuration](../config/airframe.md) that is similar to your vehicle then the vehicle will be able to fly.
-However unless the configuration precisely matches your hardware you should tune the rate and attitude controllers.
-Tuning the velocity and position controllers is less important because they are less affected by vehicle dynamics, and the default tuning configuration for a similar airframe is often sufficient.
+Однак, якщо конфігурація точно не відповідає вашому обладнанню, вам слід налаштувати регулятори швидкості та кута нахилу.
+Налаштування контролерів швидкості та позиції менш важливе, оскільки вони менше піддаються динаміці транспортного засобу, і типова конфігурація налаштування для схожого аеродинамічного корпусу часто є достатньою.
 
-Autotuning provides an automatic mechanism to tune the rate and attitude controllers.
+Автоналаштування забезпечує автоматичний механізм для налаштування регуляторів швидкості та кута нахилу.
 It can be used to tune fixed-wing and multicopter vehicles, and VTOL vehicles when flying as a multicopter or as a fixed-wing (transition between modes must be manually tuned).
-In theory it should work for other vehicle types that have a rate controller, but currently only the above types are supported.
+Теоретично це повинно працювати для інших типів транспортних засобів, які мають регулятор швидкості, але наразі підтримуються лише вищезазначені типи.
 
 Automatic tuning works well for the multicopter and fixed-wing vehicle configurations supported by PX4, provided the frame is not too flexible
 (see [below for more information](#does-autotuning-work-for-all-supported-airframes)).
@@ -266,32 +266,32 @@ The default behaviour can be configured using [parameters](#optional-configurati
 
 ### Часто Запитувані Питання
 
-#### What frames types are supported?
+#### Які типи кадрів підтримуються?
 
 Autotuning is enabled for multicopter, fixed-wing, and hybrid VTOL fixed-wing vehicles.
 
 While it is not yet enabled for other frame types, in theory it an be used with any frame that uses a rate controller.
 
-#### Does autotuning work for all supported airframes?
+#### Чи працює автоналадка для всіх підтримуваних конструкцій?
 
 The mathematical model used by autotuning to estimate the dynamics of the drone assumes this it is a linear system with no coupling between the axes (SISO), and with a limited complexity (2 poles and 2 zeros).
 If the real drone is too far from those conditions, the model will not be able to represent the real dynamics of the drone.
 
 In practise, autotuning generally works well for fixed-wing and multicopter, provided the frame is not too flexible.
 
-#### How long does autotuning take?
+#### Як довго триває автоналагодження?
 
 Tuning takes 5s-20s per axis (aborted if tuning could not be established in 20s) + 2s pause between each axis + 4s of testing if the new gains are applied in air.
 
-A multicopter must tune all three axes, and by default does not test the new gains in-air.
+Мультикоптер повинен налаштовувати всі три осі, і за замовчуванням не перевіряє нові виграші у повітрі.
 Tuning will therefore take between 19s (`5 + 2 + 5 + 2 + 5`) and 64s (`20x3 + 2x2`).
 
-By default a fixed-wing vehicle tunes all three axes and then tests the new gains in-air.
+За замовчуванням літак налагоджує всі три осі, а потім перевіряє нові коефіцієнти в повітрі.
 The range is therefore between 25s (`5 + 2 + 5 + 2 + 5 + 2 + 4`) and 70s (`20x3 + 3x2 + 4`).
 
-Note however that the above settings are defaults.
-A multicopter can choose to run the tests in air, and a fixed-wing can choose not to.
-Further, a fixed-wing can choose to tune fewer axes.
+Зверніть увагу, що вищезазначені налаштування є значеннями за замовчуванням.
+Мультикоптер може вибрати проведення тестів в повітрі, а планер може вибрати не робити цього.
+Додатково, літак з фіксованим крилом може вибрати налаштувати менше вісей.
 
 Anecdotally, it usually takes around 40s for either vehicle.
 

docs/uk/config_vtol/vtol_quad_configuration.md

@@ -84,6 +84,22 @@ Parameter: [VT_ARSP_TRANS](../advanced_config/parameter_reference.md#VT_ARSP_TRA
 Дуже важливо правильно калібрувати ваш датчик швидкості повітря.
 It is also important that you pick an airspeed that is comfortably above your airframes stall speed (check `FW_AIRSPD_MIN`) as this is currently not checked.
 
+#### Openloop Transition Time
+
+Parameter: [VT_F_TR_OL_TM](../advanced_config/parameter_reference.md#VT_F_TR_OL_TM)
+
+This specifies the duration of the front transition in seconds when no airspeed feedback is available (e.g. no airspeed sensor present).
+It should be set to a value which ensures that the vehicle reaches a high enough airspeed to complete the transition, e.g. airspeed should exceed [VT_ARSP_TRANS](../advanced_config/parameter_reference.md#VT_ARSP_TRANS).
+
+#### Transition Timeout
+
+[VT_TRANS_TIMEOUT](../advanced_config/parameter_reference.md#VT_TRANS_TIMEOUT)
+
+This specifies the upper limit for the duration of the front transition. If the vehicle has not reached the transition airspeed after this time, then the transition will be aborted and a [Quadchute](../config/safety.md#quad-chute-failsafe) event will be triggered.
+:::note
+Additionally, if an airspeed sensor is present, the transition will also be aborted if the airspeed has not reached [VT_ARSP_BLEND](../advanced_config/parameter_reference.md#VT_ARSP_BLEND) after the openloop transition time [VT_F_TR_OL_TM](../advanced_config/parameter_reference.md#VT_F_TR_OL_TM) has elapsed. This checks is used to avoid a scenario where the vehicle gains excessive speed when the airspeed sensor is faulty.
+:::
+
 ### Поради щодо переходу
 
 Як вже зазначалося, переконайтеся, що у вас добре налаштований режим багатокоптера.

docs/uk/flight_controller/ark_pi6x.md

@@ -1,6 +1,6 @@
 # ARK Pi6X Flow
 
-The [ARK Pi6X Flow](\(https://arkelectron.gitbook.io/ark-documentation/flight-controllers/ark-pi6x-flow\)) integrates a Raspberry Pi Compute Module 4 (CM4) Carrier, [ARKV6X Flight Controller](../flight_controller/ark_v6x.md), [ARK Flow sensors](../dronecan/ark_flow.md) , [ARK PAB Power Module](../power_module/ark_pab_power_module.md), and a 4-in-1 ESC, all mounted onto one compact board.
+The [ARK Pi6X Flow](https://arkelectron.gitbook.io/ark-documentation/flight-controllers/ark-pi6x-flow) integrates a Raspberry Pi Compute Module 4 (CM4) Carrier, [ARKV6X Flight Controller](../flight_controller/ark_v6x.md), [ARK Flow sensors](../dronecan/ark_flow.md) , [ARK PAB Power Module](../power_module/ark_pab_power_module.md), and a 4-in-1 ESC, all mounted onto one compact board.
 
 ![ARK Pi6X Flow Flight Controller](../../assets/flight_controller/ark_pi6x_flow/ark_pi6xflow.jpg)
 

docs/zh/config_vtol/vtol_quad_configuration.md

@@ -83,6 +83,22 @@ This is the airspeed which, when reached, will trigger the transition out of mul
 It is critical that you have properly calibrated your airspeed sensor.
 It is also important that you pick an airspeed that is comfortably above your airframes stall speed (check `FW_AIRSPD_MIN`) as this is currently not checked.
 
+#### Openloop Transition Time
+
+Parameter: [VT_F_TR_OL_TM](../advanced_config/parameter_reference.md#VT_F_TR_OL_TM)
+
+This specifies the duration of the front transition in seconds when no airspeed feedback is available (e.g. no airspeed sensor present).
+It should be set to a value which ensures that the vehicle reaches a high enough airspeed to complete the transition, e.g. airspeed should exceed [VT_ARSP_TRANS](../advanced_config/parameter_reference.md#VT_ARSP_TRANS).
+
+#### Transition Timeout
+
+[VT_TRANS_TIMEOUT](../advanced_config/parameter_reference.md#VT_TRANS_TIMEOUT)
+
+This specifies the upper limit for the duration of the front transition. If the vehicle has not reached the transition airspeed after this time, then the transition will be aborted and a [Quadchute](../config/safety.md#quad-chute-failsafe) event will be triggered.
+:::note
+Additionally, if an airspeed sensor is present, the transition will also be aborted if the airspeed has not reached [VT_ARSP_BLEND](../advanced_config/parameter_reference.md#VT_ARSP_BLEND) after the openloop transition time [VT_F_TR_OL_TM](../advanced_config/parameter_reference.md#VT_F_TR_OL_TM) has elapsed. This checks is used to avoid a scenario where the vehicle gains excessive speed when the airspeed sensor is faulty.
+:::
+
 ### 过渡模式小提示
 
 As already mentioned make sure you have a well tuned multirotor mode.

docs/zh/debug/gdb_debugging.md

@@ -1,7 +1,7 @@
 # Debugging with GDB
 
 The [GNU DeBugger (GDB)](https://sourceware.org/gdb/documentation/) comes installed with the compiler toolchain in the form of the `arm-none-eabi-gdb` binary.
-调试器读取ELF文件内的调试富豪，以了解PX4固件的静态和动态内存布局。
+调试器读取ELF文件内的调试符号，以了解PX4固件的静态和动态内存布局。
 To access the PX4 autopilot microcontroller, it needs to connect to a [Remote Target](https://sourceware.org/gdb/current/onlinedocs/gdb.html/Connecting.html), which is provided by a [SWD debug probe](swd_debug.md).
 
 信息流看起来像这样：

docs/zh/simulation/hitl.md

@@ -1,7 +1,7 @@
-# Hardware in the Loop Simulation (HITL)
+# 硬体仿真(HITL)
 
 :::warning
-HITL is [community supported and maintained](../simulation/community_supported_simulators.md).
+硬体仿真被 [社区支持和维护](../simulation/community_supported_simulators.md)
 It may or may not work with current versions of PX4.
 
 See [Toolchain Installation](../dev_setup/dev_env.md) for information about the environments and tools supported by the core development team.
@@ -10,13 +10,13 @@ See [Toolchain Installation](../dev_setup/dev_env.md) for information about the
 硬件在环仿真模式 (HITL 或 HIL) 下 PX4 固件代码运行在真实的飞行控制器硬件平台上。
 这种方法的优点是可以在实际硬件上测试大多数的实际飞行代码。
 
-PX4 supports HITL for multicopters (using [jMAVSim](../sim_jmavsim/index.md) or [Gazebo Classic](../sim_gazebo_classic/index.md)) and VTOL (using Gazebo Classic).
+PX4 支持多轴( [jMAVSim](../sim_jmavsim/index.md)或[Gazebo Classic](../sim_gazebo_classic/index.md))及VTOL (using Gazebo Classic)的仿真。
 
 <a id="compatible_airframe"></a>
 
 ## HITL兼容机架
 
-The set of compatible airframes vs simulators is:
+机架与模拟器兼容情况：
 
 | 机架                                                                                                               | `SYS_AUTOSTART` | Gazebo Classic | jMAVSim |
 | ---------------------------------------------------------------------------------------------------------------- | --------------- | -------------- | ------- |

msg/CMakeLists.txt

@@ -45,7 +45,6 @@ set(msg_files
 	ActuatorTest.msg
 	AdcReport.msg
 	Airspeed.msg
-	AirspeedValidated.msg
 	AirspeedWind.msg
 	AutotuneAttitudeControlStatus.msg
 	ButtonEvent.msg
@@ -234,6 +233,7 @@ set(msg_files
 	YawEstimatorStatus.msg
 	versioned/ActuatorMotors.msg
 	versioned/ActuatorServos.msg
+	versioned/AirspeedValidated.msg
 	versioned/ArmingCheckReply.msg
 	versioned/ArmingCheckRequest.msg
 	versioned/BatteryStatus.msg

msg/versioned/AirspeedValidated.msg

@@ -1,3 +1,5 @@
+uint32 MESSAGE_VERSION = 0
+
 uint64 timestamp				# time since system start (microseconds)
 
 float32 indicated_airspeed_m_s			# indicated airspeed in m/s (IAS), set to NAN if invalid

platforms/nuttx/src/px4/stm/stm32_common/dshot/dshot.c

@@ -86,33 +86,29 @@
 
 static void init_timer_config(uint32_t channel_mask);
 static void init_timers_dma_up(void);
-static void init_timers_dma_capt_comp(uint8_t timer_index);
 static int32_t init_timer_channels(uint8_t timer_index);
 
-static void configure_channels_round_robin(uint8_t timer_index);
+static void select_next_capture_channel(uint8_t timer_index);
+static uint8_t output_channel_from_timer_channel(uint8_t timer_index, uint8_t timer_channel);
 
 static void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg);
 static void capture_complete_callback(void *arg);
 
-static void process_capture_results(uint8_t timer_index);
+static void process_capture_results(uint8_t timer_index, uint8_t channel_index);
 static unsigned calculate_period(uint8_t timer_index, uint8_t channel_index);
 
 // Timer configuration struct
 typedef struct timer_config_t {
-	DMA_HANDLE dma_up_handle;       // DMA stream for DMA update
-	DMA_HANDLE dma_ch_handle[4];    // DMA streams for bidi CaptComp
+	DMA_HANDLE dma_handle;       	// DMA stream for DMA update and eRPM Capture Compare
 	bool enabled;                   // Timer enabled
 	bool enabled_channels[4];       // Timer Channels enabled (requested)
 	bool initialized;               // Timer initialized
 	bool initialized_channels[4];   // Timer channels initialized (successfully started)
-	bool bidirectional;             // Timer in bidi (inverted) mode
-	bool captcomp_channels[4];      // Channels configured for CaptComp
-	bool round_robin_enabled;
+	bool bidirectional;             // Timer in bidir (inverted) mode
+	int capture_channel_index;		// Timer channel currently being catured in bidirectional mode
 	uint8_t timer_index;            // Timer index. Necessary to have memory for passing pointer to hrt callback
 } timer_config_t;
 
-static uint8_t _num_dma_available = 0;
-
 static timer_config_t timer_configs[MAX_IO_TIMERS] = {};
 
 // Output buffer of interleaved motor output bytes
@@ -129,8 +125,8 @@ static uint8_t  _bidi_timer_index = 0; // TODO: BDSHOT_TIM param to select timer
 static uint32_t _dshot_frequency = 0;
 
 // eRPM data for channels on the singular timer
-static int32_t _erpms[MAX_NUM_CHANNELS_PER_TIMER] = {};
-static bool _erpms_ready[MAX_NUM_CHANNELS_PER_TIMER] = {};
+static int32_t _erpms[MAX_TIMER_IO_CHANNELS] = {};
+static bool _erpms_ready[MAX_TIMER_IO_CHANNELS] = {};
 
 // hrt callback handle for captcomp post dma processing
 static struct hrt_call _cc_call;
@@ -143,7 +139,7 @@ static uint32_t read_fail_zero[MAX_NUM_CHANNELS_PER_TIMER] = {};
 
 static void init_timer_config(uint32_t channel_mask)
 {
-	// Mark timers in use, channels in use, and timers for bidi dshot
+	// Mark timers in use, channels in use, and timers for bidir dshot
 	for (unsigned output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
 		if (channel_mask & (1 << output_channel)) {
 			uint8_t timer_index = timer_io_channels[output_channel].timer_index;
@@ -203,9 +199,9 @@ static void init_timers_dma_up(void)
 		}
 
 		// Attempt to allocate DMA for Burst
-		timer_configs[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
+		timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
 
-		if (timer_configs[timer_index].dma_up_handle == NULL) {
+		if (timer_configs[timer_index].dma_handle == NULL) {
 			PX4_DEBUG("Failed to allocate Timer %u DMA UP", timer_index);
 			continue;
 		}
@@ -216,87 +212,14 @@ static void init_timers_dma_up(void)
 
 	// Free the allocated DMA channels
 	for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
-		if (timer_configs[timer_index].dma_up_handle != NULL) {
-			stm32_dmafree(timer_configs[timer_index].dma_up_handle);
-			timer_configs[timer_index].dma_up_handle = NULL;
+		if (timer_configs[timer_index].dma_handle != NULL) {
+			stm32_dmafree(timer_configs[timer_index].dma_handle);
+			timer_configs[timer_index].dma_handle = NULL;
 			PX4_DEBUG("Freed DMA UP Timer Index %u", timer_index);
 		}
 	}
 }
 
-static void init_timers_dma_capt_comp(uint8_t timer_index)
-{
-	if (timer_configs[timer_index].enabled && timer_configs[timer_index].initialized) {
-
-		// Allocate DMA for each enabled channel
-		for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
-
-			uint8_t timer_channel = timer_io_channels[output_channel].timer_channel;
-
-			if ((timer_channel <= 0) || (timer_channel >= 5)) {
-				// invalid channel, only 1 - 4
-				continue;
-			}
-
-			// For each enabled channel on this timer, try allocating DMA
-			uint8_t timer_channel_index = timer_channel - 1;
-			bool this_timer = timer_index == timer_io_channels[output_channel].timer_index;
-			bool channel_enabled = timer_configs[timer_index].enabled_channels[timer_channel_index];
-
-			if (this_timer && channel_enabled) {
-				uint32_t dma_map_ch = io_timers[timer_index].dshot.dma_map_ch[timer_channel_index];
-
-				if (dma_map_ch == 0) {
-					// Timer channel is not mapped
-					PX4_WARN("Error! Timer %u Channel %u DMA is unmapped", timer_index, timer_channel_index);
-					continue;
-				}
-
-				PX4_DEBUG("Allocating DMA CH for Timer Index %u Channel %u", timer_index, timer_channel_index);
-				// Allocate DMA
-				timer_configs[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(dma_map_ch);
-
-				if (timer_configs[timer_index].dma_ch_handle[timer_channel_index] == NULL) {
-					PX4_WARN("Failed to allocate Timer %u Channel DMA CH %u, output_channel %u", timer_index, timer_channel_index,
-						 output_channel);
-					continue;
-				}
-
-				PX4_DEBUG("Allocated DMA CH Timer Index %u Channel %u", timer_index, timer_channel_index);
-				// Mark this timer channel as bidirectional
-				timer_configs[timer_index].captcomp_channels[timer_channel_index] = true;
-				_num_dma_available++;
-
-				if (_num_dma_available >= BOARD_DMA_NUM_DSHOT_CHANNELS) {
-					PX4_INFO("Limiting DMA channels to %u", _num_dma_available);
-					break;
-				}
-			}
-		}
-
-		// De-allocate DMA for each channel
-		for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
-			if (timer_index == timer_io_channels[output_channel].timer_index) {
-
-				uint8_t timer_channel = timer_io_channels[output_channel].timer_channel;
-
-				if ((timer_channel <= 0) || (timer_channel >= 5)) {
-					// invalid channel, only 1 - 4
-					continue;
-				}
-
-				uint8_t timer_channel_index = timer_channel - 1;
-
-				if (timer_configs[timer_index].dma_ch_handle[timer_channel_index]) {
-					stm32_dmafree(timer_configs[timer_index].dma_ch_handle[timer_channel_index]);
-					timer_configs[timer_index].dma_ch_handle[timer_channel_index] = NULL;
-					PX4_DEBUG("Freed DMA CH Timer Index %u Channel %u", timer_index, timer_channel_index);
-				}
-			}
-		}
-	}
-}
-
 static int32_t init_timer_channels(uint8_t timer_index)
 {
 	int32_t channels_init_mask = 0;
@@ -366,18 +289,6 @@ int up_dshot_init(uint32_t channel_mask, unsigned dshot_pwm_freq, bool enable_bi
 	// Initializes dshot on each timer if DShot mode is enabled and DMA is available
 	init_timers_dma_up();
 
-	// Initializes a single timer in Bidirectional DShot mode
-	if (_bidirectional) {
-		// Use first configured DShot timer (Timer index 0)
-		// TODO: BDSHOT_TIM param to select timer index?
-		init_timers_dma_capt_comp(_bidi_timer_index);
-
-		// Enable round robin if we have 1 - 3 DMA
-		if ((_num_dma_available < 4) && _num_dma_available > 0) {
-			timer_configs[_bidi_timer_index].round_robin_enabled = true;
-		}
-	}
-
 	int32_t channels_init_mask = 0;
 
 	for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) {
@@ -426,10 +337,10 @@ void up_dshot_trigger()
 			io_timer_set_dshot_burst_mode(timer_index, _dshot_frequency, channel_count);
 
 			// Allocate DMA
-			if (timer_configs[timer_index].dma_up_handle == NULL) {
-				timer_configs[timer_index].dma_up_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
+			if (timer_configs[timer_index].dma_handle == NULL) {
+				timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_up);
 
-				if (timer_configs[timer_index].dma_up_handle == NULL) {
+				if (timer_configs[timer_index].dma_handle == NULL) {
 					PX4_WARN("DMA allocation for timer %u failed", timer_index);
 					continue;
 				}
@@ -440,7 +351,7 @@ void up_dshot_trigger()
 					(uintptr_t) dshot_output_buffer[timer_index] +
 					DSHOT_OUTPUT_BUFFER_SIZE(channel_count));
 
-			px4_stm32_dmasetup(timer_configs[timer_index].dma_up_handle,
+			px4_stm32_dmasetup(timer_configs[timer_index].dma_handle,
 					   io_timers[timer_index].base + STM32_GTIM_DMAR_OFFSET,
 					   (uint32_t)(dshot_output_buffer[timer_index]),
 					   channel_count * CHANNEL_OUTPUT_BUFF_SIZE,
@@ -451,12 +362,12 @@ void up_dshot_trigger()
 
 			// Trigger DMA (DShot Outputs)
 			if (timer_configs[timer_index].bidirectional) {
-				stm32_dmastart(timer_configs[timer_index].dma_up_handle, dma_burst_finished_callback,
+				stm32_dmastart(timer_configs[timer_index].dma_handle, dma_burst_finished_callback,
 					       &timer_configs[timer_index].timer_index,
 					       false);
 
 			} else {
-				stm32_dmastart(timer_configs[timer_index].dma_up_handle,  NULL, NULL, false);
+				stm32_dmastart(timer_configs[timer_index].dma_handle,  NULL, NULL, false);
 			}
 
 			// Enable DMA update request
@@ -465,67 +376,41 @@ void up_dshot_trigger()
 	}
 }
 
-static void configure_channels_round_robin(uint8_t timer_index)
+static void select_next_capture_channel(uint8_t timer_index)
 {
-	switch (_num_dma_available) {
-	case 1: {
-			for (uint8_t i = 0; i < 4; i++) {
-				if (timer_configs[timer_index].captcomp_channels[i]) {
-					timer_configs[timer_index].captcomp_channels[i] = false;
+	bool found = false;
+	int next_index = timer_configs[timer_index].capture_channel_index;
 
-					if (i == 3) {
-						timer_configs[timer_index].captcomp_channels[0] = true;
+	while (!found) {
+		next_index++;
 
-					} else {
-						timer_configs[timer_index].captcomp_channels[i + 1] = true;
-					}
-
-					break;
-				}
-			}
-
-			break;
+		if (next_index > 3) {
+			next_index = 0;
 		}
 
-	case 2: {
-			if (timer_configs[timer_index].captcomp_channels[0]) {
-				timer_configs[timer_index].captcomp_channels[0] = false;
-				timer_configs[timer_index].captcomp_channels[1] = true;
-				timer_configs[timer_index].captcomp_channels[2] = false;
-				timer_configs[timer_index].captcomp_channels[3] = true;
-
-			} else {
-				timer_configs[timer_index].captcomp_channels[0] = true;
-				timer_configs[timer_index].captcomp_channels[1] = false;
-				timer_configs[timer_index].captcomp_channels[2] = true;
-				timer_configs[timer_index].captcomp_channels[3] = false;
-			}
-
-			break;
+		if (timer_configs[timer_index].initialized_channels[next_index]) {
+			found = true;
 		}
-
-	case 3: {
-			for (uint8_t i = 0; i < 4; i++) {
-				if (!timer_configs[timer_index].captcomp_channels[i]) {
-					timer_configs[timer_index].captcomp_channels[i] = true;
-
-					if (i == 3) {
-						timer_configs[timer_index].captcomp_channels[0] = false;
-
-					} else {
-						timer_configs[timer_index].captcomp_channels[i + 1] = false;
-					}
-
-					break;
-				}
-			}
-
-			break;
-		}
-
-	default:
-		break;
 	}
+
+	timer_configs[timer_index].capture_channel_index = next_index;
+}
+
+static uint8_t output_channel_from_timer_channel(uint8_t timer_index, uint8_t timer_channel_index)
+{
+	for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
+
+		bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index;
+		uint8_t channel_index = timer_io_channels[output_channel].timer_channel - 1;
+
+		if (is_this_timer && (channel_index == timer_channel_index)) {
+			// We found the output channel associated with this timer channel
+			return output_channel;
+		}
+	}
+
+	// TODO: error handling?
+	return 0;
 }
 
 void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
@@ -533,10 +418,10 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
 	uint8_t timer_index = *((uint8_t *)arg);
 
 	// Clean DMA UP configuration
-	if (timer_configs[timer_index].dma_up_handle != NULL) {
-		stm32_dmastop(timer_configs[timer_index].dma_up_handle);
-		stm32_dmafree(timer_configs[timer_index].dma_up_handle);
-		timer_configs[timer_index].dma_up_handle = NULL;
+	if (timer_configs[timer_index].dma_handle != NULL) {
+		stm32_dmastop(timer_configs[timer_index].dma_handle);
+		stm32_dmafree(timer_configs[timer_index].dma_handle);
+		timer_configs[timer_index].dma_handle = NULL;
 	}
 
 	// Disable DMA update request
@@ -550,63 +435,44 @@ void dma_burst_finished_callback(DMA_HANDLE handle, uint8_t status, void *arg)
 	up_clean_dcache((uintptr_t) dshot_capture_buffer,
 			(uintptr_t) dshot_capture_buffer + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER));
 
-	// If round robin is enabled reconfigure which channels we capture on
-	if (timer_configs[timer_index].round_robin_enabled) {
-		configure_channels_round_robin(timer_index);
+	// Unallocate timer channel for currently selected capture_channel
+	uint8_t capture_channel = timer_configs[timer_index].capture_channel_index;
+	uint8_t output_channel = output_channel_from_timer_channel(timer_index, capture_channel);
+
+	// Re-initialize output for CaptureDMA for next time
+	io_timer_unallocate_channel(output_channel);
+	io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL);
+
+	// Select the next capture channel
+	select_next_capture_channel(timer_index);
+
+	// Allocate DMA for currently selected capture_channel
+	capture_channel = timer_configs[timer_index].capture_channel_index;
+	timer_configs[timer_index].dma_handle = stm32_dmachannel(io_timers[timer_index].dshot.dma_map_ch[capture_channel]);
+
+	// If DMA handler is valid, start DMA
+	if (timer_configs[timer_index].dma_handle == NULL) {
+		PX4_WARN("failed to allocate dma for timer %u channel %u", timer_index, capture_channel);
+		return;
 	}
 
-	// Allocate DMA for all enabled channels on this timer
-	for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
+	// Set Capture mode for this channel
+	io_timer_set_dshot_capture_mode(timer_index, capture_channel, _dshot_frequency);
+	io_timer_capture_dma_req(timer_index, capture_channel, true);
 
-		bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index;
-		uint8_t timer_channel = timer_io_channels[output_channel].timer_channel;
+	// Choose which CC register for this DMA stream
+	uint32_t periph_addr = io_timers[timer_index].base + STM32_GTIM_CCR1_OFFSET + (4 * capture_channel);
 
-		if ((timer_channel <= 0) || (timer_channel >= 5)) {
-			// invalid channel, only 1 - 4
-			continue;
-		}
+	// Setup DMA for this channel
+	px4_stm32_dmasetup(timer_configs[timer_index].dma_handle,
+			   periph_addr,
+			   (uint32_t) dshot_capture_buffer[capture_channel],
+			   CHANNEL_CAPTURE_BUFF_SIZE,
+			   DSHOT_BIDIRECTIONAL_DMA_SCR);
 
-		uint8_t timer_channel_index = timer_channel - 1;
-		bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
-		bool captcomp_enabled = timer_configs[timer_index].captcomp_channels[timer_channel_index];
-
-		if (is_this_timer && channel_initialized && captcomp_enabled) {
-
-			// Re-initialize output for CaptureDMA
-			io_timer_unallocate_channel(output_channel);
-			io_timer_channel_init(output_channel, IOTimerChanMode_CaptureDMA, NULL, NULL);
-
-			// Allocate DMA
-			if (timer_configs[timer_index].dma_ch_handle[timer_channel_index] == NULL) {
-				timer_configs[timer_index].dma_ch_handle[timer_channel_index] = stm32_dmachannel(
-							io_timers[timer_index].dshot.dma_map_ch[timer_channel_index]);
-			}
-
-			// If DMA handler is valid, start DMA
-			if (timer_configs[timer_index].dma_ch_handle[timer_channel_index] == NULL) {
-				PX4_WARN("failed to allocate dma for timer %u channel %u", timer_index, timer_channel_index);
-				return;
-			}
-
-			// Set Capture mode for this channel
-			io_timer_set_dshot_capture_mode(timer_index, timer_channel_index, _dshot_frequency);
-			io_timer_capture_dma_req(timer_index, timer_channel_index, true);
-
-			// Choose which CC register for this DMA stream
-			uint32_t periph_addr = io_timers[timer_index].base + STM32_GTIM_CCR1_OFFSET + (4 * timer_channel_index);
-
-			// Setup DMA for this channel
-			px4_stm32_dmasetup(timer_configs[timer_index].dma_ch_handle[timer_channel_index],
-					   periph_addr,
-					   (uint32_t) dshot_capture_buffer[timer_channel_index],
-					   CHANNEL_CAPTURE_BUFF_SIZE,
-					   DSHOT_BIDIRECTIONAL_DMA_SCR);
-
-			// NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train. Instead
-			// we use an hrt callback to schedule the processing of the received and DMAd eRPM frames.
-			stm32_dmastart(timer_configs[timer_index].dma_ch_handle[timer_channel_index], NULL, NULL, false);
-		}
-	}
+	// NOTE: we can't use DMA callback since GCR encoding creates a variable length pulse train. Instead
+	// we use an hrt callback to schedule the processing of the received and DMAd eRPM frames.
+	stm32_dmastart(timer_configs[timer_index].dma_handle, NULL, NULL, false);
 
 	// Enable CaptureDMA and on all configured channels
 	io_timer_set_enable(true, IOTimerChanMode_CaptureDMA, IO_TIMER_ALL_MODES_CHANNELS);
@@ -624,33 +490,26 @@ static void capture_complete_callback(void *arg)
 	// Unallocate the timer as CaptureDMA
 	io_timer_unallocate_timer(timer_index);
 
+	uint8_t capture_channel = timer_configs[timer_index].capture_channel_index;
+
+	// Disable capture DMA
+	io_timer_capture_dma_req(timer_index, capture_channel, false);
+
+	if (timer_configs[timer_index].dma_handle != NULL) {
+		stm32_dmastop(timer_configs[timer_index].dma_handle);
+		stm32_dmafree(timer_configs[timer_index].dma_handle);
+		timer_configs[timer_index].dma_handle = NULL;
+	}
+
+	// Re-initialize all output channels on this timer
 	for (uint8_t output_channel = 0; output_channel < MAX_TIMER_IO_CHANNELS; output_channel++) {
 
 		bool is_this_timer = timer_index == timer_io_channels[output_channel].timer_index;
-		uint8_t timer_channel = timer_io_channels[output_channel].timer_channel;
-
-		if ((timer_channel <= 0) || (timer_channel >= 5)) {
-			// invalid channel, only 1 - 4
-			continue;
-		}
-
-		uint8_t timer_channel_index = timer_channel - 1;
+		uint8_t timer_channel_index = timer_io_channels[output_channel].timer_channel - 1;
 		bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
-		bool captcomp_enabled = timer_configs[timer_index].captcomp_channels[timer_channel_index];
 
 		if (is_this_timer && channel_initialized) {
 
-			if (captcomp_enabled) {
-				// Disable capture DMA
-				io_timer_capture_dma_req(timer_index, timer_channel_index, false);
-
-				if (timer_configs[timer_index].dma_ch_handle[timer_channel_index] != NULL) {
-					stm32_dmastop(timer_configs[timer_index].dma_ch_handle[timer_channel_index]);
-					stm32_dmafree(timer_configs[timer_index].dma_ch_handle[timer_channel_index]);
-					timer_configs[timer_index].dma_ch_handle[timer_channel_index] = NULL;
-				}
-			}
-
 			io_timer_unallocate_channel(output_channel);
 			// Initialize back to DShotInverted to bring IO back to the expected idle state
 			io_timer_channel_init(output_channel, IOTimerChanMode_DshotInverted, NULL, NULL);
@@ -662,39 +521,34 @@ static void capture_complete_callback(void *arg)
 			     (uintptr_t) dshot_capture_buffer + DSHOT_CAPTURE_BUFFER_SIZE(MAX_NUM_CHANNELS_PER_TIMER));
 
 	// Process eRPM frames from all channels on this timer
-	process_capture_results(timer_index);
+	process_capture_results(timer_index, capture_channel);
 
 	// Enable all channels configured as DShotInverted
 	io_timer_set_enable(true, IOTimerChanMode_DshotInverted, IO_TIMER_ALL_MODES_CHANNELS);
 }
 
-void process_capture_results(uint8_t timer_index)
+void process_capture_results(uint8_t timer_index, uint8_t channel_index)
 {
-	for (uint8_t channel_index = 0; channel_index < MAX_NUM_CHANNELS_PER_TIMER; channel_index++) {
+	const unsigned period = calculate_period(timer_index, channel_index);
 
-		if (!timer_configs[timer_index].captcomp_channels[channel_index]) {
-			continue;
-		}
+	uint8_t output_channel = output_channel_from_timer_channel(timer_index, channel_index);
 
-		// Calculate the period for each channel
-		const unsigned period = calculate_period(timer_index, channel_index);
 
-		if (period == 0) {
-			// If the parsing failed, set the eRPM to 0
-			_erpms[channel_index] = 0;
+	if (period == 0) {
+		// If the parsing failed, set the eRPM to 0
+		_erpms[output_channel] = 0;
 
-		} else if (period == 65408) {
-			// Special case for zero motion (e.g., stationary motor)
-			_erpms[channel_index] = 0;
+	} else if (period == 65408) {
+		// Special case for zero motion (e.g., stationary motor)
+		_erpms[output_channel] = 0;
 
-		} else {
-			// Convert the period to eRPM
-			_erpms[channel_index] = (1000000 * 60) / period;
-		}
-
-		// We set it ready anyway, not to hold up other channels when used in round robin.
-		_erpms_ready[channel_index] = true;
+	} else {
+		// Convert the period to eRPM
+		_erpms[output_channel] = (1000000 * 60) / period;
 	}
+
+	// We set it ready anyway, not to hold up other channels when used in round robin.
+	_erpms_ready[output_channel] = true;
 }
 
 /**
@@ -757,7 +611,7 @@ int up_bdshot_num_erpm_ready(void)
 {
 	int num_ready = 0;
 
-	for (unsigned i = 0; i < MAX_NUM_CHANNELS_PER_TIMER; ++i) {
+	for (unsigned i = 0; i < MAX_TIMER_IO_CHANNELS; ++i) {
 		if (_erpms_ready[i]) {
 			++num_ready;
 		}
@@ -773,8 +627,8 @@ int up_bdshot_get_erpm(uint8_t output_channel, int *erpm)
 	bool channel_initialized = timer_configs[timer_index].initialized_channels[timer_channel_index];
 
 	if (channel_initialized) {
-		*erpm = _erpms[timer_channel_index];
-		_erpms_ready[timer_channel_index] = false;
+		*erpm = _erpms[output_channel];
+		_erpms_ready[output_channel] = false;
 		return PX4_OK;
 	}
 
@@ -802,11 +656,6 @@ void up_bdshot_status(void)
 
 	if (_bidirectional) {
 		PX4_INFO("Bidirectional DShot enabled");
-		PX4_INFO("Available DMA: %u", _num_dma_available);
-
-		if (_num_dma_available < 4) {
-			PX4_INFO("Round robin enabled");
-		}
 	}
 
 	uint8_t timer_index = _bidi_timer_index;

src/drivers/gps/devices

@@ -1 +1 @@
-Subproject commit 91e3dbb296d74b2a6d4f8f3c7f2fddd3c6705495
+Subproject commit 4cea5de87ce7e2a52e5289bd3639fd8eb770eafa

src/lib/battery/battery.cpp

@@ -57,8 +57,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_current_average_filter_a.setParameters(expected_filter_dt, 50.f);
 	_ocv_filter_v.setParameters(expected_filter_dt, 1.f);
 	_cell_voltage_filter_v.setParameters(expected_filter_dt, 1.f);
-	_soc_derivative_filter.setParameters(expected_filter_dt, 50.f);
-	_flight_time_filter.setParameters(expected_filter_dt, 50.f);
 
 	if (index > 9 || index < 1) {
 		PX4_ERR("Battery index must be between 1 and 9 (inclusive). Received %d. Defaulting to 1.", index);
@@ -94,7 +92,6 @@ Battery::Battery(int index, ModuleParams *parent, const int sample_interval_us,
 	_param_handles.emergen_thr = param_find("BAT_EMERGEN_THR");
 
 	_param_handles.bat_avrg_current = param_find("BAT_AVRG_CURRENT");
-	_param_handles.bat_avrg_soc = param_find("BAT_AVRG_SOC");
 
 	updateParams();
 }
@@ -156,7 +153,7 @@ battery_status_s Battery::getBatteryStatus()
 	battery_status.discharged_mah = _discharged_mah;
 	battery_status.remaining = _state_of_charge;
 	battery_status.scale = _scale;
-	battery_status.time_remaining_s = computeRemainingTime(_state_of_charge);
+	battery_status.time_remaining_s = computeRemainingTime(_current_a);
 	battery_status.temperature = _temperature_c;
 	battery_status.cell_count = _params.n_cells;
 	battery_status.connected = _connected;
@@ -348,11 +345,10 @@ void Battery::computeScale()
 	}
 }
 
-float Battery::computeRemainingTime(const float state_of_charge)
+float Battery::computeRemainingTime(float current_a)
 {
 	float time_remaining_s = NAN;
-	bool reset_soc_derivative_filter = false;
-	hrt_abstime timestamp = hrt_absolute_time();
+	bool reset_current_avg_filter = false;
 
 	if (_vehicle_status_sub.updated()) {
 		vehicle_status_s vehicle_status;
@@ -361,7 +357,7 @@ float Battery::computeRemainingTime(const float state_of_charge)
 			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
 
 			if (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_vehicle_status_is_fw) {
-				reset_soc_derivative_filter = true;
+				reset_current_avg_filter = true;
 			}
 
 			_vehicle_status_is_fw = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING);
@@ -371,31 +367,28 @@ float Battery::computeRemainingTime(const float state_of_charge)
 	_flight_phase_estimation_sub.update();
 
 	// reset filter if not feasible, negative or we did a VTOL transition to FW mode
-	if (!PX4_ISFINITE(_soc_derivative_filter.getState()) || _soc_derivative_filter.getState() < FLT_EPSILON
-	    || reset_soc_derivative_filter) {
-		_soc_derivative_filter.reset(_params.bat_avrg_soc);
+	if (!PX4_ISFINITE(_current_average_filter_a.getState()) || _current_average_filter_a.getState() < FLT_EPSILON
+	    || reset_current_avg_filter) {
+		_current_average_filter_a.reset(_params.bat_avrg_current);
 	}
 
-	const float dt = math::constrain(timestamp - _last_flight_time_update, 1_ms, 5000_ms) * 1e-6f;
-
-	if (_armed && _state_of_charge < 0.99f && PX4_ISFINITE(dt)) {
+	if (_armed && PX4_ISFINITE(current_a)) {
 		// For FW only update when we are in level flight
-		if (!_vehicle_status_is_fw || ((timestamp - _flight_phase_estimation_sub.get().timestamp) < 2_s
+		if (!_vehicle_status_is_fw || ((hrt_absolute_time() - _flight_phase_estimation_sub.get().timestamp) < 2_s
 					       && _flight_phase_estimation_sub.get().flight_phase == flight_phase_estimation_s::FLIGHT_PHASE_LEVEL)) {
-			_soc_derivative_filter.update((_prev_state_of_charge - _state_of_charge) / dt);
+			// only update with positive numbers
+			_current_average_filter_a.update(fmaxf(current_a, 0.f));
 		}
-
-	} else {
-		_soc_derivative_filter.reset(_params.bat_avrg_soc);
-		_flight_time_filter.reset(_state_of_charge / fmaxf(_soc_derivative_filter.getState(), FLT_EPSILON));
 	}
 
-	time_remaining_s = _state_of_charge / fmaxf(_soc_derivative_filter.getState(), FLT_EPSILON);
-	_flight_time_filter.update(time_remaining_s);
-	_last_flight_time_update = timestamp;
-	_prev_state_of_charge = state_of_charge;
+	// Remaining time estimation only possible with capacity
+	if (_params.capacity > 0.f) {
+		const float remaining_capacity_mah = _state_of_charge * _params.capacity;
+		const float current_ma = fmaxf(_current_average_filter_a.getState() * 1e3f, FLT_EPSILON);
+		time_remaining_s = remaining_capacity_mah / current_ma * 3600.f;
+	}
 
-	return _flight_time_filter.getState();
+	return time_remaining_s;
 }
 
 void Battery::updateParams()
@@ -411,7 +404,6 @@ void Battery::updateParams()
 	param_get(_param_handles.crit_thr, &_params.crit_thr);
 	param_get(_param_handles.emergen_thr, &_params.emergen_thr);
 	param_get(_param_handles.bat_avrg_current, &_params.bat_avrg_current);
-	param_get(_param_handles.bat_avrg_soc, &_params.bat_avrg_soc);
 
 	if (n_cells != _params.n_cells) {
 		_internal_resistance_initialized = false;

src/lib/battery/battery.h

@@ -124,7 +124,6 @@ protected:
 		param_t emergen_thr;
 		param_t source;
 		param_t bat_avrg_current;
-		param_t bat_avrg_soc;
 	} _param_handles{};
 
 	struct {
@@ -138,7 +137,6 @@ protected:
 		float emergen_thr;
 		int32_t source;
 		float bat_avrg_current;
-		float bat_avrg_soc;
 	} _params{};
 
 	const int _index;
@@ -153,7 +151,7 @@ private:
 	uint8_t determineWarning(float state_of_charge);
 	uint16_t determineFaults();
 	void computeScale();
-	float computeRemainingTime(const float state_of_charge);
+	float computeRemainingTime(float current_a);
 
 	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
 	uORB::SubscriptionData<flight_phase_estimation_s> _flight_phase_estimation_sub{ORB_ID(flight_phase_estimation)};
@@ -168,8 +166,6 @@ private:
 	float _voltage_v{0.f};
 	AlphaFilter<float> _ocv_filter_v;
 	AlphaFilter<float> _cell_voltage_filter_v;
-	AlphaFilter<float> _soc_derivative_filter;
-	AlphaFilter<float> _flight_time_filter;
 	float _current_a{-1};
 	AlphaFilter<float>
 	_current_average_filter_a; ///< averaging filter for current. For FW, it is the current in level flight.
@@ -178,14 +174,12 @@ private:
 	float _discharged_mah_loop{0.f};
 	float _state_of_charge_volt_based{-1.f}; // [0,1]
 	float _state_of_charge{-1.f}; // [0,1]
-	float _prev_state_of_charge{-1.f}; // [0, 1]
 	float _scale{1.f};
 	uint8_t _warning{battery_status_s::WARNING_NONE};
 	hrt_abstime _last_timestamp{0};
 	bool _armed{false};
 	bool _vehicle_status_is_fw{false};
 	hrt_abstime _last_unconnected_timestamp{0};
-	hrt_abstime _last_flight_time_update{0};
 
 	// Internal Resistance estimation
 	void updateInternalResistanceEstimation(const float voltage_v, const float current_a);

src/lib/battery/battery_estimation_replay.py

@@ -1,310 +0,0 @@
-# Test internal resistance and flight time estimator on flight logs
-# run with:
-# python3 battery_estimation_replay.py -f <(required)pathToLogFile>
-#   -r <(optional)useLoggedRemainingForFlightTimeEstimation>
-#   -c <(optional)#batteryCells>
-#   -u <(optional)fullCellVoltage>
-#   -e <(optional)emptyCellVoltage>
-#   -l <(optional)forgettingFactor>
-#   -s <(optional)averageSocConsumption>
-#   -t <(optional)remainingFilterTimeConstant>
-#   -k <(optional)flightTimeFilterTimeConstant>
-# Note: Can lead to slightly different results than the online estimation due to the fact that
-# the log frequency of the voltage and current are not the same as the online frequency.
-
-from pyulog import ULog
-import matplotlib.pyplot as plt
-import numpy as np
-import argparse
-
-def getData(log, topic_name, variable_name, instance=0):
-    for elem in log.data_list:
-        if elem.name == topic_name and instance == elem.multi_id:
-            return elem.data[variable_name]
-    return np.array([])
-
-def us2s(time_us):
-    return time_us * 1e-6
-
-def getParam(log, param_name):
-    if param_name in log.initial_parameters:
-        return log.initial_parameters[param_name]
-    else:
-        print(f"Parameter {param_name} not found in log.")
-    return None
-
-def alphaFilter(filter_state, alpha, sample):
-    return filter_state + alpha * (sample - filter_state)
-
-def updateRLS(theta, P, x, voltage, current, lam):
-    gamma = P @ x / (lam + x.T @ P @ x)
-    error = voltage - x.T @ theta
-    data_cov = x.T @ P @ x
-    theta_temp = theta + gamma * error
-    P_temp = (P - gamma @ x.T @ P) / lam
-    if (abs(np.linalg.norm(P)) < abs(np.linalg.norm(P_temp))):
-        theta_corr = np.array([voltage + theta[1] * current, theta[1]]) # Correct OCV estimation
-        P_corr = P
-        return theta_corr, P_corr, error[0][0], data_cov[0][0], 0, 0
-    return theta_temp, P_temp, error[0][0], data_cov[0][0], gamma[0][0], gamma[1][0]
-
-def main(log_name, logged_remaining, n_cells, full_cell, empty_cell, lam, soc_consumption_avg, time_constant_ocv_derivative, time_constant_flight_time):
-    log = ULog(log_name)
-
-    log_n_cells    = getParam(log, 'BAT1_N_CELLS')
-    log_full_cell  = getParam(log, 'BAT1_V_CHARGED')
-    log_empty_cell = getParam(log, 'BAT1_V_EMPTY')
-    log_capacity   = getParam(log, 'BAT1_CAPACITY')
-
-    # Debug information
-    print("\nParameters:")
-    print(f"Extracted from log - BAT1_N_CELLS: {log_n_cells}, BAT1_V_CHARGED: {log_full_cell}, BAT1_V_EMPTY: {log_empty_cell}, BAT1_CAPACITY: {log_capacity}")
-
-    # Use log parameters unless overridden
-    if n_cells is None:
-        n_cells = log_n_cells
-    else:
-        print(f"Using override for n_cells: {n_cells}")
-    if full_cell is None:
-        full_cell = log_full_cell
-    else:
-        print(f"Using override for full_cell: {full_cell}")
-    if empty_cell is None:
-        empty_cell = log_empty_cell
-    else:
-        print(f"Using override for empty_cell: {empty_cell}")
-
-    # Debug information for final parameter values
-    print(f"Using parameters - n_cells: {n_cells}, full_cell: {full_cell}, empty_cell: {empty_cell}")
-
-    # Extract data from log
-    timestamps            = us2s(getData(log, 'battery_status', 'timestamp'))
-    current               =      getData(log, 'battery_status', 'current_a')
-    current_average       =      getData(log, 'battery_status', 'current_average_a')
-    voltage               =      getData(log, 'battery_status', 'voltage_v')
-    remaining             =      getData(log, 'battery_status', 'remaining')
-    remaining_flight_time =      getData(log, 'battery_status', 'time_remaining_s')
-
-    if not timestamps.size or not current.size or not current_average.size or not voltage.size or not remaining.size:
-        print("Error: Incomplete data.")
-        return
-
-    ## Internal resistance estimation
-
-    # Containers for plotting
-    ocv_est                    = np.zeros_like(timestamps)
-    ocv_est_filtered           = np.zeros_like(timestamps)
-    r_est                      = np.zeros_like(timestamps)
-    error_hist                 = np.zeros_like(timestamps)
-    v_est                      = np.zeros_like(timestamps)
-    internal_resistance_stable = np.zeros_like(timestamps)
-    cov_norm                   = np.zeros_like(timestamps)
-    r_cov                      = np.zeros_like(timestamps)
-    ocv_cov                    = np.zeros_like(timestamps)
-    mixed_cov                  = np.zeros_like(timestamps)
-    data_cov_hist              = np.zeros_like(timestamps)
-    gamma_ocv_hist             = np.zeros_like(timestamps)
-    gamma_r_hist               = np.zeros_like(timestamps)
-    remaining_est              = np.zeros_like(timestamps)
-    remaining_est_filtered     = np.zeros_like(timestamps)
-
-    # Initializations
-    theta = np.array([[voltage[0] + 0.005 * n_cells * current[0]], [0.005 * n_cells]])  # Initial VOC and R
-    P = np.diag([1.2 * n_cells, 0.1 * n_cells]) # Initial covariance
-    error = 0
-    sample_interval = timestamps[1] - timestamps[0]
-    alpha_ocv = sample_interval / (sample_interval + 1)
-    internal_resistance_stable[-1] = 0.005
-
-    for index in range(len(current)):
-        # RLS algorithm
-        x = np.array([[1.0], [-current[index]]]) # Input vector
-        theta, P, error, data_cov, gamma_ocv_hist[index], gamma_r_hist[index] = updateRLS(theta, P, x, voltage[index], current[index], lam) # Run RLS
-
-        # Save steps for plotting
-        ocv_est[index] = theta[0][0]
-        if (index == 0):
-            ocv_est_filtered[index] = ocv_est[index]
-        else:
-            ocv_est_filtered[index] = alphaFilter(ocv_est_filtered[index - 1], alpha_ocv, ocv_est[index])
-        r_est[index] = theta[1][0]
-        error_hist[index] = error
-        v_est[index] = (x.T @ theta)[0][0]
-        cov_norm[index] = abs(np.linalg.norm(P))
-        ocv_cov[index] = P[0][0]
-        r_cov[index] = P[1][1]
-        mixed_cov[index] = P[0][1]
-        data_cov_hist[index] = data_cov
-        internal_resistance_stable[index] = max(r_est[index]/n_cells, 0.001)
-        remaining_est[index] = np.interp((voltage[index] + internal_resistance_stable[index] * n_cells * current[index]) / n_cells, [empty_cell, full_cell], [0, 1])
-        remaining_est_filtered[index] = np.interp(ocv_est_filtered[index] / n_cells, [empty_cell, full_cell], [0, 1])
-
-    ## Flight time estimation
-
-    # Containers for plotting
-    flight_time_estimated          = np.zeros_like(timestamps)
-    flight_time_estimated_filtered = np.zeros_like(timestamps)
-    remaining_consumption          = np.zeros_like(timestamps)
-    remaining_consumption_average  = np.zeros_like(timestamps)
-
-    # Initializations
-    alpha_ocv_derivative = (sample_interval) / (sample_interval + time_constant_ocv_derivative)
-    alpha_flight_time = (sample_interval) / (sample_interval + time_constant_flight_time)
-    if (logged_remaining):
-        state_of_charge = remaining
-    else:
-        state_of_charge = remaining_est_filtered
-    flight_time_estimated[0] = remaining[0] / soc_consumption_avg
-    flight_time_estimated_filtered[0] = flight_time_estimated[0]
-    remaining_consumption_average[0] = soc_consumption_avg
-    dt = 0
-
-    for index in range(len(current)):
-        if index == 0:
-            continue
-        dt = timestamps[index] - timestamps[index - 1]
-        remaining_consumption[index] = (state_of_charge[index - 1] - state_of_charge[index]) / dt
-        if state_of_charge[index] > 0.99:
-            remaining_consumption_average[index] = soc_consumption_avg
-            flight_time_estimated[index] = state_of_charge[index] / soc_consumption_avg
-        else:
-            remaining_consumption_average[index] = np.clip(alphaFilter(remaining_consumption_average[index - 1], alpha_ocv_derivative, remaining_consumption[index]), 0.0005, 0.1)
-            flight_time_estimated[index] = state_of_charge[index] / remaining_consumption_average[index]
-        flight_time_estimated_filtered[index] = alphaFilter(flight_time_estimated_filtered[index - 1], alpha_flight_time, flight_time_estimated[index])
-
-    ### Plot data
-
-    ## Internal resistance estimation plots
-    print("\nInternal Resistance estimation:")
-    print("Internal Resistance mean (per cell): ", np.mean(r_est) / n_cells)
-    sumFig = plt.figure("Battery Estimation with RLS")
-
-    volt = plt.subplot(2, 3, 1)
-    volt.plot(timestamps, voltage, label='Measured voltage')
-    volt.plot(timestamps, v_est, label='Estimated voltage')
-    volt.plot(timestamps, np.array(voltage) + np.array(internal_resistance_stable) * np.array(current) * n_cells, label='OCV estimate')
-    volt.plot(timestamps, ocv_est_filtered, label='OCV estimate smoothed')
-    volt.plot(timestamps, np.full_like(current, full_cell * n_cells), label='100% SOC')
-    volt.set_title("Measured Voltage vs Estimated voltage vs Estimated Open circuit voltage [voltage]")
-    volt.legend()
-
-    intR = plt.subplot(2, 3, 2)
-    intR.plot(timestamps, np.array(r_est) * 1000 / n_cells, label='Internal resistance estimate')
-    intR.set_title("Internal resistance estimate (per cell) [mOhm]")
-    intR.legend()
-
-    soc = plt.subplot(2, 3, 3)
-    soc.plot(timestamps, remaining, label='SoC logged')
-    soc.plot(timestamps, remaining_est, label='SoC with estimator')
-    soc.plot(timestamps, remaining_est_filtered, label='SoC with estimator smoothed')
-    soc.set_title("State of charge")
-    soc.legend()
-
-    curr = plt.subplot(2, 3, 4)
-    curr.plot(timestamps, current, label='Measured current')
-    curr.set_title("Measured Current [A]")
-    curr.legend()
-
-    err = plt.subplot(2, 3, 5)
-    err.plot(timestamps, error_hist, label='$Error$')
-    err.set_title("Voltage estimation error [voltage]")
-    err.legend()
-
-    cov = plt.subplot(2, 3, 6)
-    cov.plot(timestamps, cov_norm, label = 'Covariance norm')
-    cov.set_title("Covariance norm")
-    cov.legend()
-
-    # # SoC estimation plots
-    # socFig = plt.figure("SoC estimation")
-    # plt.plot(timestamps, np.interp((np.array(voltage) + np.array(current) * 0.005 * n_cells) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with default $R_{int}$')
-    # plt.plot(timestamps, remaining, label='SoC logged')
-    # plt.plot(timestamps, np.interp((np.array(voltage) + np.array(internal_resistance_stable) * n_cells * np.array(current)) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator')
-    # # plt.plot(timestamps, np.convolve(np.interp((np.array(voltage) + np.array(internal_resistance_stable) * n_cells * np.array(current)) / n_cells, [empty_cell, full_cell], [0, 1]), np.ones(500)/500, mode='full')[0:np.size(timestamps)], label='SoC with estimator smoothed')
-    # # plt.plot(timestamps, np.interp((np.array(voltage) + np.array(current) * 0.0009 * n_cells) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with $R_{int}$ measured beforehand')
-    # # plt.plot(timestamps, np.interp(ocv_est/n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with VOC estimate')
-    # plt.legend()
-
-    # # Covariance plots
-    # covFig = plt.figure("Covariance plots")
-    # covR = plt.subplot(2, 2, 1)
-    # covR.plot(timestamps, r_cov, label = 'r_cov')
-    # covR.set_title("Internal resistance covariance")
-    # covR.legend()
-    # covVOC = plt.subplot(2, 2, 2)
-    # covVOC.plot(timestamps, ocv_cov, label = 'ocv_cov')
-    # covVOC.set_title("Open circuit covariance")
-    # covVOC.legend()
-    # covM = plt.subplot(2, 2, 3)
-    # covM.plot(timestamps, mixed_cov, label = 'mixed_cov')
-    # covM.set_title("Mixed covariance")
-    # covM.legend()
-    # covM = plt.subplot(2, 2, 4)
-    # covM.plot(timestamps, cov_norm, label = 'cov_norm')
-    # covM.set_title("Covariance norm")
-    # covM.legend()
-
-    # # Gain plots
-    # gainFig = plt.figure("Gain plots")
-    # gainVoc = plt.subplot(1, 2, 1)
-    # gainVoc.plot(timestamps, gamma_ocv_hist, label = 'gain_voc')
-    # gainVoc.set_title("Gain VOC")
-    # gainVoc.legend()
-    # gainR = plt.subplot(1, 2, 2)
-    # gainR.plot(timestamps, gamma_r_hist, label = 'gain_r')
-    # gainR.set_title("Gain R")
-    # gainR.legend()
-
-    ## Flight time estimation plots
-    print("\nFlight time estimation:")
-    if (logged_remaining):
-        print("Using logged remaining for flight time estimation")
-    else:
-        print("Using estimated remaining for flight time estimation")
-    print(f"Alpha for remaining derivative: {round(alpha_ocv_derivative, 6)}, Alpha for flight time estimation: {round(alpha_flight_time, 6)}")
-    print(f"In {round(timestamps[-1] - timestamps[0])} seconds, the remaining flight time estimate was reduced by {round(flight_time_estimated_filtered[0] - flight_time_estimated_filtered[-1])} seconds.")
-
-    flightTimeEstFig = plt.figure("Flight Time Estimation")
-
-    flightTime = plt.subplot(2, 2, 1)
-    if (remaining_flight_time.size):
-        flightTime.plot(timestamps, remaining_flight_time, label='Flight time remaining (logged)')
-    flightTime.plot(timestamps, flight_time_estimated, label='Flight time remaining (estimated)')
-    flightTime.plot(timestamps, flight_time_estimated_filtered, label='Flight time remaining (estimated, filtered)')
-    flightTime.set_title("Flight time remaining [s]")
-    flightTime.legend()
-
-    remainingPlot = plt.subplot(2, 2, 2)
-    remainingPlot.plot(timestamps, remaining, label='Remaining (logged)')
-    remainingPlot.plot(timestamps, remaining_est, label='Remaining (estimated)')
-    remainingPlot.plot(timestamps, remaining_est_filtered, label='Remaining (estimated, filtered)')
-    remainingPlot.set_title("Remaining [0, 1]")
-    remainingPlot.legend()
-
-    currentPlot = plt.subplot(2, 2, 3)
-    currentPlot.plot(timestamps, current, label='Current')
-    currentPlot.plot(timestamps, current_average, label='Current average')
-    currentPlot.set_title("Current [A]")
-    currentPlot.legend()
-
-    remainingPlot = plt.subplot(2, 2, 4)
-    remainingPlot.plot(timestamps, remaining_consumption, label='Remaining consumption')
-    remainingPlot.plot(timestamps, remaining_consumption_average, label='Remaining consumption average')
-    remainingPlot.set_title("Remaining consumption [1/s]")
-    remainingPlot.legend()
-
-    plt.show()
-
-if __name__ == "__main__":
-    parser = argparse.ArgumentParser(description='Estimate battery parameters from ulog file.')
-    parser.add_argument('-f', type = str,   required = True,  help = 'Full path to ulog file')
-    parser.add_argument('-r', type = bool,  required = False, help = 'Use logged remaining value for flight time estimation',              default = False)
-    parser.add_argument('-c', type = float, required = False, help = 'Number of cells in battery',                                         default = None )
-    parser.add_argument('-u', type = float, required = False, help = 'Full cell voltage',                                                  default = None )
-    parser.add_argument('-e', type = float, required = False, help = 'Empty cell voltage',                                                 default = None )
-    parser.add_argument('-l', type = float, required = False, help = 'Forgetting factor',                                                  default = 0.99 )
-    parser.add_argument('-s', type = float, required = False, help = 'Average SoC consumption per second',                                 default = 0.001)
-    parser.add_argument('-t', type = int,   required = False, help = 'Time constant for alpha filter of remaining derivative',             default = 50   )
-    parser.add_argument('-k', type = int,   required = False, help = 'Time constant for alpha filter of remaining flight time estimation', default = 5    )
-    args = parser.parse_args()
-    main(args.f, args.r, args.c, args.u, args.e, args.l, args.s, args.t, args.k)

src/lib/battery/int_res_est_replay.py

@@ -0,0 +1,208 @@
+# Test internal resistance estimator on flight logs
+# run with:
+# python3 int_res_est_replay.py -f <pathToLogFile> -c <#batteryCells>
+#   -u <(optional)fullCellVoltage> -e <(optional)emptyCellVoltage> -l <(optional)forgettingFactor> -d <(optional)filterMeasurements>
+# Note: Can lead to slightly different results than the online estimation due to the fact that
+# the log frequency of the voltage and current are not the same as the online frequency.
+
+from pyulog import ULog
+import matplotlib.pyplot as plt
+import numpy as np
+import argparse
+
+def getData(log, topic_name, variable_name, instance=0):
+    for elem in log.data_list:
+        if elem.name == topic_name and instance == elem.multi_id:
+            return elem.data[variable_name]
+    return np.array([])
+
+def us2s(time_us):
+    return time_us * 1e-6
+
+def getParam(log, param_name):
+    if param_name in log.initial_parameters:
+        return log.initial_parameters[param_name]
+    else:
+        print(f"Parameter {param_name} not found in log.")
+    return None
+
+def rls_update(theta, P, x, V, I, lam):
+    gamma = P @ x / (lam + x.T @ P @ x)
+    error = V - x.T @ theta
+    data_cov = x.T @ P @ x
+    theta_temp = theta + gamma * error
+    P_temp = (P - gamma @ x.T @ P) / lam
+    if (abs(np.linalg.norm(P)) < abs(np.linalg.norm(P_temp))):
+        theta_corr = np.array([V + theta[1] * I, theta[1]]) # Correct OCV estimation
+        P_corr = P
+        return theta_corr, P_corr, error, data_cov, 0, 0
+    return theta_temp, P_temp, error, data_cov, gamma[0], gamma[1]
+
+def main(log_name, n_cells, full_cell, empty_cell, lam):
+    log = ULog(log_name)
+
+    log_n_cells = getParam(log, 'BAT1_N_CELLS')
+    log_full_cell = getParam(log, 'BAT1_V_CHARGED')
+    log_empty_cell = getParam(log, 'BAT1_V_EMPTY')
+
+    # Debug information
+    print(f"Extracted from log - BAT1_N_CELLS: {log_n_cells}, BAT1_V_CHARGED: {log_full_cell}, BAT1_V_EMPTY: {log_empty_cell}")
+
+    # Use log parameters unless overridden
+    if n_cells is None:
+        n_cells = log_n_cells
+    else:
+        print(f"Using override for n_cells: {n_cells}")
+    if full_cell is None:
+        full_cell = log_full_cell
+    else:
+        print(f"Using override for full_cell: {full_cell}")
+    if empty_cell is None:
+        empty_cell = log_empty_cell
+    else:
+        print(f"Using override for empty_cell: {empty_cell}")
+
+    # Debug information for final parameter values
+    print(f"Using parameters - n_cells: {n_cells}, full_cell: {full_cell}, empty_cell: {empty_cell}")
+
+    timestamps = us2s(getData(log, 'battery_status', 'timestamp'))
+    I = getData(log, 'battery_status', 'current_a')
+    V = getData(log, 'battery_status', 'voltage_v')
+    remaining = getData(log, 'battery_status', 'remaining')
+
+    if not timestamps.size or not I.size or not V.size or not remaining.size:
+        print("Error: Incomplete data.")
+        return
+
+    # Initializations
+    theta = np.array([[V[0] + 0.005 * n_cells * I[0]], [0.005 * n_cells]])  # Initial VOC and R
+    P = np.diag([1.2 * n_cells, 0.1 * n_cells]) # Initial covariance
+    error = 0
+
+    # For plotting
+    VOC_est = np.zeros_like(I)
+    R_est = np.zeros_like(I)
+    error_hist = np.zeros_like(I)
+    v_est = np.zeros_like(I)
+    internal_resistance_stable = np.zeros_like(I)
+    internal_resistance_stable[-1] = 0.005
+    cov_norm = np.zeros_like(I)
+    r_cov = np.zeros_like(I)
+    ocv_cov = np.zeros_like(I)
+    mixed_cov = np.zeros_like(I)
+    data_cov_hist = np.zeros_like(I)
+    gamma_voc_hist = np.zeros_like(I)
+    gamma_r_hist = np.zeros_like(I)
+
+    for index in range(len(I)):
+        # RLS algorithm
+        x = np.array([[1.0], [-I[index]]]) # Input vector
+        theta, P, error, data_cov, gamma_voc_hist[index], gamma_r_hist[index] = rls_update(theta, P, x, V[index], I[index], lam) # Run RLS
+
+        # For plotting
+        VOC_est[index] = theta[0][0]
+        R_est[index] = theta[1][0]
+        error_hist[index] = error
+        v_est[index] = x.T @ theta
+        cov_norm[index] = abs(np.linalg.norm(P))
+        ocv_cov[index] = P[0][0]
+        r_cov[index] = P[1][1]
+        mixed_cov[index] = P[0][1]
+        data_cov_hist[index] = data_cov
+        internal_resistance_stable[index] = max(R_est[index]/n_cells, 0.001)
+
+    # Plot data
+    print("Internal Resistance mean (per cell): ", np.mean(R_est) / n_cells)
+
+    # Summary plot
+    sumFig = plt.figure("Battery Estimation with RLS")
+
+    volt = plt.subplot(2, 3, 1)
+    volt.plot(timestamps, V, label='Measured voltage')
+    volt.plot(timestamps, v_est, label='Estimated voltage')
+    volt.plot(timestamps, np.array(V) + np.array(internal_resistance_stable) * np.array(I) * n_cells, label='OCV estimate')
+    ocv_smoothed = np.convolve(np.array(V) + np.array(internal_resistance_stable) * np.array(I) * n_cells, np.ones(30)/30, mode='full')[0:np.size(timestamps)]
+    ocv_smoothed[0:30] = ocv_smoothed[31]
+    volt.plot(timestamps, ocv_smoothed, label='OCV estimate smoothed')
+    volt.plot(timestamps, np.full_like(I, full_cell * n_cells), label='100% SOC')
+    volt.set_title("Measured Voltage vs Estimated voltage vs Estimated Open circuit voltage [V]")
+    volt.legend()
+
+    intR = plt.subplot(2, 3, 2)
+    intR.plot(timestamps, np.array(R_est) * 1000 / n_cells, label='Internal resistance estimate')
+    intR.set_title("Internal resistance estimate (per cell) [mOhm]")
+    intR.legend()
+
+    soc = plt.subplot(2, 3, 3)
+    soc.plot(timestamps, remaining, label='SoC logged')
+    soc.plot(timestamps, np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator')
+    soc.plot(timestamps, np.interp(ocv_smoothed / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator smoothed')
+    soc.set_title("State of charge")
+    soc.legend()
+
+    curr = plt.subplot(2, 3, 4)
+    curr.plot(timestamps, I, label='Measured current')
+    curr.set_title("Measured Current [A]")
+    curr.legend()
+
+    err = plt.subplot(2, 3, 5)
+    err.plot(timestamps, error_hist, label='$Error$')
+    err.set_title("Voltage estimation error [V]")
+    err.legend()
+
+    cov = plt.subplot(2, 3, 6)
+    cov.plot(timestamps, cov_norm, label = 'Covariance norm')
+    cov.set_title("Covariance norm")
+    cov.legend()
+
+    # # SoC estimation plots
+    # socFig = plt.figure("SoC estimation")
+    # plt.plot(timestamps, np.interp((np.array(V) + np.array(I) * 0.005 * n_cells) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with default $R_{int}$')
+    # plt.plot(timestamps, remaining, label='SoC logged')
+    # plt.plot(timestamps, np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with estimator')
+    # # plt.plot(timestamps, np.convolve(np.interp((np.array(V) + np.array(internal_resistance_stable) * n_cells * np.array(I)) / n_cells, [empty_cell, full_cell], [0, 1]), np.ones(500)/500, mode='full')[0:np.size(timestamps)], label='SoC with estimator smoothed')
+    # # plt.plot(timestamps, np.interp((np.array(V) + np.array(I) * 0.0009 * n_cells) / n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with $R_{int}$ measured beforehand')
+    # # plt.plot(timestamps, np.interp(VOC_est/n_cells, [empty_cell, full_cell], [0, 1]), label='SoC with VOC estimate')
+    # plt.legend()
+
+    # # Covariance plots
+    # covFig = plt.figure("Covariance plots")
+    # covR = plt.subplot(2, 2, 1)
+    # covR.plot(timestamps, r_cov, label = 'r_cov')
+    # covR.set_title("Internal resistance covariance")
+    # covR.legend()
+    # covVOC = plt.subplot(2, 2, 2)
+    # covVOC.plot(timestamps, ocv_cov, label = 'ocv_cov')
+    # covVOC.set_title("Open circuit covariance")
+    # covVOC.legend()
+    # covM = plt.subplot(2, 2, 3)
+    # covM.plot(timestamps, mixed_cov, label = 'mixed_cov')
+    # covM.set_title("Mixed covariance")
+    # covM.legend()
+    # covM = plt.subplot(2, 2, 4)
+    # covM.plot(timestamps, cov_norm, label = 'cov_norm')
+    # covM.set_title("Covariance norm")
+    # covM.legend()
+
+    # # Gain plots
+    # gainFig = plt.figure("Gain plots")
+    # gainVoc = plt.subplot(1, 2, 1)
+    # gainVoc.plot(timestamps, gamma_voc_hist, label = 'gain_voc')
+    # gainVoc.set_title("Gain VOC")
+    # gainVoc.legend()
+    # gainR = plt.subplot(1, 2, 2)
+    # gainR.plot(timestamps, gamma_r_hist, label = 'gain_r')
+    # gainR.set_title("Gain R")
+    # gainR.legend()
+
+    plt.show()
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description='Estimate battery parameters from ulog file.')
+    parser.add_argument('-f', type = str,   required = True,  help = 'Full path to ulog file')
+    parser.add_argument('-c', type = float, required = False, help = 'Number of cells in battery')
+    parser.add_argument('-u', type = float, required = False, default = None, help = 'Full cell voltage')
+    parser.add_argument('-e', type = float, required = False, default = None,  help = 'Empty cell voltage')
+    parser.add_argument('-l', type = float, required = False, default = 0.99, help = 'Forgetting factor')
+    args = parser.parse_args()
+    main(args.f, args.c, args.u, args.e, args.l)

src/lib/battery/module.yaml

@@ -178,16 +178,3 @@ parameters:
             decimal: 2
             increment: 0.1
             default: 15
-
-        BAT_AVRG_SOC:
-            description:
-                short: Expected average SoC derivative during flight.
-                long: |
-                    This value is used to initialize the in-flight average SoC derivative,
-                    which in turn is used for estimating remaining flight time and RTL triggering.
-            type: float
-            min: 0
-            max: 100
-            decimal: 4
-            increment: 0.0001
-            default: 0.001

src/modules/commander/commander_params.c

@@ -615,7 +615,7 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
  * Specify modes in which RC loss is ignored and the failsafe action not triggered.
  *
  * @min 0
- * @max 31
+ * @max 7
  * @bit 0 Mission
  * @bit 1 Hold
  * @bit 2 Offboard
@@ -623,6 +623,20 @@ PARAM_DEFINE_INT32(NAV_RCL_ACT, 2);
  */
 PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0);
 
+/**
+ * Datalink loss exceptions
+ *
+ * Specify modes in which datalink loss is ignored and the failsafe action not triggered.
+ *
+ * @min 0
+ * @max 7
+ * @bit 0 Mission
+ * @bit 1 Hold
+ * @bit 2 Offboard
+ * @group Commander
+ */
+PARAM_DEFINE_INT32(COM_DLL_EXCEPT, 0);
+
 /**
  * Set the actuator failure failsafe mode
  *

src/modules/commander/failsafe/failsafe.cpp

@@ -452,8 +452,9 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 
 	// Do not enter failsafe while doing a vtol takeoff after the vehicle has started a transition and before it reaches the loiter
 	// altitude. The vtol takeoff navigaton mode will set mission_finished to true as soon as the loiter is established
-	const bool ignore_link_failsafe = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
-					  && in_forward_flight && !state.mission_finished;
+	const bool ignore_any_link_loss_vtol_takeoff_fixedwing = state.user_intended_mode ==
+			vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF
+			&& in_forward_flight && !state.mission_finished;
 
 	// Manual control (RC) loss
 	if (!status_flags.manual_control_signal_lost) {
@@ -470,8 +471,10 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 	const bool rc_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
 					      state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
 					     && (_param_com_rcl_except.get() & (int)ManualControlLossExceptionBits::Hold);
+
 	const bool rc_loss_ignored = rc_loss_ignored_mission || rc_loss_ignored_loiter || rc_loss_ignored_offboard ||
-				     rc_loss_ignored_takeoff || ignore_link_failsafe || _manual_control_lost_at_arming;
+				     rc_loss_ignored_takeoff || ignore_any_link_loss_vtol_takeoff_fixedwing
+				     || _manual_control_lost_at_arming;
 
 	if (_param_com_rc_in_mode.get() != int32_t(RcInMode::StickInputDisabled) && !rc_loss_ignored) {
 		CHECK_FAILSAFE(status_flags, manual_control_signal_lost,
@@ -479,10 +482,23 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state,
 	}
 
 	// GCS connection loss
-	const bool gcs_connection_loss_ignored = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND ||
-			state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND || ignore_link_failsafe;
+	const bool dll_loss_ignored_land = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
+					   || state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND;
 
-	if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !gcs_connection_loss_ignored) {
+	const bool dll_loss_ignored_mission = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
+					      && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Mission);
+	const bool dll_loss_ignored_loiter = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
+					     && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold);
+	const bool dll_loss_ignored_offboard = state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_OFFBOARD
+					       && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Offboard);
+	const bool dll_loss_ignored_takeoff = (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF ||
+					       state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_VTOL_TAKEOFF)
+					      && (_param_com_dll_except.get() & (int)DatalinkLossExceptionBits::Hold);
+
+	const bool dll_loss_ignored = dll_loss_ignored_mission || dll_loss_ignored_loiter || dll_loss_ignored_offboard ||
+				      dll_loss_ignored_takeoff || ignore_any_link_loss_vtol_takeoff_fixedwing || dll_loss_ignored_land;
+
+	if (_param_nav_dll_act.get() != int32_t(gcs_connection_loss_failsafe_mode::Disabled) && !dll_loss_ignored) {
 		CHECK_FAILSAFE(status_flags, gcs_connection_lost,
 			       fromNavDllOrRclActParam(_param_nav_dll_act.get()).causedBy(Cause::GCSConnectionLoss));
 	}

src/modules/commander/failsafe/failsafe.h

@@ -59,6 +59,12 @@ private:
 		Offboard = (1 << 2)
 	};
 
+	enum class DatalinkLossExceptionBits : int32_t {
+		Mission = (1 << 0),
+		Hold = (1 << 1),
+		Offboard = (1 << 2)
+	};
+
 	// COM_LOW_BAT_ACT parameter values
 	enum class LowBatteryAction : int32_t {
 		Warning = 0,        // Warning
@@ -193,6 +199,7 @@ private:
 					(ParamInt<px4::params::NAV_DLL_ACT>) 	_param_nav_dll_act,
 					(ParamInt<px4::params::NAV_RCL_ACT>) 	_param_nav_rcl_act,
 					(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
+					(ParamInt<px4::params::COM_DLL_EXCEPT>) _param_com_dll_except,
 					(ParamInt<px4::params::COM_RC_IN_MODE>) _param_com_rc_in_mode,
 					(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl,
 					(ParamInt<px4::params::GF_ACTION>)  	_param_gf_action,

src/modules/control_allocator/module.yaml

@@ -40,7 +40,7 @@ parameters:
                 short: Control allocation method
                 long: |
                   Selects the algorithm and desaturation method.
-                  If set to Automtic, the selection is based on the airframe (CA_AIRFRAME).
+                  If set to Automatic, the selection is based on the airframe (CA_AIRFRAME).
             type: enum
             values:
                 0: Pseudo-inverse with output clipping

src/modules/dataman/parameters.c

@@ -34,10 +34,14 @@
 /**
  * Dataman storage backend
  *
+ * If the board supports persistent storage (i.e., the KConfig variable DATAMAN_PERSISTENT_STORAGE is set),
+ * the 'Default storage' backend uses a file on persistent storage. If not supported, this backend uses
+ * non-persistent storage in RAM.
+ *
  * @group System
- * @value -1 Disabled
- * @value 0 default (SD card)
- * @value 1 RAM (not persistent)
+ * @value -1 Dataman disabled
+ * @value 0 Default storage
+ * @value 1 RAM storage
  * @boolean
  * @reboot_required true
  */

src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp

@@ -108,7 +108,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed
 					bool reset = false;
 
 					if (!fused && !ekf.isOtherSourceOfHorizontalPositionAidingThan(ekf.control_status_flags().aux_gpos)) {
-						ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, pos_var, sq(sample.epv));
+						ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance));
 						ekf.resetAidSourceStatusZeroInnovation(aid_src);
 						reset = true;
 					}

src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp

@@ -75,6 +75,12 @@ void Ekf::controlMagFusion(const imuSample &imu_sample)
 			_mag_lpf.reset(mag_sample.mag);
 			_mag_counter = 1;
 
+			if (!_control_status.flags.in_air) {
+				// Assume that a reset on the ground is caused by a change in mag calibration
+				// Clear alignment to force a clean reset
+				_control_status.flags.yaw_align = false;
+			}
+
 		} else {
 			_mag_lpf.update(mag_sample.mag);
 			_mag_counter++;

src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp

@@ -46,18 +46,17 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 {
 	const auto state_vector = _state.vector();
 
-	_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
-	_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
-
 	// if either axis fails we abort the fusion
 	if (_aid_src_optical_flow.innovation_rejected) {
+		_innov_check_fail_status.flags.reject_optflow_X = true;
+		_innov_check_fail_status.flags.reject_optflow_Y = true;
 		return false;
 	}
 
 	// fuse observation axes sequentially
 	for (uint8_t index = 0; index <= 1; index++) {
 		if (index == 0) {
-			// everything was already computed above
+			// everything was already computed before
 
 		} else if (index == 1) {
 			// recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes)
@@ -69,6 +68,16 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 			const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias;
 			_aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast<float>
 							      (_aid_src_optical_flow.observation[1]);
+
+			// recalculate the test ratio as the measurement jacobian is highly non linear
+			// when close to the ground (singularity at 0) and the innovation can suddenly become really
+			// large and destabilize the filter
+			_aid_src_optical_flow.test_ratio[1] = sq(_aid_src_optical_flow.innovation[1]) / (sq(
+					_params.flow_innov_gate) * _aid_src_optical_flow.innovation_variance[1]);
+
+			if (_aid_src_optical_flow.test_ratio[1] > 1.f) {
+				continue;
+			}
 		}
 
 		if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) {
@@ -91,6 +100,9 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 	_fault_status.flags.bad_optflow_X = false;
 	_fault_status.flags.bad_optflow_Y = false;
 
+	_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
+	_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
+
 	_aid_src_optical_flow.time_last_fuse = _time_delayed_us;
 	_aid_src_optical_flow.fused = true;
 
@@ -103,7 +115,7 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain)
 	return true;
 }
 
-float Ekf::predictFlowRange() const
+float Ekf::predictFlowHagl() const
 {
 	// calculate the sensor position relative to the IMU
 	const Vector3f pos_offset_body = _params.flow_pos_body - _params.imu_pos_body;
@@ -113,17 +125,19 @@ float Ekf::predictFlowRange() const
 
 	// calculate the height above the ground of the optical flow camera. Since earth frame is NED
 	// a positive offset in earth frame leads to a smaller height above the ground.
-	const float height_above_gnd_est = getHagl() - pos_offset_earth(2);
+	const float height_above_gnd_est = fabsf(getHagl() - pos_offset_earth(2));
 
+	// Never return a really small value to avoid generating insanely large flow innovations
+	// that could destabilize the filter
+	constexpr float min_hagl = 1e-2f;
+
+	return fmaxf(height_above_gnd_est, min_hagl);
+}
+float Ekf::predictFlowRange() const
+{
 	// calculate range from focal point to centre of image
-	float flow_range = height_above_gnd_est / _R_to_earth(2, 2); // absolute distance to the frame region in view
-
-	// avoid the flow prediction singularity at range = 0
-	if (fabsf(flow_range) < FLT_EPSILON) {
-		flow_range = signNoZero(flow_range) * FLT_EPSILON;
-	}
-
-	return flow_range;
+	// absolute distance to the frame region in view
+	return predictFlowHagl() / _R_to_earth(2, 2);
 }
 
 Vector2f Ekf::predictFlow(const Vector3f &flow_gyro) const
@@ -142,9 +156,9 @@ Vector2f Ekf::predictFlow(const Vector3f &flow_gyro) const
 	const Vector2f vel_body = _state.quat_nominal.rotateVectorInverse(vel_rel_earth).xy();
 
 	// calculate range from focal point to centre of image
-	const float range = predictFlowRange();
+	const float scale = _R_to_earth(2, 2) / predictFlowHagl();
 
-	return Vector2f(vel_body(1) / range, -vel_body(0) / range);
+	return Vector2f(vel_body(1) * scale, -vel_body(0) * scale);
 }
 
 float Ekf::calcOptFlowMeasVar(const flowSample &flow_sample) const

src/modules/ekf2/EKF/ekf.h

@@ -791,6 +791,7 @@ private:
 	// calculate optical flow body angular rate compensation
 	void calcOptFlowBodyRateComp(const flowSample &flow_sample);
 
+	float predictFlowHagl() const;
 	float predictFlowRange() const;
 	Vector2f predictFlow(const Vector3f &flow_gyro) const;
 

src/modules/ekf2/test/test_EKF_flow_generated.cpp

@@ -71,3 +71,22 @@ TEST(FlowGenerated, distBottom0y)
 	sym::ComputeFlowYInnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var, &H);
 	EXPECT_GT(innov_var, 1e12);
 }
+
+TEST(FlowGenerated, distBottomNeg)
+{
+	// GIVEN: a small negative distance to the ground (singularity)
+	StateSample state{};
+	state.quat_nominal = Quatf();
+	state.pos(2) = 1e-3f;
+
+	const float R = sq(radians(sq(0.5f)));
+	SquareMatrixState P = createRandomCovarianceMatrix();
+
+	VectorState H;
+	Vector2f innov_var;
+	sym::ComputeFlowXyInnovVarAndHx(state.vector(), P, R, FLT_EPSILON, &innov_var, &H);
+	EXPECT_GT(innov_var(0), 1e6);
+	EXPECT_GT(innov_var(1), 1e6);
+	sym::ComputeFlowYInnovVarAndH(state.vector(), P, R, FLT_EPSILON, &innov_var(1), &H);
+	EXPECT_GT(innov_var(1), 1e6);
+}

src/modules/fw_rate_control/FixedwingRateControl.cpp

@@ -179,10 +179,16 @@ float FixedwingRateControl::get_airspeed_and_update_scaling()
 	 *
 	 * Forcing the scaling to this value allows reasonable handheld tests.
 	 */
-	const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(),
-					   _param_fw_airspd_max.get()), 0.1f, 1000.0f);
 
-	_airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f;
+
+	if (_param_fw_arsp_scale_en.get()) {
+		const float min_airspeed = math::max(_param_fw_airspd_stall.get(), 0.1f);
+		const float airspeed_constrained = math::max(airspeed, min_airspeed);
+		_airspeed_scaling = _param_fw_airspd_trim.get() / airspeed_constrained;
+
+	} else {
+		_airspeed_scaling = 1.0f;
+	}
 
 	return airspeed;
 }

src/modules/mc_pos_control/MulticopterPositionControl.cpp

@@ -198,6 +198,8 @@ void MulticopterPositionControl::parameters_update(bool force)
 			Vector3f(_param_mpc_xy_vel_p_acc.get(), _param_mpc_xy_vel_p_acc.get(), _param_mpc_z_vel_p_acc.get()),
 			Vector3f(_param_mpc_xy_vel_i_acc.get(), _param_mpc_xy_vel_i_acc.get(), _param_mpc_z_vel_i_acc.get()),
 			Vector3f(_param_mpc_xy_vel_d_acc.get(), _param_mpc_xy_vel_d_acc.get(), _param_mpc_z_vel_d_acc.get()));
+		_control.setAccelerationGains(_param_mpc_xy_acc_p.get());
+		_control.setAccelerationCutoff(_param_mpc_acc_cutoff.get());
 		_control.setHorizontalThrustMargin(_param_mpc_thr_xy_marg.get());
 		_control.decoupleHorizontalAndVecticalAcceleration(_param_mpc_acc_decouple.get());
 		_goto_control.setParamMpcAccHor(_param_mpc_acc_hor.get());
@@ -413,6 +415,8 @@ void MulticopterPositionControl::Run()
 		}
 
 		_vehicle_land_detected_sub.update(&_vehicle_land_detected);
+		_vehicle_acceleration_sub.update(&_vehicle_acceleration);
+		_vehicle_attitude_sub.update(&_vehicle_attitude);
 
 		if (_param_mpc_use_hte.get()) {
 			hover_thrust_estimate_s hte;
@@ -542,6 +546,8 @@ void MulticopterPositionControl::Run()
 				math::max(speed_down, 0.f));
 
 			_control.setInputSetpoint(_setpoint);
+			_control.setAttitude(Quatf(_vehicle_attitude.q));
+			_control.setBodySpecificForce(Vector3f(_vehicle_acceleration.xyz), dt);
 
 			// update states
 			if (!PX4_ISFINITE(_setpoint.position[2])

src/modules/mc_pos_control/MulticopterPositionControl.hpp

@@ -61,6 +61,8 @@
 #include <uORB/topics/hover_thrust_estimate.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/trajectory_setpoint.h>
+#include <uORB/topics/vehicle_acceleration.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_constraints.h>
 #include <uORB/topics/vehicle_control_mode.h>
@@ -105,6 +107,8 @@ private:
 
 	uORB::Subscription _hover_thrust_estimate_sub{ORB_ID(hover_thrust_estimate)};
 	uORB::Subscription _trajectory_setpoint_sub{ORB_ID(trajectory_setpoint)};
+	uORB::Subscription _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_constraints_sub{ORB_ID(vehicle_constraints)};
 	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
@@ -130,6 +134,9 @@ private:
 		.landed = true,
 	};
 
+	vehicle_attitude_s _vehicle_attitude{};
+	vehicle_acceleration_s _vehicle_acceleration{};
+
 	DEFINE_PARAMETERS(
 		// Position Control
 		(ParamFloat<px4::params::MPC_XY_P>)         _param_mpc_xy_p,
@@ -140,6 +147,8 @@ private:
 		(ParamFloat<px4::params::MPC_Z_VEL_P_ACC>)  _param_mpc_z_vel_p_acc,
 		(ParamFloat<px4::params::MPC_Z_VEL_I_ACC>)  _param_mpc_z_vel_i_acc,
 		(ParamFloat<px4::params::MPC_Z_VEL_D_ACC>)  _param_mpc_z_vel_d_acc,
+		(ParamFloat<px4::params::MPC_XY_ACC_P>)     _param_mpc_xy_acc_p,
+		(ParamFloat<px4::params::MPC_ACC_CUTOFF>)   _param_mpc_acc_cutoff,
 		(ParamFloat<px4::params::MPC_XY_VEL_MAX>)   _param_mpc_xy_vel_max,
 		(ParamFloat<px4::params::MPC_Z_V_AUTO_UP>)  _param_mpc_z_v_auto_up,
 		(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,

src/modules/mc_pos_control/PositionControl/PositionControl.cpp

@@ -149,7 +149,7 @@ void PositionControl::_velocityControl(const float dt)
 	// No control input from setpoints or corresponding states which are NAN
 	ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);
 
-	_accelerationControl();
+	_accelerationControl(dt);
 
 	// Integrator anti-windup in vertical direction
 	if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.f) ||
@@ -201,7 +201,7 @@ void PositionControl::_velocityControl(const float dt)
 	_vel_int += vel_error.emult(_gain_vel_i) * dt;
 }
 
-void PositionControl::_accelerationControl()
+void PositionControl::_accelerationControl(const float dt)
 {
 	// Assume standard acceleration due to gravity in vertical direction for attitude generation
 	float z_specific_force = -CONSTANTS_ONE_G;
@@ -211,7 +211,12 @@ void PositionControl::_accelerationControl()
 		z_specific_force += _acc_sp(2);
 	}
 
-	Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), -z_specific_force).normalized();
+	const Vector3f sf_sp_ned = Vector3f(_acc_sp(0), _acc_sp(1), z_specific_force);
+	const Vector3f sf_sp_body = _attitude.rotateVectorInverse(sf_sp_ned);
+
+	Quatf att_error(AxisAnglef(AxisAnglef(Quatf(_body_specific_force_lpf.getState(), sf_sp_body)) * _gain_acc_xy));
+	Quatf desired_att = _attitude * att_error;
+	Vector3f body_z = desired_att.dcm_z();
 	ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _lim_tilt);
 	// Convert to thrust assuming hover thrust produces standard gravity
 	const float thrust_ned_z = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;

src/modules/mc_pos_control/PositionControl/PositionControl.hpp

@@ -40,6 +40,7 @@
 #pragma once
 
 #include <lib/mathlib/mathlib.h>
+#include <mathlib/math/filter/AlphaFilter.hpp>
 #include <matrix/matrix/math.hpp>
 #include <uORB/topics/trajectory_setpoint.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
@@ -93,6 +94,9 @@ public:
 	 */
 	void setVelocityGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);
 
+	void setAccelerationGains(const float gain_xy) { _gain_acc_xy = gain_xy; }
+	void setAccelerationCutoff(const float cutoff_frequency) { _body_specific_force_lpf.setCutoffFreq(cutoff_frequency); }
+
 	/**
 	 * Set the maximum velocity to execute with feed forward and position control
 	 * @param vel_horizontal horizontal velocity limit
@@ -146,6 +150,9 @@ public:
 	 */
 	void setInputSetpoint(const trajectory_setpoint_s &setpoint);
 
+	void setBodySpecificForce(const matrix::Vector3f &specific_force, const float dt) { _body_specific_force_lpf.update(specific_force, dt); }
+	void setAttitude(const matrix::Quatf &attitude) { _attitude = attitude; }
+
 	/**
 	 * Apply P-position and PID-velocity controller that updates the member
 	 * thrust, yaw- and yawspeed-setpoints.
@@ -199,7 +206,7 @@ private:
 
 	void _positionControl(); ///< Position proportional control
 	void _velocityControl(const float dt); ///< Velocity PID control
-	void _accelerationControl(); ///< Acceleration setpoint processing
+	void _accelerationControl(float dt); ///< Acceleration setpoint processing
 
 	// Gains
 	matrix::Vector3f _gain_pos_p; ///< Position control proportional gain
@@ -218,6 +225,8 @@ private:
 
 	float _hover_thrust{}; ///< Thrust [HOVER_THRUST_MIN, HOVER_THRUST_MAX] with which the vehicle hovers not accelerating down or up with level orientation
 	bool _decouple_horizontal_and_vertical_acceleration{true}; ///< Ignore vertical acceleration setpoint to remove its effect on the tilt setpoint
+	float _gain_acc_xy{1.f};
+	AlphaFilter<matrix::Vector3f> _body_specific_force_lpf{};
 
 	// States
 	matrix::Vector3f _pos; /**< current position */
@@ -233,4 +242,6 @@ private:
 	matrix::Vector3f _thr_sp; /**< desired thrust */
 	float _yaw_sp{}; /**< desired heading */
 	float _yawspeed_sp{}; /** desired yaw-speed */
+
+	matrix::Quatf _attitude;
 };

src/modules/mc_pos_control/multicopter_position_control_gain_params.c

@@ -135,3 +135,29 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D_ACC, 0.f);
  * @group Multicopter Position Control
  */
 PARAM_DEFINE_FLOAT(MPC_XY_VEL_D_ACC, 0.2f);
+
+/**
+ * Gain for xy acceleration control
+ *
+ * Reduce to lower the sensitivity to accelerometer noise
+ *
+ * @min 0.1
+ * @max 1
+ * @decimal 2
+ * @increment 0.05
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_XY_ACC_P, 1.f);
+
+/**
+ * Acceleration feedback cutoff frequency
+ *
+ * 0 to disable
+ *
+ * @min 0
+ * @max 100
+ * @decimal 1
+ * @increment 1.0
+ * @group Multicopter Position Control
+ */
+PARAM_DEFINE_FLOAT(MPC_ACC_CUTOFF, 0.1f);

src/modules/navigator/rtl.cpp

@@ -318,7 +318,7 @@ void RTL::setRtlTypeAndDestination()
 
 	uint8_t safe_point_index{0U};
 
-	if (_param_rtl_type.get() != 2) {
+	if (_param_rtl_type.get() != 2 && _param_rtl_type.get() != 4) {
 		// check the closest allowed destination.
 		DestinationType destination_type{DestinationType::DESTINATION_TYPE_HOME};
 		PositionYawSetpoint rtl_position;
@@ -566,6 +566,14 @@ void RTL::init_rtl_mission_type()
 		} else {
 			new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
 		}
+
+	} else if (_param_rtl_type.get() == 4) {
+		if (hasMissionLandStart() && reverseIsFurther()) {
+			new_rtl_mission_type = RtlType::RTL_MISSION_FAST;
+
+		} else {
+			new_rtl_mission_type = RtlType::RTL_MISSION_FAST_REVERSE;
+		}
 	}
 
 	if (_set_rtl_mission_type == new_rtl_mission_type) {
@@ -630,6 +638,12 @@ bool RTL::hasMissionLandStart() const
 	       && _navigator->get_mission_result()->valid;
 }
 
+bool RTL::reverseIsFurther() const
+{
+	return (_mission_sub.get().land_start_index - _mission_sub.get().current_seq) < _mission_sub.get().current_seq;
+}
+
+
 bool RTL::hasVtolLandApproach(const PositionYawSetpoint &rtl_position) const
 {
 	return readVtolLandApproaches(rtl_position).isAnyApproachValid();

src/modules/navigator/rtl.h

@@ -105,6 +105,14 @@ private:
 	 */
 	bool hasMissionLandStart() const;
 
+
+	/**
+	 * @brief Check whether there are more waypoints between current waypoint
+	 *        and the takeoff location than the end/land location.
+	 * @return true if the reverse is more items away.
+	 */
+	bool reverseIsFurther() const;
+
 	/**
 	 * @brief function to call regularly to do background work
 	 */

src/modules/navigator/rtl_params.c

@@ -112,6 +112,7 @@ PARAM_DEFINE_FLOAT(RTL_MIN_DIST, 10.0f);
  * @value 1 Return to closest safe point other than home (mission landing pattern or rally point), via direct path. If no mission landing or rally points are defined return home via direct path. Always chose closest safe landing point if vehicle is a VTOL in hover mode.
  * @value 2 Return to a planned mission landing, if available, using the mission path, else return to home via the reverse mission path. Do not consider rally points.
  * @value 3 Return via direct path to closest destination: home, start of mission landing pattern or safe point. If the destination is a mission landing pattern, follow the pattern to land.
+ * @value 4 Return to the planned mission landing, or to home via the reverse mission path, whichever is closer by counting waypoints. Do not consider rally points.
  * @group Return Mode
  */
 PARAM_DEFINE_INT32(RTL_TYPE, 0);

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -191,6 +191,13 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
 
 			if (calibration_updated) {
 				_last_calibration_update = hrt_absolute_time();
+
+				for (int instance = 0; instance < MAX_SENSOR_COUNT; instance++) {
+					// avoid mixing data currected using old calibration
+					_timestamp_sample_sum[instance] = 0;
+					_data_sum[instance].zero();
+					_data_sum_count[instance] = 0;
+				}
 			}
 		}
 

src/modules/simulation/gz_plugins/CMakeLists.txt

@@ -34,7 +34,7 @@
 project(px4_gz_plugins)
 
 if(NOT DEFINED ENV{GZ_DISTRO} OR NOT "$ENV{GZ_DISTRO}"  STREQUAL "harmonic")
-    find_package(gz-transport NAMES gz-transport14 gz-transport13 gz-transport12)
+    find_package(gz-transport NAMES gz-transport14 gz-transport13)
     find_package(gz-sim NAMES gz-sim9 gz-sim8)
     find_package(gz-sensors NAMES gz-sensors9 gz-sensors8)
     find_package(gz-plugin NAMES gz-plugin3 gz-plugin2 COMPONENTS register)

src/modules/spacecraft/module.yaml

@@ -41,7 +41,7 @@ parameters:
                 short: Control allocation method
                 long: |
                   Selects the algorithm and desaturation method.
-                  If set to Automtic, the selection is based on the airframe (CA_AIRFRAME).
+                  If set to Automatic, the selection is based on the airframe (CA_AIRFRAME).
             type: enum
             values:
                 0: Pseudo-inverse with output clipping

src/modules/uxrce_dds_client/dds_topics.yaml

@@ -74,6 +74,9 @@ publications:
   - topic: /fmu/out/airspeed_validated
     type: px4_msgs::msg::AirspeedValidated
 
+  - topic: /fmu/out/vtol_vehicle_status
+    type: px4_msgs::msg::VtolVehicleStatus
+
   - topic: /fmu/out/home_position
     type: px4_msgs::msg::HomePosition
 

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -427,9 +427,16 @@ VtolAttitudeControl::Run()
 		_vehicle_thrust_setpoint0_pub.publish(_thrust_setpoint_0);
 		_vehicle_thrust_setpoint1_pub.publish(_thrust_setpoint_1);
 
-		// Advertise/Publish vtol vehicle status
-		_vtol_vehicle_status.timestamp = hrt_absolute_time();
-		_vtol_vehicle_status_pub.publish(_vtol_vehicle_status);
+		// Advertise/publish vtol vehicle status -- immediately if changed, otherwise at 1 Hz
+		const bool vtol_vehicle_status_changed =
+			(_vtol_vehicle_status.vehicle_vtol_state != _prev_published_vtol_vehicle_status.vehicle_vtol_state) ||
+			(_vtol_vehicle_status.fixed_wing_system_failure != _prev_published_vtol_vehicle_status.fixed_wing_system_failure);
+
+		if (vtol_vehicle_status_changed || hrt_elapsed_time(&_prev_published_vtol_vehicle_status.timestamp) >= 1_s) {
+			_vtol_vehicle_status.timestamp = hrt_absolute_time();
+			_vtol_vehicle_status_pub.publish(_vtol_vehicle_status);
+			_prev_published_vtol_vehicle_status = _vtol_vehicle_status;
+		}
 
 		// Publish flaps/spoiler setpoint with configured deflection in Hover if in Auto.
 		// In Manual always published in FW rate controller, and in Auto FW in FW Position Controller.

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -205,6 +205,7 @@ private:
 	vehicle_local_position_setpoint_s	_local_pos_sp{};
 	vehicle_status_s 			_vehicle_status{};
 	vtol_vehicle_status_s 			_vtol_vehicle_status{};
+	vtol_vehicle_status_s 			_prev_published_vtol_vehicle_status{};
 	float _home_position_z{NAN};
 
 	float _air_density{atmosphere::kAirDensitySeaLevelStandardAtmos};	// [kg/m^3]