Merge branch 'master' into master_JRE

.github/workflows/test_environment.yml

@@ -28,14 +28,14 @@ jobs:
       fail-fast: false  # don't cancel if a job from the matrix fails
       matrix:
         include:
-          - os: ubuntu
-            name: bionic
           - os: ubuntu
             name: focal
           - os: ubuntu
             name: jammy
           - os: ubuntu
             name: lunar
+          - os: ubuntu
+            name: mantic
           - os: archlinux
             name: latest
           - os: debian
@@ -77,7 +77,7 @@ jobs:
         with:
             submodules: 'recursive'
       - name: test install environment ${{matrix.os}}.${{matrix.name}}
-        timeout-minutes: 30
+        timeout-minutes: 45
         env:
           DISABLE_MAVNATIVE: True
           DEBIAN_FRONTEND: noninteractive

ArduPlane/Plane.h

@@ -735,6 +735,9 @@ class Plane : public AP_Vehicle {
         // are we trying to follow terrain?
         bool terrain_following;
 
+        // are we waiting to load terrain data to init terrain following
+        bool terrain_following_pending;
+
         // target altitude above terrain in cm, valid if terrain_following
         // is set
         int32_t terrain_alt_cm;
@@ -855,7 +858,7 @@ class Plane : public AP_Vehicle {
     void reset_offset_altitude(void);
     void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
     bool above_location_current(const Location &loc);
-    void setup_terrain_target_alt(Location &loc) const;
+    void setup_terrain_target_alt(Location &loc);
     int32_t adjusted_altitude_cm(void);
     int32_t adjusted_relative_altitude_cm(void);
     float mission_alt_offset(void);

ArduPlane/ReleaseNotes.txt

@@ -1,3 +1,31 @@
+Release 4.4.2-beta1 13th October 2023
+-------------------------------------
+
+Changes from 4.4.1
+
+- BETAFPV-F405 support
+- MambaF405v2 battery and serial setup corrected
+- mRo Control Zero OEM H7 bdshot support
+- SpeedyBee-F405-Wing gets VTX power control
+- SpeedyBee-F405-Mini support
+- T-Motor H743 Mini support
+- EKF3 supports baroless boards
+- INA battery monitor supports config of shunt resistor used (see BATTx_SHUNT)
+- BMI088 IMU error value handling fixed to avoid occasional negative spike
+- Dev environment CI autotest stability improvements
+- OSD correct DisplayPort BF MSP symbols
+- OSD option to correct direction arrows for BF font set
+- Sensor status reporting to GCS fixed for baroless boards
+- added opendroneid option to auto-store IDs in persistent flash
+- fixed TECS bug that could cause inability to climb or descend
+- fixed race condition when starting TECS controlled mode
+- fixed RTL with rally point and terrain follow
+- protect against invalid data in SBUS for first 4 channels
+- added build type to VER message
+- allow moving baseline rover at 3Hz
+- use RC deadzones in stick mixing
+
+
 Release 4.4.1 26th September 2023
 ---------------------------------
 

ArduPlane/altitude.cpp

@@ -212,6 +212,12 @@ void Plane::set_target_altitude_location(const Location &loc)
         target_altitude.amsl_cm += home.alt;
     }
 #if AP_TERRAIN_AVAILABLE
+    if (target_altitude.terrain_following_pending) {
+        /* we didn't get terrain data to init when we started on this
+           target, retry
+        */
+        setup_terrain_target_alt(next_WP_loc);
+    }
     /*
       if this location has the terrain_alt flag set and we know the
       terrain altitude of our current location then treat it as a
@@ -469,12 +475,16 @@ bool Plane::above_location_current(const Location &loc)
   modify a destination to be setup for terrain following if
   TERRAIN_FOLLOW is enabled
  */
-void Plane::setup_terrain_target_alt(Location &loc) const
+void Plane::setup_terrain_target_alt(Location &loc)
 {
 #if AP_TERRAIN_AVAILABLE
     if (terrain_enabled_in_current_mode()) {
-        loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN);
+        if (!loc.change_alt_frame(Location::AltFrame::ABOVE_TERRAIN)) {
+            target_altitude.terrain_following_pending = true;
+            return;
+        }
     }
+    target_altitude.terrain_following_pending = false;
 #endif
 }
 

ArduPlane/commands_logic.cpp

@@ -8,7 +8,11 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
     // default to non-VTOL loiter
     auto_state.vtol_loiter = false;
 
-        // log when new commands start
+#if AP_TERRAIN_AVAILABLE
+    plane.target_altitude.terrain_following_pending = false;
+#endif
+
+    // log when new commands start
     if (should_log(MASK_LOG_CMD)) {
         logger.Write_Mission_Cmd(mission, cmd);
     }

