From dac1b2e99cce7d7bc370c394db18f57e94fd926c Mon Sep 17 00:00:00 2001
From: jfbblue0922 <jfbblue0922@gmail.com>
Date: Thu, 22 Feb 2024 19:03:27 +0900
Subject: [PATCH 01/26] AP_HAL_ChibiOS/hwdef: update JFB110 board definition

---
 libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat | 5 +----
 1 file changed, 1 insertion(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat
index 6b8189410a301..714b561d303cd 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/JFB110/hwdef.dat
@@ -120,11 +120,8 @@ define HAL_HAVE_BOARD_VOLTAGE 1
 PB3 VBUS_RESERVED INPUT
 
 # JFB110 has SERVORAIL ADC
-# Set SENSOR_3.3V power signal insted.
 PC1  SCALED_V3V3 ADC1 SCALE(2)       # ADC1_11
-PA3  FMU_SERVORAIL_VCC ADC1 SCALE(2) # ADC1_15
-define FMU_SERVORAIL_ADC_PIN 15
-define HAL_HAVE_SERVO_VOLTAGE 1
+PA3  FMU_SERVORAIL_VCC_SENS ADC1 SCALE(2) # ADC1_15
 
 PC0  RSSI_IN ADC1 SCALE(1)       # ADC1_10
 define RSSI_ANA_PIN 10

From 749c7428ddf2b29900791101fcca13b52a53e3a2 Mon Sep 17 00:00:00 2001
From: Gone4Dirt <matthewnkear@gmail.com>
Date: Thu, 22 Feb 2024 17:57:27 +0000
Subject: [PATCH 02/26] SITL: FlightAxis: Fix helidemix

---
 libraries/SITL/SIM_FlightAxis.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp
index 36d63e0d67cdc..d60793c006dc6 100644
--- a/libraries/SITL/SIM_FlightAxis.cpp
+++ b/libraries/SITL/SIM_FlightAxis.cpp
@@ -316,10 +316,12 @@ void FlightAxis::exchange_data(const struct sitl_input &input)
         float swash3 = scaled_servos[2];
 
         float roll_rate = swash1 - swash2;
-        float pitch_rate = -((swash1+swash2) / 2.0f - swash3);
+        float pitch_rate = ((swash1+swash2) / 2.0f - swash3);
+        float col = (swash1 + swash2 + swash3) / 3.0;
 
         scaled_servos[0] = constrain_float(roll_rate + 0.5, 0, 1);
         scaled_servos[1] = constrain_float(pitch_rate + 0.5, 0, 1);
+        scaled_servos[2] = constrain_float(col, 0, 1);
     }
 
     const uint16_t channels = hal.scheduler->is_system_initialized()?4095:0;

From a517d5fed18900a6de35dc82eec1568ae0a79eed Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 26 Feb 2024 14:17:54 +1100
Subject: [PATCH 03/26] AP_NavEKF3: correct initialisation of BeaconFusion data
 structure

the fusionReport object is not being cleared when the filter undergoes a forced reset.

So delete and recreate the object.  Will also make this sensitive to the number of beacons changing.

Also don't attempt to allocate zero bytes.
---
 libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
index 714fc41711e91..bc495582acf3e 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
@@ -37,10 +37,11 @@ void NavEKF3_core::BeaconFusion::InitialiseVariables()
     posOffsetMinVar = 0.0f;
     minOffsetStateChangeFilt = 0.0f;
     fuseDataReportIndex = 0;
-    if (AP::dal().beacon()) {
-        if (fusionReport == nullptr) {
-            fusionReport = new BeaconFusion::FusionReport[AP::dal().beacon()->count()];
-        }
+    delete[] fusionReport;
+    fusionReport = nullptr;
+    auto *beacon = AP::dal().beacon();
+    if (beacon != nullptr) {
+        fusionReport = new BeaconFusion::FusionReport[beacon->count()];
     }
     posOffsetNED.zero();
     originEstInit = false;

From 21edc6aee158454651fdad15d47d467f2e35faae Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Mon, 26 Feb 2024 15:09:46 +1100
Subject: [PATCH 04/26] AP_NavEKF3: do not trust number of beacons to not
 change

if the count from the beacon library changes we may end up looking at memory we shouldn't
---
 libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp      |  3 ++-
 libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp | 11 ++++++++---
 libraries/AP_NavEKF3/AP_NavEKF3_core.h           |  1 +
 3 files changed, 11 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
index 1f60ae65f932c..93e3db98d5b90 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Logging.cpp
@@ -238,7 +238,8 @@ void NavEKF3_core::Log_Write_Beacon(uint64_t time_us)
     }
 
     // Ensure that beacons are not skipped due to calling this function at a rate lower than the updates
-    if (rngBcn.fuseDataReportIndex >= rngBcn.N) {
+    if (rngBcn.fuseDataReportIndex >= rngBcn.N ||
+        rngBcn.fuseDataReportIndex > rngBcn.numFusionReports) {
         rngBcn.fuseDataReportIndex = 0;
     }
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
index bc495582acf3e..6fc98f0814829 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp
@@ -39,9 +39,14 @@ void NavEKF3_core::BeaconFusion::InitialiseVariables()
     fuseDataReportIndex = 0;
     delete[] fusionReport;
     fusionReport = nullptr;
+    numFusionReports = 0;
     auto *beacon = AP::dal().beacon();
     if (beacon != nullptr) {
-        fusionReport = new BeaconFusion::FusionReport[beacon->count()];
+        const uint8_t count = beacon->count();
+        fusionReport = new BeaconFusion::FusionReport[count];
+        if (fusionReport != nullptr) {
+            numFusionReports = count;
+        }
     }
     posOffsetNED.zero();
     originEstInit = false;
@@ -311,7 +316,7 @@ void NavEKF3_core::FuseRngBcn()
         }
 
         // Update the fusion report
-        if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) {
+        if (rngBcn.dataDelayed.beacon_ID < rngBcn.numFusionReports) {
             auto &report = rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID];
             report.beaconPosNED = rngBcn.dataDelayed.beacon_posNED;
             report.innov = rngBcn.innov;
@@ -550,7 +555,7 @@ void NavEKF3_core::FuseRngBcnStatic()
             rngBcn.alignmentCompleted = true;
         }
         // Update the fusion report
-        if (rngBcn.fusionReport && rngBcn.dataDelayed.beacon_ID < dal.beacon()->count()) {
+        if (rngBcn.dataDelayed.beacon_ID < rngBcn.numFusionReports) {
             auto &report = rngBcn.fusionReport[rngBcn.dataDelayed.beacon_ID];
             report.beaconPosNED = rngBcn.dataDelayed.beacon_posNED;
             report.innov = rngBcn.innov;
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
index d9304f12383cf..a59de0a82dcc0 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h
@@ -1372,6 +1372,7 @@ class NavEKF3_core : public NavEKF_core_common
             ftype testRatio;    // innovation consistency test ratio
             Vector3F beaconPosNED; // beacon NED position
         } *fusionReport;
+        uint8_t numFusionReports;
     } rngBcn;
 #endif  // if EK3_FEATURE_BEACON_FUSION
 

From 2cb5f434fdc787013673968282d303b5a0e2b089 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 27 Feb 2024 14:56:34 +1100
Subject: [PATCH 05/26] AP_Filesystem: don't show directory entries for empty @
 filesystems

---
 libraries/AP_Filesystem/AP_Filesystem.cpp | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp
index 45c58654bf786..b7642da19cfcf 100644
--- a/libraries/AP_Filesystem/AP_Filesystem.cpp
+++ b/libraries/AP_Filesystem/AP_Filesystem.cpp
@@ -242,6 +242,14 @@ struct dirent *AP_Filesystem::readdir(DirHandle *dirp)
         if (prefix[0] != '@') {
             continue;
         }
+
+        // only return @ entries in root if we can successfully opendir them:
+        auto *d = backends[virtual_dirent.backend_ofs].fs.opendir("");
+        if (d == nullptr) {
+            continue;
+        }
+        backends[virtual_dirent.backend_ofs].fs.closedir(d);
+
         // found a virtual directory we haven't returned yet
         strncpy_noterm(virtual_dirent.de.d_name, prefix, sizeof(virtual_dirent.de.d_name));
         virtual_dirent.d_off++;

From b359ec89a5cc296311b512972ce9db4139a1a11b Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Mon, 26 Feb 2024 14:30:07 +0000
Subject: [PATCH 06/26] AP_HAL_ChibiOS: use standard FPV config for
 SpeedyBeeF405WING

---
 .../hwdef/SpeedyBeeF405WING/hwdef.dat         | 59 +++++--------------
 1 file changed, 14 insertions(+), 45 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat
index f6842833c8d38..73a85c5dd3456 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/SpeedyBeeF405WING/hwdef.dat
@@ -169,50 +169,19 @@ PC13 PINIO1 OUTPUT GPIO(81) LOW
 
 define STM32_PWM_USE_ADVANCED TRUE
 
