From 440179383a5601032fbca4ef97884286be3cf024 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Henry=20Kotz=C3=A9?= Date: Mon, 9 Nov 2026 20:13:16 +0200 Subject: [PATCH] feat(SIH): Added optical flow sensor continues on #28270 (#29676) * feat(sih): model the distance sensor range limits, quality and noise The simulated distance sensor reported a noise free reading with no signal quality, and saturated only against the upper range limit. Readings below the minimum range were passed through unchanged, which is not what a range finder does. Report the raw saturated value with a zero quality outside of the measurement range, derate the quality over the last 20% of the range, and add configurable Gaussian noise (SIH_DISTSNSR_NOI). The quality is decided on the true distance so that a measurement sitting at a range limit does not flicker in and out of validity. The sensor is now enabled with SIH_DISTSNSR_EN instead of by an out of band value of the override parameter, and the ground distance computation moves into a helper. * feat(sih): add optical flow simulation Simulate a downward facing optical flow sensor in SIH so that flow aided navigation can be exercised without a rendered camera image. The flow rate is the sum of the body rates and of the ground relative velocity scaled by the distance to the ground, integrated over one publication interval and published on sensor_optical_flow with configurable rate (SIH_OF_RATE) and noise (SIH_OF_NOISE). The 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. Quality drops to zero above SIH_OF_H_MAX or when the flow rate exceeds what the sensor can correlate. The topic is advertised before the simulated clock starts, because the sensors module brings up its flow pipeline on the first appearance of the topic, and publishing during startup deadlocks the lockstep clock this thread advances. Add the quadx_flow and standard_vtol_flow models with matching airframes: SYS_AUTOSTART 10046 flies the multicopter on flow only with no GNSS, 10047 fuses the flow next to GNSS on a VTOL. * prune configs Signed-off-by: RomanBapst * more config cleanup Signed-off-by: RomanBapst * added EKF2_HGT_REF to vtol flow config Signed-off-by: RomanBapst * feat(simulation): added tests for optical flow for both quadx and stanard vtol modes and enable them in CI - for the test we monitor error on ground truth position and velocity Signed-off-by: RomanBapst * feat(docs): document addition of optical flow in SIH Signed-off-by: RomanBapst * fix(mavsdk_tests): unsubscribe flow ground truth comparison on destruction The ground truth comparison callbacks capture `this` and touch members of AutopilotTesterFlow. If a REQUIRE fails between start and stop, the tester is destroyed during unwinding while the subscriptions are still active, so a callback could run on the destroyed mutex and sample buffers. Make stop idempotent and call it from the destructor, and stop any running comparison before starting a new one. Assisted-by: Claude:claude-opus-5-5 Signed-off-by: RomanBapst * tests for python * fix(mavsdk_tests): start the VTOL flow loiter already on its radius The loiter was centered on the vehicle's position right after the transition, so it started at the center and had to fly straight out to the commanded radius before the lateral guidance curved onto the track. Assisted-by: Claude:claude-sonnet-5 * fix(mavsdk_tests): resend the flow test EKF origin until it is taken The SITL startup script starts ekf2 after MAVLink. When MAVSDK connected first, the single SET_GPS_GLOBAL_ORIGIN was published before ekf2 existed and MAVSDK's RUN_PREARM_CHECKS followed it. Ekf2 then subscribed, saw only the newest vehicle command and never got an origin. The Coverage build starts slowly enough to hit this every time. Assisted-by: Claude:claude-opus-5-5 * ci(sitl_tests): run the VTOL flow test without coverage As a Coverage job it built with coverage on every PR and uploaded the sitl-vtol flag from PRs, which replaced main's carried-forward VTOL coverage with the flow-only flight. The quadx_flow entry also sat between the Coverage comment and the standard_vtol entry it describes. Assisted-by: Claude:claude-opus-5-5 * fix(mavsdk_tests): sum the flow test heading sweep with its sign Adding up the absolute heading changes counted yaw oscillation toward the 360 degrees, so the wait could end before a full orbit was flown. Assisted-by: Claude:claude-opus-5-5 * refactor(airframes): keep only the non-default params in the SIH flow airframes SIH_OF_H_MAX, SIH_DISTSNSR_EN and EKF2_OF_CTRL were set to their defaults and SENS_EN_GPSSIM is already set by the quadx base airframe. The quadx forced EKF2_RNG_CTRL to 2 while the VTOL used the conditional default for no stated reason, so both now use the default. Assisted-by: Claude:claude-opus-5-5 * docs(sim_sih): mark the SIH flow airframes as new in PX4 v2.0 release/1.18 has branched, so these airframes first ship in v2.0, not v1.17. Assisted-by: Claude:claude-opus-5-5 --------- Signed-off-by: RomanBapst Co-authored-by: RomanBapst Co-authored-by: Jacob Dahl --- .github/workflows/sitl_tests.yml | 2 + .../airframes/10046_sihsim_quadx_flow | 17 + .../airframes/10047_sihsim_standard_vtol_flow | 19 ++ .../init.d-posix/airframes/CMakeLists.txt | 2 + docs/en/sensor/optical_flow.md | 19 ++ docs/en/sim_sih/index.md | 23 +- .../simulation/simulator_sih/CMakeLists.txt | 2 + src/modules/simulation/simulator_sih/sih.cpp | 178 ++++++++++- src/modules/simulation/simulator_sih/sih.hpp | 42 ++- .../simulation/simulator_sih/sih_params.yaml | 73 ++++- test/mavsdk_tests/autopilot_tester_flow.py | 299 ++++++++++++++++++ test/mavsdk_tests/configs/sih-sitl.json | 10 + test/mavsdk_tests/conftest.py | 10 +- test/mavsdk_tests/pytest.ini | 2 + .../test_multicopter_optical_flow.py | 70 ++++ test/mavsdk_tests/test_vtol_optical_flow.py | 80 +++++ 16 files changed, 833 insertions(+), 15 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/10046_sihsim_quadx_flow create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/10047_sihsim_standard_vtol_flow create mode 100644 test/mavsdk_tests/autopilot_tester_flow.py create mode 100644 test/mavsdk_tests/test_multicopter_optical_flow.py create mode 100644 test/mavsdk_tests/test_vtol_optical_flow.py 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)