diff --git a/.github/workflows/colcon.yml b/.github/workflows/colcon.yml index 4cd2df25af2f6..052726313c003 100644 --- a/.github/workflows/colcon.yml +++ b/.github/workflows/colcon.yml @@ -191,7 +191,7 @@ jobs: - name: install Micro-XRCE-DDS-Gen from source run: | cd /tmp - git clone --depth 1 --recurse-submodules --branch v4.7.0 https://github.com/ardupilot/Micro-XRCE-DDS-Gen.git + git clone --depth 1 --recurse-submodules --branch v4.7.1 https://github.com/ardupilot/Micro-XRCE-DDS-Gen.git cd Micro-XRCE-DDS-Gen ./gradlew assemble - name: Build with colcon diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index b45a55dd6386b..bee3f93112f48 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -9,4 +9,9 @@ jobs: steps: - uses: actions/checkout@v5 - uses: actions/setup-python@v5 + with: + python-version: 3.x - uses: pre-commit/action@v3.0.1 + - run: python3 -m pip install --upgrade pip + - run: python3 -m pip install pytest + - run: PYTHONPATH="." pytest tests/test_pre_commit_copyright.py diff --git a/.github/workflows/test_dds.yml b/.github/workflows/test_dds.yml index a64b956abf6de..1307214daa0ab 100644 --- a/.github/workflows/test_dds.yml +++ b/.github/workflows/test_dds.yml @@ -173,7 +173,7 @@ jobs: - name: install Micro-XRCE-DDS-Gen from source run: | cd /tmp - git clone --recurse-submodules https://github.com/ardupilot/Micro-XRCE-DDS-Gen.git + git clone --depth 1 --recurse-submodules --branch v4.7.1 https://github.com/ardupilot/Micro-XRCE-DDS-Gen.git cd Micro-XRCE-DDS-Gen ./gradlew assemble - name: test ${{matrix.config}} diff --git a/.github/workflows/test_sitl_rover.yml b/.github/workflows/test_sitl_rover.yml index 55c3a3250209a..affdc87f325e0 100644 --- a/.github/workflows/test_sitl_rover.yml +++ b/.github/workflows/test_sitl_rover.yml @@ -195,6 +195,7 @@ jobs: run: | sudo apt-get update || true sudo apt-get -y install ppp || true + pppd --help # fail with `command not found` if ppp install failed git config --global --add safe.directory ${GITHUB_WORKSPACE} if [[ ${{ matrix.toolchain }} == "clang" ]]; then export CC=clang diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e0116c0fbb6c0..e9d0001879d8f 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -16,7 +16,7 @@ exclude: | repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v5.0.0 + rev: v6.0.0 hooks: #- id: trailing-whitespace #- id: end-of-file-fixer @@ -55,6 +55,44 @@ repos: rev: 7.3.0 hooks: - id: flake8 + + - repo: local + hooks: + - id: ament-pep257 + name: Run ament_pep257 on Tools/ros2 + entry: bash -c 'ament_pep257 Tools/ros2 > /dev/null' + language: python + additional_dependencies: + - ament-lint-pep257 + - tomli;python_version<'3.11' + pass_filenames: false + files: ^Tools/ros2/ + + - repo: https://github.com/codespell-project/codespell + rev: v2.4.1 + hooks: + - id: codespell # See pyproject.toml for args + types_or: [python] + args: + - --ignore-words-list=afile,bodgy,chek,commitish,edn,empy,finis,fram,mot,ned,parm,releas,parms,rin,ser,sitl,siz + additional_dependencies: + - tomli + + - repo: local + hooks: + - id: check-copyright + name: check copyright notice in files + entry: Tools/gittools/pre_commit_copyright.py + language: python + files: | + (?x)^( + Tools/ros2/.*\.py + )$ + args: [ + --ignore=Tools/ros2/ardupilot_dds_tests/setup.py, + --ignore=Tools/ros2/ardupilot_dds_tests/test/test_pep257.py, + --ignore=Tools/ros2/ardupilot_dds_tests/test/test_copyright.py, + ] # # Use to sort python imports by name and put system import first. # - repo: https://github.com/pycqa/isort diff --git a/Dockerfile b/Dockerfile index c1545dd54aa01..22c0d317e7604 100644 --- a/Dockerfile +++ b/Dockerfile @@ -48,7 +48,7 @@ RUN git config --global --add safe.directory $PWD RUN echo "if [ -d \"\$HOME/.local/bin\" ] ; then\nPATH=\"\$HOME/.local/bin:\$PATH\"\nfi" >> ~/.ardupilot_env # Clone & install Micro-XRCE-DDS-Gen dependency -RUN git clone --recurse-submodules https://github.com/ardupilot/Micro-XRCE-DDS-Gen.git /home/${USER_NAME}/Micro-XRCE-DDS-Gen \ +RUN git clone --recurse-submodules --depth 1 --branch v4.7.1 https://github.com/ardupilot/Micro-XRCE-DDS-Gen.git /home/${USER_NAME}/Micro-XRCE-DDS-Gen \ && cd /home/${USER_NAME}/Micro-XRCE-DDS-Gen \ && ./gradlew assemble \ && export AP_ENV_LOC="/home/${USER_NAME}/.ardupilot_env" \ diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 09758436f2363..809e8640ec232 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -98,7 +98,7 @@ def srcpath(path): # need to check the hwdef.h file for the board to see if dds is enabled # the issue here is that we need to configure the env properly to include # the DDS library, but the definition is the the hwdef file - # and can be overriden by the commandline options + # and can be overridden by the commandline options with open(env.BUILDROOT + "/hwdef.h", 'r', encoding="utf8") as file: if "#define AP_DDS_ENABLED 1" in file.read(): # Enable DDS if the hwdef file has it enabled @@ -833,8 +833,7 @@ def configure_env(self, cfg, env): # wrap malloc to ensure memory is zeroed if cfg.env.DEST_OS == 'cygwin': - # on cygwin we need to wrap _malloc_r instead - env.LINKFLAGS += ['-Wl,--wrap,_malloc_r'] + pass # handled at runtime in libraries/AP_Common/c++.cpp elif platform.system() != 'Darwin': env.LINKFLAGS += ['-Wl,--wrap,malloc'] diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 74a81d6c04489..3f27e5214fa15 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -9537,11 +9537,12 @@ def TestTetherStuck(self): # Simulate vehicle getting stuck by increasing RC throttle self.set_rc(3, 1900) - self.delay_sim_time(5, reason='let tether get stuck') + self.delay_sim_time(10, reason='let tether get stuck') # Monitor behavior for 10 seconds tstart = self.get_sim_time() initial_alt = self.get_altitude() + self.progress(f"initial_alt={initial_alt}") stuck = True # Assume it's stuck unless proven otherwise while self.get_sim_time() - tstart < 10: @@ -9556,7 +9557,7 @@ def TestTetherStuck(self): # Check if the vehicle is stuck. # We assume the vehicle is stuck if the current is high and the altitude is not changing - if battery_status and (battery_status.current_battery < 6500 or abs(current_alt - initial_alt) > 2): + if battery_status and (battery_status.current_battery < 6500 or abs(current_alt - initial_alt) > 3): stuck = False # Vehicle moved or current is abnormal break @@ -13999,7 +14000,7 @@ def RudderDisarmMidair(self): self.wait_ready_to_arm() self.change_mode('STABILIZE') - # Set home current location, this gives a large home vs orgin difference + # Set home current location, this gives a large home vs origin difference self.set_home(self.mav.location()) self.set_rc(4, 2000) diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py index a7f351e0dffa5..303860499f873 100644 --- a/Tools/autotest/quadplane.py +++ b/Tools/autotest/quadplane.py @@ -2835,8 +2835,8 @@ def TerrainAvoidApplet(self): "SIM_SONAR_SCALE": 10, }) - # This mission triggers an intersting selection of "Pitching", "Quading" and "CMTC" events - # it's not always consistent, perhaps due to wind, so the tests try to accomodate variances. + # This mission triggers an interesting selection of "Pitching", "Quading" and "CMTC" events + # it's not always consistent, perhaps due to wind, so the tests try to accommodate variances. filename = "TopOfTheWorldShort.waypoints" self.progress("Flying mission %s" % filename) num_wp = self.load_mission(filename) diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 91f748182a362..2dfcd34ec4653 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -415,6 +415,9 @@ if $IS_DOCKER; then echo "# ArduPilot env file. Need to be loaded by your Shell." > ~/$SHELL_LOGIN fi +SCRIPT_DIR="$(dirname "$(realpath "${BASH_SOURCE[0]}")")" +ARDUPILOT_ROOT=$(realpath "$SCRIPT_DIR/../../") + PIP_USER_ARGUMENT="--user" # create a Python venv on more recent releases: @@ -433,14 +436,31 @@ fi if [ -n "$PYTHON_VENV_PACKAGE" ]; then $APT_GET install $PYTHON_VENV_PACKAGE - python3 -m venv --system-site-packages $HOME/venv-ardupilot + + # Check if venv already exists in ARDUPILOT_ROOT (check both venv-ardupilot and venv) + VENV_PATH="" + if [ -d "$ARDUPILOT_ROOT/venv-ardupilot" ]; then + VENV_PATH="$ARDUPILOT_ROOT/venv-ardupilot" + echo "Found existing venv at $VENV_PATH" + elif [ -d "$ARDUPILOT_ROOT/venv" ]; then + VENV_PATH="$ARDUPILOT_ROOT/venv" + echo "Found existing venv at $VENV_PATH" + elif [ -d "$ARDUPILOT_ROOT/.venv" ]; then + VENV_PATH="$ARDUPILOT_ROOT/.venv" + echo "Found existing venv at $VENV_PATH" + else + VENV_PATH="$HOME/venv-ardupilot" + echo "Creating new venv at $VENV_PATH" + python3 -m venv --system-site-packages "$VENV_PATH" + fi + + SOURCE_LINE="source $VENV_PATH/bin/activate" # activate it: - SOURCE_LINE="source $HOME/venv-ardupilot/bin/activate" $SOURCE_LINE PIP_USER_ARGUMENT="" - if [[ -z "${DO_PYTHON_VENV_ENV}" ]] && maybe_prompt_user "Make ArduPilot venv default for python [N/y]?" ; then + if [[ -z "${DO_PYTHON_VENV_ENV}" ]] && maybe_prompt_user "Make ArduPilot venv default for python [N/y]?\nThis means that any terminal will open and load ArduPilot venv" ; then DO_PYTHON_VENV_ENV=1 fi @@ -478,8 +498,32 @@ fi for PACKAGE in $PYTHON_PKGS; do if [ "$PACKAGE" == "wxpython" ]; then echo "##### $PACKAGE takes a *VERY* long time to install (~30 minutes). Be patient." + + # Use wheel repository for specific supported Ubuntu releases only + case ${RELEASE_CODENAME} in + focal) + echo "##### Adding wxpython wheel repository for faster installation" + WXPYTHON_WHEEL_REPO="https://extras.wxpython.org/wxPython4/extras/linux/gtk3/ubuntu-20.04" + time $PIP install $PIP_USER_ARGUMENT -U -f $WXPYTHON_WHEEL_REPO $PACKAGE + ;; + jammy) + echo "##### Adding wxpython wheel repository for faster installation" + WXPYTHON_WHEEL_REPO="https://extras.wxpython.org/wxPython4/extras/linux/gtk3/ubuntu-22.04" + time $PIP install $PIP_USER_ARGUMENT -U -f $WXPYTHON_WHEEL_REPO $PACKAGE + ;; + noble) + echo "##### Adding wxpython wheel repository for faster installation" + WXPYTHON_WHEEL_REPO="https://extras.wxpython.org/wxPython4/extras/linux/gtk3/ubuntu-24.04" + time $PIP install $PIP_USER_ARGUMENT -U -f $WXPYTHON_WHEEL_REPO $PACKAGE + ;; + *) + echo "##### Installing wxpython from PyPI (no specific wheel repository for this release)" + time $PIP install $PIP_USER_ARGUMENT -U $PACKAGE + ;; + esac + else + time $PIP install $PIP_USER_ARGUMENT -U $PACKAGE fi - time $PIP install $PIP_USER_ARGUMENT -U $PACKAGE done # somehow Plucky really wants Pillow reinstalled or MAVProxy's map @@ -509,9 +553,6 @@ fi heading "Adding ArduPilot Tools to environment" -SCRIPT_DIR="$(dirname "$(realpath "${BASH_SOURCE[0]}")")" -ARDUPILOT_ROOT=$(realpath "$SCRIPT_DIR/../../") - if [[ $DO_AP_STM_ENV -eq 1 ]]; then exportline="export PATH=$OPT/$ARM_ROOT/bin:\$PATH"; grep -Fxq "$exportline" ~/$SHELL_LOGIN 2>/dev/null || { diff --git a/Tools/gittools/pre_commit_copyright.py b/Tools/gittools/pre_commit_copyright.py new file mode 100755 index 0000000000000..a64bcf2ac98e5 --- /dev/null +++ b/Tools/gittools/pre_commit_copyright.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 + +# CAUTION: The first docstring in this file will be the copyright notice that will be +# looked for in all file paths that the pre-commit hook passes in. If all files +# contain the __doc__ text below, the commit will be allowed to proceed. + +"""ArduPilot.org. +# +# 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 . +""" + +# --- Do not merges these two docstrings. See CAUTION above. --- + +from argparse import ArgumentParser +from pathlib import Path + +r""" +A pre-commit hook that will use the content of __doc__ and then ensure all files +processed contain that same copyright information. This is useful to ensure that all +those files consistent copyright notices, which is important for legal and compliance +reasons. If any files does not contain the expected copyright information, the commit +will be aborted and an error message will be displayed with the file paths of all files +that do not match. + +Place an entry into the local `.pre-commit-config.yaml` file to run this job. +```yaml + - repo: local + hooks: + - id: check-copyright + name: check copyright notice in files + entry: Tools/gittools/pre_commit_copyright.py + language: python + files: | + (?x)^( + Tools/ros2/.*\.py + )$ + args: [ + --ignore=excluded_file.py, + --ignore=another_excluded.py, + ] +``` + +Usage: +1. Place this script in the `Tools/gittools/pre_commit_copyright.py` file. +2. Ensure the docstring of this file is set to the expected copyright notice. +3. Add the entry to your `.pre-commit-config.yaml` file as shown above. +4. Run `pre-commit install` to set up the pre-commit hooks. +5. Now, when you try to commit changes, this hook will check for the copyright notice. +6. If any files do not contain the expected copyright notice, the commit will be aborted. +7. The error message will list all files that do not match the expected copyright notice. +8. Use --ignore=file_path to exclude specific files from copyright checking. + +Command line usage: +python pre_commit_copyright.py file1.py file2.py --ignore=excluded_file.py \ + --ignore=another_excluded.py +""" + + +def get_file_paths() -> list[str]: + """Parse command line arguments and return a sorted list of file paths excluding + ignored files.""" + parser = ArgumentParser(description="Check copyright notice in files") + parser.add_argument("files", nargs="+", help="File paths to check") + parser.add_argument( + "--ignore", + action="append", + default=[], + help="File paths to ignore (can be used multiple times)", + ) + args = parser.parse_args() + return sorted(set(args.files) - set(args.ignore)) + + +def check_copyright(file_path: Path) -> bool: + """Check if the file contains the expected copyright notice.""" + try: + return __doc__ in file_path.read_text() + except Exception as e: # noqa: BLE001 Path.read_text() can raise many exceptions + print(f"Error reading {file_path}: {e}") + return False + + +def main(): + """Main function to check all file paths from command line arguments. + + pre-commit will repeatedly call this function and each time it will pass ~four file + paths to be checked. + + Supports --ignore flags to exclude specific files from copyright checking. + """ + if failed_files := [ + path for path in get_file_paths() if not check_copyright(Path(path)) + ]: + raise SystemExit(f"Copyright not found in: {', '.join(failed_files)}") + + +if __name__ == "__main__": + main() diff --git a/Tools/scripts/build_ci.sh b/Tools/scripts/build_ci.sh index aedfbb72ddc89..376005fdccbe1 100755 --- a/Tools/scripts/build_ci.sh +++ b/Tools/scripts/build_ci.sh @@ -188,6 +188,7 @@ for t in $CI_BUILD_TARGET; do if [ "$t" == "sitltest-rover" ]; then sudo apt-get update || /bin/true sudo apt-get install -y ppp || /bin/true + pppd --help # fail with `command not found` if ppp install failed run_autotest "Rover" "build.Rover" "test.Rover" continue fi diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 036d360f8851a..bd2b84c43dd14 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -140,7 +140,7 @@ def config_option(self): Feature('AP_Periph', 'LONG_TEXT', 'HAL_PERIPH_SUPPORT_LONG_CAN_PRINTF', 'Enable extended length text strings', 0, None), Feature('AP_Periph', 'PERIPH_DEVICE_TEMPERATURE', 'AP_PERIPH_DEVICE_TEMPERATURE_ENABLED', 'Emit DroneCAN Temperature Messages for AP_Temperature sensors', 0, 'TEMP'), # noqa Feature('AP_Periph', 'PERIPH_MSP', 'AP_PERIPH_MSP_ENABLED', 'Emit MSP protocol messages from AP_Periph', 0, 'MSP'), - Feature('AP_Periph', 'PERIPH_NOTIFY', 'AP_PERIPH_NOTIFY_ENABLED', 'Handle DroneCAN messages for notification equipments (e.g. buzzers, lights etc.)', 0, None), # noqa + Feature('AP_Periph', 'PERIPH_NOTIFY', 'AP_PERIPH_NOTIFY_ENABLED', 'Handle DroneCAN messages for notification equipment (e.g. buzzers, lights etc.)', 0, None), # noqa Feature('AP_Periph', 'PERIPH_SERIAL_OPTIONS', 'AP_PERIPH_SERIAL_OPTIONS_ENABLED', 'Enable Serial Options on AP_Periph', 0, None), # noqa Feature('AP_Periph', 'PERIPH_BATTERY', 'AP_PERIPH_BATTERY_ENABLED', 'Emit DroneCAN battery info messages using AP_BattMonitor', 0, None), # noqa Feature('AP_Periph', 'PERIPH_RELAY', 'AP_PERIPH_RELAY_ENABLED', 'Handle DroneCAN hardpoint command', 0, 'RELAY'), diff --git a/Tools/scripts/size_compare_branches.py b/Tools/scripts/size_compare_branches.py index b27430e445166..ec85b4b6e7f5d 100755 --- a/Tools/scripts/size_compare_branches.py +++ b/Tools/scripts/size_compare_branches.py @@ -13,7 +13,7 @@ Starting in the ardupilot directory. ~/ardupilot $ python Tools/scripts/size_compare_branches.py --branch=[PR_BRANCH_NAME] --vehicle=copter -Output is placed into ../ELF_DIFF_[VEHICLE_NAME] +Output is placed into ELF_DIFF_[VEHICLE_NAME] ''' import copy @@ -98,9 +98,6 @@ def __init__(self, self.jobs = jobs self.features = features - if self.bin_dir is None: - self.bin_dir = self.find_bin_dir() - self.boards_by_name = {} for board in board_list.BoardList().boards: self.boards_by_name[board.name] = board @@ -247,9 +244,9 @@ def esp32_board_names(self): 'esp32diy', ] - def find_bin_dir(self): + def find_bin_dir(self, toolchain_prefix="arm-none-eabi-"): '''attempt to find where the arm-none-eabi tools are''' - binary = shutil.which("arm-none-eabi-g++") + binary = shutil.which(toolchain_prefix + "g++") if binary is None: return None return os.path.dirname(binary) @@ -629,6 +626,13 @@ def elf_diff_results(self, result_master, result_branch): toolchain = "" else: toolchain += "-" + + if self.bin_dir is None: + self.bin_dir = self.find_bin_dir(toolchain_prefix=toolchain) + if self.bin_dir is None: + self.progress(f"Gtoolchain: {self.toolchain}") + raise ValueError("Crap") + elf_diff_commandline = [ "time", "python3", @@ -637,7 +641,7 @@ def elf_diff_results(self, result_master, result_branch): f'--bin_prefix={toolchain}', "--old_alias", "%s %s" % (master_branch, elf_filename), "--new_alias", "%s %s" % (branch, elf_filename), - "--html_dir", "../ELF_DIFF_%s_%s" % (board, vehicle_name), + "--html_dir", "ELF_DIFF_%s_%s" % (board, vehicle_name), ] try: @@ -703,10 +707,7 @@ def compare_task_results_elf_diff(self, pairs): if "master" not in pair or "branch" not in pair: # probably incomplete: continue - try: - self.elf_diff_results(pair["master"], pair["branch"]) - except Exception as e: - print(f"Exception calling elf_diff: {e}") + self.elf_diff_results(pair["master"], pair["branch"]) def emit_csv_for_results(self, results): '''emit dictionary of dictionaries as a CSV''' diff --git a/libraries/AP_CANManager/AP_CANManager.h b/libraries/AP_CANManager/AP_CANManager.h index 367e357e0e198..34d70791d0e1b 100644 --- a/libraries/AP_CANManager/AP_CANManager.h +++ b/libraries/AP_CANManager/AP_CANManager.h @@ -26,10 +26,6 @@ #include #include "AP_SLCANIface.h" #include "AP_CANDriver.h" -#include -#if HAL_GCS_ENABLED -#include "AP_MAVLinkCAN.h" -#endif #include "AP_CAN.h" @@ -104,23 +100,6 @@ class AP_CANManager static const struct AP_Param::GroupInfo var_info[]; -#if HAL_GCS_ENABLED - inline bool handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg) - { - return mavlink_can.handle_can_forward(chan, packet, msg); - } - - inline void handle_can_frame(const mavlink_message_t &msg) - { - mavlink_can.handle_can_frame(msg); - } - - inline void handle_can_filter_modify(const mavlink_message_t &msg) - { - mavlink_can.handle_can_filter_modify(msg); - } -#endif - private: // Parameter interface for CANIfaces @@ -192,11 +171,6 @@ class AP_CANManager HAL_Semaphore _sem; -#if HAL_GCS_ENABLED - // Class for handling MAVLink CAN frames - AP_MAVLinkCAN mavlink_can; -#endif // HAL_GCS_ENABLED - #if AP_CAN_LOGGING_ENABLED && HAL_LOGGING_ENABLED /* handler for CAN frames for logging diff --git a/libraries/AP_CANManager/AP_CANManager_config.h b/libraries/AP_CANManager/AP_CANManager_config.h index a33ff66888a0d..06805aea20161 100644 --- a/libraries/AP_CANManager/AP_CANManager_config.h +++ b/libraries/AP_CANManager/AP_CANManager_config.h @@ -10,3 +10,7 @@ #ifndef AP_CAN_LOGGING_ENABLED #define AP_CAN_LOGGING_ENABLED HAL_MAX_CAN_PROTOCOL_DRIVERS && HAL_LOGGING_ENABLED #endif + +#ifndef AP_MAVLINKCAN_ENABLED +#define AP_MAVLINKCAN_ENABLED (HAL_CANMANAGER_ENABLED && HAL_GCS_ENABLED) +#endif // AP_MAVLINKCAN_ENABLED diff --git a/libraries/AP_CANManager/AP_MAVLinkCAN.cpp b/libraries/AP_CANManager/AP_MAVLinkCAN.cpp index 358cd0c7b21cc..5940157968aa0 100644 --- a/libraries/AP_CANManager/AP_MAVLinkCAN.cpp +++ b/libraries/AP_CANManager/AP_MAVLinkCAN.cpp @@ -17,14 +17,52 @@ #include #include -#if HAL_CANMANAGER_ENABLED && HAL_GCS_ENABLED +#if AP_MAVLINKCAN_ENABLED extern const AP_HAL::HAL& hal; +static AP_MAVLinkCAN *singleton; + +AP_MAVLinkCAN *AP_MAVLinkCAN::ensure_singleton() +{ + if (singleton == nullptr) { + singleton = NEW_NOTHROW AP_MAVLinkCAN(); + } + return singleton; +} + +bool AP_MAVLinkCAN::handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg) +{ + auto *s = ensure_singleton(); + if (s == nullptr) { + return false; + } + return singleton->_handle_can_forward(chan, packet, msg); +} + +void AP_MAVLinkCAN::handle_can_frame(const mavlink_message_t &msg) +{ + auto *s = ensure_singleton(); + if (s == nullptr) { + return; + } + singleton->_handle_can_frame(msg); +} + +void AP_MAVLinkCAN::handle_can_filter_modify(const mavlink_message_t &msg) +{ + auto *s = ensure_singleton(); + if (s == nullptr) { + return; + } + singleton->_handle_can_filter_modify(msg); +} + + /* handle MAV_CMD_CAN_FORWARD mavlink long command */ -bool AP_MAVLinkCAN::handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg) +bool AP_MAVLinkCAN::_handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg) { WITH_SEMAPHORE(can_forward.sem); const int8_t bus = int8_t(packet.param1)-1; @@ -71,7 +109,7 @@ bool AP_MAVLinkCAN::handle_can_forward(mavlink_channel_t chan, const mavlink_com /* handle a CAN_FRAME packet */ -void AP_MAVLinkCAN::handle_can_frame(const mavlink_message_t &msg) +void AP_MAVLinkCAN::_handle_can_frame(const mavlink_message_t &msg) { if (frame_buffer == nullptr) { // allocate frame buffer @@ -164,7 +202,7 @@ void AP_MAVLinkCAN::process_frame_buffer() /* handle a CAN_FILTER_MODIFY packet */ -void AP_MAVLinkCAN::handle_can_filter_modify(const mavlink_message_t &msg) +void AP_MAVLinkCAN::_handle_can_filter_modify(const mavlink_message_t &msg) { mavlink_can_filter_modify_t p; mavlink_msg_can_filter_modify_decode(&msg, &p); @@ -319,4 +357,4 @@ void AP_MAVLinkCAN::can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &fram } } -#endif // HAL_CANMANAGER_ENABLED && HAL_GCS_ENABLED +#endif // AP_MAVLINKCAN_ENABLED diff --git a/libraries/AP_CANManager/AP_MAVLinkCAN.h b/libraries/AP_CANManager/AP_MAVLinkCAN.h index 2ce59958af84b..d238d9c889f09 100644 --- a/libraries/AP_CANManager/AP_MAVLinkCAN.h +++ b/libraries/AP_CANManager/AP_MAVLinkCAN.h @@ -15,12 +15,12 @@ #pragma once -#include -#include #include "AP_CANManager_config.h" -#if HAL_CANMANAGER_ENABLED && HAL_GCS_ENABLED +#if AP_MAVLINKCAN_ENABLED +#include +#include #include #include #include @@ -29,18 +29,15 @@ class AP_MAVLinkCAN { public: AP_MAVLinkCAN() {} - - // Process CAN frame forwarding - void process_frame_buffer(); // Handle commands to forward CAN frames to GCS - bool handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg); - + static bool handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg); + // Handle received MAVLink CAN frames - void handle_can_frame(const mavlink_message_t &msg); - + static void handle_can_frame(const mavlink_message_t &msg); + // Handle CAN filter modification - void handle_can_filter_modify(const mavlink_message_t &msg); + static void handle_can_filter_modify(const mavlink_message_t &msg); private: // Callback for receiving CAN frames from CAN bus and sending to GCS @@ -71,6 +68,20 @@ class AP_MAVLinkCAN { // Frame buffer for queuing frames HAL_Semaphore frame_buffer_sem; ObjectBuffer *frame_buffer; + + static AP_MAVLinkCAN *ensure_singleton(); + + // Process CAN frame forwarding + void process_frame_buffer(); + + // Handle commands to forward CAN frames to GCS + bool _handle_can_forward(mavlink_channel_t chan, const mavlink_command_int_t &packet, const mavlink_message_t &msg); + + // Handle received MAVLink CAN frames + void _handle_can_frame(const mavlink_message_t &msg); + + // Handle CAN filter modification + void _handle_can_filter_modify(const mavlink_message_t &msg); }; -#endif // HAL_CANMANAGER_ENABLED && HAL_GCS_ENABLED +#endif // AP_MAVLINKCAN_ENABLED diff --git a/libraries/AP_Common/c++.cpp b/libraries/AP_Common/c++.cpp index 2490005cfe99a..d00e9d46977bd 100644 --- a/libraries/AP_Common/c++.cpp +++ b/libraries/AP_Common/c++.cpp @@ -85,31 +85,50 @@ void operator delete[](void * ptr) } #if defined(CYGWIN_BUILD) && CONFIG_HAL_BOARD == HAL_BOARD_SITL -/* - wrapper around malloc to ensure all memory is initialised as zero - cygwin needs to wrap _malloc_r - */ -#undef _malloc_r -extern "C" { - void *__wrap__malloc_r(_reent *r, size_t size); - void *__real__malloc_r(_reent *r, size_t size); - void *_malloc_r(_reent *r, size_t size); + +// intercept malloc to ensure memory so allocated is zeroed. this is rather +// nasty hack that hopefully does not live long! if it breaks again, that's it! +// this is completely unsupported! + +// __imp_malloc is a pointer to a function that will be called to actually +// do the malloc work. we can't replace it (or malloc itself) at build time as +// that will cause cygwin to disable its own malloc. we therefore use a +// constructor function that runs before main but after cygwin's disable check +// to set the pointer to our own malloc which calls back into cygwin's allocator +// through calloc to actually do the work and the zeroing. + +#include // perror +#include // mprotect and constants + +extern "C" void *__imp_malloc; // the pointer of our affection + +static void *do_the_malloc(size_t size) { // replacement for malloc + // allocate zeroed using calloc (malloc would of course recurse infinitely) + return calloc(1, size); } -void *__wrap__malloc_r(_reent *r, size_t size) -{ - void *ret = __real__malloc_r(r, size); - if (ret != nullptr) { - memset(ret, 0, size); + +// called before main to set __imp_malloc. priority of 101 guarantees execution +// before C++ constructors. +__attribute__((constructor(101))) static void hack_in_malloc() { + // __imp_malloc is in .text which is read-only so make it read-write first + + // compute address of its memory page as mprotect mandates page alignment + size_t page_size = (size_t)sysconf(_SC_PAGESIZE); + void *page_base = (void *)((size_t)&__imp_malloc & (~(page_size-1))); + + // make it writable and executable as we (unlikely) may be executing near it + int res = mprotect(page_base, page_size, PROT_READ|PROT_WRITE|PROT_EXEC); + if (res) { + perror("hack_in_malloc() mprotect writable"); } - return ret; -} -void *_malloc_r(_reent *x, size_t size) -{ - void *ret = __real__malloc_r(x, size); - if (ret != nullptr) { - memset(ret, 0, size); + + *(void **)&__imp_malloc = (void *)&do_the_malloc; // set the pointer now + + // put the page back to read-only (and let execution keep happening) + res = mprotect(page_base, page_size, PROT_READ|PROT_EXEC); + if (res) { + perror("hack_in_malloc() mprotect readable"); } - return ret; } #elif CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS && CONFIG_HAL_BOARD != HAL_BOARD_QURT diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 899877147be80..63ac60c93adb0 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -111,8 +111,35 @@ static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D == (uint32_t)GPS_FIX_T static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D_DGPS == (uint32_t)GPS_FIX_TYPE_DGPS, "FIX_DGPS incorrect"); static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FLOAT == (uint32_t)GPS_FIX_TYPE_RTK_FLOAT, "FIX_RTK_FLOAT incorrect"); static_assert((uint32_t)AP_GPS::GPS_Status::GPS_OK_FIX_3D_RTK_FIXED == (uint32_t)GPS_FIX_TYPE_RTK_FIXED, "FIX_RTK_FIXED incorrect"); + +static_assert(static_cast(AP_GPS::Errors::GPS_SYSTEM_ERROR_NONE) == (uint32_t)0x00, "GPS_SYSTEM_ERROR_NONE incorrect"); //not ideal but state doesn't exist in mavlink +static_assert(static_cast(AP_GPS::Errors::INCOMING_CORRECTIONS) == (uint32_t)GPS_SYSTEM_ERROR_INCOMING_CORRECTIONS, "INCOMING_CORRECTIONS incorrect"); +static_assert(static_cast(AP_GPS::Errors::CONFIGURATION) == (uint32_t)GPS_SYSTEM_ERROR_CONFIGURATION, "CONFIGURATION incorrect"); +static_assert(static_cast(AP_GPS::Errors::SOFTWARE) == (uint32_t)GPS_SYSTEM_ERROR_SOFTWARE, "SOFTWARE incorrect"); +static_assert(static_cast(AP_GPS::Errors::ANTENNA) == (uint32_t)GPS_SYSTEM_ERROR_ANTENNA, "ANTENNA incorrect"); +static_assert(static_cast(AP_GPS::Errors::EVENT_CONGESTION) == (uint32_t)GPS_SYSTEM_ERROR_EVENT_CONGESTION, "EVENT_CONGESTION incorrect"); +static_assert(static_cast(AP_GPS::Errors::CPU_OVERLOAD) == (uint32_t)GPS_SYSTEM_ERROR_CPU_OVERLOAD, "CPU_OVERLOAD incorrect"); +static_assert(static_cast(AP_GPS::Errors::OUTPUT_CONGESTION) == (uint32_t)GPS_SYSTEM_ERROR_OUTPUT_CONGESTION, "OUTPUT_CONGESTION inccorect"); + +static_assert(static_cast(AP_GPS::Authentication::UNKNOWN) == (uint8_t)GPS_AUTHENTICATION_STATE_UNKNOWN, "UNKNOWN incorrect"); +static_assert(static_cast(AP_GPS::Authentication::INITIALIZING) == (uint8_t)GPS_AUTHENTICATION_STATE_INITIALIZING, "INITIALIZING incorrect"); +static_assert(static_cast(AP_GPS::Authentication::ERROR) == (uint8_t)GPS_AUTHENTICATION_STATE_ERROR, "ERROR incorrect"); +static_assert(static_cast(AP_GPS::Authentication::OK) == (uint8_t)GPS_AUTHENTICATION_STATE_OK, "GPS_AUTHENTICATION_STATE_OK incorrect"); +static_assert(static_cast(AP_GPS::Authentication::DISABLED) == (uint8_t)GPS_AUTHENTICATION_STATE_DISABLED, "DISABLED incorrect"); + +static_assert(static_cast(AP_GPS::Jamming::UNKNOWN) == (uint8_t)GPS_JAMMING_STATE_UNKNOWN, "UNKNOWN incorrect"); +static_assert(static_cast(AP_GPS::Jamming::OK) == (uint8_t)GPS_JAMMING_STATE_OK, "OK incorrect"); +static_assert(static_cast(AP_GPS::Jamming::DETECTED) == (uint8_t)GPS_JAMMING_STATE_DETECTED, "DETECTED incorrect"); +static_assert(static_cast(AP_GPS::Jamming::MITIGATED) == (uint8_t)GPS_JAMMING_STATE_MITIGATED, "MITIGATED incorrect"); + +static_assert(static_cast(AP_GPS::Spoofing::UNKNOWN) == (uint8_t)GPS_SPOOFING_STATE_UNKNOWN, "UNKNOWN incorrect"); +static_assert(static_cast(AP_GPS::Spoofing::OK) == (uint8_t)GPS_SPOOFING_STATE_OK, "OK incorrect"); +static_assert(static_cast(AP_GPS::Spoofing::DETECTED) == (uint8_t)GPS_SPOOFING_STATE_DETECTED, "DETECTED incorrect"); +static_assert(static_cast(AP_GPS::Spoofing::MITIGATED) == (uint8_t)GPS_SPOOFING_STATE_MITIGATED, "MITIGATED incorrect"); + #endif + // ensure that our own enum-class status is equivalent to the // ArduPilot-scoped AP_GPS_FixType enumeration: static_assert((uint32_t)AP_GPS::GPS_Status::NO_GPS == (uint8_t)AP_GPS_FixType::NO_GPS, "NO_GPS incorrect"); @@ -1460,6 +1487,27 @@ void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst) } #endif +#if AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED +void AP_GPS::send_mavlink_gnss_integrity(mavlink_channel_t chan, uint8_t instance) { + if(state[instance].has_gnss_integrity) { + + mavlink_msg_gnss_integrity_send(chan, + instance, + state[instance].system_errors, + state[instance].authentication_state, + state[instance].jamming_state, + state[instance].spoofing_state, + UINT8_MAX, //raim_state not implemented yet + UINT16_MAX, //raim_hfom not implemented yet + UINT16_MAX, //raim_vfom not implemented yet + UINT8_MAX, //corrections_quality not implemented yet + UINT8_MAX, //systems_status_summary not implemented yet + UINT8_MAX, //gnss_signal_quality not implemented yet + UINT8_MAX); //post_processing_quality not implemented yet + } +} +#endif // AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + bool AP_GPS::first_unconfigured_gps(uint8_t &instance) const { for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 929ac4394b06d..0e3c9cee4f128 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -156,6 +156,47 @@ class AP_GPS GPS_OK_FIX_3D_RTK_FIXED = 6, ///< Receiving valid messages and 3D RTK Fixed }; + /// GPS error bits. These are kept aligned with MAVLink by + /// static_assert in AP_GPS.cpp + enum class Errors { + GPS_SYSTEM_ERROR_NONE = 0, + INCOMING_CORRECTIONS = 1 << 0, ///< There are problems with incoming correction streams + CONFIGURATION = 1 << 1, ///< There are problems with the configuration + SOFTWARE = 1 << 2, ///< There are problems with the software on the GPS receiver + ANTENNA = 1 << 3, ///< There are problems with an antenna connected to the GPS receiver + EVENT_CONGESTION = 1 << 4, ///< There are problems handling all incoming events + CPU_OVERLOAD = 1 << 5, ///< The GPS receiver CPU is overloaded + OUTPUT_CONGESTION = 1 << 6, ///< The GPS receiver is experiencing output congestion + }; + + /// GPS authentication status. These are kept aligned with MAVLink by + /// static_assert in AP_GPS.cpp + enum class Authentication { + UNKNOWN = 0, ///< The GPS receiver does not provide GPS signal authentication info + INITIALIZING = 1, ///< The GPS receiver is initializing signal authentication + ERROR = 2, ///< The GPS receiver encountered an error while initializing signal authentication + OK = 3, ///< The GPS receiver has correctly authenticated all signals + DISABLED = 4, ///< GPS signal authentication is disabled on the receiver + }; + + /// GPS jamming status. These are kept aligned with MAVLink by + /// static_assert in AP_GPS.cpp + enum class Jamming { + UNKNOWN = 0, ///< The GPS receiver does not provide GPS signal jamming info + OK = 1, ///< The GPS receiver detected no signal jamming + MITIGATED = 2, ///< The GPS receiver detected and mitigated signal jamming + DETECTED = 3, ///< The GPS receiver detected signal jamming + }; + + /// GPS spoofing status. These are kept aligned with MAVLink by + /// static_assert in AP_GPS.cpp + enum class Spoofing { + UNKNOWN = 0, ///< The GPS receiver does not provide GPS signal spoofing info + OK = 1, ///< The GPS receiver detected no signal spoofing + MITIGATED = 2, ///< The GPS receiver detected and mitigated signal spoofing + DETECTED = 3, ///< The GPS receiver detected signal spoofing but still has a fix + }; + // GPS navigation engine settings. Not all GPS receivers support // this enum GPS_Engine_Setting { @@ -242,6 +283,16 @@ class AP_GPS float relPosD; ///< Reported Vertical distance in meters float accHeading; ///< Reported Heading Accuracy in degrees uint32_t relposheading_ts; ///< True if new data has been received since last time it was false + +#if AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + // Integrity message information + // all the following fields must only all be filled by backend drivers + bool has_gnss_integrity; ///< True if the receiver supports GNSS integrity msg + uint32_t system_errors; ///< System errors + uint8_t authentication_state; ///< Authentication state of GNSS signals + uint8_t jamming_state; ///< Jamming state of GNSS signals + uint8_t spoofing_state; ///< Spoofing state of GNSS signals +#endif // AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED }; /// Startup initialisation. @@ -486,6 +537,7 @@ class AP_GPS bool have_gps_yaw_configured(uint8_t instance) const { return state[instance].gps_yaw_configured; } + // the expected lag (in seconds) in the position and velocity readings from the gps // return true if the GPS hardware configuration is known or the lag parameter has been set manually @@ -506,6 +558,8 @@ class AP_GPS void send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst); + void send_mavlink_gnss_integrity(mavlink_channel_t chan, uint8_t inst); + // Returns true if there is an unconfigured GPS, and provides the instance number of the first non configured GPS bool first_unconfigured_gps(uint8_t &instance) const WARN_IF_UNUSED; void broadcast_first_configuration_failure_reason(void) const; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 55d23b33cfe05..9fafb1a83fd22 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -46,6 +46,10 @@ do { \ #define GPS_SBF_STREAM_NUMBER 1 #endif +#ifndef GPS_SBF_STATUS_STREAM_NUMBER + #define GPS_SBF_STATUS_STREAM_NUMBER 3 +#endif + #define SBF_EXCESS_COMMAND_BYTES 5 // 2 start bytes + validity byte + space byte + endline byte #define RX_ERROR_MASK (CONGESTION | \ @@ -54,10 +58,34 @@ do { \ INVALIDCONFIG | \ OUTOFGEOFENCE) +#define EXT_ERROR_MASK (SISERROR | \ + DIFFCORRERROR | \ + EXTSENSORERROR | \ + SETUPERROR) + constexpr const char *AP_GPS_SBF::portIdentifiers[]; constexpr const char* AP_GPS_SBF::_initialisation_blob[]; constexpr const char* AP_GPS_SBF::sbas_on_blob[]; +const AP_GPS_SBF::SBF_Error_Map AP_GPS_SBF::sbf_error_map[] = { + { AP_GPS_SBF::INVALIDCONFIG, AP_GPS::Errors::CONFIGURATION }, + { AP_GPS_SBF::SOFTWARE, AP_GPS::Errors::SOFTWARE }, + { AP_GPS_SBF::ANTENNA, AP_GPS::Errors::ANTENNA }, + { AP_GPS_SBF::MISSEDEVENT, AP_GPS::Errors::EVENT_CONGESTION }, + { AP_GPS_SBF::CPUOVERLOAD, AP_GPS::Errors::CPU_OVERLOAD }, + { AP_GPS_SBF::CONGESTION, AP_GPS::Errors::OUTPUT_CONGESTION }, +}; + +const AP_GPS_SBF::Auth_State_Map AP_GPS_SBF::auth_state_map[] = { + {0, AP_GPS::Authentication::DISABLED}, + {1, AP_GPS::Authentication::INITIALIZING}, + {2, AP_GPS::Authentication::INITIALIZING}, + {3, AP_GPS::Authentication::ERROR}, + {4, AP_GPS::Authentication::ERROR}, + {5, AP_GPS::Authentication::ERROR}, + {6, AP_GPS::Authentication::OK}, +}; + AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, @@ -71,6 +99,11 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, // if we ever parse RTK observations it will always be of type NED, so set it once state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED; +#if AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + //all septentrio receivers support GNSS integrity msg + state.has_gnss_integrity = true; +#endif //AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + // yaw available when option bit set or using dual antenna if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw) || (get_type() == AP_GPS::GPS_Type::GPS_TYPE_SBF_DUAL_ANTENNA)) { @@ -138,6 +171,13 @@ AP_GPS_SBF::read(void) config_string = nullptr; } break; + case Config_State::SSO_Status: + if (asprintf(&config_string, "sso,Stream%d,COM%d,GalAuthStatus+RFStatus+QualityInd+ReceiverStatus,sec1\n", + (int)GPS_SBF_STATUS_STREAM_NUMBER, + (int)params.com_port) == -1) { + config_string = nullptr; + } + break; case Config_State::Constellation: if ((params.gnss_mode&0x6F)!=0) { //IMES not taken into account by Septentrio receivers @@ -387,8 +427,11 @@ AP_GPS_SBF::parse(uint8_t temp) config_step = Config_State::SSO; break; case Config_State::SSO: - config_step = Config_State::Constellation; + config_step = Config_State::SSO_Status; break; + case Config_State::SSO_Status: + config_step = Config_State::Constellation; + break; case Config_State::Constellation: // we can also move to // Config_State::Blob if we choose @@ -559,12 +602,95 @@ AP_GPS_SBF::process_message(void) check_new_itow(temp.TOW, sbf_msg.length); RxState = temp.RxState; if ((RxError & RX_ERROR_MASK) != (temp.RxError & RX_ERROR_MASK)) { - GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF error changed (0x%08x/0x%08x)", (unsigned int)(state.instance + 1), + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF internal error changed (0x%08x/0x%08x)", (unsigned int)(state.instance + 1), (unsigned int)(RxError & RX_ERROR_MASK), (unsigned int)(temp.RxError & RX_ERROR_MASK)); } RxError = temp.RxError; + if ((ExtError & EXT_ERROR_MASK) != (temp.ExtError & EXT_ERROR_MASK)) { + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: SBF external error changed (0x%08x/0x%08x)", (unsigned int)(state.instance + 1), + (unsigned int)(ExtError & EXT_ERROR_MASK), (unsigned int)(temp.ExtError & EXT_ERROR_MASK)); + } + ExtError = temp.ExtError; +#if AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + state.system_errors = static_cast(AP_GPS::Errors::GPS_SYSTEM_ERROR_NONE); + + if (ExtError & DIFFCORRERROR) { + state.system_errors |= static_cast(AP_GPS::Errors::INCOMING_CORRECTIONS); + } + + for (const auto &map_entry : sbf_error_map) { + if (RxError & map_entry.sbf_error_bit) { + state.system_errors |= static_cast(map_entry.ap_error_enum); + } + } +#endif // AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + break; + } + + case RFStatus: + { + const msg4092 &temp = sbf_msg.data.msg4092u; + check_new_itow(temp.TOW, sbf_msg.length); +#if AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + if (temp.Flags==0) { + state.spoofing_state = static_cast(AP_GPS::Spoofing::OK); + } + else { + state.spoofing_state = static_cast(AP_GPS::Spoofing::DETECTED); + } + state.jamming_state = static_cast(AP_GPS::Jamming::OK); + + if (temp.SBLength < sizeof(RFStatusRFBandSubBlock)) { + break; + } + + const uint8_t *p = (const uint8_t *)&temp.RFBand; + const uint8_t *packet_end = (const uint8_t *)&temp + (sbf_msg.length - 8); + + for (uint8_t i = 0; i < temp.N; i++) { + if (p + temp.SBLength > packet_end) { + break; + } + + const RFStatusRFBandSubBlock &rf_band_data = *(const RFStatusRFBandSubBlock *)p; + + switch (rf_band_data.Info & (uint8_t)0b1111) { + case 1: + case 2: + // As long as there is indicated but unmitigated spoofing in one band, don't report the overall state as mitigated + if (state.jamming_state == static_cast(AP_GPS::Jamming::OK)) { + state.jamming_state = static_cast(AP_GPS::Jamming::MITIGATED); + } + break; + case 8: + state.jamming_state = static_cast(AP_GPS::Jamming::DETECTED); + break; + } + p += temp.SBLength; + } +#endif // AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED break; } + + case GALAuthStatus: + { + const msg4245 &temp = sbf_msg.data.msg4245u; + check_new_itow(temp.TOW, sbf_msg.length); +#if AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + state.authentication_state = static_cast(AP_GPS::Authentication::UNKNOWN); + + const uint16_t osnma_status = temp.OSNMAStatus & (uint16_t)0b111; + + for (const auto &map_entry : auth_state_map) { + if (osnma_status == map_entry.osnma_status_code) { + state.authentication_state = static_cast(map_entry.auth_state); + break; + } + } +#endif // AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + break; + } + case VelCovGeodetic: { const msg5908 &temp = sbf_msg.data.msg5908u; diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index b4e1bb28c62b1..052c827f1de62 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -73,6 +73,7 @@ class AP_GPS_SBF : public AP_GPS_Backend enum class Config_State { Baud_Rate, SSO, + SSO_Status, Blob, SBAS, SGA, @@ -103,6 +104,7 @@ class AP_GPS_SBF : public AP_GPS_Backend uint32_t crc_error_counter = 0; uint32_t RxState; uint32_t RxError; + uint8_t ExtError; void mount_disk(void) const; void unmount_disk(void) const; @@ -113,6 +115,8 @@ class AP_GPS_SBF : public AP_GPS_Backend PVTGeodetic = 4007, ReceiverStatus = 4014, BaseVectorGeod = 4028, + RFStatus = 4092, + GALAuthStatus = 4245, VelCovGeodetic = 5908, AttEulerCov = 5939, AuxAntPositions = 5942, @@ -205,6 +209,36 @@ class AP_GPS_SBF : public AP_GPS_Backend VectorInfoGeod info; // there can be multiple baselines here, but we will only consume the first one, so don't worry about anything after }; + struct PACKED RFStatusRFBandSubBlock // RFBand sub-blocks of RFStatus message + { + uint32_t Frequency; + uint16_t Bandwidth; + uint8_t Info; + }; + + struct PACKED msg4092 // RFStatus + { + uint32_t TOW; + uint16_t WNc; + uint8_t N; + uint8_t SBLength; + uint8_t Flags; + uint8_t Reserved[3]; + uint8_t RFBand; // So we can use address and parse sub-blocks manually + }; + + struct PACKED msg4245 // GALAuthStatus + { + uint32_t TOW; + uint16_t WNc; + uint16_t OSNMAStatus; + float TrustedTimeDelta; + uint64_t GalActiveMask; + uint64_t GalAuthenticMask; + uint64_t GpsActiveMask; + uint64_t GpsAuthenticMask; + }; + struct PACKED msg5908 // VelCovGeodetic { uint32_t TOW; @@ -265,6 +299,8 @@ class AP_GPS_SBF : public AP_GPS_Backend msg4001 msg4001u; msg4014 msg4014u; msg4028 msg4028u; + msg4092 msg4092u; + msg4245 msg4245u; msg5908 msg5908u; msg5939 msg5939u; msg5942 msg5942u; @@ -297,6 +333,7 @@ class AP_GPS_SBF : public AP_GPS_Backend enum { SOFTWARE = (1 << 3), // set upon detection of a software warning or error. This bit is reset by the command lif, error WATCHDOG = (1 << 4), // set when the watch-dog expired at least once since the last power-on. + ANTENNA = (1 << 5), // set when antenna overcurrent condition is detected CONGESTION = (1 << 6), // set when an output data congestion has been detected on at least one of the communication ports of the receiver during the last second. MISSEDEVENT = (1 << 8), // set when an external event congestion has been detected during the last second. It indicates that the receiver is receiving too many events on its EVENTx pins. CPUOVERLOAD = (1 << 9), // set when the CPU load is larger than 90%. If this bit is set, receiver operation may be unreliable and the user must decrease the processing load by following the recommendations in the User Manual. @@ -304,6 +341,27 @@ class AP_GPS_SBF : public AP_GPS_Backend OUTOFGEOFENCE = (1 << 11), // set if the receiver is currently out of its permitted region of operation (geo-fencing). }; + enum { + SISERROR = (1 << 0), // set if a violation of the signal-in-space ICD has been detected + DIFFCORRERROR = (1 << 1), // set when an anomaly has been detected in an incoming differential correction stream + EXTSENSORERROR = (1 << 2), // set when a malfunction has been detected on at least one of the external sensors connected to the receiver + SETUPERROR = (1 << 3), // set when a configuration/setup error has been detected + }; + + struct SBF_Error_Map { + const uint32_t sbf_error_bit; + const AP_GPS::Errors ap_error_enum; + }; + + static const SBF_Error_Map sbf_error_map[]; + + struct Auth_State_Map { + const uint16_t osnma_status_code; + const AP_GPS::Authentication auth_state; + }; + + static const Auth_State_Map auth_state_map[]; + static constexpr const char *portIdentifiers[] = { "COM", "USB", "IP1", "NTR", "IPS", "IPR" }; char portIdentifier[5]; uint8_t portLength; diff --git a/libraries/AP_GPS/AP_GPS_config.h b/libraries/AP_GPS/AP_GPS_config.h index ad6b5364a29df..4a5a75e12881e 100644 --- a/libraries/AP_GPS/AP_GPS_config.h +++ b/libraries/AP_GPS/AP_GPS_config.h @@ -120,4 +120,4 @@ #ifndef AP_GPS_GPS2_RTK_SENDING_ENABLED #define AP_GPS_GPS2_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1 && (AP_GPS_SBF_ENABLED || AP_GPS_ERB_ENABLED) -#endif +#endif \ No newline at end of file diff --git a/libraries/AP_Generator/scripts/test-loweheiser.py b/libraries/AP_Generator/scripts/test-loweheiser.py index 4f2531fce33df..8bfc46e52a695 100755 --- a/libraries/AP_Generator/scripts/test-loweheiser.py +++ b/libraries/AP_Generator/scripts/test-loweheiser.py @@ -219,7 +219,7 @@ def send_heartbeats(self): return self.last_heartbeat_sent = now - # self.progress("Sending heatbeat") + # self.progress("Sending heartbeat") self.conn.mav.heartbeat_send( mavutil.mavlink.MAV_TYPE_GCS, mavutil.mavlink.MAV_AUTOPILOT_GENERIC, diff --git a/libraries/AP_HAL/board/esp32.h b/libraries/AP_HAL/board/esp32.h index 60d6d83308247..0c42061463943 100644 --- a/libraries/AP_HAL/board/esp32.h +++ b/libraries/AP_HAL/board/esp32.h @@ -19,7 +19,7 @@ #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_ESP32_S3EMPTY #include "esp32s3empty.h" #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_ESP32_S3M5STAMPFLY -#include "esp32s3m5stampfly.h" // https://shop.m5stack.com/products/m5stamp-fly-with-m5stamps3 +// no include required for stampfly; it is all in hwdef.dat #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_ESP32_IMU_MODULE_V11 #include "esp32imu_module_v11.h" //makerfabs esp32 imu module v1.1 #else diff --git a/libraries/AP_HAL/utility/Socket.cpp b/libraries/AP_HAL/utility/Socket.cpp index 46f71ab9aa167..6935d4154b76c 100644 --- a/libraries/AP_HAL/utility/Socket.cpp +++ b/libraries/AP_HAL/utility/Socket.cpp @@ -159,6 +159,9 @@ bool SOCKET_CLASS_NAME::connect(const char *address, uint16_t port) ret = CALL_PREFIX(connect)(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); if (ret != 0) { + if (errno == EINPROGRESS) { + pending_connect = true; + } return false; } connected = true; @@ -479,6 +482,17 @@ bool SOCKET_CLASS_NAME::pollout(uint32_t timeout_ms) if (CALL_PREFIX(select)(fd+1, nullptr, &fds, nullptr, &tv) != 1) { return false; } + + if (pending_connect) { + int sock_error = 0; + socklen_t len = sizeof(sock_error); + if (CALL_PREFIX(getsockopt)(fd, SOL_SOCKET, SO_ERROR, (void*)&sock_error, &len) == 0 && + sock_error == 0) { + connected = true; + } + pending_connect = false; + } + return true; } diff --git a/libraries/AP_HAL/utility/Socket.hpp b/libraries/AP_HAL/utility/Socket.hpp index 480ef365cb4c3..4c513b5f0cab0 100644 --- a/libraries/AP_HAL/utility/Socket.hpp +++ b/libraries/AP_HAL/utility/Socket.hpp @@ -84,6 +84,10 @@ class SOCKET_CLASS_NAME { return connected; } + bool is_pending(void) const { + return pending_connect; + } + // access to inet_ntop static const char *inet_addr_to_str(uint32_t addr, char *dst, uint16_t len); @@ -104,6 +108,8 @@ class SOCKET_CLASS_NAME { bool connected; + bool pending_connect; + void make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr); }; diff --git a/libraries/AP_HAL_ESP32/boards/esp32s3m5stampfly.h b/libraries/AP_HAL_ESP32/boards/esp32s3m5stampfly.h deleted file mode 100644 index dbad5639a39fd..0000000000000 --- a/libraries/AP_HAL_ESP32/boards/esp32s3m5stampfly.h +++ /dev/null @@ -1,120 +0,0 @@ -/* - * This file 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 file 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 . - */ -#pragma once - -// GROVE EXPANSION -// -// The Stampfly has two Seeed Studio Grove connectors to attach peripherals. By -// default, the left (red) connector is I2C (with no other devices on the bus) -// and the right (black) connector is SERIAL3. However, the two signal pins in -// each can be reassigned by editing this file. -// -// Grove Red (I2C) -// 1 (toward front): GPIO_NUM_15, SCL (has 4.7kΩ pullup to 3.3V) -// 2 : GPIO_NUM_13, SDA (has 4.7kΩ pullup to 3.3V) -// 3 : +V (+5V if plugged into USB, else +Vbat) -// 4 (toward rear) : GND -// -// Grove Black (SERIAL3) -// 1 (toward rear) : GPIO_NUM_1, RX -// 2 : GPIO_NUM_3, TX -// 3 : +V (+5V if plugged into USB, else +Vbat) -// 4 (toward front): GND - -// make sensor selection clearer -#define PROBE_IMU_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,GET_I2C_DEVICE(bus, addr),##args)) -#define PROBE_IMU_SPI(driver, devname, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,hal.spi->get_device(devname),##args)) -#define PROBE_IMU_SPI2(driver, devname1, devname2, args ...) ADD_BACKEND(AP_InertialSensor_ ## driver::probe(*this,hal.spi->get_device(devname1),hal.spi->get_device(devname2),##args)) - -#define PROBE_BARO_I2C(driver, bus, addr, args ...) ADD_BACKEND(AP_Baro_ ## driver::probe(*this,std::move(GET_I2C_DEVICE(bus, addr)),##args)) -#define PROBE_BARO_SPI(driver, devname, args ...) ADD_BACKEND(AP_Baro_ ## driver::probe(*this,std::move(hal.spi->get_device(devname)),##args)) - -#define PROBE_MAG_I2C(driver, bus, addr, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe(GET_I2C_DEVICE(bus, addr),##args)) -#define PROBE_MAG_SPI(driver, devname, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe(hal.spi->get_device(devname),##args)) -#define PROBE_MAG_IMU(driver, imudev, imu_instance, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe_ ## imudev(imu_instance,##args)) -#define PROBE_MAG_IMU_I2C(driver, imudev, bus, addr, args ...) ADD_BACKEND(DRIVER_ ##driver, AP_Compass_ ## driver::probe_ ## imudev(GET_I2C_DEVICE(bus,addr),##args)) -//------------------------------------ - - -#define CONFIG_HAL_BOARD_SUBTYPE HAL_BOARD_SUBTYPE_ESP32_S3M5STAMPFLY -// make sensor selection clearer - -//- these are missing from esp-idf......will not be needed later -#define RTC_WDT_STG_SEL_OFF 0 -#define RTC_WDT_STG_SEL_INT 1 -#define RTC_WDT_STG_SEL_RESET_CPU 2 -#define RTC_WDT_STG_SEL_RESET_SYSTEM 3 -#define RTC_WDT_STG_SEL_RESET_RTC 4 - -#define HAL_ESP32_BOARD_NAME "esp32s3m5stampfly" - -#define HAL_INS_PROBE_LIST PROBE_IMU_SPI(BMI270, "bmi270", ROTATION_ROLL_180_YAW_90) - -#define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(BMP280, 0, 0x76) - -#define AP_COMPASS_ENABLE_DEFAULT 0 -#define ALLOW_ARM_NO_COMPASS - -// no ADC -#define HAL_DISABLE_ADC_DRIVER 1 -#define HAL_USE_ADC 0 - -#define AP_BATTERY_INA3221_ENABLED 1 -#define HAL_BATTMON_INA3221_SHUNT_OHMS 0.01 -#define HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_4156US -#define HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_4156US -#define HAL_BATTMON_INA3221_AVG_MODE_SEL HAL_BATTMON_INA3221_AVG_MODE_16 - -// 2 use udp, 1 use tcp... for udp,client needs to connect as UDPCL in missionplanner etc to 192.168.4.1 port 14550 -#define HAL_ESP32_WIFI 2 - -#define WIFI_SSID "ardupilot123" -#define WIFI_PWD "ardupilot123" - -//RCOUT which pins are used? -// r-up, l-down, l-up, r-down (quad X order) -#define HAL_ESP32_RCOUT { GPIO_NUM_42, GPIO_NUM_10, GPIO_NUM_5, GPIO_NUM_41 } - -// SPI BUS setup, including gpio, dma, etc is in the hwdef.dat -// SPI per-device setup, including speeds, etc. is in the hwdef.dat - -//I2C bus list. bus 0 is internal, bus 1 is the red grove connector -#define HAL_ESP32_I2C_BUSES \ - {.port=I2C_NUM_0, .sda=GPIO_NUM_3, .scl=GPIO_NUM_4, .speed=400*KHZ, .internal=true}, \ - {.port=I2C_NUM_1, .sda=GPIO_NUM_13, .scl=GPIO_NUM_15, .speed=400*KHZ, .internal=false} - -// rcin on what pin? enable AP_RCPROTOCOL_ENABLED below if using -// currently on another unmapped pin -//#define HAL_ESP32_RCIN GPIO_NUM_16 -#define HAL_ESP32_RMT_RX_PIN_NUMBER GPIO_NUM_16 - - -// HARDWARE UARTS. UART 0 apparently goes over USB? so we assign it to pins we -// don't have mapped to anything. might be fixable in the HAL... -// UART 1 (SERIAL3) is the black grove connector -#define HAL_ESP32_UART_DEVICES \ - {.port=UART_NUM_0, .rx=GPIO_NUM_18, .tx=GPIO_NUM_17 }, \ - {.port=UART_NUM_1, .rx=GPIO_NUM_1, .tx=GPIO_NUM_2 } - -// log over MAVLink by default -#define HAL_LOGGING_FILESYSTEM_ENABLED 0 -#define HAL_LOGGING_DATAFLASH_ENABLED 0 -#define HAL_LOGGING_MAVLINK_ENABLED 1 -#define HAL_LOGGING_BACKENDS_DEFAULT 2 - -#define AP_RCPROTOCOL_ENABLED 0 - -#define AP_FILESYSTEM_ESP32_ENABLED 0 -#define AP_SCRIPTING_ENABLED 0 diff --git a/libraries/AP_HAL_ESP32/hwdef/esp32s3m5stampfly/hwdef.dat b/libraries/AP_HAL_ESP32/hwdef/esp32s3m5stampfly/hwdef.dat index 4c9bf77972729..47e26fa2047ea 100644 --- a/libraries/AP_HAL_ESP32/hwdef/esp32s3m5stampfly/hwdef.dat +++ b/libraries/AP_HAL_ESP32/hwdef/esp32s3m5stampfly/hwdef.dat @@ -1,3 +1,23 @@ +# https://shop.m5stack.com/products/m5stamp-fly-with-m5stamps3 + +# GROVE EXPANSION +# +# The Stampfly has two Seeed Studio Grove connectors to attach peripherals. By +# default, the left (red) connector is I2C (with no other devices on the bus) +# and the right (black) connector is SERIAL3. However, the two signal pins in +# each can be reassigned by editing this file. +# +# Grove Red (I2C) +# 1 (toward front): GPIO_NUM_15, SCL (has 4.7kΩ pullup to 3.3V) +# 2 : GPIO_NUM_13, SDA (has 4.7kΩ pullup to 3.3V) +# 3 : +V (+5V if plugged into USB, else +Vbat) +# 4 (toward rear) : GND +# +# Grove Black (SERIAL3) +# 1 (toward rear) : GPIO_NUM_1, RX +# 2 : GPIO_NUM_3, TX +# 3 : +V (+5V if plugged into USB, else +Vbat) +# 4 (toward front): GND # SPI Buses; https://docs.espressif.com/projects/esp-idf/en/v5.3.4/esp32/api-reference/peripherals/spi_master.html # Host DMA Channel MOSI MISO SCLK @@ -12,8 +32,70 @@ ESP32_SPIBUS SPI2_HOST SPI_DMA_CH_AUTO GPIO_NUM_14 GPIO_NUM_43 GPIO_NUM_44 ESP32_SPIDEV bmi270 0 0 GPIO_NUM_46 3 10*MHZ 10*MHZ ESP32_SPIDEV pixartflow_disabled 0 1 GPIO_NUM_12 3 2*MHZ 2*MHZ +# i2c Buses +# Port SDA SCL SPEED INTERNAL +ESP32_I2CBUS I2C_NUM_0 GPIO_NUM_3 GPIO_NUM_4 400*KHZ true +ESP32_I2CBUS I2C_NUM_1 GPIO_NUM_13 GPIO_NUM_15 400*KHZ false + +# one IMU: +IMU BMI270 SPI:bmi270 ROTATION_ROLL_180_YAW_90 + +# one Baro: +BARO BMP280 I2C:0:0x76 + +# HARDWARE UARTS. UART 0 apparently goes over USB? so we assign it to pins we +# don't have mapped to anything. might be fixable in the HAL... +# UART 1 (SERIAL3) is the black grove connector +# PORT RXPIN TXPIN +ESP32_SERIAL UART_NUM_0 GPIO_NUM_18 GPIO_NUM_17 +ESP32_SERIAL UART_NUM_1 GPIO_NUM_1 GPIO_NUM_2 + +# RC Output (r-up, l-down, l-up, r-down (quad X order)) +ESP32_RCOUT GPIO_NUM_42 +ESP32_RCOUT GPIO_NUM_10 +ESP32_RCOUT GPIO_NUM_5 +ESP32_RCOUT GPIO_NUM_41 + # the stampfly does not have an Airspeed sensor: define AP_AIRSPEED_ENABLED 0 +define HAL_ESP32_BOARD_NAME "esp32s3m5stampfly" + +define AP_COMPASS_ENABLE_DEFAULT 0 +define ALLOW_ARM_NO_COMPASS + +# no ADC +define HAL_DISABLE_ADC_DRIVER 1 +define HAL_USE_ADC 0 + +define AP_BATTERY_INA3221_ENABLED 1 +define HAL_BATTMON_INA3221_SHUNT_OHMS 0.01 +define HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_4156US +define HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_4156US +define HAL_BATTMON_INA3221_AVG_MODE_SEL HAL_BATTMON_INA3221_AVG_MODE_16 + +# 2 use udp, 1 use tcp... for udp,client needs to connect as UDPCL in missionplanner etc to 192.168.4.1 port 14550 +define HAL_ESP32_WIFI 2 + +define WIFI_SSID "ardupilot123" +define WIFI_PWD "ardupilot123" + +# rcin on what pin? enable AP_RCPROTOCOL_ENABLED below if using +# currently on another unmapped pin +# define HAL_ESP32_RCIN GPIO_NUM_16 +define HAL_ESP32_RMT_RX_PIN_NUMBER GPIO_NUM_16 + + +# log over MAVLink by default +define HAL_LOGGING_FILESYSTEM_ENABLED 0 +define HAL_LOGGING_DATAFLASH_ENABLED 0 +define HAL_LOGGING_MAVLINK_ENABLED 1 +define HAL_LOGGING_BACKENDS_DEFAULT 2 + +define AP_RCPROTOCOL_ENABLED 0 + +define AP_FILESYSTEM_ESP32_ENABLED 0 +define AP_SCRIPTING_ENABLED 0 + # This is a board that's not really intended for anything other than copter AUTOBUILD_TARGETS Copter diff --git a/libraries/AP_HAL_ESP32/hwdef/scripts/esp32_hwdef.py b/libraries/AP_HAL_ESP32/hwdef/scripts/esp32_hwdef.py index a179c636f93ab..f92f6c80f10df 100755 --- a/libraries/AP_HAL_ESP32/hwdef/scripts/esp32_hwdef.py +++ b/libraries/AP_HAL_ESP32/hwdef/scripts/esp32_hwdef.py @@ -20,19 +20,31 @@ class ESP32HWDef(hwdef.HWDef): def __init__(self, quiet=False, outdir=None, hwdef=[]): super(ESP32HWDef, self).__init__(quiet=quiet, outdir=outdir, hwdef=hwdef) - # lists of ESP32_SPIDEV buses and devices + # lists of ESP32_SPIBUS buses and ESP32_SPIDEV devices self.esp32_spibus = [] self.esp32_spidev = [] + # lists of ESP32_I2CBUS buses + self.esp32_i2cbus = [] + + # list of ESP32_SERIAL devices + self.esp32_serials = [] + + # list of ESP32_RCOUT declarations + self.esp32_rcout = [] + def write_hwdef_header_content(self, f): for d in self.alllines: if d.startswith('define '): f.write('#define %s\n' % d[7:]) self.write_SPI_config(f) + self.write_I2C_config(f) self.write_IMU_config(f) self.write_MAG_config(f) self.write_BARO_config(f) + self.write_SERIAL_config(f) + self.write_RCOUT_config(f) self.write_env_py(os.path.join(self.outdir, "env.py")) @@ -43,14 +55,38 @@ def process_line(self, line, depth): self.alllines.append(line) a = shlex.split(line, posix=False) + if a[0] == 'ESP32_I2CBUS': + self.process_line_esp32_i2cbus(line, depth, a) + if a[0] == 'ESP32_SPIBUS': self.process_line_esp32_spibus(line, depth, a) if a[0] == 'ESP32_SPIDEV': self.process_line_esp32_spidev(line, depth, a) + if a[0] == 'ESP32_SERIAL': + self.process_line_esp32_serial(line, depth, a) + + if a[0] == 'ESP32_RCOUT': + self.process_line_esp32_rcout(line, depth, a) + super(ESP32HWDef, self).process_line(line, depth) - # SPI_BUS support: + # ESP32_I2CBUS support: + def process_line_esp32_i2cbus(self, line, depth, a): + self.esp32_i2cbus.append(a[1:]) + + def write_I2C_bus_table(self, f): + '''write I2C bus table''' + buslist = [] + for bus in self.esp32_i2cbus: + if len(bus) != 5: + self.error(f"Badly formed ESP32_I2CBUS line {bus} {len(bus)=}") + (port, sda, scl, speed, internal) = bus + buslist.append(f"{{ .port={port}, .sda={sda}, .scl={scl}, .speed={speed}, .internal={internal} }}") + + self.write_device_table(f, "i2c buses", "HAL_ESP32_I2C_BUSES", buslist) + + # ESP32_SPI_BUS support: def process_line_esp32_spibus(self, line, depth, a): self.esp32_spibus.append(a[1:]) @@ -73,6 +109,13 @@ def write_SPI_bus_table(self, f): def process_line_esp32_spidev(self, line, depth, a): self.esp32_spidev.append(a[1:]) + def process_line_esp32_serial(self, line, depth, a): + self.esp32_serials.append(a[1:]) + + # ESP32_RCOUT support: + def process_line_esp32_rcout(self, line, depth, a): + self.esp32_rcout.append(a[1:]) + def write_SPI_device_table(self, f): '''write SPI device table''' devlist = [] @@ -97,6 +140,11 @@ def write_SPI_device_table(self, f): self.write_device_table(f, 'SPI devices', 'HAL_ESP32_SPI_DEVICES', devlist) + def write_I2C_config(self, f): + '''write I2C config defines''' + + self.write_I2C_bus_table(f) + def write_SPI_config(self, f): '''write SPI config defines''' @@ -106,6 +154,32 @@ def write_SPI_config(self, f): if len(self.esp32_spidev): self.write_SPI_device_table(f) + def write_SERIAL_config(self, f): + '''write serial config defines''' + + seriallist = [] + for serial in self.esp32_serials: + if len(serial) != 3: + self.error(f"Badly formed ESP32_SERIALS line {serial} {len(serial)=}") + (port, rxpin, txpin) = serial + seriallist.append(f"{{ .port={port}, .rx={rxpin}, .tx={txpin} }}") + + self.write_device_table(f, 'serial devices', 'HAL_ESP32_UART_DEVICES', seriallist) + + def write_RCOUT_config(self, f): + '''write rc output defines''' + rcout_list = [] + for rcout in self.esp32_rcout: + if len(rcout) != 1: + self.error(f"Badly formed ESP32_RCOUT line {rcout} {len(rcout)=}") + (gpio_num, ) = rcout + rcout_list.append(gpio_num) + + if len(rcout_list) == 0: + f.write("// No rc outputs\n") + return + f.write(f"#define HAL_ESP32_RCOUT {{ {', '.join(rcout_list)} }}") + if __name__ == '__main__': diff --git a/libraries/AP_Networking/AP_Networking_port.cpp b/libraries/AP_Networking/AP_Networking_port.cpp index fd56d86a1f178..18bc64592b607 100644 --- a/libraries/AP_Networking/AP_Networking_port.cpp +++ b/libraries/AP_Networking/AP_Networking_port.cpp @@ -296,7 +296,7 @@ void AP_Networking::Port::tcp_client_loop(void) } if (!connected) { const char *dest = ip.get_str(); - connected = sock->connect(dest, port.get()); + connected = sock->connect_timeout(dest, port.get(), 3000); if (connected) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "TCP[%u]: connected to %s:%u", unsigned(state.idx), dest, unsigned(port.get())); sock->set_blocking(false); diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h index 5f915094198e4..ef049d6da9557 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Backend_CAN.h @@ -36,10 +36,6 @@ class AP_RangeFinder_Backend_CAN : public AP_RangeFinder_Backend // maximum time between readings before we change state to NoData: virtual uint32_t read_timeout_ms() const { return 200; } - virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { - return MAV_DISTANCE_SENSOR_RADAR; - } - // return true if the CAN ID is correct bool is_correct_id(uint32_t can_id) const; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h index a5c6441258e58..e2f5eabbd26ba 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_CAN.h @@ -15,6 +15,12 @@ class AP_RangeFinder_Benewake_CAN : public AP_RangeFinder_Backend_CAN { // handler for incoming frames. Return true if consumed bool handle_frame(AP_HAL::CANFrame &frame) override; bool handle_frame_H30(AP_HAL::CANFrame &frame); + +protected: + + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_LASER; + } }; #endif // AP_RANGEFINDER_BENEWAKE_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h index c587759706199..2fe8fc722f0ad 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NRA24_CAN.h @@ -18,6 +18,12 @@ class AP_RangeFinder_NRA24_CAN : public AP_RangeFinder_Backend_CAN { static const struct AP_Param::GroupInfo var_info[]; +protected: + + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_RADAR; + } + private: uint32_t get_radar_id(uint32_t id) const { return ((id & 0xF0U) >> 4U); } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h index ff88245ef54cd..6a5aae3857b3a 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_TOFSenseP_CAN.h @@ -16,6 +16,12 @@ class AP_RangeFinder_TOFSenseP_CAN : public AP_RangeFinder_Backend_CAN { bool handle_frame(AP_HAL::CANFrame &frame) override; static const struct AP_Param::GroupInfo var_info[]; + +protected: + + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_LASER; + } }; #endif // AP_RANGEFINDER_USD1_CAN_ENABLED diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h index 445f7d3f50789..09ff767f383c5 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_USD1_CAN.h @@ -16,6 +16,12 @@ class AP_RangeFinder_USD1_CAN : public AP_RangeFinder_Backend_CAN { bool handle_frame(AP_HAL::CANFrame &frame) override; static const struct AP_Param::GroupInfo var_info[]; + +protected: + + MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { + return MAV_DISTANCE_SENSOR_RADAR; + } }; #endif // AP_RANGEFINDER_USD1_CAN_ENABLED diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 5c61d2529bd61..ece6aae7f8f93 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -569,6 +569,11 @@ function SocketAPM_ud:send(str, len) end ---@return integer function SocketAPM_ud:sendto(str, len, ipaddr, port) end +-- return true if a socket is in a pending connect state +-- used for non-blocking TCP connections +---@return boolean +function SocketAPM_ud:is_pending() end + -- bind to an address. Use "0.0.0.0" for wildcard bind ---@param IP_address string ---@param port integer diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 81ba4bdb63ecc..e99bd8bbf1b3b 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -647,6 +647,7 @@ ap_object SocketAPM method sendto int32_t string uint32_t'skip_check uint32_t'sk ap_object SocketAPM method listen boolean uint8_t'skip_check ap_object SocketAPM method set_blocking boolean boolean ap_object SocketAPM method is_connected boolean +ap_object SocketAPM method is_pending boolean ap_object SocketAPM method pollout boolean uint32_t'skip_check ap_object SocketAPM method pollin boolean uint32_t'skip_check ap_object SocketAPM method reuseaddress boolean diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 1471fe7aa2459..cf538e94cdf72 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -711,10 +711,10 @@ class GCS_MAVLINK handle MAV_CMD_CAN_FORWARD and CAN_FRAME messages for CAN over MAVLink */ void can_frame_callback(uint8_t bus, const AP_HAL::CANFrame &); -#if HAL_CANMANAGER_ENABLED +#if AP_MAVLINKCAN_ENABLED MAV_RESULT handle_can_forward(const mavlink_command_int_t &packet, const mavlink_message_t &msg); -#endif void handle_can_frame(const mavlink_message_t &msg) const; +#endif // AP_MAVLINKCAN_ENABLED void handle_optical_flow(const mavlink_message_t &msg); diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index b7d43d6a30a8e..5822406d14ca5 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -73,6 +73,8 @@ #include "MissionItemProtocol_Rally.h" #include "MissionItemProtocol_Fence.h" +#include + #include #include @@ -1053,6 +1055,9 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_MISSION_CURRENT, MSG_CURRENT_WAYPOINT}, { MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, MSG_SERVO_OUTPUT_RAW}, { MAVLINK_MSG_ID_RC_CHANNELS, MSG_RC_CHANNELS}, +#if AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + { MAVLINK_MSG_ID_GNSS_INTEGRITY, MSG_GNSS_INTEGRITY}, +#endif #if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED { MAVLINK_MSG_ID_RC_CHANNELS_RAW, MSG_RC_CHANNELS_RAW}, #endif @@ -1096,11 +1101,15 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_ATTITUDE, MSG_ATTITUDE}, { MAVLINK_MSG_ID_ATTITUDE_QUATERNION, MSG_ATTITUDE_QUATERNION}, { MAVLINK_MSG_ID_GLOBAL_POSITION_INT, MSG_LOCATION}, +#if AP_AHRS_ENABLED { MAVLINK_MSG_ID_LOCAL_POSITION_NED, MSG_LOCAL_POSITION}, +#endif // AP_AHRS_ENABLED { MAVLINK_MSG_ID_VFR_HUD, MSG_VFR_HUD}, #endif { MAVLINK_MSG_ID_HWSTATUS, MSG_HWSTATUS}, +#if AP_MAVLINK_MSG_WIND_ENABLED { MAVLINK_MSG_ID_WIND, MSG_WIND}, +#endif // AP_MAVLINK_MSG_WIND_ENABLED #if AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED { MAVLINK_MSG_ID_RANGEFINDER, MSG_RANGEFINDER}, #endif // AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED @@ -1137,7 +1146,9 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_MAG_CAL_PROGRESS, MSG_MAG_CAL_PROGRESS}, { MAVLINK_MSG_ID_MAG_CAL_REPORT, MSG_MAG_CAL_REPORT}, #endif +#if AP_AHRS_ENABLED { MAVLINK_MSG_ID_EKF_STATUS_REPORT, MSG_EKF_STATUS_REPORT}, +#endif // AP_AHRS_ENABLED { MAVLINK_MSG_ID_PID_TUNING, MSG_PID_TUNING}, { MAVLINK_MSG_ID_VIBRATION, MSG_VIBRATION}, #if AP_RPM_ENABLED @@ -4198,13 +4209,13 @@ MAV_RESULT GCS_MAVLINK::handle_command_mag_cal(const mavlink_command_int_t &pack } #endif // COMPASS_CAL_ENABLED -#if HAL_CANMANAGER_ENABLED +#if AP_MAVLINKCAN_ENABLED /* handle MAV_CMD_CAN_FORWARD */ MAV_RESULT GCS_MAVLINK::handle_can_forward(const mavlink_command_int_t &packet, const mavlink_message_t &msg) { - return AP::can().handle_can_forward(chan, packet, msg) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED; + return AP_MAVLinkCAN::handle_can_forward(chan, packet, msg) ? MAV_RESULT_ACCEPTED : MAV_RESULT_FAILED; } /* @@ -4212,9 +4223,9 @@ MAV_RESULT GCS_MAVLINK::handle_can_forward(const mavlink_command_int_t &packet, */ void GCS_MAVLINK::handle_can_frame(const mavlink_message_t &msg) const { - AP::can().handle_can_frame(msg); + AP_MAVLinkCAN::handle_can_frame(msg); } -#endif // HAL_CANMANAGER_ENABLED +#endif // AP_MAVLINKCAN_ENABLED void GCS_MAVLINK::handle_distance_sensor(const mavlink_message_t &msg) { @@ -4571,11 +4582,11 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg) break; #endif -#if HAL_CANMANAGER_ENABLED +#if AP_MAVLINKCAN_ENABLED case MAVLINK_MSG_ID_CAN_FILTER_MODIFY: - AP::can().handle_can_filter_modify(msg); + AP_MAVLinkCAN::handle_can_filter_modify(msg); break; -#endif +#endif // AP_MAVLINKCAN_ENABLED #if AP_OPENDRONEID_ENABLED case MAVLINK_MSG_ID_OPEN_DRONE_ID_ARM_STATUS: @@ -5587,10 +5598,10 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p return handle_command_battery_reset(packet); #endif -#if HAL_CANMANAGER_ENABLED +#if AP_MAVLINKCAN_ENABLED case MAV_CMD_CAN_FORWARD: return handle_can_forward(packet, msg); -#endif +#endif // AP_MAVLINKCAN_ENABLED #if HAL_HIGH_LATENCY2_ENABLED case MAV_CMD_CONTROL_HIGH_LATENCY: @@ -6489,7 +6500,12 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) AP::gps().send_mavlink_gps_rtk(chan, 1); break; #endif // AP_GPS_GPS2_RTK_SENDING_ENABLED - +#if AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + case MSG_GNSS_INTEGRITY: + CHECK_PAYLOAD_SIZE(GNSS_INTEGRITY); + AP::gps().send_mavlink_gnss_integrity(chan, 0); + break; +#endif // AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED #if AP_AHRS_ENABLED case MSG_LOCAL_POSITION: CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED); diff --git a/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp b/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp index dd259cb90b534..0d816402f13ec 100644 --- a/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp +++ b/libraries/GCS_MAVLink/GCS_MAVLink_Parameters.cpp @@ -17,6 +17,7 @@ #include #include #include +#include const struct AP_Param::GroupInfo *GCS::_chan_var_info[MAVLINK_COMM_NUM_BUFFERS]; @@ -212,7 +213,7 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = { // @Description: Bitmask for configuring this telemetry channel. For having effect on all channels, set the relevant mask in all MAVx_OPTIONS parameters. Keep in mind that part of the flags may require a reboot to take action. // @RebootRequired: True // @User: Standard - // @Bitmask: 1:Don't forward mavlink to/from, 2:Ignore Streamrate + // @Bitmask: 0:Accept unsigned MAVLink2 messages, 1:Don't forward mavlink to/from, 2:Ignore Streamrate AP_GROUPINFO("_OPTIONS", 20, GCS_MAVLINK, options, 0), // PARAMETER_CONVERSION - Added: May-2025 for ArduPilot-4.7 @@ -271,8 +272,8 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { static const ap_message STREAM_POSITION_msgs[] = { #if AP_AHRS_ENABLED MSG_LOCATION, -#endif // AP_AHRS_ENABLED MSG_LOCAL_POSITION +#endif // AP_AHRS_ENABLED }; static const ap_message STREAM_RAW_CONTROLLER_msgs[] = { @@ -283,10 +284,12 @@ static const ap_message STREAM_RAW_CONTROLLER_msgs[] = { static const ap_message STREAM_RC_CHANNELS_msgs[] = { MSG_SERVO_OUTPUT_RAW, +#if AP_RC_CHANNEL_ENABLED MSG_RC_CHANNELS, #if AP_MAVLINK_MSG_RC_CHANNELS_RAW_ENABLED MSG_RC_CHANNELS_RAW, // only sent on a mavlink1 connection #endif +#endif // AP_RC_CHANNEL_ENABLED }; static const ap_message STREAM_EXTRA1_msgs[] = { @@ -340,7 +343,9 @@ static const ap_message STREAM_EXTRA3_msgs[] = { #if AP_AHRS_ENABLED MSG_AHRS, #endif // AP_AHRS_ENABLED +#if AP_MAVLINK_MSG_WIND_ENABLED MSG_WIND, +#endif // AP_MAVLINK_MSG_WIND_ENABLED #if AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED MSG_RANGEFINDER, #endif // AP_MAVLINK_MSG_RANGEFINDER_SENDING_ENABLED @@ -366,10 +371,15 @@ static const ap_message STREAM_EXTRA3_msgs[] = { MSG_MAG_CAL_REPORT, MSG_MAG_CAL_PROGRESS, #endif +#if AP_AHRS_ENABLED MSG_EKF_STATUS_REPORT, +#endif // AP_AHRS_ENABLED #if !APM_BUILD_TYPE(APM_BUILD_AntennaTracker) MSG_VIBRATION, #endif +#if AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED + MSG_GNSS_INTEGRITY, +#endif // AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED }; static const ap_message STREAM_PARAMS_msgs[] = { diff --git a/libraries/GCS_MAVLink/GCS_config.h b/libraries/GCS_MAVLink/GCS_config.h index e16a36b22c38c..b7b3dc980b26e 100644 --- a/libraries/GCS_MAVLink/GCS_config.h +++ b/libraries/GCS_MAVLink/GCS_config.h @@ -56,6 +56,10 @@ #define AP_MAVLINK_MSG_RELAY_STATUS_ENABLED HAL_GCS_ENABLED && AP_RELAY_ENABLED #endif +#ifndef AP_MAVLINK_MSG_WIND_ENABLED +#define AP_MAVLINK_MSG_WIND_ENABLED AP_AHRS_ENABLED +#endif + // allow removal of developer-centric mavlink commands #ifndef AP_MAVLINK_FAILURE_CREATION_ENABLED #define AP_MAVLINK_FAILURE_CREATION_ENABLED 1 @@ -140,3 +144,7 @@ #ifndef AP_MAVLINK_SET_GPS_GLOBAL_ORIGIN_MESSAGE_ENABLED #define AP_MAVLINK_SET_GPS_GLOBAL_ORIGIN_MESSAGE_ENABLED (HAL_GCS_ENABLED && AP_AHRS_ENABLED) #endif // AP_MAVLINK_SET_GPS_GLOBAL_ORIGIN_MESSAGE_ENABLED + +#ifndef AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED +#define AP_MAVLINK_MSG_GNSS_INTEGRITY_ENABLED AP_GPS_SBF_ENABLED && HAL_GCS_ENABLED +#endif \ No newline at end of file diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h index aeba8593979e7..e9b9fac5bae1d 100644 --- a/libraries/GCS_MAVLink/ap_message.h +++ b/libraries/GCS_MAVLink/ap_message.h @@ -113,5 +113,6 @@ enum ap_message : uint8_t { #if AP_MAVLINK_MSG_FLIGHT_INFORMATION_ENABLED MSG_FLIGHT_INFORMATION = 100, #endif + MSG_GNSS_INTEGRITY = 101, MSG_LAST // MSG_LAST must be the last entry in this enum }; diff --git a/tests/test_pre_commit_copyright.py b/tests/test_pre_commit_copyright.py new file mode 100644 index 0000000000000..fe9bf55a31dd7 --- /dev/null +++ b/tests/test_pre_commit_copyright.py @@ -0,0 +1,42 @@ +import pytest +from unittest.mock import patch + +from Tools.gittools.pre_commit_copyright import get_file_paths + + +def test_get_file_paths_no_args(): + """Test get_file_paths with no command line arguments.""" + with patch('sys.argv', ["pre_commit_copyright.py"]): + with pytest.raises(SystemExit, match="2"): + _ = get_file_paths() + + +@pytest.mark.parametrize( + "argv, expected", + [ + ( + ["pre_commit_copyright.py", "file1.py", "file2.py", "file3.py"], + ["file1.py", "file2.py", "file3.py"] + ), + ( + ["pre_commit_copyright.py", "file1.py", "file2.py", "--ignore=file2.py"], + ["file1.py"] + ), + ( + ["pre_commit_copyright.py", "file1.py", "file2.py", "file3.py", "--ignore=file1.py", "--ignore=file3.py"], + ["file2.py"] + ), + ( + ["pre_commit_copyright.py", "file1.py", "file2.py", "--ignore=excluded_file.py", "--ignore=another_excluded.py"], + ["file1.py", "file2.py"] + ), + ( + ["pre_commit_copyright.py", "file1.py", "--ignore=file1.py"], + [] + ), + ] +) +def test_get_file_paths_parametrized(argv, expected): + """Test get_file_paths with various command line argument combinations.""" + with patch('sys.argv', argv): + assert get_file_paths() == expected