-define HAL_WITH_DSP FALSE
-
-# disable parachute and sprayer to save flash
-define HAL_PARACHUTE_ENABLED 0
-define HAL_SPRAYER_ENABLED 0
-define AP_GRIPPER_ENABLED 0
-define HAL_GENERATOR_ENABLED 0
-define AP_ICENGINE_ENABLED 0
-#define LANDING_GEAR_ENABLED 0
-define WINCH_ENABLED 0
-define HAL_ADSB_ENABLED  0
-
-define AC_OAPATHPLANNER_ENABLED 0
-define PRECISION_LANDING 0
-#define HAL_BARO_WIND_COMP_ENABLED 0
-define AP_OPTICALFLOW_ENABLED 0
-
-
-# Disable un-needed hardware drivers
-define HAL_WITH_ESC_TELEM 0
-define AP_FETTEC_ONEWIRE_ENABLED 0
-
-define AP_VOLZ_ENABLED 0
-define AP_ROBOTISSERVO_ENABLE 0
-define HAL_PICCOLO_CAN_ENABLE 0
-define HAL_TORQEEDO_ENABLED 0
-define HAL_RUNCAM_ENABLED 0
-define HAL_HOTT_TELEM_ENABLED 0
-define HAL_NMEA_OUTPUT_ENABLED 0
-define HAL_BUTTON_ENABLED 0
-define AP_NOTIFY_OREOLED_ENABLED 0
-
-#only support MS4525 ANALOG ASP5033 driver
-define AP_AIRSPEED_BACKEND_DEFAULT_ENABLED 0
-define AP_AIRSPEED_MS4525_ENABLED 1
-define AP_AIRSPEED_ANALOG_ENABLED 1
-define AP_AIRSPEED_ASP5033_ENABLED 1
-
-#only support UBLOX and NMEA GPS driver
-define AP_GPS_BACKEND_DEFAULT_ENABLED 0 
-define AP_GPS_UBLOX_ENABLED 1
-define AP_GPS_NMEA_ENABLED 1
-define HAL_MSP_GPS_ENABLED 1
-
-define AP_TRAMP_ENABLED 1
+# minimal drivers to reduce flash usage
+include ../include/minimize_fpv_osd.inc
+
+undef AP_CAMERA_MOUNT_ENABLED
+undef AP_LANDINGGEAR_ENABLED
+undef HAL_MOUNT_ENABLED
+undef HAL_MOUNT_SERVO_ENABLED
+undef QAUTOTUNE_ENABLED
+
+define AP_CAMERA_MOUNT_ENABLED 1
+define AP_LANDINGGEAR_ENABLED 1
+define HAL_MOUNT_ENABLED 1
+define HAL_MOUNT_SERVO_ENABLED 1
+define QAUTOTUNE_ENABLED 1
 
 define DEFAULT_NTF_LED_TYPES 257

From de657689ce6678357e5d3838e1c46ceb8c6bf12e Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 21 Feb 2024 08:53:30 +0900
Subject: [PATCH 07/26] SITL: vicon param descriptions

---
 libraries/SITL/SITL.cpp | 83 +++++++++++++++++++++++++++++++++++++----
 1 file changed, 76 insertions(+), 7 deletions(-)

diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 46ff0cba1294f..f9c0b4b7afc3b 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -332,28 +332,97 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
     // Scenario for thermalling simulation, for soaring
     AP_GROUPINFO("THML_SCENARI",  12, SIM,  thermal_scenario, 0),
 
-    // vicon sensor position (position offsets in body frame)
+    // @Param: VICON_POS_X
+    // @DisplayName: SITL vicon position on vehicle in Forward direction
+    // @Description: SITL vicon position on vehicle in Forward direction
+    // @Units: m
+    // @Range: 0 10
+    // @User: Advanced
+
+    // @Param: VICON_POS_Y
+    // @DisplayName: SITL vicon position on vehicle in Right direction
+    // @Description: SITL vicon position on vehicle in Right direction
+    // @Units: m
+    // @Range: 0 10
+    // @User: Advanced
+
+    // @Param: VICON_POS_Z
+    // @DisplayName: SITL vicon position on vehicle in Down direction
+    // @Description: SITL vicon position on vehicle in Down direction
+    // @Units: m
+    // @Range: 0 10
+    // @User: Advanced    
     AP_GROUPINFO("VICON_POS",     14, SIM,  vicon_pos_offset, 0),
 
     // Buyoancy for submarines
     AP_GROUPINFO_FRAME("BUOYANCY", 15, SIM, buoyancy, 1, AP_PARAM_FRAME_SUB),
 
-    // vicon glitch in NED frame
+    // @Param: VICON_GLIT_X
+    // @DisplayName: SITL vicon position glitch North
+    // @Description: SITL vicon position glitch North
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: VICON_GLIT_Y
+    // @DisplayName: SITL vicon position glitch East
+    // @Description: SITL vicon position glitch East
+    // @Units: m
+    // @User: Advanced
+
+    // @Param: VICON_GLIT_Z
+    // @DisplayName: SITL vicon position glitch Down
+    // @Description: SITL vicon position glitch Down
+    // @Units: m
+    // @User: Advanced
     AP_GROUPINFO("VICON_GLIT",    16, SIM,  vicon_glitch, 0),
 
-    // vicon failure
+    // @Param: VICON_FAIL
+    // @DisplayName: SITL vicon failure
+    // @Description: SITL vicon failure
+    // @Values: 0:Vicon Healthy, 1:Vicon Failed
+    // @User: Advanced
     AP_GROUPINFO("VICON_FAIL",    17, SIM,  vicon_fail, 0),
 
-    // vicon yaw (in earth frame)
+    // @Param: VICON_YAW
+    // @DisplayName: SITL vicon yaw angle in earth frame
+    // @Description: SITL vicon yaw angle in earth frame
+    // @Units: deg
+    // @Range: 0 360
+    // @User: Advanced
     AP_GROUPINFO("VICON_YAW",     18, SIM,  vicon_yaw, 0),
 
-    // vicon yaw error in degrees (added to reported yaw sent to vehicle)
+    // @Param: VICON_YAWERR
+    // @DisplayName: SITL vicon yaw error
+    // @Description: SITL vicon yaw added to reported yaw sent to vehicle
+    // @Units: deg
+    // @Range: -180 180
+    // @User: Advanced
     AP_GROUPINFO("VICON_YAWERR",  19, SIM,  vicon_yaw_error, 0),
 
-    // vicon message type mask
+    // @Param: VICON_TMASK
+    // @DisplayName: SITL vicon type mask
+    // @Description: SITL vicon messages sent
+    // @Bitmask: 0:VISION_POSITION_ESTIMATE, 1:VISION_SPEED_ESTIMATE, 2:VICON_POSITION_ESTIMATE, 3:VISION_POSITION_DELTA, 4:ODOMETRY
+    // @User: Advanced
     AP_GROUPINFO("VICON_TMASK",   20, SIM,  vicon_type_mask, 3),
 
-    // vicon velocity glitch in NED frame
+    // @Param: VICON_VGLI_X
+    // @DisplayName: SITL vicon velocity glitch North
+    // @Description: SITL vicon velocity glitch North
+    // @Units: m/s
+    // @User: Advanced
+
+    // @Param: VICON_VGLI_Y
+    // @DisplayName: SITL vicon velocity glitch East
+    // @Description: SITL vicon velocity glitch East
+    // @Units: m/s
+    // @User: Advanced
+
+    // @Param: VICON_VGLI_Z
+    // @DisplayName: SITL vicon velocity glitch Down
+    // @Description: SITL vicon velocity glitch Down
+    // @Units: m/s
+    // @User: Advanced
     AP_GROUPINFO("VICON_VGLI",    21, SIM,  vicon_vel_glitch, 0),
 
     AP_GROUPINFO("RATE_HZ",  22, SIM,  loop_rate_hz, SIM_RATE_HZ_DEFAULT),

From e1e7882b7b3eea90111bcedeed0d82089deb4405 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 21 Feb 2024 14:13:58 +0900
Subject: [PATCH 08/26] Tools: remove vicon params from whitelist

---
 Tools/autotest/vehicle_test_suite.py | 13 -------------
 1 file changed, 13 deletions(-)

diff --git a/Tools/autotest/vehicle_test_suite.py b/Tools/autotest/vehicle_test_suite.py
index e886a7f1682ee..4d51a6a9b9c65 100644
--- a/Tools/autotest/vehicle_test_suite.py
+++ b/Tools/autotest/vehicle_test_suite.py
@@ -2438,19 +2438,6 @@ def get_sim_parameter_documentation_get_whitelist(self):
             "SIM_VIB_MOT_MASK",
             "SIM_VIB_MOT_MAX",
             "SIM_VIB_MOT_MULT",
-            "SIM_VICON_FAIL",
-            "SIM_VICON_GLIT_X",
-            "SIM_VICON_GLIT_Y",
-            "SIM_VICON_GLIT_Z",
-            "SIM_VICON_POS_X",
-            "SIM_VICON_POS_Y",
-            "SIM_VICON_POS_Z",
-            "SIM_VICON_TMASK",
-            "SIM_VICON_VGLI_X",
-            "SIM_VICON_VGLI_Y",
-            "SIM_VICON_VGLI_Z",
-            "SIM_VICON_YAW",
-            "SIM_VICON_YAWERR",
             "SIM_WAVE_AMP",
             "SIM_WAVE_DIR",
             "SIM_WAVE_ENABLE",

From 81b0d599a8d40edb7c7023baee0b2bc6305e08fa Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 21 Feb 2024 08:55:51 +0900
Subject: [PATCH 09/26] SITL: vicon send odometry with quality of 50

---
 libraries/SITL/SIM_Vicon.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/libraries/SITL/SIM_Vicon.cpp b/libraries/SITL/SIM_Vicon.cpp
