diff --git a/.github/workflows/sitl_tests.yml b/.github/workflows/sitl_tests.yml
index 12c1e176498..89f44d6158d 100644
--- a/.github/workflows/sitl_tests.yml
+++ b/.github/workflows/sitl_tests.yml
@@ -37,9 +37,11 @@ jobs:
config:
- {model: "quadx", latitude: "59.617693", longitude: "-151.145316", altitude: "48", build_type: "AddressSanitizer" } # Alaska
- {model: "hex", latitude: "47.397742", longitude: "8.545594", altitude: "488", build_type: "ThreadSanitizer" } # Zurich
+ - {model: "quadx_flow", latitude: "59.617693", longitude: "-151.145316", altitude: "48", build_type: "RelWithDebInfo" } # Alaska
# Coverage on main only; PRs reuse main's numbers through Codecov carryforward (codecov.yml)
- {model: "standard_vtol", latitude: "47.397742", longitude: "8.545594", altitude: "488", build_type: "${{ github.event_name == 'push' && 'Coverage' || 'RelWithDebInfo' }}" } # Zurich
- {model: "xvert", latitude: "47.397742", longitude: "8.545594", altitude: "488", build_type: "UndefinedBehaviorSanitizer" } # Zurich
+ - {model: "standard_vtol_flow", latitude: "47.397742", longitude: "8.545594", altitude: "488", build_type: "RelWithDebInfo" } # Zurich
env:
PX4_CMAKE_BUILD_TYPE: ${{ matrix.config.build_type }}
PX4_SBOM_DISABLE: 1
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10046_sihsim_quadx_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/10046_sihsim_quadx_flow
new file mode 100644
index 00000000000..648c71aef62
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10046_sihsim_quadx_flow
@@ -0,0 +1,17 @@
+#!/bin/sh
+#
+# @name QuadrotorX SITL for SIH with optical flow
+#
+# @type Simulation
+# @class Quadrotor
+#
+
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadx_flow}
+
+. ${R}etc/init.d-posix/airframes/10040_sihsim_quadx
+
+param set-default SIH_OF_EN 1
+param set-default SIH_DISTSNSR_MAX 30
+
+param bitclear EKF2_SENS_EN 3 # disable gps fusion
+param set-default EKF2_HGT_REF 0
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10047_sihsim_standard_vtol_flow b/ROMFS/px4fmu_common/init.d-posix/airframes/10047_sihsim_standard_vtol_flow
new file mode 100644
index 00000000000..02a9edaddc4
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10047_sihsim_standard_vtol_flow
@@ -0,0 +1,19 @@
+#!/bin/sh
+#
+# @name SIH Standard VTOL with optical flow
+#
+# @type Simulation
+# @class VTOL
+#
+# @board px4_fmu-v2 exclude
+#
+
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol_flow}
+
+. ${R}etc/init.d-posix/airframes/10043_sihsim_standard_vtol
+
+param set-default SIH_OF_EN 1
+param set-default SIH_DISTSNSR_MAX 30
+
+param bitclear EKF2_SENS_EN 3 # disable gps fusion
+param set-default EKF2_HGT_REF 0
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
index 84ca19e30b7..55ab76ebf5c 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
@@ -112,6 +112,8 @@ px4_add_romfs_files(
10043_sihsim_standard_vtol
10044_sihsim_hex
10045_sihsim_rover_ackermann
+ 10046_sihsim_quadx_flow
+ 10047_sihsim_standard_vtol_flow
17001_flightgear_tf-g1
17002_flightgear_tf-g2
diff --git a/docs/en/sensor/optical_flow.md b/docs/en/sensor/optical_flow.md
index e1b9289215e..c49c07765f1 100644
--- a/docs/en/sensor/optical_flow.md
+++ b/docs/en/sensor/optical_flow.md
@@ -113,3 +113,22 @@ If your optical flow sensor is offset from the vehicle centre, you can set this
| [EKF2_OF_POS_Z](../advanced_config/parameter_reference.md#EKF2_OF_POS_Z) | Z position of optical flow focal point in body frame (default is 0.0m). |
See [Using PX4's Navigation Filter (EKF2) > Optical flow](../advanced_config/tuning_the_ecl_ekf.md#optical-flow) for more information.
+
+## Simulation
+
+Optical flow can be simulated with [SIH](../sim_sih/index.md), which computes the flow from the simulated vehicle motion instead of rendering a camera image.
+As with real hardware, the simulated sensor reports the integrated flow only, so a downward facing [distance sensor](../sensor/rangefinders.md) has to be simulated alongside it — SIH provides one.
+
+Two ready-made airframes have flow enabled and the estimator configured for it:
+
+```sh
+# Multicopter navigating on optical flow only (GNSS fusion disabled)
+make px4_sitl_sih sihsim_quadx_flow
+
+# Standard VTOL navigating on optical flow only (GNSS fusion disabled)
+make px4_sitl_sih sihsim_standard_vtol_flow
+```
+
+The sensor itself is configured with the [`SIH_OF_*` parameters](../sim_sih/index.md#optical-flow) (rate, noise, and maximum ground distance).
+
+Optical flow is also simulated by [Gazebo Classic](../sim_gazebo_classic/index.md), which renders the downward facing camera image and runs flow computation on it.
diff --git a/docs/en/sim_sih/index.md b/docs/en/sim_sih/index.md
index 4edfd36227a..8a854dcb1e6 100644
--- a/docs/en/sim_sih/index.md
+++ b/docs/en/sim_sih/index.md
@@ -14,7 +14,8 @@ See [PX4 Simulation QuickStart](../simulation/px4_simulation_quickstart.md) for
## Overview
SIH runs as a PX4 module that replaces real sensor and actuator hardware with a simulated physics model.
-It provides simulated IMU, GPS, barometer, magnetometer, and airspeed sensor data via uORB, and reads actuator outputs to update the vehicle state at each timestep.
+It provides simulated IMU, GPS, barometer, magnetometer, airspeed, distance sensor, and optical flow data via uORB, and reads actuator outputs to update the vehicle state at each timestep.
+The distance sensor and the downward facing [optical flow sensor](#optical-flow) let you exercise flow aided navigation without a rendered camera image.
The simulation runs in lockstep with PX4, ensuring deterministic and reproducible results.
It also integrates seamlessly with ROS 2 with no additional configuration (see [ROS 2 Integration](#ros-2-integration) below).
@@ -38,6 +39,8 @@ The following vehicle types are supported:
| Tailsitter VTOL | `make px4_sitl_sih sihsim_xvert` | Experimental |
| Standard VTOL (QuadPlane) | `make px4_sitl_sih sihsim_standard_vtol` | Experimental |
| Ackermann Rover | `make px4_sitl_sih sihsim_rover_ackermann` | Experimental |
+| Quadrotor X with optical flow | `make px4_sitl_sih sihsim_quadx_flow` | Experimental |
+| Standard VTOL with optical flow | `make px4_sitl_sih sihsim_standard_vtol_flow` | Experimental |
::: warning
Only the quadrotor vehicle type is stable and recommended for development. All other vehicle types (hexarotor, fixed-wing, VTOL, rover) are experimental and may have aerodynamic model or controller interaction issues that produce unrealistic flight behaviour.
@@ -117,6 +120,24 @@ PX4_SIM_SPEED_FACTOR=10 make px4_sitl_sih sihsim_quadx
SIH supports setting a wind velocity with the PX4 parameters [`SIH_WIND_N`](../advanced_config/parameter_reference.md#SIH_WIND_E) and [`SIH_WIND_E`](../advanced_config/parameter_reference.md#SIH_WIND_E) [m/s]. The parameters can also be changed during flight to simulate changing wind.
+#### Optical Flow
+
+SIH can simulate a downward facing optical flow sensor, so that flow aided navigation can be exercised without a rendered camera image.
+The sensor reports the integrated flow only: the flow module completes the sample with the vehicle gyro and a downward facing distance sensor, which therefore has to be enabled as well ([`SIH_DISTSNSR_EN`](../advanced_config/parameter_reference.md#SIH_DISTSNSR_EN)).
+
+The flow sensor is configured with [`SIH_OF_EN`](../advanced_config/parameter_reference.md#SIH_OF_EN), [`SIH_OF_RATE`](../advanced_config/parameter_reference.md#SIH_OF_RATE), [`SIH_OF_NOISE`](../advanced_config/parameter_reference.md#SIH_OF_NOISE), and [`SIH_OF_H_MAX`](../advanced_config/parameter_reference.md#SIH_OF_H_MAX).
+Quality drops to zero above `SIH_OF_H_MAX` or when the flow rate exceeds what the sensor can correlate.
+
+Two ready-made airframes use it:
+
+```sh
+# Multicopter navigating on optical flow only (GNSS fusion disabled)
+make px4_sitl_sih sihsim_quadx_flow
+
+# Standard VTOL navigating on optical flow only (GNSS fusion disabled)
+make px4_sitl_sih sihsim_standard_vtol_flow
+```
+
#### Set Custom Takeoff Location
The default takeoff location can be set using environment variables:
diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt
index f6f4c5c6fd6..6532bf8b30a 100644
--- a/src/modules/simulation/simulator_sih/CMakeLists.txt
+++ b/src/modules/simulation/simulator_sih/CMakeLists.txt
@@ -54,8 +54,10 @@ if(PX4_PLATFORM MATCHES "posix")
set(models
airplane
quadx
+ quadx_flow
xvert
standard_vtol
+ standard_vtol_flow
hex
rover_ackermann
)
diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp
index b7502d0d632..11b00218da0 100644
--- a/src/modules/simulation/simulator_sih/sih.cpp
+++ b/src/modules/simulation/simulator_sih/sih.cpp
@@ -47,6 +47,7 @@
#include
#include // to get PWM flags
+#include // to get the simulated sensor device types
#include
using namespace math;
@@ -58,7 +59,14 @@ ModuleBase::Descriptor Sih::desc{task_spawn, custom_command, print_usage};
Sih::Sih() :
ModuleParams(nullptr)
{
- srand(1234); // initialize the random seed once before calling math::generate_wgn()
+ srand(1234); // initialize the random seed once before calling generate_wgn()
+
+ device::Device::DeviceId device_id{};
+ device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
+ device_id.devid_s.bus = 0;
+ device_id.devid_s.address = 0;
+ device_id.devid_s.devtype = DRV_FLOW_DEVTYPE_SIM;
+ _optical_flow_device_id = device_id.devid;
}
Sih::~Sih()
@@ -74,10 +82,19 @@ void Sih::run()
parameters_updated();
+ if (_sih_optical_flow_en.get()) {
+ // Advertise before the simulated clock starts running: the sensors module brings up its
+ // optical flow pipeline when the topic first appears, and it only looks for it while
+ // updating its parameters. Advertising here makes it visible for the startup scan even
+ // though the first sample is only published once the control pipeline is alive.
+ _optical_flow_pub.advertise();
+ }
+
const hrt_abstime task_start = hrt_absolute_time();
_last_run = task_start;
_airspeed_time = task_start;
_dist_snsr_time = task_start;
+ _optical_flow_time = task_start;
_ranging_beacon_time = task_start;
_vehicle = static_cast(constrain(_sih_vtype.get(),
static_cast(VehicleType::First),
@@ -224,6 +241,8 @@ void Sih::updateFailureConfig()
== failure_injection::Mode::Off);
_distance_sensor_blocked = (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_DISTANCE_SENSOR, 1)
== failure_injection::Mode::Off);
+ _optical_flow_blocked = (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_OPTICAL_FLOW, 1)
+ == failure_injection::Mode::Off);
_accel_blocked = (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_ACCEL, 1)
== failure_injection::Mode::Off);
_gyro_blocked = (_failure_config.mode(failure_injection_s::FAILURE_UNIT_SENSOR_GYRO, 1)
@@ -276,12 +295,30 @@ void Sih::sensor_step()
}
// distance sensor published at 50 Hz
- if (now - _dist_snsr_time >= 20_ms
- && fabs(_distance_snsr_override) < 10000) {
+ if (_sih_distance_snsr_en.get() && now - _dist_snsr_time >= 20_ms) {
_dist_snsr_time = now;
send_dist_snsr(now);
}
+ // The optical flow is integrated every iteration and published at SIH_OF_RATE, but only
+ // once the control pipeline is alive. Publishing while the system is still starting up
+ // races with the sensors module bringing up its optical flow pipeline and deadlocks the
+ // simulation, as this thread is the one advancing the lockstep clock.
+ if (_sih_optical_flow_en.get()) {
+ if (_last_actuator_output_time > 0) {
+ accumulate_optical_flow(dt);
+
+ if (now - _optical_flow_time >= _optical_flow_interval_us) {
+ _optical_flow_time = now;
+ send_optical_flow(now);
+ }
+
+ } else {
+ // keep the integration interval aligned with the first publication
+ _optical_flow_time = now;
+ }
+ }
+
// ranging beacon published at 2 Hz (each beacon at 0.5 Hz)
if (now - _ranging_beacon_time >= 500_ms) {
_ranging_beacon_time = now;
@@ -367,9 +404,12 @@ void Sih::parameters_updated()
_distance_snsr_min = _sih_distance_snsr_min.get();
_distance_snsr_max = _sih_distance_snsr_max.get();
_distance_snsr_override = _sih_distance_snsr_override.get();
+ _distance_snsr_noise = _sih_distance_snsr_noise.get();
_px4_rangefinder.set_min_distance(_distance_snsr_min);
_px4_rangefinder.set_max_distance(_distance_snsr_max);
+ _optical_flow_interval_us = static_cast(roundf(1e6f / math::max(_sih_optical_flow_rate.get(), 1.f)));
+
_T_TAU = _sih_thrust_tau.get();
_v_wind_N = Vector3f(_sih_wind_n.get(), _sih_wind_e.get(), 0.f);
@@ -820,6 +860,21 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us)
_airspeed_pub.publish(airspeed);
}
+float Sih::ground_distance() const
+{
+ // the ground is assumed flat at the altitude of the simulation reference (SIH_LOC_H0)
+ const float cos_tilt = _q.dcm_z()(2);
+
+ if (cos_tilt < FLT_EPSILON) {
+ // the body down axis points away from the ground
+ return NAN;
+ }
+
+ // the fake ground lets the vehicle settle marginally below the reference altitude, clamp it
+ // so that a landed vehicle reads zero instead of dropping out of range
+ return math::max(-_lpos(2), 0.f) / cos_tilt;
+}
+
void Sih::send_dist_snsr(const hrt_abstime &time_now_us)
{
if (_distance_sensor_blocked) {
@@ -827,20 +882,127 @@ void Sih::send_dist_snsr(const hrt_abstime &time_now_us)
}
float current_distance;
+ int8_t signal_quality;
if (_distance_snsr_override >= 0.f) {
+ // forced reading, let the driver flag it if it falls outside of the configured range
current_distance = _distance_snsr_override;
+ signal_quality = -1;
} else {
- current_distance = -_lpos(2) / _q.dcm_z()(2);
+ // the quality is decided on the true distance rather than on the noisy reading, so
+ // that a measurement sitting at a range limit does not flicker in and out of validity
+ const float true_distance = ground_distance();
- if (current_distance > _distance_snsr_max) {
- // this is based on lightware lw20 behaviour
- current_distance = UINT16_MAX / 100.f;
+ if (!PX4_ISFINITE(true_distance) || true_distance > _distance_snsr_max) {
+ // nothing within reach, the sensor reports its saturated raw reading
+ current_distance = DISTSNSR_OUT_OF_RANGE;
+ signal_quality = 0;
+
+ } else if (true_distance < _distance_snsr_min) {
+ // closer than the minimum range: the reading saturates and cannot be trusted
+ current_distance = _distance_snsr_min;
+ signal_quality = 0;
+
+ } else {
+ current_distance = math::max(true_distance + generate_wgn() * _distance_snsr_noise, 0.f);
+
+ // the returned signal gets weaker as the target moves away
+ const float derate_start = DISTSNSR_QUALITY_DERATE * _distance_snsr_max;
+ const float derate_span = _distance_snsr_max - derate_start;
+
+ if (true_distance > derate_start && derate_span > FLT_EPSILON) {
+ const float derated = (true_distance - derate_start) / derate_span;
+ signal_quality = static_cast(constrain(roundf(100.f - 99.f * derated), 1.f, 100.f));
+
+ } else {
+ signal_quality = 100;
+ }
}
}
- _px4_rangefinder.update(hrt_absolute_time(), current_distance);
+ _px4_rangefinder.update(time_now_us, current_distance, signal_quality);
+}
+
+void Sih::accumulate_optical_flow(const float dt)
+{
+ // An optical flow sensor integrates the angular displacement of the ground image over its
+ // integration interval. The sign convention of sensor_optical_flow is such that a positive
+ // value is produced by a right handed rotation of the sensor about the body axis, which
+ // gives the translation over the ground the opposite sign (see the flow observation model
+ // in the EKF).
+ const float distance = ground_distance();
+
+ if (!PX4_ISFINITE(distance) || distance > _sih_optical_flow_h_max.get()) {
+ // too high above the ground, or not looking at it: no image to track
+ _optical_flow_tracking = false;
+ _optical_flow_dt += dt;
+ return;
+ }
+
+ const Vector3f v_B = _q.rotateVectorInverse(_v_N); // ground relative velocity in body frame
+
+ // the flow produced by the translation over the ground scales with the inverse of the
+ // distance, which is kept bounded to stay well behaved when landed
+ const Vector2f translation_flow = Vector2f(-v_B(1), v_B(0)) / math::max(distance, FLOW_MIN_DISTANCE);
+ const Vector2f flow_rate = Vector2f(_w_B(0), _w_B(1)) + translation_flow;
+
+ if (flow_rate.norm() > FLOW_MAX_RATE) {
+ // the image moves too fast for the sensor to correlate two consecutive frames
+ _optical_flow_tracking = false;
+ }
+
+ _optical_flow_integral += flow_rate * dt;
+ _optical_flow_dt += dt;
+}
+
+void Sih::send_optical_flow(const hrt_abstime &time_now_us)
+{
+ Vector2f pixel_flow = _optical_flow_integral;
+ const float integration_time = _optical_flow_dt;
+ const bool tracking = _optical_flow_tracking;
+
+ // start a new integration interval
+ _optical_flow_integral.setZero();
+ _optical_flow_dt = 0.f;
+ _optical_flow_tracking = true;
+
+ if (_optical_flow_blocked || integration_time < FLT_EPSILON) {
+ return;
+ }
+
+ sensor_optical_flow_s flow{};
+ flow.timestamp_sample = time_now_us;
+ flow.device_id = _optical_flow_device_id;
+
+ if (tracking) {
+ // white noise on the flow rate estimate, integrated over the interval
+ pixel_flow += Vector2f(generate_wgn(), generate_wgn()) * (_sih_optical_flow_noise.get() * integration_time);
+ flow.quality = UINT8_MAX;
+
+ } else {
+ pixel_flow.setZero();
+ flow.quality = 0;
+ }
+
+ pixel_flow.copyTo(flow.pixel_flow);
+ flow.integration_timespan_us = static_cast(roundf(integration_time * 1e6f));
+
+ // the sensor has neither a gyro nor a rangefinder: the flow module completes the sample
+ // with the vehicle gyro and the downward facing distance sensor
+ flow.delta_angle[0] = flow.delta_angle[1] = flow.delta_angle[2] = NAN;
+ flow.delta_angle_available = false;
+ flow.distance_m = NAN;
+ flow.distance_available = false;
+
+ flow.max_flow_rate = FLOW_MAX_RATE;
+ flow.min_ground_distance = 0.f;
+ flow.max_ground_distance = _sih_optical_flow_h_max.get();
+ flow.mode = sensor_optical_flow_s::MODE_LOWLIGHT;
+ flow.error_count = 0;
+
+ flow.timestamp = hrt_absolute_time();
+ _optical_flow_pub.publish(flow);
}
void Sih::send_ranging_beacon(const hrt_abstime &time_now_us)
diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp
index 82795a10ec9..e33cbc67737 100644
--- a/src/modules/simulation/simulator_sih/sih.hpp
+++ b/src/modules/simulation/simulator_sih/sih.hpp
@@ -80,6 +80,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -129,9 +130,10 @@ private:
PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Gyroscope _px4_gyro{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
PX4Rangefinder _px4_rangefinder{10092548}; // 10092548: DRV_DIST_DEVTYPE_SIM, BUS: 0, ADDR: 0, TYPE: SIMULATION
- uORB::Publication _airspeed_pub{ORB_ID(airspeed)};
- uORB::Publication _ranging_beacon_pub{ORB_ID(ranging_beacon)};
- uORB::Publication _esc_status_pub{ORB_ID(esc_status)};
+ uORB::Publication _airspeed_pub{ORB_ID(airspeed)};
+ uORB::Publication _ranging_beacon_pub{ORB_ID(ranging_beacon)};
+ uORB::Publication _esc_status_pub{ORB_ID(esc_status)};
+ uORB::Publication _optical_flow_pub{ORB_ID(sensor_optical_flow)};
// groundtruth
uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
@@ -145,6 +147,7 @@ private:
bool _airspeed_blocked{false};
bool _distance_sensor_blocked{false};
+ bool _optical_flow_blocked{false};
bool _accel_blocked{false};
bool _gyro_blocked{false};
uint16_t _stuck_motor_mask{0};
@@ -175,6 +178,16 @@ private:
};
RangingBeaconConfig _ranging_beacons[NUM_RANGING_BEACONS] {};
+ // Distance sensor simulation constants
+ // fraction of the measurement range above which the returned signal starts to get weaker
+ static constexpr float DISTSNSR_QUALITY_DERATE = 0.8f;
+ // out of range reading, this is based on lightware lw20 behaviour
+ static constexpr float DISTSNSR_OUT_OF_RANGE = UINT16_MAX / 100.f;
+
+ // Optical flow simulation constants, the values are taken from the PAW3902
+ static constexpr float FLOW_MAX_RATE = 7.4f; // maximum measurable angular flow rate [rad/s]
+ static constexpr float FLOW_MIN_DISTANCE = 0.1f; // lower bound on the range scaling the flow [m]
+
static constexpr float T1_C = 15.0f; // ground temperature in Celsius
static constexpr float T1_K = T1_C - atmosphere::kAbsoluteNullCelsius; // ground temperature in Kelvin
static constexpr float TEMP_GRADIENT = -6.5f / 1000.0f; // temperature gradient in degrees per metre
@@ -202,7 +215,14 @@ private:
void reconstruct_sensors_signals(const hrt_abstime &time_now_us);
void send_airspeed(const hrt_abstime &time_now_us);
void send_dist_snsr(const hrt_abstime &time_now_us);
+ void accumulate_optical_flow(const float dt);
+ void send_optical_flow(const hrt_abstime &time_now_us);
void send_ranging_beacon(const hrt_abstime &time_now_us);
+
+ // distance from the vehicle to the ground measured along the body down axis, NAN if the
+ // body down axis does not point at the ground anymore
+ float ground_distance() const;
+
void publish_ground_truth(const hrt_abstime &time_now_us);
void generate_fw_aerodynamics(const float roll_cmd, const float pitch_cmd, const float yaw_cmd, const float thrust_for_prowash);
void generate_ts_aerodynamics();
@@ -233,6 +253,13 @@ private:
hrt_abstime _ranging_beacon_time{0};
uint8_t _ranging_beacon_idx{0};
+ // optical flow accumulation over one integration interval
+ hrt_abstime _optical_flow_time{0};
+ matrix::Vector2f _optical_flow_integral{};
+ float _optical_flow_dt{0.f};
+ bool _optical_flow_tracking{true};
+ uint32_t _optical_flow_device_id{0};
+
bool _grounded{true}; // whether the vehicle is on the ground
// Quantities in body frame (FRD)
@@ -310,7 +337,8 @@ private:
matrix::Matrix3f _I; // vehicle inertia matrix
matrix::Matrix3f _Im1; // inverse of the inertia matrix
- float _distance_snsr_min, _distance_snsr_max, _distance_snsr_override;
+ float _distance_snsr_min, _distance_snsr_max, _distance_snsr_override, _distance_snsr_noise;
+ hrt_abstime _optical_flow_interval_us{20_ms};
esc_status_s _esc_status{};
@@ -335,9 +363,15 @@ private:
(ParamFloat) _sih_lon0,
(ParamFloat) _sih_h0,
(ParamFloat) _sih_yaw0,
+ (ParamInt) _sih_distance_snsr_en,
(ParamFloat) _sih_distance_snsr_min,
(ParamFloat) _sih_distance_snsr_max,
(ParamFloat) _sih_distance_snsr_override,
+ (ParamFloat) _sih_distance_snsr_noise,
+ (ParamInt) _sih_optical_flow_en,
+ (ParamFloat) _sih_optical_flow_rate,
+ (ParamFloat) _sih_optical_flow_noise,
+ (ParamFloat) _sih_optical_flow_h_max,
(ParamFloat) _sih_thrust_tau,
// forward propeller
(ParamFloat) _sih_f_thrust_max,
diff --git a/src/modules/simulation/simulator_sih/sih_params.yaml b/src/modules/simulation/simulator_sih/sih_params.yaml
index ce5be19a30f..e52f98ddaa1 100644
--- a/src/modules/simulation/simulator_sih/sih_params.yaml
+++ b/src/modules/simulation/simulator_sih/sih_params.yaml
@@ -227,9 +227,20 @@ parameters:
max: 3.14159
decimal: 3
increment: 0.01
+ SIH_DISTSNSR_EN:
+ description:
+ short: Enable the simulated downward facing distance sensor
+ type: enum
+ values:
+ 0: Disabled
+ 1: Enabled
+ default: 1
SIH_DISTSNSR_MIN:
description:
short: distance sensor minimum range
+ long: |-
+ Readings closer than this saturate at the minimum range
+ and are reported with a zero signal quality.
type: float
default: 0.0
unit: m
@@ -240,6 +251,9 @@ parameters:
SIH_DISTSNSR_MAX:
description:
short: distance sensor maximum range
+ long: |-
+ The signal quality degrades over the last 20% of the range. Beyond the maximum range
+ the sensor reports a saturated reading with a zero signal quality.
type: float
default: 100.0
unit: m
@@ -250,10 +264,67 @@ parameters:
SIH_DISTSNSR_OVR:
description:
short: if >= 0 the distance sensor measures will be overridden by this value
- long: Absolute value superior to 10000 will disable distance sensor
+ long: Set SIH_DISTSNSR_EN to 0 to disable the distance sensor.
type: float
default: -1.0
unit: m
+ SIH_DISTSNSR_NOI:
+ description:
+ short: Distance sensor measurement noise standard deviation
+ long: |-
+ Gaussian noise added to the simulated distance sensor measurements.
+ Set to 0 to disable noise.
+ type: float
+ default: 0.01
+ unit: m
+ min: 0.0
+ max: 10.0
+ decimal: 3
+ SIH_OF_EN:
+ description:
+ short: Enable the simulated optical flow sensor
+ long: |-
+ The simulated sensor reports the integrated flow only. The flow module completes the
+ sample with the vehicle gyro and the downward facing distance sensor, which therefore
+ has to be enabled as well (SIH_DISTSNSR_EN) for the flow to be usable by the estimator.
+ type: enum
+ values:
+ 0: Disabled
+ 1: Enabled
+ default: 0
+ SIH_OF_RATE:
+ description:
+ short: Optical flow publication rate
+ long: The simulated sensor integrates the flow over one publication interval.
+ type: float
+ default: 12.0
+ unit: Hz
+ min: 1.0
+ max: 100.0
+ decimal: 1
+ SIH_OF_NOISE:
+ description:
+ short: Optical flow measurement noise standard deviation
+ long: |-
+ Gaussian noise added to the simulated flow rate.
+ Set to 0 to disable noise.
+ type: float
+ default: 0.05
+ unit: rad/s
+ min: 0.0
+ max: 5.0
+ decimal: 3
+ SIH_OF_H_MAX:
+ description:
+ short: Optical flow maximum ground distance
+ long: |-
+ Above this distance to the ground the simulated sensor cannot track the ground image
+ anymore and reports a zero quality.
+ type: float
+ default: 30.0
+ unit: m
+ min: 1.0
+ decimal: 1
SIH_T_TAU:
description:
short: thruster time constant tau
diff --git a/test/mavsdk_tests/autopilot_tester_flow.py b/test/mavsdk_tests/autopilot_tester_flow.py
new file mode 100644
index 00000000000..a0186415630
--- /dev/null
+++ b/test/mavsdk_tests/autopilot_tester_flow.py
@@ -0,0 +1,299 @@
+"""AutopilotTester for optical flow aided navigation (sihsim_quadx_flow, sihsim_standard_vtol_flow).
+
+The flow models fly without GNSS fusion, so the estimator never picks a global origin on its own
+and everything that needs a global position (arming checks, home position, orbit center) stays
+unavailable until a GCS provides one.
+
+Flow quality can only be judged indirectly: the velocity the estimator reports is compared against
+the SIH ground truth velocity, which is streamed out as HIL_STATE_QUATERNION.
+"""
+
+import json
+import math
+import threading
+import time
+from typing import Any, List, Optional, Tuple
+
+from autopilot_tester import AutopilotTester, log
+from geometry import CoordinateTransformation
+
+AXES = ("north", "east", "down")
+
+MAV_CMD_DO_ORBIT = 34
+
+# Wall time between SET_GPS_GLOBAL_ORIGIN resends while the estimator has not taken the origin.
+ORIGIN_RESEND_INTERVAL_S = 1.0
+
+# ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER: the nose follows the circle center, so the
+# heading turns by a full revolution per orbit.
+ORBIT_YAW_FACE_CENTER = 0.0
+
+# Cap on the recorded ground truth trajectory so that a hanging test cannot grow it without bound.
+MAX_GROUND_TRUTH_SAMPLES = 100000
+
+Local2 = Tuple[float, float]
+
+
+def _heading_from_quaternion(q: List[float]) -> float:
+ w, x, y, z = q
+ return math.atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))
+
+
+class AutopilotTesterFlow(AutopilotTester):
+
+ def __init__(self, speed_factor: Optional[float] = None) -> None:
+ super().__init__(speed_factor)
+ self._velocity_lock = threading.Lock()
+ self._estimate_valid = False
+ self._estimated_velocity_m_s = [0.0, 0.0, 0.0]
+ self._error_sum_m_s = [0.0, 0.0, 0.0]
+ self._error_max_m_s = [0.0, 0.0, 0.0]
+ self._error_samples = 0
+ self._ground_truth_lla: List[Tuple[float, float]] = []
+ self._heading_valid = False
+ self._previous_heading_rad = 0.0
+ self._heading_swept_rad = 0.0
+ self._comparison_active = False
+ self._position_velocity_handle: Any = None
+ self._ground_truth_handle: Any = None
+
+ # ------------------------------------------------------------------
+ # EKF origin / arming
+ # ------------------------------------------------------------------
+
+ def _home_frame(self) -> CoordinateTransformation:
+ return CoordinateTransformation(*self._home_or_fail())
+
+ def set_ekf_origin_to_home(self, timeout_s: float = 20) -> None:
+ """Set the EKF origin to the stored (ground truth) home position with
+ SET_GPS_GLOBAL_ORIGIN and wait until the vehicle reports a global position. Requires
+ store_home() to have run."""
+ home_latitude_deg, home_longitude_deg = self._home_or_fail()
+ origin = {
+ "target_system": self._target_system_id(),
+ "latitude": round(home_latitude_deg * 1e7),
+ "longitude": round(home_longitude_deg * 1e7),
+ "altitude": round(self.home_absolute_altitude_m() * 1e3),
+ "time_usec": 0,
+ }
+
+ # The SITL startup script starts the estimator after MAVLink, and a subscriber that joins
+ # late only sees the newest vehicle command. SET_GPS_GLOBAL_ORIGIN is not acknowledged, so
+ # it is resent until it is taken. The estimator only publishes a global position once it
+ # has an origin, so a finite global position confirms it.
+ log("Waiting for the EKF origin to be set")
+ last_sent_s = -math.inf
+
+ def got_origin() -> bool:
+ nonlocal last_sent_s
+ position = self._telemetry.position()
+ if math.isfinite(position.latitude_deg) and math.isfinite(position.longitude_deg):
+ return True
+
+ now_s = time.monotonic()
+ if now_s - last_sent_s >= ORIGIN_RESEND_INTERVAL_S:
+ self.send_mavlink_message("SET_GPS_GLOBAL_ORIGIN", origin)
+ last_sent_s = now_s
+ return False
+
+ assert self.poll_condition_with_timeout(got_origin, timeout_s), \
+ "EKF origin not set within {} s".format(timeout_s)
+
+ def wait_until_armable(self, timeout_s: float = 60) -> None:
+ """Wait until the autopilot itself reports that it can be armed.
+
+ wait_until_ready() cannot be used on a flow only vehicle: MAVSDK derives both the global
+ and the local position health from the GPS / optical flow / vision flags in SYS_STATUS,
+ and PX4 only ever fills in the GPS one, so health_all_ok() stays false no matter how good
+ the flow aided estimate is."""
+ log("Waiting for the vehicle to be armable")
+ assert self.poll_condition_with_timeout(
+ lambda: bool(self._telemetry.health().is_armable), timeout_s), \
+ "Not armable within {} s".format(timeout_s)
+
+ # ------------------------------------------------------------------
+ # Orbit
+ # ------------------------------------------------------------------
+
+ def command_orbit(self, center: Local2, radius_m: float, velocity_m_s: float,
+ rel_alt_m: float) -> None:
+ """Command an orbit (MAV_CMD_DO_ORBIT) around a home-relative center. On a multicopter it
+ is flown with the nose pointing at the center so that the heading turns with the vehicle,
+ on a fixed wing it turns into a loiter of that radius. Pass NaN as the velocity to leave
+ the tangential velocity to the vehicle."""
+ latitude_deg, longitude_deg = self._home_frame().global_from_local(*center)
+ self.send_command_int(
+ MAV_CMD_DO_ORBIT, param1=radius_m, param2=velocity_m_s,
+ param3=ORBIT_YAW_FACE_CENTER, param4=float("nan"),
+ x=round(latitude_deg * 1e7), y=round(longitude_deg * 1e7),
+ z=self.home_absolute_altitude_m() + rel_alt_m)
+
+ def tangent_circle_center(self, radius_m: float) -> Local2:
+ """Center of a clockwise circle of radius_m that passes through the current position
+ with the current velocity already tangent to it (center() is 90 degrees clockwise of
+ the velocity, matching command_orbit's positive-radius/clockwise convention).
+
+ Commanding an orbit around this center is joined immediately instead of being flown out
+ to from wherever it happened to be requested."""
+ north_m, east_m, _down_m = self.get_current_position_ned()
+ velocity = self._telemetry.position_velocity_ned().velocity
+ speed_m_s = math.hypot(velocity.north_m_s, velocity.east_m_s)
+ assert speed_m_s > 1.0, \
+ "Not moving fast enough to determine a heading: {:.2f} m/s".format(speed_m_s)
+ north_unit = velocity.north_m_s / speed_m_s
+ east_unit = velocity.east_m_s / speed_m_s
+ return (north_m - radius_m * east_unit, east_m + radius_m * north_unit)
+
+ def wait_until_on_circle(self, center: Local2, radius_m: float, tolerance_m: float,
+ timeout_s: float) -> None:
+ """Wait until the vehicle is on the circle around center, i.e. its distance to the
+ center is within tolerance_m of radius_m.
+
+ Flying out to the circle only crosses the band once and then stays on it, so the first
+ time the vehicle is in the band it is established on it."""
+ log("Waiting for the vehicle to get onto the circle")
+
+ def on_circle() -> bool:
+ position = self._telemetry.position_velocity_ned().position
+ distance_m = math.hypot(position.north_m - center[0], position.east_m - center[1])
+ return bool(abs(distance_m - radius_m) < tolerance_m)
+
+ assert self.poll_condition_with_timeout(on_circle, timeout_s), \
+ "Did not reach the circle within {} s".format(timeout_s)
+
+ # ------------------------------------------------------------------
+ # Ground truth comparison
+ # ------------------------------------------------------------------
+
+ def start_ground_truth_comparison(self, rate_hz: float = 30.0) -> None:
+ """Start comparing the estimated NED velocity against the ground truth velocity, and
+ start tracking how far the (ground truth) heading turns. Both streams are requested at
+ rate_hz so that the two samples being compared are close in time."""
+ self.stop_ground_truth_comparison()
+
+ self._telemetry.set_rate_position_velocity_ned(rate_hz)
+ self._telemetry.set_rate_ground_truth(rate_hz)
+
+ with self._velocity_lock:
+ self._estimate_valid = False
+ self._error_sum_m_s = [0.0, 0.0, 0.0]
+ self._error_max_m_s = [0.0, 0.0, 0.0]
+ self._error_samples = 0
+ self._heading_valid = False
+ self._heading_swept_rad = 0.0
+ self._ground_truth_lla = []
+
+ def on_position_velocity_ned(sample: Any, _user_data: Any) -> None:
+ with self._velocity_lock:
+ self._estimated_velocity_m_s = [
+ sample.velocity.north_m_s, sample.velocity.east_m_s,
+ sample.velocity.down_m_s]
+ self._estimate_valid = True
+
+ def on_ground_truth(message: Any, _user_data: Any) -> None:
+ fields = json.loads(message.fields_json)
+ # HIL_STATE_QUATERNION carries the ground truth velocity in cm/s.
+ truth_m_s = (fields["vx"] * 1e-2, fields["vy"] * 1e-2, fields["vz"] * 1e-2)
+ heading_rad = _heading_from_quaternion(fields["attitude_quaternion"])
+
+ with self._velocity_lock:
+ if len(self._ground_truth_lla) < MAX_GROUND_TRUTH_SAMPLES:
+ self._ground_truth_lla.append(
+ (fields["lat"] * 1e-7, fields["lon"] * 1e-7))
+
+ if self._heading_valid:
+ # Signed, so that yaw oscillations cancel out instead of adding up to a turn.
+ self._heading_swept_rad += math.remainder(
+ heading_rad - self._previous_heading_rad, 2.0 * math.pi)
+
+ self._previous_heading_rad = heading_rad
+ self._heading_valid = True
+
+ if not self._estimate_valid:
+ return
+
+ for i in range(3):
+ error = abs(self._estimated_velocity_m_s[i] - truth_m_s[i])
+ self._error_sum_m_s[i] += error
+ self._error_max_m_s[i] = max(self._error_max_m_s[i], error)
+
+ self._error_samples += 1
+
+ self._position_velocity_handle = self._telemetry.subscribe_position_velocity_ned(
+ on_position_velocity_ned)
+ self._ground_truth_handle = self._mavlink_direct.subscribe_message(
+ "HIL_STATE_QUATERNION", on_ground_truth)
+ self._comparison_active = True
+
+ def stop_ground_truth_comparison(self) -> None:
+ """Stop the comparison. Safe to call when it is not running."""
+ if not self._comparison_active:
+ return
+
+ self._comparison_active = False
+ self._telemetry.unsubscribe_position_velocity_ned(self._position_velocity_handle)
+ self._mavlink_direct.unsubscribe_message(self._ground_truth_handle)
+
+ def _heading_swept_deg(self) -> float:
+ with self._velocity_lock:
+ return math.degrees(abs(self._heading_swept_rad))
+
+ def wait_until_heading_swept(self, min_sweep_deg: float, timeout_s: float) -> None:
+ """Wait until the heading has turned by min_sweep_deg in total since the comparison
+ started."""
+ log("Waiting for the heading to turn by {} deg".format(min_sweep_deg))
+ assert self.poll_condition_with_timeout(
+ lambda: self._heading_swept_deg() > min_sweep_deg, timeout_s), \
+ "Heading did not turn by {} deg within {} s".format(min_sweep_deg, timeout_s)
+
+ def check_ground_truth_on_circle(self, center: Local2, radius_m: float,
+ tolerance_m: float) -> None:
+ """Assert the ground truth trajectory stayed on the commanded circle: every recorded
+ sample was within tolerance_m of radius_m around center. Together with the heading sweep
+ this is what tells a flown orbit apart from a vehicle that only turned on the spot."""
+ ct = self._home_frame()
+
+ with self._velocity_lock:
+ ground_truth_lla = list(self._ground_truth_lla)
+
+ assert len(ground_truth_lla) > 100, \
+ "Only {} ground truth samples recorded".format(len(ground_truth_lla))
+
+ distances_m = []
+ for latitude_deg, longitude_deg in ground_truth_lla:
+ north_m, east_m = ct.local_from_global(latitude_deg, longitude_deg)
+ distances_m.append(math.hypot(north_m - center[0], east_m - center[1]))
+
+ min_distance_m = min(distances_m)
+ max_distance_m = max(distances_m)
+ log("ground truth distance to the orbit center: {:.2f} m to {:.2f} m (radius {} m)"
+ .format(min_distance_m, max_distance_m, radius_m))
+
+ assert min_distance_m > radius_m - tolerance_m, \
+ "Minimum distance to center {:.2f} m, expected > {} m".format(
+ min_distance_m, radius_m - tolerance_m)
+ assert max_distance_m < radius_m + tolerance_m, \
+ "Maximum distance to center {:.2f} m, expected < {} m".format(
+ max_distance_m, radius_m + tolerance_m)
+
+ def check_velocity_error_below(self, mean_tol_m_s: float, max_tol_m_s: float) -> None:
+ """Assert on the collected velocity error: the mean absolute error of every axis has to
+ stay below mean_tol_m_s and no single sample may be off by more than max_tol_m_s."""
+ with self._velocity_lock:
+ error_samples = self._error_samples
+ error_sum_m_s = list(self._error_sum_m_s)
+ error_max_m_s = list(self._error_max_m_s)
+
+ # Without samples the checks below would pass vacuously.
+ assert error_samples > 100, "Only {} velocity error samples".format(error_samples)
+
+ for i, axis in enumerate(AXES):
+ mean_m_s = error_sum_m_s[i] / error_samples
+ log("velocity error {}: mean {:.3f} m/s, max {:.3f} m/s ({} samples)".format(
+ axis, mean_m_s, error_max_m_s[i], error_samples))
+ assert mean_m_s < mean_tol_m_s, \
+ "Mean {} velocity error {:.3f} m/s, expected < {} m/s".format(
+ axis, mean_m_s, mean_tol_m_s)
+ assert error_max_m_s[i] < max_tol_m_s, \
+ "Max {} velocity error {:.3f} m/s, expected < {} m/s".format(
+ axis, error_max_m_s[i], max_tol_m_s)
diff --git a/test/mavsdk_tests/configs/sih-sitl.json b/test/mavsdk_tests/configs/sih-sitl.json
index 3d14e49ebe7..3658095898c 100644
--- a/test/mavsdk_tests/configs/sih-sitl.json
+++ b/test/mavsdk_tests/configs/sih-sitl.json
@@ -45,6 +45,16 @@
"test_filter": "motor_failure_injection",
"timeout_min": 15
},
+ {
+ "model": "quadx_flow",
+ "test_filter": "optical_flow",
+ "timeout_min": 10
+ },
+ {
+ "model": "standard_vtol_flow",
+ "test_filter": "optical_flow_vtol",
+ "timeout_min": 10
+ },
{
"model": "standard_vtol",
"test_filter": "vtol",
diff --git a/test/mavsdk_tests/conftest.py b/test/mavsdk_tests/conftest.py
index fbb489d84b4..ec7646a29cc 100644
--- a/test/mavsdk_tests/conftest.py
+++ b/test/mavsdk_tests/conftest.py
@@ -3,11 +3,13 @@ from typing import Iterator, Optional, Type, TypeVar
import pytest
# Show the values in failed asserts inside the testers, not just in tests.
-pytest.register_assert_rewrite("autopilot_tester", "autopilot_tester_follow_me",
+pytest.register_assert_rewrite("autopilot_tester", "autopilot_tester_flow",
+ "autopilot_tester_follow_me",
"autopilot_tester_loiter", "autopilot_tester_rtl",
"autopilot_tester_vision_target")
from autopilot_tester import AutopilotTester # noqa: E402
+from autopilot_tester_flow import AutopilotTesterFlow # noqa: E402
from autopilot_tester_follow_me import AutopilotTesterFollowMe # noqa: E402
from autopilot_tester_loiter import AutopilotTesterLoiter # noqa: E402
from autopilot_tester_rtl import AutopilotTesterRtl # noqa: E402
@@ -57,6 +59,12 @@ def tester_loiter(request: pytest.FixtureRequest) -> Iterator[AutopilotTesterLoi
yield from connected_tester(request, AutopilotTesterLoiter)
+@pytest.fixture
+def tester_flow(request: pytest.FixtureRequest) -> Iterator[AutopilotTesterFlow]:
+ """A tester for optical flow aided navigation, connected to the vehicle."""
+ yield from connected_tester(request, AutopilotTesterFlow)
+
+
@pytest.fixture
def tester_follow_me(request: pytest.FixtureRequest) -> Iterator[AutopilotTesterFollowMe]:
"""A tester with follow-me support, connected to the vehicle."""
diff --git a/test/mavsdk_tests/pytest.ini b/test/mavsdk_tests/pytest.ini
index 796e93afb47..53b6f32d9c4 100644
--- a/test/mavsdk_tests/pytest.ini
+++ b/test/mavsdk_tests/pytest.ini
@@ -16,3 +16,5 @@ markers =
sih_home_yaw: SIH home yaw tests
sih_xvert_transition: SIH XVert transition tests
vision_target: vision target estimator precision landing tests
+ optical_flow: optical flow aided multicopter navigation tests
+ optical_flow_vtol: optical flow aided VTOL navigation tests
diff --git a/test/mavsdk_tests/test_multicopter_optical_flow.py b/test/mavsdk_tests/test_multicopter_optical_flow.py
new file mode 100644
index 00000000000..9c68893afa9
--- /dev/null
+++ b/test/mavsdk_tests/test_multicopter_optical_flow.py
@@ -0,0 +1,70 @@
+"""Optical flow aided navigation on a multicopter (sihsim_quadx_flow).
+
+The model has no GNSS fusion: the horizontal velocity is observed by the simulated downward
+facing optical flow sensor together with the distance sensor, and the height comes from the
+barometer. The test provides the EKF origin the way a GCS would (SET_GPS_GLOBAL_ORIGIN, the
+estimator never picks one on its own without GNSS), takes off and flies an orbit with the nose
+pointing at the circle center. Circling that way keeps the vehicle translating in every direction
+while it also keeps turning, so the flow rate the sensor reports is a mix of ground relative
+velocity and body rates. Throughout the manoeuvre the estimated NED velocity is compared against
+the SIH ground truth velocity.
+"""
+
+import pytest
+
+from autopilot_tester_flow import AutopilotTesterFlow
+
+# Well below SIH_OF_H_MAX / SIH_DISTSNSR_MAX (30 m) so the flow stays valid throughout.
+FLIGHT_ALTITUDE_M = 10.0
+
+# The center is one radius north of home, so the takeoff position is already on the circle.
+ORBIT_RADIUS_M = 20.0
+ORBIT_VELOCITY_M_S = 5.0 # ~25 s and, with the nose on the center, ~14 deg/s per turn
+ORBIT_CENTER = (ORBIT_RADIUS_M, 0.0)
+
+# The estimate has to track the truth: the two streams are paired by arrival, so a short
+# acceleration between the two samples being compared already shows up as an error. Observed is
+# ~0.1 m/s mean and ~0.3 m/s worst case, the tolerances leave room for the sampling jitter.
+MEAN_VELOCITY_ERROR_TOLERANCE_M_S = 0.3
+MAX_VELOCITY_ERROR_TOLERANCE_M_S = 1.0
+
+# The circle the vehicle actually flew (ground truth) is checked against the commanded one. The
+# band has to swallow the initial acceleration onto the circle and whatever the flow only position
+# estimate drifts over one orbit, observed is ~2.5 m.
+ORBIT_RADIUS_TOLERANCE_M = 8.0
+
+# A full turn of the heading, which the vehicle only reaches by flying a whole orbit.
+HEADING_SWEEP_DEG = 360.0
+
+
+@pytest.mark.optical_flow
+def test_orbit_tracks_ground_truth_velocity(tester_flow: AutopilotTesterFlow) -> None:
+ tester = tester_flow
+
+ # The ground truth is streamed independently of the estimator, so home can be stored before
+ # there is any global position, and it is the position the EKF origin gets set to.
+ tester.store_home()
+ tester.set_ekf_origin_to_home()
+
+ tester.wait_until_armable()
+ tester.set_takeoff_altitude(FLIGHT_ALTITUDE_M)
+ tester.arm()
+ tester.takeoff()
+ tester.wait_until_hovering()
+ tester.wait_until_altitude(FLIGHT_ALTITUDE_M, 60)
+ tester.sleep_for(3) # let the position controller settle into the hover
+
+ tester.command_orbit(ORBIT_CENTER, ORBIT_RADIUS_M, ORBIT_VELOCITY_M_S, FLIGHT_ALTITUDE_M)
+ tester.start_ground_truth_comparison()
+
+ # Fly a full circle. The heading turns with it because the nose follows the circle center, so
+ # the sweep doubles as the "the orbit is really being flown" check.
+ tester.wait_until_heading_swept(HEADING_SWEEP_DEG, 120)
+ tester.stop_ground_truth_comparison()
+
+ tester.check_ground_truth_on_circle(ORBIT_CENTER, ORBIT_RADIUS_M, ORBIT_RADIUS_TOLERANCE_M)
+ tester.check_velocity_error_below(MEAN_VELOCITY_ERROR_TOLERANCE_M_S,
+ MAX_VELOCITY_ERROR_TOLERANCE_M_S)
+
+ tester.land()
+ tester.wait_until_disarmed()
diff --git a/test/mavsdk_tests/test_vtol_optical_flow.py b/test/mavsdk_tests/test_vtol_optical_flow.py
new file mode 100644
index 00000000000..132325c8a01
--- /dev/null
+++ b/test/mavsdk_tests/test_vtol_optical_flow.py
@@ -0,0 +1,80 @@
+"""Optical flow aided navigation on a VTOL (sihsim_standard_vtol_flow).
+
+Like the multicopter flow test this model flies without GNSS fusion, so the EKF origin has to be
+provided the way a GCS would. The vehicle takes off in multicopter mode, transitions to fixed wing
+on command and flies a loiter, which keeps it banked and turning while it translates much faster
+than a multicopter would. The whole loiter is flown low enough for the flow to stay in range
+(SIH_OF_H_MAX / SIH_DISTSNSR_MAX are 30 m on this model), and the estimated NED velocity is
+compared against the SIH ground truth velocity throughout.
+"""
+
+import math
+
+import pytest
+
+from autopilot_tester_flow import AutopilotTesterFlow
+
+FLIGHT_ALTITUDE_M = 10.0
+
+# A wide loiter keeps the bank angle small, which keeps the downward facing flow sensor looking at
+# the ground and the distance it measures close to the altitude.
+LOITER_RADIUS_M = 100.0
+
+# Band the vehicle has to enter to count as established on the loiter, and the band the flown
+# (ground truth) circle is checked against afterwards. The latter has to swallow how tightly the
+# fixed wing tracks the circle plus the drift of the flow only position estimate, observed is
+# ~1.6 m.
+LOITER_CAPTURE_TOLERANCE_M = 20.0
+LOITER_RADIUS_TOLERANCE_M = 10.0
+
+# The estimate has to track the truth: the two streams are paired by arrival, so a short
+# acceleration between the two samples being compared already shows up as an error. Observed is
+# ~0.1 m/s mean and ~0.3 m/s worst case, the tolerances leave room for the sampling jitter.
+MEAN_VELOCITY_ERROR_TOLERANCE_M_S = 0.3
+MAX_VELOCITY_ERROR_TOLERANCE_M_S = 1.0
+
+# A full turn of the heading, which the vehicle only reaches by flying a whole loiter.
+HEADING_SWEEP_DEG = 360.0
+
+
+@pytest.mark.optical_flow_vtol
+def test_loiter_tracks_ground_truth_velocity(tester_flow: AutopilotTesterFlow) -> None:
+ tester = tester_flow
+
+ # The ground truth is streamed independently of the estimator, so home can be stored before
+ # there is any global position, and it is the position the EKF origin gets set to.
+ tester.store_home()
+ tester.set_ekf_origin_to_home()
+
+ tester.wait_until_armable()
+ tester.set_takeoff_altitude(FLIGHT_ALTITUDE_M)
+ tester.arm()
+ tester.takeoff()
+ tester.wait_until_hovering()
+ tester.wait_until_altitude(FLIGHT_ALTITUDE_M, 60)
+
+ tester.transition_to_fixedwing()
+ tester.wait_until_fixedwing(10)
+ tester.sleep_for(5) # let the vehicle accelerate to its cruise speed and settle its heading
+
+ # Center the loiter so that the current position and heading are already on and tangent to
+ # the circle: the vehicle joins it immediately instead of flying out to it from the (near-)
+ # center, which would otherwise dominate the ground truth comparison below.
+ loiter_center = tester.tangent_circle_center(LOITER_RADIUS_M)
+
+ tester.command_orbit(loiter_center, LOITER_RADIUS_M, math.nan, FLIGHT_ALTITUDE_M)
+ tester.wait_until_on_circle(loiter_center, LOITER_RADIUS_M, LOITER_CAPTURE_TOLERANCE_M, 180)
+
+ # Brief settle for the lateral guidance to lock onto the new path precisely.
+ tester.sleep_for(3)
+
+ tester.start_ground_truth_comparison()
+
+ # Fly a full circle: the heading of a fixed wing follows the circle, so the sweep doubles as
+ # the "the loiter is really being flown" check.
+ tester.wait_until_heading_swept(HEADING_SWEEP_DEG, 180)
+ tester.stop_ground_truth_comparison()
+
+ tester.check_ground_truth_on_circle(loiter_center, LOITER_RADIUS_M, LOITER_RADIUS_TOLERANCE_M)
+ tester.check_velocity_error_below(MEAN_VELOCITY_ERROR_TOLERANCE_M_S,
+ MAX_VELOCITY_ERROR_TOLERANCE_M_S)