Merge branch 'master' into master

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

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",

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

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

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);
 
     }
 }

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++;

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

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

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

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;

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;
@@ -296,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();
     }

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()

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();

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;
     }
 

libraries/AP_NavEKF3/AP_NavEKF3_RngBcnFusion.cpp

@@ -37,9 +37,15 @@ 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;
+    numFusionReports = 0;
+    auto *beacon = AP::dal().beacon();
+    if (beacon != nullptr) {
+        const uint8_t count = beacon->count();
+        fusionReport = new BeaconFusion::FusionReport[count];
+        if (fusionReport != nullptr) {
+            numFusionReports = count;
         }
     }
     posOffsetNED.zero();
@@ -310,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;
@@ -549,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;

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
 

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