index b54374baf8752..c2c4d29da3f43 100644
--- a/libraries/SITL/SIM_Vicon.cpp
+++ b/libraries/SITL/SIM_Vicon.cpp
@@ -233,7 +233,8 @@ void Vicon::update_vicon_position_estimate(const Location &loc,
             MAV_FRAME_LOCAL_FRD,
             MAV_FRAME_BODY_FRD,
             0,
-            MAV_ESTIMATOR_TYPE_VIO
+            MAV_ESTIMATOR_TYPE_VIO,
+            50 // quality hardcoded to 50%
         };
         mavlink_msg_odometry_encode_status(
             system_id,

From d1360c14f8e2b0ccfc545c57f5b771dfbb7f6ec7 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 20 Feb 2024 16:08:30 +0900
Subject: [PATCH 10/26] AP_VisualOdom: use Odometry quality

---
 libraries/AP_VisualOdom/AP_VisualOdom.cpp     | 35 +++++++++++++++----
 libraries/AP_VisualOdom/AP_VisualOdom.h       | 16 +++++++--
 .../AP_VisualOdom/AP_VisualOdom_Backend.h     | 17 ++++++---
 .../AP_VisualOdom/AP_VisualOdom_IntelT265.cpp | 20 +++++++----
 .../AP_VisualOdom/AP_VisualOdom_IntelT265.h   |  6 ++--
 .../AP_VisualOdom/AP_VisualOdom_Logging.cpp   | 10 +++---
 libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp | 32 ++++++++++++-----
 libraries/AP_VisualOdom/AP_VisualOdom_MAV.h   |  6 ++--
 libraries/AP_VisualOdom/LogStructure.h        | 14 +++++---
 9 files changed, 116 insertions(+), 40 deletions(-)

diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.cpp b/libraries/AP_VisualOdom/AP_VisualOdom.cpp
index d8cbcb05c48d7..35c97d4875ce9 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom.cpp
+++ b/libraries/AP_VisualOdom/AP_VisualOdom.cpp
@@ -106,6 +106,14 @@ const AP_Param::GroupInfo AP_VisualOdom::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_YAW_M_NSE", 7, AP_VisualOdom, _yaw_noise, 0.2f),
 
+    // @Param: _QUAL_MIN
+    // @DisplayName: Visual odometry minimum quality
+    // @Description: Visual odometry will only be sent to EKF if over this value. -1 to always send (even bad values), 0 to send if good or unknown
+    // @Units: %
+    // @Range: -1 100
+    // @User: Advanced
+    AP_GROUPINFO("_QUAL_MIN", 8, AP_VisualOdom, _quality_min, 0),
+
     AP_GROUPEND
 };
 
@@ -161,6 +169,16 @@ bool AP_VisualOdom::healthy() const
     return _driver->healthy();
 }
 
+// return quality as a measure from 0 ~ 100
+// -1 means failed, 0 means unknown, 1 is worst, 100 is best
+int8_t AP_VisualOdom::quality() const
+{
+    if (_driver == nullptr) {
+        return 0;
+    }
+    return _driver->quality();
+}
+
 #if HAL_GCS_ENABLED
 // consume vision_position_delta mavlink messages
 void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &msg)
@@ -179,7 +197,8 @@ void AP_VisualOdom::handle_vision_position_delta_msg(const mavlink_message_t &ms
 
 // general purpose method to consume position estimate data and send to EKF
 // distances in meters, roll, pitch and yaw are in radians
-void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter)
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
 {
     // exit immediately if not enabled
     if (!enabled()) {
@@ -191,12 +210,13 @@ void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_
         // convert attitude to quaternion and call backend
         Quaternion attitude;
         attitude.from_euler(roll, pitch, yaw);
-        _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
+        _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);
     }
 }
 
 // general purpose method to consume position estimate data and send to EKF
-void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
 {
     // exit immediately if not enabled
     if (!enabled()) {
@@ -205,11 +225,14 @@ void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_
 
     // call backend
     if (_driver != nullptr) {
-        _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter);
+        _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);
     }
 }
 
-void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter)
+// general purpose methods to consume velocity estimate data and send to EKF
+// velocity in NED meters per second
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
 {
     // exit immediately if not enabled
     if (!enabled()) {
@@ -218,7 +241,7 @@ void AP_VisualOdom::handle_vision_speed_estimate(uint64_t remote_time_us, uint32
 
     // call backend
     if (_driver != nullptr) {
-        _driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter);
+        _driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter, quality);
     }
 }
 
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom.h b/libraries/AP_VisualOdom/AP_VisualOdom.h
index 47606abb97f81..3f7323bf7d2f8 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom.h
+++ b/libraries/AP_VisualOdom/AP_VisualOdom.h
@@ -83,6 +83,13 @@ class AP_VisualOdom
     // return yaw measurement noise in rad
     float get_yaw_noise() const { return _yaw_noise; }
 
+    // return quality threshold
+    int8_t get_quality_min() const { return _quality_min; }
+
+    // return quality as a measure from -1 ~ 100
+    // -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    int8_t quality() const;
+
 #if HAL_GCS_ENABLED
     // consume vision_position_delta mavlink messages
     void handle_vision_position_delta_msg(const mavlink_message_t &msg);
@@ -90,12 +97,14 @@ class AP_VisualOdom
 
     // general purpose methods to consume position estimate data and send to EKF
     // distances in meters, roll, pitch and yaw are in radians
-    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter);
-    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter);
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality);
+    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality);
     
     // general purpose methods to consume velocity estimate data and send to EKF
     // velocity in NED meters per second
-    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter);
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality);
 
     // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
     void request_align_yaw_to_ahrs();
@@ -126,6 +135,7 @@ class AP_VisualOdom
     AP_Float _vel_noise;        // velocity measurement noise in m/s
     AP_Float _pos_noise;        // position measurement noise in meters
     AP_Float _yaw_noise;        // yaw measurement noise in radians
+    AP_Int8 _quality_min;       // positions and velocities will only be sent to EKF if over this value.  if 0 all values sent to EKF
 
     // reference to backends
     AP_VisualOdom_Backend *_driver;
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
index 9f5225aa9e9a4..c669b8babb329 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_Backend.h
@@ -29,16 +29,22 @@ class AP_VisualOdom_Backend
     // return true if sensor is basically healthy (we are receiving data)
     bool healthy() const;
 
+    // return quality as a measure from -1 ~ 100
+    // -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    int8_t quality() const { return _quality; }
+
 #if HAL_GCS_ENABLED
     // consume vision_position_delta mavlink messages
     void handle_vision_position_delta_msg(const mavlink_message_t &msg);
 #endif
 
     // consume vision pose estimate data and send to EKF. distances in meters
-    virtual void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) = 0;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    virtual void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality) = 0;
 
     // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
-    virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) = 0;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    virtual void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) = 0;
 
     // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
     virtual void request_align_yaw_to_ahrs() {}
@@ -62,8 +68,8 @@ class AP_VisualOdom_Backend
 #if HAL_LOGGING_ENABLED
     // Logging Functions
     void Write_VisualOdom(float time_delta, const Vector3f &angle_delta, const Vector3f &position_delta, float confidence);
-    void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored);
-    void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored);
+    void Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored, int8_t quality);
+    void Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality);
 #endif
 
     AP_VisualOdom &_frontend;   // reference to frontend
@@ -72,6 +78,9 @@ class AP_VisualOdom_Backend
     // reset counter handling
     uint8_t _last_reset_counter;    // last sensor reset counter received
     uint32_t _reset_timestamp_ms;   // time reset counter was received
+
+    // quality
+    int8_t _quality;                // last recorded quality
 };
 
 #endif  // HAL_VISUALODOM_ENABLED
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
index cc063d8590104..c26bd38b715f1 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp
@@ -28,7 +28,8 @@
 extern const AP_HAL::HAL& hal;
 
 // consume vision pose estimate data and send to EKF. distances in meters
-void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
 {
     const float scale_factor = _frontend.get_pos_scale();
     Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
@@ -59,8 +60,11 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
     posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
     angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
 
+    // record quality
+    _quality = quality;
+
     // check for recent position reset
-    bool consume = should_consume_sensor_data(true, reset_counter);
+    bool consume = should_consume_sensor_data(true, reset_counter) && (_quality >= _frontend.get_quality_min());
     if (consume) {
         // send attitude and position to EKF
         AP::ahrs().writeExtNavData(pos, att, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
@@ -74,7 +78,7 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
 
 #if HAL_LOGGING_ENABLED
     // log sensor data
-    Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume);
+    Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), wrap_360(degrees(yaw)), posErr, angErr, reset_counter, !consume, _quality);
 #endif
 
     // store corrected attitude for use in pre-arm checks
@@ -85,14 +89,18 @@ void AP_VisualOdom_IntelT265::handle_pose_estimate(uint64_t remote_time_us, uint
 }
 
 // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
-void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter)
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
 {
     // rotate velocity to align with vehicle
     Vector3f vel_corrected = vel;
     rotate_velocity(vel_corrected);
 
+    // record quality
+    _quality = quality;
+
     // check for recent position reset
-    bool consume = should_consume_sensor_data(false, reset_counter);
+    bool consume = should_consume_sensor_data(false, reset_counter) && (_quality >= _frontend.get_quality_min());
     if (consume) {
         // send velocity to EKF
         AP::ahrs().writeExtNavVelData(vel_corrected, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
@@ -102,7 +110,7 @@ void AP_VisualOdom_IntelT265::handle_vision_speed_estimate(uint64_t remote_time_
     _last_update_ms = AP_HAL::millis();
 
 #if HAL_LOGGING_ENABLED
-    Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume);
+    Write_VisualVelocity(remote_time_us, time_ms, vel_corrected, reset_counter, !consume, _quality);
 #endif
 }
 
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
index 8a78bd1b7ae37..6103ec05dfa1b 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.h
@@ -14,10 +14,12 @@ class AP_VisualOdom_IntelT265 : public AP_VisualOdom_Backend
     using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
 
     // consume vision pose estimate data and send to EKF. distances in meters
-    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality) override;
 
     // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
-    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) override;
 
     // request sensor's yaw be aligned with vehicle's AHRS/EKF attitude
     void request_align_yaw_to_ahrs() override { _align_yaw = true; }
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp
index 4a92a983606f8..80b9e1d70f182 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_Logging.cpp
@@ -24,7 +24,7 @@ void AP_VisualOdom_Backend::Write_VisualOdom(float time_delta, const Vector3f &a
 }
 
 // Write visual position sensor data.  x,y,z are in meters, angles are in degrees