ArduPlane/mode.cpp

@@ -97,6 +97,10 @@ bool Mode::enter()
     quadplane.mode_enter();
 #endif
 
+#if AP_TERRAIN_AVAILABLE
+    plane.target_altitude.terrain_following_pending = false;
+#endif
+
     bool enter_result = _enter();
 
     if (enter_result) {

Tools/autotest/arducopter.py

@@ -8653,9 +8653,10 @@ def verify_yaw(mav, m):
                 if m.yawspeed > yawspeed_thresh_rads:
                     raise NotAchievedException("Excessive yaw on takeoff: %f deg/s > %f deg/s (frame=%s)" %
                                                (math.degrees(m.yawspeed), math.degrees(yawspeed_thresh_rads), frame))
-            self.install_message_hook(verify_yaw)
+            self.context_push()
+            self.install_message_hook_context(verify_yaw)
             self.takeoff(10)
-            self.remove_message_hook(verify_yaw)
+            self.context_pop()
             self.hover()
             self.change_mode('ALT_HOLD')
             self.delay_sim_time(1)
@@ -8671,13 +8672,14 @@ def verify_rollpitch(mav, m):
                 if m.roll > roll_thresh_rad:
                     raise NotAchievedException("Excessive roll %f deg > %f deg" %
                                                (math.degrees(m.roll), math.degrees(roll_thresh_rad)))
-            self.install_message_hook(verify_rollpitch)
+            self.context_push()
+            self.install_message_hook_context(verify_rollpitch)
             for i in range(5):
                 self.set_rc(4, 2000)
                 self.delay_sim_time(0.5)
                 self.set_rc(4, 1500)
                 self.delay_sim_time(5)
-            self.remove_message_hook(verify_rollpitch)
+            self.context_pop()
 
             self.do_RTL()
 

Tools/autotest/arduplane.py

@@ -1836,6 +1836,82 @@ def FenceRetRally(self):
         self.do_fence_disable() # Disable fence so we can land
         self.fly_home_land_and_disarm() # Pack it up, we're going home.
 
+    def TerrainRally(self):
+        """ Tests terrain follow with a rally point """
+        self.context_push()
+        self.install_terrain_handlers_context()
+
+        def terrain_following_above_80m(mav, m):
+            if m.get_type() == 'TERRAIN_REPORT':
+                if m.current_height < 50:
+                    raise NotAchievedException(
+                        "TERRAIN_REPORT.current_height below 50m %fm" % m.current_height)
+            if m.get_type() == 'VFR_HUD':
+                if m.groundspeed < 2:
+                    raise NotAchievedException("hit ground")
+
+        def terrain_wait_path(loc1, loc2, steps):
+            '''wait till we have terrain for N steps from loc1 to loc2'''
+            tstart = self.get_sim_time_cached()
+            self.progress("Waiting for terrain data")
+            while True:
+                now = self.get_sim_time_cached()
+                if now - tstart > 60:
+                    raise NotAchievedException("Did not get correct required terrain")
+                for i in range(steps):
+                    lat = loc1.lat + i * (loc2.lat-loc1.lat)/steps
+                    lon = loc1.lng + i * (loc2.lng-loc1.lng)/steps
+                    self.mav.mav.terrain_check_send(int(lat*1.0e7), int(lon*1.0e7))
+
+                report = self.assert_receive_message('TERRAIN_REPORT', timeout=60)
+                self.progress("Terrain pending=%u" % report.pending)
+                if report.pending == 0:
+                    break
+            self.progress("Got required terrain")
+
+        self.wait_ready_to_arm()
+        self.homeloc = self.mav.location()
+
+        guided_loc = mavutil.location(-35.39723762, 149.07284612, 99.0, 0)
+        rally_loc = mavutil.location(-35.3654952000, 149.1558698000, 100, 0)
+
+        terrain_wait_path(self.homeloc, rally_loc, 10)
+
+        # set a rally point to the west of home
+        self.upload_rally_points_from_locations([rally_loc])
+
+        self.set_parameter("TKOFF_ALT", 100)
+        self.change_mode("TAKEOFF")
+        self.wait_ready_to_arm()
+        self.arm_vehicle()
+        self.set_parameter("TERRAIN_FOLLOW", 1)
+        self.wait_altitude(90, 120, timeout=30, relative=True)
+        self.progress("Done takeoff")
+
+        self.install_message_hook_context(terrain_following_above_80m)
+
+        self.change_mode("GUIDED")
+        self.do_reposition(guided_loc, frame=mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT)
+        self.progress("Flying to guided location")
+        self.wait_location(guided_loc,
+                           accuracy=200,
+                           target_altitude=None,
+                           timeout=600)
+
+        self.progress("Reached guided location")
+        self.set_parameter("RALLY_LIMIT_KM", 50)
+        self.change_mode("RTL")
+        self.progress("Flying to rally point")
+        self.wait_location(rally_loc,
+                           accuracy=200,
+                           target_altitude=None,
+                           timeout=600)
+        self.progress("Reached rally point")
+
+        self.context_pop()
+        self.disarm_vehicle(force=True)
+        self.reboot_sitl()
+
     def Parachute(self):
         '''Test Parachute'''
         self.set_rc(9, 1000)
@@ -5125,6 +5201,7 @@ def tests(self):
             self.MAV_CMD_DO_LAND_START,
             self.InteractTest,
             self.MAV_CMD_MISSION_START,
+            self.TerrainRally,
         ])
         return ret
 

