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)