-void AP_VisualOdom_Backend::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored)
+void AP_VisualOdom_Backend::Write_VisualPosition(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float pos_err, float ang_err, uint8_t reset_counter, bool ignored, int8_t quality)
 {
     const struct log_VisualPosition pkt_visualpos {
         LOG_PACKET_HEADER_INIT(LOG_VISUALPOS_MSG),
@@ -40,13 +40,14 @@ void AP_VisualOdom_Backend::Write_VisualPosition(uint64_t remote_time_us, uint32
         pos_err         : pos_err,
         ang_err         : ang_err,
         reset_counter   : reset_counter,
-        ignored         : (uint8_t)ignored
+        ignored         : (uint8_t)ignored,
+        quality         : quality
     };
     AP::logger().WriteBlock(&pkt_visualpos, sizeof(log_VisualPosition));
 }
 
 // Write visual velocity sensor data, velocity in NED meters per second
-void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored)
+void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, bool ignored, int8_t quality)
 {
     const struct log_VisualVelocity pkt_visualvel {
         LOG_PACKET_HEADER_INIT(LOG_VISUALVEL_MSG),
@@ -58,7 +59,8 @@ void AP_VisualOdom_Backend::Write_VisualVelocity(uint64_t remote_time_us, uint32
         vel_z           : vel.z,
         vel_err         : _frontend.get_vel_noise(),
         reset_counter   : reset_counter,
-        ignored         : (uint8_t)ignored
+        ignored         : (uint8_t)ignored,
+        quality         : quality
     };
     AP::logger().WriteBlock(&pkt_visualvel, sizeof(log_VisualVelocity));
 }
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
index 5e4af959fabcd..48b6ec04e0a57 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.cpp
@@ -24,15 +24,23 @@
 #include <AP_Logger/AP_Logger.h>
 
 // consume vision pose estimate data and send to EKF. distances in meters
-void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter)
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
 {
     const float scale_factor =  _frontend.get_pos_scale();
     Vector3f pos{x * scale_factor, y * scale_factor, z * scale_factor};
 
     posErr = constrain_float(posErr, _frontend.get_pos_noise(), 100.0f);
     angErr = constrain_float(angErr, _frontend.get_yaw_noise(), 1.5f);
-    // send attitude and position to EKF
-    AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
+
+    // record quality
+    _quality = quality;
+
+    // send attitude and position to EKF if quality OK
+    bool consume = (_quality >= _frontend.get_quality_min());
+    if (consume) {
+        AP::ahrs().writeExtNavData(pos, attitude, posErr, angErr, time_ms, _frontend.get_delay_ms(), get_reset_timestamp_ms(reset_counter));
+    }
 
     // calculate euler orientation for logging
     float roll;
@@ -42,23 +50,31 @@ void AP_VisualOdom_MAV::handle_pose_estimate(uint64_t remote_time_us, uint32_t t
 
 #if HAL_LOGGING_ENABLED
     // log sensor data
-    Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, false);
+    Write_VisualPosition(remote_time_us, time_ms, pos.x, pos.y, pos.z, degrees(roll), degrees(pitch), degrees(yaw), posErr, angErr, reset_counter, !consume, _quality);
 #endif
 
     // record time for health monitoring
     _last_update_ms = AP_HAL::millis();
 }
 
-void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter)
+// consume vision velocity estimate data and send to EKF, velocity in NED meters per second
+// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+void AP_VisualOdom_MAV::handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality)
 {
-    // send velocity to EKF
-    AP::ahrs().writeExtNavVelData(vel, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
+    // record quality
+    _quality = quality;
+
+    // send velocity to EKF if quality OK
+    bool consume = (_quality >= _frontend.get_quality_min());
+    if (consume) {
+        AP::ahrs().writeExtNavVelData(vel, _frontend.get_vel_noise(), time_ms, _frontend.get_delay_ms());
+    }
 
     // record time for health monitoring
     _last_update_ms = AP_HAL::millis();
 
 #if HAL_LOGGING_ENABLED
-    Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, false);
+    Write_VisualVelocity(remote_time_us, time_ms, vel, reset_counter, !consume, _quality);
 #endif
 }
 
diff --git a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
index 18d5fed7096fd..c1d1c131dc4ff 100644
--- a/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
+++ b/libraries/AP_VisualOdom/AP_VisualOdom_MAV.h
@@ -14,10 +14,12 @@ class AP_VisualOdom_MAV : public AP_VisualOdom_Backend
     using AP_VisualOdom_Backend::AP_VisualOdom_Backend;
 
     // consume vision pose estimate data and send to EKF. distances in meters
-    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter) override;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality) override;
 
     // consume vision velocity estimate data and send to EKF, velocity in NED meters per second
-    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter) override;
+    // quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
+    void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality) override;
 };
 
 #endif  // AP_VISUALODOM_MAV_ENABLED
diff --git a/libraries/AP_VisualOdom/LogStructure.h b/libraries/AP_VisualOdom/LogStructure.h
index cb15e181c4b49..776c3f82b53d2 100644
--- a/libraries/AP_VisualOdom/LogStructure.h
+++ b/libraries/AP_VisualOdom/LogStructure.h
@@ -40,13 +40,14 @@ struct PACKED log_VisualOdom {
 // @Field: PX: Position X-axis (North-South)
 // @Field: PY: Position Y-axis (East-West)
 // @Field: PZ: Position Z-axis (Down-Up)
-// @Field: Roll: Roll lean angle
-// @Field: Pitch: Pitch lean angle
-// @Field: Yaw: Yaw angle
+// @Field: R: Roll lean angle
+// @Field: P: Pitch lean angle
+// @Field: Y: Yaw angle
 // @Field: PErr: Position estimate error
 // @Field: AErr: Attitude estimate error
 // @Field: Rst: Position reset counter
 // @Field: Ign: Ignored
+// @Field: Q: Quality
 struct PACKED log_VisualPosition {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -62,6 +63,7 @@ struct PACKED log_VisualPosition {
     float ang_err;  // radians
     uint8_t reset_counter;
     uint8_t ignored;
+    int8_t quality;
 };
 
 // @LoggerMessage: VISV
@@ -75,6 +77,7 @@ struct PACKED log_VisualPosition {
 // @Field: VErr: Velocity estimate error
 // @Field: Rst: Velocity reset counter
 // @Field: Ign: Ignored
+// @Field: Q: Quality
 struct PACKED log_VisualVelocity {
     LOG_PACKET_HEADER;
     uint64_t time_us;
@@ -86,6 +89,7 @@ struct PACKED log_VisualVelocity {
     float vel_err;
     uint8_t reset_counter;
     uint8_t ignored;
+    int8_t quality;
 };
 
 #if HAL_VISUALODOM_ENABLED
@@ -93,9 +97,9 @@ struct PACKED log_VisualVelocity {
     { LOG_VISUALODOM_MSG, sizeof(log_VisualOdom), \
       "VISO", "Qffffffff", "TimeUS,dt,AngDX,AngDY,AngDZ,PosDX,PosDY,PosDZ,conf", "ssrrrmmm-", "FF000000-" }, \
     { LOG_VISUALPOS_MSG, sizeof(log_VisualPosition), \
-      "VISP", "QQIffffffffBB", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,Roll,Pitch,Yaw,PErr,AErr,Rst,Ign", "sssmmmddhmd--", "FFC00000000--" }, \
+      "VISP", "QQIffffffffBBb", "TimeUS,RTimeUS,CTimeMS,PX,PY,PZ,R,P,Y,PErr,AErr,Rst,Ign,Q", "sssmmmddhmd--%", "FFC00000000--0" }, \
     { LOG_VISUALVEL_MSG, sizeof(log_VisualVelocity), \
-      "VISV", "QQIffffBB", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,Rst,Ign", "sssnnnn--", "FFC0000--" },
+      "VISV", "QQIffffBBb", "TimeUS,RTimeUS,CTimeMS,VX,VY,VZ,VErr,Rst,Ign,Q", "sssnnnn--%", "FFC0000--0" },
 #else
 #define LOG_STRUCTURE_FROM_VISUALODOM
 #endif

From 442c90ff1b4c5fd0c84ba792d25199eb03acd689 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 20 Feb 2024 16:09:08 +0900
Subject: [PATCH 11/26] GCS_MAVLink: send Odometry quality to VisualOdom

---
 libraries/GCS_MAVLink/GCS_Common.cpp | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index a907c003f3d6d..a4d700c6d478e 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -3694,12 +3694,12 @@ void GCS_MAVLINK::handle_odometry(const mavlink_message_t &msg)
     }
 
     const uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.time_usec, PAYLOAD_SIZE(chan, ODOMETRY));
-    visual_odom->handle_pose_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, q, posErr, angErr, m.reset_counter);
+    visual_odom->handle_pose_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, q, posErr, angErr, m.reset_counter, m.quality);
 
     // convert velocity vector from FRD to NED frame
     Vector3f vel{m.vx, m.vy, m.vz};
     vel = q * vel;
-    visual_odom->handle_vision_speed_estimate(m.time_usec, timestamp_ms, vel, m.reset_counter);
+    visual_odom->handle_vision_speed_estimate(m.time_usec, timestamp_ms, vel, m.reset_counter, m.quality);
 }
 
 // there are several messages which all have identical fields in them.
@@ -3731,7 +3731,7 @@ void GCS_MAVLINK::handle_common_vision_position_estimate_data(const uint64_t use
         angErr = cbrtf(sq(covariance[15])+sq(covariance[18])+sq(covariance[20]));
     }
 
-    visual_odom->handle_pose_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, posErr, angErr, reset_counter);
+    visual_odom->handle_pose_estimate(usec, timestamp_ms, x, y, z, roll, pitch, yaw, posErr, angErr, reset_counter, 0);
 }
 
 void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)
@@ -3747,7 +3747,7 @@ void GCS_MAVLINK::handle_att_pos_mocap(const mavlink_message_t &msg)
         return;
     }
     // note: att_pos_mocap does not include reset counter
-    visual_odom->handle_pose_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, m.q, 0, 0, 0);
+    visual_odom->handle_pose_estimate(m.time_usec, timestamp_ms, m.x, m.y, m.z, m.q, 0, 0, 0, 0);
 }
 
 void GCS_MAVLINK::handle_vision_speed_estimate(const mavlink_message_t &msg)