Tools/autotest/common.py

@@ -7085,6 +7085,35 @@ def assert_rc_channel_value(self, channel, value):
             raise NotAchievedException("Expected %s to be %u got %u" %
                                        (channel, value, m_value))
 
+    def do_reposition(self,
+                      loc,
+                      frame=mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT):
+        '''send a DO_REPOSITION command for a location'''
+        self.run_cmd_int(
+            mavutil.mavlink.MAV_CMD_DO_REPOSITION,
+            0,
+            0,
+            0,
+            0,
+            int(loc.lat*1e7), # lat* 1e7
+            int(loc.lng*1e7), # lon* 1e7
+            loc.alt,
+            frame=frame
+        )
+
+    def add_rally_point(self, loc, seq, total):
+        '''add a rally point at the given location'''
+        self.mav.mav.rally_point_send(1, # target system
+                                      0, # target component
+                                      seq, # sequence number
+                                      total, # total count
+                                      int(loc.lat * 1e7),
+                                      int(loc.lng * 1e7),
+                                      loc.alt, # relative alt
+                                      0, # "break" alt?!
+                                      0, # "land dir"
+                                      0) # flags
+
     def wait_location(self,
                       loc,
                       accuracy=5.0,

Tools/environment_install/install-prereqs-ubuntu.sh

@@ -104,6 +104,11 @@ elif [ ${RELEASE_CODENAME} == 'lunar' ]; then
     SITLCFML_VERSION="2.5"
     PYTHON_V="python3"
     PIP=pip3
+elif [ ${RELEASE_CODENAME} == 'mantic' ]; then
+    SITLFML_VERSION="2.5"
+    SITLCFML_VERSION="2.5"
+    PYTHON_V="python3"
+    PIP=pip3
 elif [ ${RELEASE_CODENAME} == 'groovy' ] ||
          [ ${RELEASE_CODENAME} == 'bullseye' ]; then
     SITLFML_VERSION="2.5"
@@ -157,7 +162,8 @@ fi
 ARM_LINUX_PKGS="g++-arm-linux-gnueabihf $INSTALL_PKG_CONFIG"
 # python-wxgtk packages are added to SITL_PKGS below
 
-if [ ${RELEASE_CODENAME} == 'lunar' ]; then
+if [ ${RELEASE_CODENAME} == 'lunar' ] ||
+   [ ${RELEASE_CODENAME} == 'mantic' ]; then
     # on Lunar (and presumably later releases), we install in venv, below
     PYTHON_PKGS+=" numpy pyparsing psutil"
     SITL_PKGS="python3-dev"
@@ -167,7 +173,8 @@ fi
 
 # add some packages required for commonly-used MAVProxy modules:
 if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then
-  if [ ${RELEASE_CODENAME} == 'lunar' ]; then
+    if [ ${RELEASE_CODENAME} == 'lunar' ] ||
+       [ ${RELEASE_CODENAME} == 'mantic' ]; then
         PYTHON_PKGS+=" matplotlib scipy opencv-python pyyaml"
         SITL_PKGS+=" xterm libcsfml-dev libcsfml-audio${SITLCFML_VERSION} libcsfml-dev libcsfml-graphics${SITLCFML_VERSION} libcsfml-network${SITLCFML_VERSION} libcsfml-system${SITLCFML_VERSION} libcsfml-window${SITLCFML_VERSION} libsfml-audio${SITLFML_VERSION} libsfml-dev libsfml-graphics${SITLFML_VERSION} libsfml-network${SITLFML_VERSION} libsfml-system${SITLFML_VERSION} libsfml-window${SITLFML_VERSION}"
   else
@@ -258,7 +265,7 @@ elif [ ${RELEASE_CODENAME} == 'lunar' ]; then
     SITL_PKGS+=" libpython3-stdlib" # for argparse
 elif [ ${RELEASE_CODENAME} == 'buster' ]; then
     SITL_PKGS+=" libpython3-stdlib" # for argparse
-else
+elif [ ${RELEASE_CODENAME} != 'mantic' ]; then
   SITL_PKGS+=" python-argparse"
 fi
 
@@ -272,6 +279,9 @@ if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then
     SITL_PKGS+=" libjpeg8-dev"
   elif [ ${RELEASE_CODENAME} == 'lunar' ]; then
     SITL_PKGS+=" libgtk-3-dev libwxgtk3.2-dev "
+  elif [ ${RELEASE_CODENAME} == 'mantic' ]; then
+    SITL_PKGS+=" libgtk-3-dev libwxgtk3.2-dev "
+    # see below
   elif apt-cache search python-wxgtk3.0 | grep wx; then
       SITL_PKGS+=" python-wxgtk3.0"
   elif apt-cache search python3-wxgtk4.0 | grep wx; then
@@ -287,6 +297,10 @@ if [[ $SKIP_AP_GRAPHIC_ENV -ne 1 ]]; then
       PYTHON_PKGS+=" opencv-python"
       SITL_PKGS+=" python3-wxgtk4.0"
       SITL_PKGS+=" fonts-freefont-ttf libfreetype6-dev libpng16-16 libportmidi-dev libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsdl1.2-dev"  # for pygame
+  elif [ ${RELEASE_CODENAME} == 'mantic' ]; then
+      PYTHON_PKGS+=" wxpython opencv-python"
+      SITL_PKGS+=" python3-wxgtk4.0"
+      SITL_PKGS+=" fonts-freefont-ttf libfreetype6-dev libpng16-16 libportmidi-dev libsdl-image1.2-dev libsdl-mixer1.2-dev libsdl-ttf2.0-dev libsdl1.2-dev"  # for pygame
   elif [ ${RELEASE_CODENAME} == 'bullseye' ] ||
          [ ${RELEASE_CODENAME} == 'groovy' ] ||
          [ ${RELEASE_CODENAME} == 'buster' ] ||
@@ -329,7 +343,8 @@ fi
 PIP_USER_ARGUMENT="--user"
 
 # create a Python venv on more recent releases:
-if [ ${RELEASE_CODENAME} == 'lunar' ]; then
+if [ ${RELEASE_CODENAME} == 'lunar' ] ||
+   [ ${RELEASE_CODENAME} == 'mantic' ]; then
     $APT_GET install python3.11-venv
     python3 -m venv $HOME/venv-ardupilot
 
@@ -354,7 +369,8 @@ if [ "$GITHUB_ACTIONS" == "true" ]; then
     PIP_USER_ARGUMENT+=" --progress-bar off"
 fi
 
-if [ ${RELEASE_CODENAME} == 'lunar' ]; then
+if [ ${RELEASE_CODENAME} == 'lunar' ] ||
+   [ ${RELEASE_CODENAME} == 'mantic' ]; then
     # must do this ahead of wxPython pip3 run :-/
     $PIP install $PIP_USER_ARGUMENT -U attrdict3
 fi

Tools/vagrant/initvagrant.sh

@@ -42,6 +42,8 @@ sudo -u $VAGRANT_USER ln -fs /vagrant/Tools/vagrant/screenrc /home/$VAGRANT_USER
 perl -pe 's/kernel.yama.ptrace_scope = ./kernel.yama.ptrace_scope = 0/' -i /etc/sysctl.d/10-ptrace.conf
 echo 0 > /proc/sys/kernel/yama/ptrace_scope
 
+RELEASE_CODENAME=$(lsb_release -c -s)
+
 if [ ${RELEASE_CODENAME} != 'bionic' ]; then
     # build JSB sim
     apt-get install -y libtool automake autoconf libexpat1-dev cmake
@@ -63,8 +65,6 @@ echo "source $BASHRC_GIT" |
 # link a half-way decent .mavinit.scr into place:
 sudo --login -u $VAGRANT_USER ln -sf /vagrant/Tools/vagrant/mavinit.scr /home/$VAGRANT_USER/.mavinit.scr
 
-RELEASE_CODENAME=$(lsb_release -c -s)
-
 # no multipath available, stop mutlipathd complaining about lack of data:
 if [ ${RELEASE_CODENAME} == 'jammy' ]; then
     cat >>/etc/multipath.conf <<EOF