@@ -3760,7 +3760,7 @@ void GCS_MAVLINK::handle_vision_speed_estimate(const mavlink_message_t &msg)
     mavlink_msg_vision_speed_estimate_decode(&msg, &m);
     const Vector3f vel = {m.x, m.y, m.z};
     uint32_t timestamp_ms = correct_offboard_timestamp_usec_to_ms(m.usec, PAYLOAD_SIZE(chan, VISION_SPEED_ESTIMATE));
-    visual_odom->handle_vision_speed_estimate(m.usec, timestamp_ms, vel, m.reset_counter);
+    visual_odom->handle_vision_speed_estimate(m.usec, timestamp_ms, vel, m.reset_counter, 0);
 }
 #endif  // HAL_VISUALODOM_ENABLED
 

From 6c5425cd68f8343434eaefef9709a7ac6e9a04d7 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 20 Feb 2024 16:14:21 +0900
Subject: [PATCH 12/26] AP_DDS: send quality of zero to AP_VisualOdom

---
 libraries/AP_DDS/AP_DDS_External_Odom.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/AP_DDS/AP_DDS_External_Odom.cpp b/libraries/AP_DDS/AP_DDS_External_Odom.cpp
index 8d8563e5e866c..7a868e14a5194 100644
--- a/libraries/AP_DDS/AP_DDS_External_Odom.cpp
+++ b/libraries/AP_DDS/AP_DDS_External_Odom.cpp
@@ -40,7 +40,7 @@ void AP_DDS_External_Odom::handle_external_odom(const tf2_msgs_msg_TFMessage& ms
         const uint8_t reset_counter {0};
         // TODO implement jitter correction similar to GCS_MAVLINK::correct_offboard_timestamp_usec_to_ms(remote_time_us, sizeof(msg));
         const uint32_t time_ms {static_cast<uint32_t>(remote_time_us * 1E-3)};
-        visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter);
+        visual_odom->handle_pose_estimate(remote_time_us, time_ms, ap_position.x, ap_position.y, ap_position.z, ap_rotation, posErr, angErr, reset_counter, 0);
 
     }
 }

From f2b66fd179d5ae504631d594c7c5bda79ffbb6ae Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 20 Feb 2024 16:14:47 +0900
Subject: [PATCH 13/26] AP_Scripting: add VisualOdom bindings

---
 libraries/AP_Scripting/docs/docs.lua                 | 12 ++++++++++++
 .../AP_Scripting/generator/description/bindings.desc |  6 ++++++
 2 files changed, 18 insertions(+)

diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua
index f3481947289f9..7375e0a0d273a 100644
--- a/libraries/AP_Scripting/docs/docs.lua
+++ b/libraries/AP_Scripting/docs/docs.lua
@@ -3448,3 +3448,15 @@ function networking:get_netmask_active() end
 -- desc
 ---@return uint32_t_ud
 function networking:get_ip_active() end
+
+-- visual odometry object
+--@class visual_odom
+visual_odom = {}
+
+-- visual odometry health
+---@return boolean
+function visual_odom:healthy() end
+
+-- visual odometry quality as a percentage from 1 to 100 or 0 if unknown
+---@return integer
+function visual_odom:quality() end
diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc
index c9e9867df5179..8e01c48ebbecf 100644
--- a/libraries/AP_Scripting/generator/description/bindings.desc
+++ b/libraries/AP_Scripting/generator/description/bindings.desc
@@ -915,3 +915,9 @@ singleton AP_Networking method get_ip_active uint32_t
 singleton AP_Networking method get_netmask_active uint32_t
 singleton AP_Networking method get_gateway_active uint32_t
 singleton AP_Networking method address_to_str string uint32_t'skip_check
+
+include AP_VisualOdom/AP_VisualOdom.h
+singleton AP_VisualOdom depends HAL_VISUALODOM_ENABLED
+singleton AP_VisualOdom rename visual_odom
+singleton AP_VisualOdom method healthy boolean
+singleton AP_VisualOdom method quality int8_t

From 07cd050f63705bfe107c5b92d1271a0be46f90d2 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 20 Feb 2024 16:49:46 +0900
Subject: [PATCH 14/26] AP_Scripting: EKF source switching for ExtNav and
 optflow

---
 .../applets/ahrs-source-extnav-optflow.lua    | 263 ++++++++++++++++++
 .../applets/ahrs-source-extnav-optflow.md     |  47 ++++
 2 files changed, 310 insertions(+)
 create mode 100644 libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua
 create mode 100644 libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.md

diff --git a/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua b/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua
new file mode 100644
index 0000000000000..759cb6d0a5b03
--- /dev/null
+++ b/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.lua
@@ -0,0 +1,263 @@
+-- switches between AHRS/EKF sources based on the pilot's source selection switch or using an automatic source selection algorithm
+-- this script is intended to help vehicles automatically switch between ExternalNav and optical flow
+--
+-- configure a downward facing lidar with a range of at least 5m
+-- setup RCx_OPTION = 90 (EKF Pos Source) to select the source (low=external nav, middle=opticalflow, high=Not Used)
+-- setup RCx_OPTION = 300 (Scripting1).  When this switch is pulled high, the source will be automatically selected
+-- SRC_ENABLE = 1 (enable scripting)
+-- setup EK3_SRCn_ parameters so that ExternalNav is the primary source, opticalflow is secondary
+--     EK3_SRC1_POSXY = 6 (ExternalNav)
+--     EK3_SRC1_VELXY = 6 (ExternalNav)
+--     EK3_SRC1_VELZ  = 6 (ExternalNav)
+--     EK3_SRC1_POSZ  = 6 (ExternalNav) or 1 (Baro)
+--     EK3_SRC1_YAW   = 6 (ExternalNav) or 1 (Compass)
+--     EK3_SRC2_POSXY = 0 (None)
+--     EK3_SRC2_VELXY = 5 (OpticalFlow)
+--     EK3_SRC2_VELZ  = 0 (None)
+--     EK3_SRC2_POSZ  = 1 (Baro)
+--     EK3_SRC2_YAW   = 1 (Compass)
+--     EK3_SRC_OPTIONS = 0 (Do not fuse all velocities)
+--
+-- When the 2nd auxiliary switch (300/Scripting1) is pulled high automatic source selection is used based on the following criteria:
+--     ESRC_EXTN_THRESH holds the threshold for ExternalNav innovation threshold (around 0.3 is a good choice)
+--     ESRC_EXTN_QUAL holds the ExternalNav quality threshold (about 10 is a good choice)
+--     ESRC_FLOW_QUAL holds the optical flow quality threshold (about 50 is a good choice)
+--     ESRC_FLOW_THRESH holds the threshold for optical flow innovations (about 0.15 is a good choice)
+--     ESRC_RNGFND_MAX holds the threshold (in meters) for rangefinder altitude
+--
+--     If ExternalNav's quality is above ESRC_EXTN_QUAL and innovations are below ESRC_EXTN_THRESH, ExternalNav is used
+--     Optical flow is used if the above is not true and:
+--         optical flow's quality is above ESRC_FLOW_QUAL
+--         innovations are below ESRC_FLOW_THRESH
+--         rangefinder distance is below ESRC_RNGFND_MAX
+
+local PARAM_TABLE_KEY = 81
+PARAM_TABLE_PREFIX = "ESRC_"
+local MAV_SEVERITY = {EMERGENCY=0, ALERT=1, CRITICAL=2, ERROR=3, WARNING=4, NOTICE=5, INFO=6, DEBUG=7}
+
+-- bind a parameter to a variable
+function bind_param(name)
+  local p = Parameter()
+  assert(p:init(name), string.format('could not find %s parameter', name))
+  return p
+end
+
+-- add a parameter and bind it to a variable
+local function bind_add_param(name, idx, default_value)
+  assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', name))
+  return bind_param(PARAM_TABLE_PREFIX .. name)
+end
+
+-- add param table
+assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 5), 'ahrs-source-extnav-optflow: could not add param table')
+
+--[[
+  // @Param: ESRC_EXTN_THRESH
+  // @DisplayName: EKF Source ExternalNav Innovation Threshold
+  // @Description: ExternalNav may be used if innovations are below this threshold
+  // @Range: 0 1
+  // @User: Standard
+--]]
+local ESRC_EXTN_THRESH = bind_add_param('EXTN_THRESH', 1, 0.3)
+
+--[[
+  // @Param: ESRC_EXTN_QUAL
+  // @DisplayName: EKF Source ExternalNav Quality Threshold
+  // @Description: ExternalNav may be used if quality is above this threshold
+  // @Range: 0 100
+  // @Units: %
+  // @User: Standard
+--]]
+local ESRC_EXTN_QUAL = bind_add_param('EXTN_QUAL', 2, 10)
+
+--[[
+  // @Param: ESRC_FLOW_THRESH
+  // @DisplayName: EKF Source OpticalFlow Innovation Threshold
+  // @Description: OpticalFlow may be used if innovations are below this threshold
+  // @Range: 0 1
+  // @User: Standard
+--]]
+local ESRC_FLOW_THRESH = bind_add_param('FLOW_THRESH', 3, 0.3)
+
+--[[
+  // @Param: ESRC_FLOW_QUAL
+  // @DisplayName: EKF Source OpticalFlow Quality Threshold
+  // @Description: OpticalFlow may be used if quality is above this threshold
+  // @Range: 0 100
+  // @Units: %
+  // @User: Standard
+--]]
+local ESRC_FLOW_QUAL = bind_add_param('FLOW_QUAL', 4, 50)
+
+--[[
+  // @Param: ESRC_RNGFND_MAX
+  // @DisplayName: EKF Source Rangefinder Max
+  // @Description: OpticalFlow may be used if rangefinder distance is below this threshold
+  // @Range: 0 50
+  // @Units: m
+  // @User: Standard
+--]]
+local ESRC_RNGFND_MAX = bind_add_param('RNGFND_MAX', 5, 7)
+
+local last_rc_ekfsrc_pos = 0        -- last known position of EKF source select switch
+local last_rc_autosrc_pos = 0       -- last known position of automatic source select switch
+local rangefinder_rotation = 25     -- check downward (25) facing lidar
+local auto_switch = false           -- true when auto switching between sources is active
+local source_prev = 0               -- previous source, defaults to primary source (ExternalNav)
+local vote_counter_max = 20         -- when a vote counter reaches this number (i.e. 2sec) source may be switched
+local extnav_vs_opticalflow_vote = 0  -- vote counter for external nav vs optical (-20 = external nav, +20 = optical flow)
+
+assert(visual_odom, 'could not access optical flow')
+assert(optical_flow, 'could not access optical flow')
+
+-- play tune on buzzer to alert user to change in active source set
+function play_source_tune(source)
+  if (source) then
+    if (source == 0) then
+      notify:play_tune("L8C")       -- one long lower tone
+    elseif (source == 1) then
+      notify:play_tune("L12DD")     -- two fast medium tones
+    elseif (source == 2) then
+      notify:play_tune("L16FFF")    -- three very fast, high tones
+    end
+  end
+end
+
+-- convert a boolean to a number
+function bool_to_int(b)
+  if b then
+    return 1
+  end
+  return 0
+end
+
+-- the main update function
+function update()
+
+  -- check for EKF Source Select switch position change
+  local rc_ekfsrc_pos = rc:get_aux_cached(90)  -- RCx_OPTION = 90 (EKF Pos Source)
+  if rc_ekfsrc_pos == nil then
+    rc_ekfsrc_pos = 0
+  end
+  local rc_ekfsrc_pos_changed = (rc_ekfsrc_pos ~= last_rc_ekfsrc_pos)
+  last_rc_ekfsrc_pos = rc_ekfsrc_pos
+
+  -- check for EKF automatic source select switch position change
+  local rc_autosrc_pos = rc:get_aux_cached(300) -- RCx_OPTION = 300 (Scripting1)
+  if rc_autosrc_pos == nil then
+    rc_autosrc_pos = 0
+  end
+  local rc_autosrc_pos_changed = (rc_autosrc_pos ~= last_rc_autosrc_pos)
+  last_rc_autosrc_pos = rc_autosrc_pos
+
+  -- check external nav quality and innovations
+  local extnav_over_threshold = true
+  local extnav_innov
+  extnav_innov, _ = ahrs:get_vel_innovations_and_variances_for_source(6)
+  if (extnav_innov) then
+    local extnav_xyz_innov = extnav_innov:length()
+    extnav_over_threshold = (extnav_xyz_innov == 0.0) or (extnav_xyz_innov > ESRC_EXTN_THRESH:get())
+    gcs:send_named_float("ExtNInnov", extnav_xyz_innov)
+  end
+  local extnav_usable = (not extnav_over_threshold) and visual_odom and visual_odom:healthy() and (visual_odom:quality() >= ESRC_EXTN_QUAL:get())
+  if visual_odom then
+    gcs:send_named_float("ExtNQuality", visual_odom:quality())
+  end
+  gcs:send_named_float("ExtNUsable", bool_to_int(extnav_usable))
+
+  -- check optical flow quality and innovations
+  local opticalflow_quality_good = false
+  if (optical_flow) then
+    opticalflow_quality_good = (optical_flow:enabled() and optical_flow:healthy() and optical_flow:quality() >= ESRC_FLOW_QUAL:get())
+  end
+
+  -- get opticalflow innovations from ahrs (only x and y values are valid)
+  local opticalflow_over_threshold = true
+  local opticalflow_innov
+  opticalflow_innov, _ = ahrs:get_vel_innovations_and_variances_for_source(5)
+  if (opticalflow_innov) then
+    local opticalflow_xy_innov = math.sqrt(opticalflow_innov:x() * opticalflow_innov:x() + opticalflow_innov:y() * opticalflow_innov:y())
+    opticalflow_over_threshold = (opticalflow_xy_innov == 0.0) or (opticalflow_xy_innov > ESRC_FLOW_THRESH:get())
+    gcs:send_named_float("FlowInnov", opticalflow_xy_innov)
+  end
+
+  -- get rangefinder distance
+  local rngfnd_distance_m = 0
+  if rangefinder:has_data_orient(rangefinder_rotation) then
+    rngfnd_distance_m = rangefinder:distance_cm_orient(rangefinder_rotation) * 0.01
+  end
+  local rngfnd_over_threshold = (rngfnd_distance_m == 0) or (rngfnd_distance_m > ESRC_RNGFND_MAX:get())
+
+  -- opticalflow is usable if quality and innovations are good and rangefinder is in range
+  local opticalflow_usable = opticalflow_quality_good and (not opticalflow_over_threshold) and (not rngfnd_over_threshold)
+  gcs:send_named_float("FlowUsable", bool_to_int(opticalflow_usable))
+
+  -- automatic selection logic --
+
+  -- ExtNav vs opticalflow vote. "-1" to move towards EXtNav, "+1" to move to opticalflow
+  if (extnav_usable) then
+    -- vote for external nav if usable and innovations are low
+    extnav_vs_opticalflow_vote = math.max(extnav_vs_opticalflow_vote - 1, -vote_counter_max)
+  elseif opticalflow_usable then
+    -- vote for opticalflow if usable
+    extnav_vs_opticalflow_vote = math.min(extnav_vs_opticalflow_vote + 1, vote_counter_max)
+  end
+
+  -- auto source vote collation
+  local auto_source = -1                         -- auto source undecided if -1
+  if extnav_vs_opticalflow_vote <= -vote_counter_max then
+    auto_source = 0                              -- external nav
+  elseif extnav_vs_opticalflow_vote >= vote_counter_max then
+    auto_source = 1                              -- opticalflow
+  end
+
+  -- read source switch position from user
+  if rc_ekfsrc_pos_changed then                 -- check for changes in source switch position
+    auto_switch = false                         -- disable auto switching of source
+    if source_prev ~= rc_ekfsrc_pos then        -- check if switch position does not match source (there is a one-to-one mapping of switch position to source)
+      source_prev = rc_ekfsrc_pos                -- record what source should now be (changed by ArduPilot vehicle code)
+      gcs:send_text(MAV_SEVERITY.INFO, "Pilot switched to Source " .. string.format("%d", source_prev+1))
+    else
+      gcs:send_text(MAV_SEVERITY.INFO, "Pilot switched but already Source " .. string.format("%d", source_prev+1))
+    end
+    play_source_tune(source_prev)                -- alert user of source whether changed or not
+  end
+
+  -- if auto switch exists then read auto source switch position from RCx_FUNCTION = 300 (Scripting1)
+  if rc_autosrc_pos_changed  then                -- check for changes in source auto switch position
+    if rc_autosrc_pos == 0 then                  -- pilot has pulled switch low
+      auto_switch = false                        -- disable auto switching of source
+      if rc_ekfsrc_pos ~= source_prev then       -- check if source will change
+        source_prev = rc_ekfsrc_pos              -- record pilot's selected source
+        ahrs:set_posvelyaw_source_set(source_prev)   -- switch to pilot's selected source
+        gcs:send_text(MAV_SEVERITY.INFO, "Auto source disabled, switched to Source " .. string.format("%d", source_prev+1))
+      else
+        gcs:send_text(MAV_SEVERITY.INFO, "Auto source disabled, already Source " .. string.format("%d", source_prev+1))
+      end
+    elseif rc_autosrc_pos == 2 then              -- pilot has pulled auto switch high
+      auto_switch = true                         -- enable auto switching of source
+      if auto_source < 0 then
+        gcs:send_text(MAV_SEVERITY.INFO, "Auto source enabled, undecided, Source " .. string.format("%d", source_prev+1))
+      elseif auto_source ~= source_prev then     -- check if source will change
+        source_prev = auto_source                -- record pilot's selected source
+        ahrs:set_posvelyaw_source_set(source_prev)   -- switch to pilot's selected source
+        gcs:send_text(MAV_SEVERITY.INFO, "Auto source enabled, switched to Source " .. string.format("%d", source_prev+1))
+      else
+        gcs:send_text(MAV_SEVERITY.INFO, "Auto source enabled, already Source " .. string.format("%d", source_prev+1))
+      end
+    end
+    play_source_tune(source_prev)
+  end
+
+  -- auto switching
+  if auto_switch and (auto_source >= 0) and (auto_source ~= source_prev) then
+    source_prev = auto_source                  -- record selected source
+    ahrs:set_posvelyaw_source_set(source_prev) -- switch to pilot's selected source
+    gcs:send_text(MAV_SEVERITY.INFO, "Auto switched to Source " .. string.format("%d", source_prev+1))
+    play_source_tune(source_prev)
+  end
+
+  return update, 100
+end
+
+return update()
diff --git a/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.md b/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.md
new file mode 100644
index 0000000000000..37eb93d157d2b
--- /dev/null
+++ b/libraries/AP_Scripting/applets/ahrs-source-extnav-optflow.md
@@ -0,0 +1,47 @@
+# ExternalNav / Optical flow source switching
+
+Switches between AHRS/EKF sources based on the pilot's source selection switch or using an automatic source selection algorithm
+This script is intended to help vehicles automatically switch between ExternalNav and optical flow
+
+## Parmeter Descriptions
+
+-- ESRC_EXTN_THRESH : ExternalNav innovation threshold
+-- ESRC_EXTN_QUAL : ExternalNav quality threshold
+-- ESRC_FLOW_THRESH : OpticalFlow innovation threshold
+-- ESRC_FLOW_QUAL : OpticalFlow quality threshold
+-- ESRC_RNGFND_MAX : Rangefinder altitude threshold (in meters)
+
+## How to use
+
+Configure a downward facing lidar with a range of at least 5m
+Set RCx_OPTION = 90 (EKF Pos Source) to select the source (low=ExternalNav, middle=opticalflow, high=Not Used)
+Set RCx_OPTION = 300 (Scripting1).  When this switch is pulled high, the source will be automatically selected
+Set SRC_ENABLE = 1 (enable scripting)
+Set EK3_SRCn_ parameters so that ExternalNav is the primary source, opticalflow is secondary
+
+  - EK3_SRC1_POSXY = 6 (ExternalNav)
+  - EK3_SRC1_VELXY = 6 (ExternalNav)
+  - EK3_SRC1_VELZ  = 6 (ExternalNav)
+  - EK3_SRC1_POSZ  = 6 (ExternalNav) or 1 (Baro)
+  - EK3_SRC1_YAW   = 6 (ExternalNav) or 1 (Compass)
+  - EK3_SRC2_POSXY = 0 (None)
+  - EK3_SRC2_VELXY = 5 (OpticalFlow)
+  - EK3_SRC2_VELZ  = 0 (None)
+  - EK3_SRC2_POSZ  = 1 (Baro)
+  - EK3_SRC2_YAW   = 1 (Compass)
+  - EK3_SRC_OPTIONS = 0 (Do not fuse all velocities)
+
+When the 2nd auxiliary switch (300/Scripting1) is pulled high automatic source selection is used based on the following criteria:
+
+  - ESRC_EXTN_THRESH holds the threshold for ExternalNav innovation threshold (around 0.3 is a good choice)
+  - ESRC_EXTN_QUAL holds the ExternalNav quality threshold (about 10 is a good choice)
+  - ESRC_FLOW_QUAL holds the optical flow quality threshold (about 50 is a good choice)
+  - ESRC_FLOW_THRESH holds the threshold for optical flow innovations (about 0.15 is a good choice)
+  - ESRC_RNGFND_MAX holds the threshold (in meters) for rangefinder altitude
+
+  - If ExternalNav's quality is above ESRC_EXTN_QUAL and innovations are below ESRC_EXTN_THRESH, ExternalNav is used
+  - Optical flow is used if the above is not true and:
+
+    - Quality is above ESRC_FLOW_QUAL
+    - Innovations are below ESRC_FLOW_THRESH
+    - Rangefinder distance is below ESRC_RNGFND_MAX

From fb5df809320b77c305621955790193a461ab19d3 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 21 Feb 2024 17:02:51 +0900
Subject: [PATCH 15/26] AP_HAL_ChibiOS: AP_Periph does not use AP_VisualOdom

---
 libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h
index 136987f000d94..47e90a8828899 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/defaults_periph.h
@@ -329,6 +329,7 @@
 #define AP_RPM_ENABLED defined(HAL_PERIPH_ENABLE_RPM)
 #define AP_RCPROTOCOL_ENABLED defined(HAL_PERIPH_ENABLE_RCIN)
 #define AP_RTC_ENABLED defined(HAL_PERIPH_ENABLE_RTC)
+#define HAL_VISUALODOM_ENABLED defined(HAL_PERIPH_ENABLE_VISUALODOM)
 
 #ifndef AP_BOOTLOADER_ALWAYS_ERASE
 #define AP_BOOTLOADER_ALWAYS_ERASE 1

From 9ea8e61cfc2bea76c949968f1930ee80061e5eca Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 28 Feb 2024 07:41:44 +1100
Subject: [PATCH 16/26] AP_Scripting: example showing how to use readstring
 method

a fairly common requirement for scripts
---
 .../AP_Scripting/examples/readstring_test.lua | 50 +++++++++++++++++++
 1 file changed, 50 insertions(+)
 create mode 100644 libraries/AP_Scripting/examples/readstring_test.lua

diff --git a/libraries/AP_Scripting/examples/readstring_test.lua b/libraries/AP_Scripting/examples/readstring_test.lua
new file mode 100644
index 0000000000000..d95a0687be8e5
--- /dev/null
+++ b/libraries/AP_Scripting/examples/readstring_test.lua
@@ -0,0 +1,50 @@
+--[[
+   example demonstrating how to read from a serial port into a lua string
+--]]
+
+local baud_rate = 57600
+
+local port = assert(serial:find_serial(0), "Could not find Scripting Serial Port")
+
+port:begin(baud_rate)
+port:set_flow_control(0)
+
+--[[
+  get a string by concatenating bytes from the serial port,
+  suitable for ArduPilot 4.4.x
+--]]
+local function get_string_44(n)
+   local ret = ""
+   for _ = 1, n do
+      local b = port:read()
+      ret = ret .. string.char(b)
+   end
+   return ret
+end
+
+--[[
+  get a string directly uisng readstring
+  suitable for ArduPilot 4.5.x and later
+--]]
+local function get_string_45(n)
+   return port:readstring(n)
+end
+
+function update()
+   -- test using 4.5 method
+   local n = port:available():toint()
+   if n > 0 then
+      local str = get_string_45(n)
+      gcs:send_text(0, string.format("Received: '%s'", str))
+   end
+
+   -- test using 4.4 method (just so we don't have an unused function in lua check)
+   n = port:available():toint()
+   if n > 0 then
+      local str = get_string_44(n)
+      gcs:send_text(0, string.format("Received: '%s'", str))
+    end
+   return update, 100
+end
+
+return update, 100

From c91a6fdba72f25bba673f7cac5f2bb7a9e166351 Mon Sep 17 00:00:00 2001
From: Andrew Piper <github@andypiper.com>
Date: Sat, 24 Feb 2024 18:05:38 +0000
Subject: [PATCH 17/26] environment_install: fixup mac xcode install text

---
 Tools/environment_install/install-prereqs-mac.sh | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/environment_install/install-prereqs-mac.sh b/Tools/environment_install/install-prereqs-mac.sh
index de2cd666ef760..7c0701b38a161 100755
--- a/Tools/environment_install/install-prereqs-mac.sh
+++ b/Tools/environment_install/install-prereqs-mac.sh
@@ -52,7 +52,7 @@ echo "Checking CLI Tools installed..."
     ERROR=$(xcode-select --install 2>&1 > /dev/null)
 } ||
 {
-if [[ $ERROR != *"command line tools are already installed"* ]]; then
+if [[ $ERROR != *"ommand line tools are already installed"* ]]; then
     echo "$ERROR" 1>&2
     exit 1
 fi

From 07f34a2f74afef6e8bda04253a783138262e1cf7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Feb 2024 11:16:58 +1100
Subject: [PATCH 18/26] SITL: added NoVehicle

---
 libraries/SITL/SIM_NoVehicle.h | 40 ++++++++++++++++++++++++++++++++++
 1 file changed, 40 insertions(+)
 create mode 100644 libraries/SITL/SIM_NoVehicle.h

diff --git a/libraries/SITL/SIM_NoVehicle.h b/libraries/SITL/SIM_NoVehicle.h
new file mode 100644
index 0000000000000..0d37392b0f9ae
--- /dev/null
+++ b/libraries/SITL/SIM_NoVehicle.h
@@ -0,0 +1,40 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+/*
+  empty vehicle for example CI testing
+*/
+
+#pragma once
+
+#include "SIM_Aircraft.h"
+
+namespace SITL {
+
+/*
+  empty vehicle
+ */
+class NoVehicle : public Aircraft {
+public:
+    NoVehicle(const char *frame_str) : Aircraft(frame_str) {}
+
+    void update(const struct sitl_input &input) override {}
+
+    /* static object creator */
+    static Aircraft *create(const char *frame_str) {
+        return new NoVehicle(frame_str);
+    }
+};
+
+} // namespace SITL

From e4c103972c13ffcf57da3b45399b51ea0a1dd0a6 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Feb 2024 11:17:28 +1100
Subject: [PATCH 19/26] HAL_SITL: use NoVehicle for example tests and replay

allows for example runs without -C --model XXX
---
 libraries/AP_HAL_SITL/SITL_cmdline.cpp | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp
index f4c5529197ff4..8eebf19967b73 100644
--- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp
+++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp
@@ -37,6 +37,7 @@
 #include <SITL/SIM_Webots_Python.h>
 #include <SITL/SIM_JSON.h>
 #include <SITL/SIM_Blimp.h>
+#include <SITL/SIM_NoVehicle.h>
 #include <AP_Filesystem/AP_Filesystem.h>
 
 #include <AP_Vehicle/AP_Vehicle_Type.h>
@@ -181,6 +182,7 @@ static const struct {
     { "webots",             Webots::create },
     { "JSON",               JSON::create },
     { "blimp",              Blimp::create },
+    { "novehicle",          NoVehicle::create },
 };
 
 void SITL_State::_set_signal_handlers(void) const
@@ -239,7 +241,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
     static struct timeval first_tv;
     gettimeofday(&first_tv, nullptr);
     time_t start_time_UTC = first_tv.tv_sec;
-    const bool is_replay = APM_BUILD_TYPE(APM_BUILD_Replay);
+    const bool is_example = APM_BUILD_TYPE(APM_BUILD_Replay) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN);
 
     enum long_options {
         CMDLINE_GIMBAL = 1,
@@ -348,8 +350,8 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
         {0, false, 0, 0}
     };
 
-    if (is_replay) {
-        model_str = "quad";
+    if (is_example) {
+        model_str = "novehicle";
         HALSITL::UARTDriver::_console = true;
     }
 
@@ -371,7 +373,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
 
     GetOptLong gopt(argc, argv, "hwus:r:CI:P:SO:M:F:c:v:",
                     options);
-    while (!is_replay && (opt = gopt.getoption()) != -1) {
+    while (!is_example && (opt = gopt.getoption()) != -1) {
         switch (opt) {
         case 'w':
             erase_all_storage = true;

From 39efe75e79b7eeb97dcccc71bf8735b88e7fd5f4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Feb 2024 11:33:46 +1100
Subject: [PATCH 20/26] HAL_SITL: allow delay() and delay_microseconds() to
 work in examples

when we have no sitl object we need to directly call stop_clock()
---
 libraries/AP_HAL_SITL/Scheduler.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp
index c925a0ca5b418..12a77a8d0c16b 100644
--- a/libraries/AP_HAL_SITL/Scheduler.cpp
+++ b/libraries/AP_HAL_SITL/Scheduler.cpp
@@ -128,6 +128,11 @@ bool Scheduler::semaphore_wait_hack_required() const
 
 void Scheduler::delay_microseconds(uint16_t usec)
 {
+    if (_sitlState->_sitl == nullptr) {
+        // this allows examples to run
+        hal.scheduler->stop_clock(AP_HAL::micros64()+usec);
+        return;
+    }
     uint64_t start = AP_HAL::micros64();
     do {
         uint64_t dtime = AP_HAL::micros64() - start;

From d0cec297a8cbb08767b2675631692fb4010eafec Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Feb 2024 11:39:33 +1100
Subject: [PATCH 21/26] AP_JSON: made test pass/fail

---
 libraries/AP_JSON/examples/XPlane/XPlane.cpp | 21 +++++++++++++++++---
 1 file changed, 18 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_JSON/examples/XPlane/XPlane.cpp b/libraries/AP_JSON/examples/XPlane/XPlane.cpp
index d7af16a4f5722..197876434ac8d 100644
--- a/libraries/AP_JSON/examples/XPlane/XPlane.cpp
+++ b/libraries/AP_JSON/examples/XPlane/XPlane.cpp
@@ -12,12 +12,16 @@ void loop();
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
-static void test_xplane(void)
+static uint32_t test_count;
+static uint32_t label_count;
+
+static bool test_xplane(void)
 {
     const uint32_t m1 = hal.util->available_memory();
     auto *obj = AP_JSON::load_json("@ROMFS/models/xplane_plane.json");
     if (obj == nullptr) {
-        ::printf("Failed to parse json\n");
+        ::printf("Failed to load or parse json\n");
+        return false;
     }
     const uint32_t m2 = hal.util->available_memory();
     ::printf("Used %u bytes\n", unsigned(m1-m2));
@@ -28,8 +32,10 @@ static void test_xplane(void)
          ++i) {
         const char *label = i->first.c_str();
         ::printf("Label: %s\n", label);
+        label_count++;
     }
     delete obj;
+    return label_count > 10;
 }
 
 /*
@@ -43,7 +49,16 @@ void setup(void)
 void loop(void)
 {
     ::printf("Memory: %u\n", (unsigned)hal.util->available_memory());
-    test_xplane();
+    if (test_xplane()) {
+        test_count++;
+    } else {
+        ::printf("Test FAILED\n");
+        exit(1);
+    }
+    if (test_count > 10) {
+        ::printf("Test PASSED\n");
+        exit(0);
+    }
 }
 
 AP_HAL_MAIN();

From a74dab8c0a2ff999767b839b37b4ffee23a14953 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Feb 2024 11:59:06 +1100
Subject: [PATCH 22/26] HAL_SITL: don't run IO procs in examples

---
 libraries/AP_HAL_SITL/Scheduler.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_SITL/Scheduler.cpp b/libraries/AP_HAL_SITL/Scheduler.cpp
index 12a77a8d0c16b..32eeebfa7088f 100644
--- a/libraries/AP_HAL_SITL/Scheduler.cpp
+++ b/libraries/AP_HAL_SITL/Scheduler.cpp
@@ -301,7 +301,7 @@ void Scheduler::_run_io_procs()
 void Scheduler::stop_clock(uint64_t time_usec)
 {
     _stopped_clock_usec = time_usec;
-    if (time_usec - _last_io_run > 10000) {
+    if (_sitlState->_sitl != nullptr && time_usec - _last_io_run > 10000) {
         _last_io_run = time_usec;
         _run_io_procs();
     }

From 66ff84e6fb19310a7aabb744dd81342a91f0b485 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Feb 2024 11:59:22 +1100
Subject: [PATCH 23/26] AP_InertialSensor: fixed wait_for_sample() in examples

---
 libraries/AP_InertialSensor/AP_InertialSensor.cpp | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index 5b3c4f8e7c53e..b8262548e9676 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -1973,6 +1973,13 @@ void AP_InertialSensor::update(void)
  */
 void AP_InertialSensor::wait_for_sample(void)
 {
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    auto *sitl = AP::sitl();
+    if (sitl == nullptr) {
+        hal.scheduler->delay_microseconds(1000);
+        return;
+    }
+#endif
     if (_have_sample) {
         // the user has called wait_for_sample() again without
         // consuming the sample with update()

From 7226c5107d396db87785766b80b5de1a7b03eac0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Feb 2024 11:59:42 +1100
Subject: [PATCH 24/26] AP_Schedule: fixed loop in example with sitl NULL

---
 libraries/AP_Scheduler/AP_Scheduler.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/AP_Scheduler/AP_Scheduler.cpp b/libraries/AP_Scheduler/AP_Scheduler.cpp
index 5fd91703b114b..62a702bb86b19 100644
--- a/libraries/AP_Scheduler/AP_Scheduler.cpp
+++ b/libraries/AP_Scheduler/AP_Scheduler.cpp
@@ -360,7 +360,7 @@ void AP_Scheduler::loop()
           for testing low CPU conditions we can add an optional delay in SITL
         */
         auto *sitl = AP::sitl();
-        uint32_t loop_delay_us = sitl->loop_delay.get();
+        uint32_t loop_delay_us = sitl? sitl->loop_delay.get() : 1000U;
         hal.scheduler->delay_microseconds(loop_delay_us);
     }
 #endif

From 09500df4a85c751c4eaadef59128065ce2d475e2 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Feb 2024 11:59:56 +1100
Subject: [PATCH 25/26] AP_Scheduler: fixed example test to pass/fail

---
 .../Scheduler_test/Scheduler_test.cpp         | 29 ++++++++++++++++---
 1 file changed, 25 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp
index 5a9eb79d20ee7..b0cc45e2fb752 100644
--- a/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp
+++ b/libraries/AP_Scheduler/examples/Scheduler_test/Scheduler_test.cpp
@@ -9,6 +9,7 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS_Dummy.h>
+#include <stdio.h>
 
 const struct AP_Param::GroupInfo        GCS_MAVLINK_Parameters::var_info[] = {
     AP_GROUPEND
@@ -26,13 +27,14 @@ class SchedTest {
 
 private:
 
-    AP_InertialSensor ins;
 #if HAL_EXTERNAL_AHRS_ENABLED
     AP_ExternalAHRS eAHRS;
 #endif // HAL_EXTERNAL_AHRS_ENABLED
     AP_Scheduler scheduler;
 
     uint32_t ins_counter;
+    uint32_t count_5s;
+    uint32_t count_1s;
     static const AP_Scheduler::Task scheduler_tasks[];
 
     void ins_update(void);
@@ -81,8 +83,6 @@ void SchedTest::setup(void)
 
     board_config.init();
 
-    ins.init(scheduler.get_loop_rate_hz());
-
     // initialise the scheduler
     scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks), (uint32_t)-1);
 }
@@ -91,6 +91,24 @@ void SchedTest::loop(void)
 {
     // run all tasks
     scheduler.loop();
+    if (ins_counter == 1000) {
+        bool ok = true;
+        if (count_5s != 4) {
+            ::printf("ERROR: count_5s=%u\n", (unsigned)count_5s);
+            ok = false;
+        }
+        if (count_1s != 20) {
+            ::printf("ERROR: count_1s=%u\n", (unsigned)count_1s);
+            ok = false;
+        }
+        if (!ok) {
+            ::printf("Test FAILED\n");
+            exit(1);
+        } else {
+            ::printf("Test PASSED\n");
+            exit(0);
+        }
+    }
 }
 
 /*
@@ -99,7 +117,6 @@ void SchedTest::loop(void)
 void SchedTest::ins_update(void)
 {
     ins_counter++;
-    ins.update();
 }
 
 /*
@@ -108,6 +125,7 @@ void SchedTest::ins_update(void)
 void SchedTest::one_hz_print(void)
 {
     hal.console->printf("one_hz: t=%lu\n", (unsigned long)AP_HAL::millis());
+    count_1s++;
 }
 
 /*
@@ -116,6 +134,7 @@ void SchedTest::one_hz_print(void)
 void SchedTest::five_second_call(void)
 {
     hal.console->printf("five_seconds: t=%lu ins_counter=%u\n", (unsigned long)AP_HAL::millis(), (unsigned)ins_counter);
+    count_5s++;
 }
 
 /*
@@ -128,8 +147,10 @@ void setup(void)
 {
     schedtest.setup();
 }
+
 void loop(void)
 {
     schedtest.loop();
 }
+
 AP_HAL_MAIN();

From 03910b8df4afa08d6594c7dd3ac36d2f7eefbdda Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 23 Feb 2024 12:00:13 +1100
Subject: [PATCH 26/26] Tools: run examples with SITL

---
 Tools/autotest/examples.py | 2 +-
 Tools/scripts/build_ci.sh  | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/Tools/autotest/examples.py b/Tools/autotest/examples.py
index 590205b331cc3..b7c69fc6a2db9 100644
--- a/Tools/autotest/examples.py
+++ b/Tools/autotest/examples.py
@@ -51,7 +51,7 @@ def run_example(filepath, valgrind=False, gdb=False):
 
 
 def run_examples(debug=False, valgrind=False, gdb=False):
-    dirpath = util.reltopdir(os.path.join('build', 'linux', 'examples'))
+    dirpath = util.reltopdir(os.path.join('build', 'sitl', 'examples'))
 
     print("Running Hello")
     # explicitly run helloworld and check for output
diff --git a/Tools/scripts/build_ci.sh b/Tools/scripts/build_ci.sh
index 75a42b516c6bb..f45383f6ab822 100755
--- a/Tools/scripts/build_ci.sh
+++ b/Tools/scripts/build_ci.sh
@@ -176,7 +176,7 @@ for t in $CI_BUILD_TARGET; do
     fi
 
     if [ "$t" == "examples" ]; then
-        ./waf configure --board=linux --debug
+        ./waf configure --board=sitl --debug
         ./waf examples
         run_autotest "Examples" "--no-clean" "run.examples"
         continue