diff --git a/.github/workflows/esp32_build.yml b/.github/workflows/esp32_build.yml deleted file mode 100644 index 12f6dd91054a0..0000000000000 --- a/.github/workflows/esp32_build.yml +++ /dev/null @@ -1,272 +0,0 @@ -name: ESP32 Build - -on: - push: - paths-ignore: - # remove non copter and plane vehicles - - 'AntennaTracker/**' - - 'ArduSub/**' - - 'Blimp/**' - - 'Rover/**' - # remove non esp32 HAL - - 'libraries/AP_HAL_ChibiOS/**' - - 'libraries/AP_HAL_SITL/**' - # remove non SITL directories - - 'Tools/AP_Bootloader/**' - - 'Tools/AP_Periph/**' - - 'Tools/bootloaders/**' - - 'Tools/CHDK-Script/**' - - 'Tools/CodeStyle/**' - - 'Tools/completion/**' - - 'Tools/CPUInfo/**' - - 'Tools/debug/**' - - 'Tools/environment_install/**' - - 'Tools/FilterTestTool/**' - - 'Tools/Frame_params/**' - - 'Tools/geotag/**' - - 'Tools/GIT_Test/**' - - 'Tools/gittools/**' - - 'Tools/Hello/**' - - 'Tools/IO_Firmware/**' - - 'Tools/Linux_HAL_Essentials/**' - - 'Tools/LogAnalyzer/**' - - 'Tools/mavproxy_modules/**' - - 'Tools/Pozyx/**' - - 'Tools/PrintVersion.py' - - 'Tools/Replay/**' - - 'Tools/simulink/**' - - 'Tools/UDP_Proxy/**' - - 'Tools/vagrant/**' - - 'Tools/Vicon/**' - # Discard python file from Tools/scripts as not used - - 'Tools/scripts/**.py' - - 'Tools/scripts/build_sizes/**' - - 'Tools/scripts/build_tests/**' - - 'Tools/scripts/CAN/**' - - 'Tools/scripts/signing/**' - # Remove autotest - - 'Tools/autotest/**' - # Remove markdown files as irrelevant - - '**.md' - # Remove dotfile at root directory - - './.dir-locals.el' - - './.dockerignore' - - './.editorconfig' - - './.flake8' - - './.gitattributes' - - './.github' - - './.gitignore' - - './.pre-commit-config.yaml' - - './.pydevproject' - - './.valgrind-suppressions' - - './.valgrindrc' - - 'Dockerfile' - - 'Vagrantfile' - - 'Makefile' - # Remove some directories check - - '.vscode/**' - - '.github/ISSUE_TEMPLATE/**' - # Remove change on other workflows - - '.github/workflows/test_environment.yml' - - pull_request: - paths-ignore: - # remove non copter and plane vehicles - - 'AntennaTracker/**' - - 'ArduSub/**' - - 'Blimp/**' - - 'Rover/**' - # remove non esp32 HAL - - 'libraries/AP_HAL_ChibiOS/**' - - 'libraries/AP_HAL_SITL/**' - # remove non SITL directories - - 'Tools/AP_Bootloader/**' - - 'Tools/AP_Periph/**' - - 'Tools/bootloaders/**' - - 'Tools/CHDK-Script/**' - - 'Tools/CodeStyle/**' - - 'Tools/completion/**' - - 'Tools/CPUInfo/**' - - 'Tools/debug/**' - - 'Tools/environment_install/**' - - 'Tools/FilterTestTool/**' - - 'Tools/Frame_params/**' - - 'Tools/geotag/**' - - 'Tools/GIT_Test/**' - - 'Tools/gittools/**' - - 'Tools/Hello/**' - - 'Tools/IO_Firmware/**' - - 'Tools/Linux_HAL_Essentials/**' - - 'Tools/LogAnalyzer/**' - - 'Tools/mavproxy_modules/**' - - 'Tools/Pozyx/**' - - 'Tools/PrintVersion.py' - - 'Tools/Replay/**' - - 'Tools/simulink/**' - - 'Tools/UDP_Proxy/**' - - 'Tools/vagrant/**' - - 'Tools/Vicon/**' - # Discard python file from Tools/scripts as not used - - 'Tools/scripts/**.py' - - 'Tools/scripts/build_sizes/**' - - 'Tools/scripts/build_tests/**' - - 'Tools/scripts/CAN/**' - - 'Tools/scripts/signing/**' - # Remove autotest - - 'Tools/autotest/**' - # Remove markdown files as irrelevant - - '**.md' - # Remove dotfile at root directory - - './.dir-locals.el' - - './.dockerignore' - - './.editorconfig' - - './.flake8' - - './.gitattributes' - - './.github' - - './.gitignore' - - './.pre-commit-config.yaml' - - './.pydevproject' - - './.valgrind-suppressions' - - './.valgrindrc' - - 'Dockerfile' - - 'Vagrantfile' - - 'Makefile' - # Remove some directories check - - '.vscode/**' - - '.github/ISSUE_TEMPLATE/**' - # Remove change on other workflows - - '.github/workflows/test_environment.yml' - - workflow_dispatch: - -concurrency: - group: ci-${{github.workflow}}-${{ github.ref }} - cancel-in-progress: true - -jobs: - build: - runs-on: ubuntu-22.04 - strategy: - fail-fast: false # don't cancel if a job from the matrix fails - matrix: - config: [ - esp32buzz, - esp32s3empty, - ] - gcc: [10] - - steps: - - uses: actions/checkout@v4 - with: - submodules: 'recursive' - - - name: Register gcc problem matcher - run: echo "::add-matcher::.github/problem-matchers/gcc.json" - - - name: Register python problem matcher - run: echo "::add-matcher::.github/problem-matchers/python.json" - - - name: Register autotest warn matcher - run: echo "::add-matcher::.github/problem-matchers/autotestwarn.json" - - - name: Register autotest fail matcher - run: echo "::add-matcher::.github/problem-matchers/autotestfail.json" - - - name: Install Prerequisites - shell: bash - run: | - sudo apt-get install git wget libncurses-dev flex bison gperf python3 python3-pip python3-venv python3-setuptools python3-serial python3-gevent python3-cryptography python3-future python3-pyparsing python3-pyelftools cmake ninja-build ccache libffi-dev libssl-dev dfu-util libusb-1.0-0 - sudo update-alternatives --install /usr/bin/python python /usr/bin/python3 10 - update-alternatives --query python - python --version - pip3 install gevent - - # we actualy want 3.11 .. but the above only gave us 3.10, not ok with esp32 builds. - sudo add-apt-repository ppa:deadsnakes/ppa - sudo apt-get update - sudo apt-get install python3.11 python3.11-venv python3.11-distutils -y - sudo apt-get install python3 python3-pip python3-venv python3-setuptools python3-serial python3-cryptography python3-future python3-pyparsing python3-pyelftools - update-alternatives --query python - pip3 install gevent - python --version - python3.11 --version - which python3.11 - sudo update-alternatives --install /usr/bin/python python /usr/bin/python3.11 11 - update-alternatives --query python - - rm -rf /usr/local/bin/cmake - sudo apt-get remove --purge --auto-remove cmake - sudo apt-get update && \ - sudo apt-get install -y software-properties-common lsb-release && \ - sudo apt-get clean all - wget -O - https://apt.kitware.com/keys/kitware-archive-latest.asc 2>/dev/null | gpg --dearmor - | sudo tee /etc/apt/trusted.gpg.d/kitware.gpg >/dev/null - sudo apt-add-repository "deb https://apt.kitware.com/ubuntu/ $(lsb_release -cs) main" - sudo apt-get update - sudo apt-get install cmake - - - git submodule update --init --recursive --depth=1 - ./Tools/scripts/esp32_get_idf.sh - - sudo ln -s /usr/bin/ninja /usr/bin/ninja-build - - cd modules/esp_idf - echo "Installing ESP-IDF tools...." - ./install.sh esp32,esp32s3 - cd ../.. - - - - name: build ${{matrix.config}} - env: - CI_BUILD_TARGET: ${{matrix.config}} - shell: bash - run: | - source ~/.bash_profile - PATH="/github/home/.local/bin:$PATH" - echo $PATH - echo "### Configure ${{matrix.config}} :rocket:" >> $GITHUB_STEP_SUMMARY - cd modules/esp_idf - ./install.sh - source ./export.sh - cd ../.. - python -m pip install --progress-bar off future lxml pymavlink MAVProxy pexpect flake8 geocoder empy==3.3.4 dronecan - which cmake - ./waf configure --board ${{matrix.config}} - echo './waf configure --board ${{matrix.config}}' >> $GITHUB_STEP_SUMMARY - echo "### Build Plane ${{matrix.config}} :rocket:" >> $GITHUB_STEP_SUMMARY - echo './waf plane' >> $GITHUB_STEP_SUMMARY - ./waf plane | tee >( grep -vE 'Compiling|Generat|includes.list|Parsing|Validation|Building|Linking|Detecting|linker|\*\*\*\*\*\*\*|Partition|SubType|Checking|esp-idf' --color=never --line-buffered > summary.log) - cat summary.log >> $GITHUB_STEP_SUMMARY - - cp build/${{matrix.config}}/esp-idf_build/ardupilot.bin ./ArduPlane.${{matrix.config}}.bin - cp build/${{matrix.config}}/esp-idf_build/ardupilot.elf ./ArduPlane.${{matrix.config}}.elf - echo "### Build Copter ${{matrix.config}} :rocket:" >> $GITHUB_STEP_SUMMARY - echo './waf copter' >> $GITHUB_STEP_SUMMARY - ./waf copter | tee >( grep -vE 'Compiling|Generat|includes.list|Parsing|Validation|Building|Linking|Detecting|linker|\*\*\*\*\*\*\*|Partition|SubType|Checking|esp-idf' --color=never --line-buffered > summary2.log) - - cat summary2.log >> $GITHUB_STEP_SUMMARY - - cp build/${{matrix.config}}/esp-idf_build/ardupilot.bin ./ArduCopter.${{matrix.config}}.bin - cp build/${{matrix.config}}/esp-idf_build/ardupilot.elf ./ArduCopter.${{matrix.config}}.elf - - cp build/${{matrix.config}}/esp-idf_build/bootloader/bootloader.bin ./bootloader.${{matrix.config}}.bin - cp build/${{matrix.config}}/esp-idf_build/partition_table/partition-table.bin ./partition-table.${{matrix.config}}.bin - - echo "### Assets avail in artifact:" >> $GITHUB_STEP_SUMMARY - ls bootloader* partition* Ardu*.elf Ardu*.bin >> $GITHUB_STEP_SUMMARY - - - name: Archive artifacts - uses: actions/upload-artifact@v4 - with: - name: esp32-binaries -${{matrix.config}} - path: | - /home/runner/work/ardupilot/ardupilot/ArduPlane.${{matrix.config}}.bin - /home/runner/work/ardupilot/ardupilot/ArduPlane.${{matrix.config}}.elf - /home/runner/work/ardupilot/ardupilot/ArduCopter.${{matrix.config}}.bin - /home/runner/work/ardupilot/ardupilot/ArduCopter.${{matrix.config}}.elf - /home/runner/work/ardupilot/ardupilot/bootloader.${{matrix.config}}.bin - /home/runner/work/ardupilot/ardupilot/partition-table.${{matrix.config}}.bin - - - retention-days: 14 - diff --git a/.github/workflows/test_environment.yml b/.github/workflows/test_environment.yml deleted file mode 100644 index bf5ffa81c2037..0000000000000 --- a/.github/workflows/test_environment.yml +++ /dev/null @@ -1,139 +0,0 @@ -name: test environment setup -on: - schedule: - - cron: '0 0 * * 6' # every saturday at midnight - workflow_dispatch: - push: - paths: - - '.github/workflows/test_environment.yml' - - 'Tools/environment_install/**' - - pull_request: - paths: - - '.github/workflows/test_environment.yml' - - 'Tools/environment_install/**' - - -concurrency: - group: ci-${{github.workflow}}-${{ github.ref }} - cancel-in-progress: true - -jobs: - build: - runs-on: ubuntu-22.04 - container: - image: ${{matrix.os}}:${{matrix.name}} - options: --privileged - strategy: - fail-fast: false # don't cancel if a job from the matrix fails - matrix: - include: - - os: ubuntu - name: focal - - os: ubuntu - name: jammy - - os: ubuntu - name: lunar - - os: ubuntu - name: mantic - - os: debian - name: bullseye - - os: debian - name: buster - steps: - - name: Install Git - timeout-minutes: 30 - env: - DEBIAN_FRONTEND: noninteractive - TZ: Europe/Paris - shell: 'script -q -e -c "bash {0}"' - run: | - case ${{matrix.os}} in - *"ubuntu"*) - apt-get update && apt-get install --no-install-recommends -qy \ - lsb-release \ - sudo \ - git \ - software-properties-common - add-apt-repository ppa:git-core/ppa -y - apt-get update && apt-get install --no-install-recommends -qy git - ;; - *"debian"*) - apt-get update && apt-get install --no-install-recommends -qy \ - lsb-release \ - sudo \ - git \ - software-properties-common - ;; - *"archlinux"*) - pacman -Syu --noconfirm --needed git sudo - ;; - esac - - # git checkout the PR - - uses: actions/checkout@v4 - with: - submodules: 'recursive' - - name: test install environment ${{matrix.os}}.${{matrix.name}} - timeout-minutes: 45 - env: - DISABLE_MAVNATIVE: True - DEBIAN_FRONTEND: noninteractive - TZ: Europe/Paris - SKIP_AP_GIT_CHECK: 1 - shell: 'script -q -e -c "bash {0}"' - run: | - PATH="/github/home/.local/bin:$PATH" - ln -snf /usr/share/zoneinfo/$TZ /etc/localtime && echo $TZ > /etc/timezone - sed -i 's/\$EUID/\$ID/' Tools/environment_install/install-prereqs-ubuntu.sh - sed -i 's/sudo usermod/\#sudo usermod/' Tools/environment_install/install-prereqs-ubuntu.sh - sed -i 's/sudo usermod/\#sudo usermod/' Tools/environment_install/install-prereqs-arch.sh - case ${{matrix.os}} in - *"ubuntu"*) - echo 'debconf debconf/frontend select Noninteractive' | debconf-set-selections - Tools/environment_install/install-prereqs-ubuntu.sh -qy - ;; - *"debian"*) - Tools/environment_install/install-prereqs-ubuntu.sh -qy - ;; - *"archlinux"*) - cp /etc/skel/.bashrc /root - cp /etc/skel/.bashrc /github/home - git config --global --add safe.directory /__w/ardupilot/ardupilot - Tools/environment_install/install-prereqs-arch.sh -qy - ;; - esac - - - name: test build STIL ${{matrix.os}}.${{matrix.name}} - env: - DISABLE_MAVNATIVE: True - DEBIAN_FRONTEND: noninteractive - TZ: Europe/Paris - shell: 'script -q -e -c "bash {0}"' - run: | - git config --global --add safe.directory ${GITHUB_WORKSPACE} - source ~/.bashrc - source $HOME/venv-ardupilot/bin/activate || true - git config --global --add safe.directory /__w/ardupilot/ardupilot - ./waf configure - ./waf rover - - - name: test build Chibios ${{matrix.os}}.${{matrix.name}} - env: - DISABLE_MAVNATIVE: True - DEBIAN_FRONTEND: noninteractive - TZ: Europe/Paris - shell: 'script -q -e -c "bash {0}"' - run: | - git config --global --add safe.directory ${GITHUB_WORKSPACE} - source ~/.bashrc - source $HOME/venv-ardupilot/bin/activate || true - case ${{matrix.os}} in - *"archlinux"*) - export PATH=/opt/gcc-arm-none-eabi-10-2020-q4-major/bin:$PATH - export PATH=/__w/ardupilot/ardupilot/ardupilot/Tools/autotest:$PATH - ;; - esac - git config --global --add safe.directory /__w/ardupilot/ardupilot - ./waf configure --board CubeOrange - ./waf plane diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index f57a987613f0f..32fbbca08d6f1 100644 --- a/Tools/autotest/arducopter.py +++ b/Tools/autotest/arducopter.py @@ -10733,12 +10733,25 @@ def MAV_CMD_NAV_TAKEOFF_command_int(self): def LuaParamSet(self): '''test param-set.lua applet''' - self.install_applet_script_context("param-set.lua") self.set_parameters({ 'SCR_ENABLE': 1, }) + + self.context_push() + + self.install_applet_script_context("param-set.lua") self.reboot_sitl() + # collect original parameter values for everything other than + # SCR_ENABLE and PARAM_SET_ENABLE. This resolves an issue at + # the end of the test where the popped context wants to set + # PARAM_SET_ENABLE back to its original value (enabled!), + # which can cause problems resetting + orig_parameter_values = self.get_parameters([ + 'RTL_ALT', + 'DISARM_DELAY', + ]) + self.wait_ready_to_arm() # scripts will be ready by now! self.start_subtest("set RTL_ALT freely") self.set_parameter("RTL_ALT", 23) @@ -10772,8 +10785,17 @@ def LuaParamSet(self): self.reboot_sitl() self.assert_parameter_value('DISARM_DELAY', 111) - # very bad things happen if we don't turn things off at the end.. - self.set_parameter("PARAM_SET_ENABLE", 0) + # set parameters to their original value so the following + # context pop should only need to set PARAM_SET_ENABLE back to + # its default value of 1. That prevents permission errors + # created by param-set.lua! + self.set_parameters(orig_parameter_values) + + self.context_pop() + + # disable the script but don't add it to the context so it + # doesn't get re-enabled: + self.set_parameter('PARAM_SET_ENABLE', 0, add_to_context=False) def LUAConfigProfile(self): '''test the config_profiles.lua example script''' diff --git a/Tools/environment_install/install-prereqs-mac.sh b/Tools/environment_install/install-prereqs-mac.sh index 75fb19bea15e7..ee74ad13e6b58 100755 --- a/Tools/environment_install/install-prereqs-mac.sh +++ b/Tools/environment_install/install-prereqs-mac.sh @@ -120,7 +120,7 @@ if maybe_prompt_user "Install python using pyenv [N/y]?" ; then pushd $HOME/.pyenv git fetch --tags - git checkout v2.3.12 + git checkout v2.6.7 popd exportline="export PYENV_ROOT=\$HOME/.pyenv" echo $exportline >> ~/$SHELL_LOGIN @@ -134,10 +134,10 @@ if maybe_prompt_user "Install python using pyenv [N/y]?" ; then } echo "pyenv installed" { - $(pyenv global 3.10.4) + $(pyenv global 3.10.18) } || { - env PYTHON_CONFIGURE_OPTS="--enable-framework" pyenv install 3.10.4 - pyenv global 3.10.4 + env PYTHON_CONFIGURE_OPTS="--enable-framework" pyenv install 3.10.18 + pyenv global 3.10.18 } fi diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.4/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.4/hwdef.dat index 024ab6027cdbe..4ff9aee02e3ee 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.4/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.4/hwdef.dat @@ -1,3 +1,3 @@ include ../Callisto/hwdef.inc -define AP_CUSTOM_FIRMWARE_STRING "ArduCopter V4.5.7-C2.4r9" +define AP_CUSTOM_FIRMWARE_STRING "ArduCopter V4.5.7-C2.4r11" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.5/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.5/hwdef.dat index af210cc007191..2f4df5ec3a5bf 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.5/hwdef.dat +++ b/libraries/AP_HAL_ChibiOS/hwdef/Callisto-2.5/hwdef.dat @@ -1,3 +1,3 @@ include ../Callisto/hwdef.inc -define AP_CUSTOM_FIRMWARE_STRING "ArduCopter V4.5.7-C2.5r9" +define AP_CUSTOM_FIRMWARE_STRING "ArduCopter V4.5.7-C2.5r11" diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Callisto/ReleaseNotes.txt b/libraries/AP_HAL_ChibiOS/hwdef/Callisto/ReleaseNotes.txt new file mode 100644 index 0000000000000..d2160361f27fb --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/hwdef/Callisto/ReleaseNotes.txt @@ -0,0 +1,166 @@ +July 31st, 2025 : Callisto: V4.5.7-C2.4r11 +August 25th, 2025 : AP_Logger: be 'in expected delay' when doing log transfer to avoid the monitor thread's wrath when the main thread has not run for 1.9 seconds + +July 31st, 2025 : Callisto: V4.5.7-C2.4r10 +July 31st, 2025 : Callisto: Param: ATC_LAND and TERRAIN_OFS_MAX +July 16th, 2025 : Copter: WP_NAVALT_MIN only apply to takeoff +July 30th, 2025 : autotest: LUA ConfigProfile +July 29th, 2025 : AP_Scripting: Check Firmware Checksum +July 29th, 2025 : AP_Scripting: ConfigProfile: add a script that changes vehicle between configurations different battery configurations, different payload configurations and different arm configurations +July 27th, 2025 : AP_Param: add support for LUA ConfigProfile + +July 2nd, 2025 : Callisto: Version: ArduCopter V4.5.7-C2.4r9 +July 2nd, 2025 : Callisto: Remove unwanted Features +July 2nd, 2025 : Callisto: Use SBUS only +May 13th, 2025 : Callisto: Enable network and PPP + +May 22nd, 2025 : Callisto: Version: ArduCopter V4.5.7-C2.4r8 +May 6th, 2025 : AP_Param: add option to ignore PARAM_SET +May 6th, 2025 : GCS_MAVLink: disallow setting of readonly/internal parameters via mavftp +May 6th, 2025 : AP_Scripting: add param-set-filter LUA applet and bindings for new applet + +March 22nd, 2025 : Callisto: Version: ArduCopter V4.5.7-C2.Xr7 +March 22nd, 2025 : AP_GPS: Remove GPS[bla] part of mavlink message. + +March 21st, 2025 : Callisto: Version: ArduCopter V4.5.7-C2.Xr6 +March 20th, 2025 : AP_DroneCAN: Remove CAN[bla] part of mavlink message. +February 18th, 2025 : Callisto: add Callisto-2.5 variant +February 18th, 2025 : Callisto: add Callisto-2.4 variant +February 18th, 2025 : Callisto: remove unnecessary flight modes +February 18th, 2025 : Callisto: add base Callisto files for Callisto variants to inherit from +August 7th, 2024 : AP_BattMonitor: add and use Battery backend internal-use-only flag +June 5th, 2024 : AP_BattMonitor: avoid casting DroneCAN backend to incorrect type +February 10th, 2024 : GCS_MAVLink: use can_printf_severity() for AP_Periph +February 10th, 2024 : AP_Periph: map MAV_SEVERITY to DroneCAN debug level +July 23rd, 2024 : Copter: Ground oscillation gain reduction +July 23rd, 2024 : AC_AttitudeControl: Add Landed Gain Backoff +October 8th, 2024 : GCS_MAVLink: raise number of MAVLink ports +May 8th, 2024 : GCS_MAVLink: deny attempt to do partial upload while mission transfer in progress +February 10th, 2024 : chibios_hwdef.py: allow re-use of bootloader from other boards + +November 30th, 2024 : ArduCopter V4.5.7-C2.4r4 +November 30th, 2024 : Callisto: Param: Relay fix + +November 28th, 2024 : ArduCopter V4.5.7-C2.4r3 +July 23rd, 2024 : AC_AttitudeControl: Add Landed Gain Backoff +April 24th, 2023 : Filter: Increase triple notch spacing +October 7th, 2024 : Copter: version to 4.5.7 +December 19th, 2022 : Copter: Payload Place enhancements +December 13th, 2022 : AP_Gripper: Add Neutral state after init +December 19th, 2022 : Copter: Use wp_start() in do_nav_wp() +December 19th, 2022 : Copter: Fix Auto Takeoff when complete_alt_cm is current altitude + +August 19th, 2024 : Version: V4.4.4 Cal1S6 +August 25th, 2024 : Remove velocity output limit + +August 23rd, 2024 : Version: V4.4.4 Cal1S5 +August 23rd, 2024 : ShipOps: Add Vel and Accel limits + +August 21st, 2024 : Version: V4.4.4 Cal1S4 +August 9th, 2024 : ShipOps: Add Failsafe + +August 19th, 2024 : Version: V4.4.4 Cal1S3 +August 19th, 2024 : ShipOps: LUA script +August 11th, 2024 : AP_Follow: Add get_target_sysid +August 9th, 2024 : ShipOps: Fix exit stratagy +August 9th, 2024 : ShipOps: Add Vel and Accel limits +August 9th, 2024 : ShipOps: Improve error handling +August 9th, 2024 : autotest: add test for ship operations +August 8th, 2024 : Copter: Fix payload place bug +August 4th, 2024 : ShipOps: Handle bad KOZ +August 4th, 2024 : ShipOps: RTL height when ship lost +July 17th, 2024 : AutoTest: ShipOps +July 17th, 2024 : ShipOps: Add KOZ + +July 11th, 2024 : Version: 4.4.4 Cal1S2 +July 11th, 2024 : ShipOps: Fix lidar payload trigger + +January 23rd, 2024 : Version: 4.4.4 Cal1 +June 27th, 2023 : Copter: Release Tie Down on take-off +July 10th, 2023 : Copter: Add Tie Down Release +March 2nd, 2024 : Copter: Payload Place: Add logging +March 2nd, 2024 : Copter: Payload Place: Payload Place: Add Drop Range +June 3rd, 2024 : Copter: Payload Place: Add thrust detection threshold filtering + +January 3rd, 2024 : Version: V4.4.0-Cal3 +January 3rd, 2024 : Callisto: Update VEL_XY_D and RCX_DZ +December 18th, 2023 : Copter: version to 4.4.4 +December 6th, 2023 : board_types.txt: reserve ID for FreeSpace PowerStack +September 20th, 2023 : AC_Autotune: Reduce max rates +September 28th, 2023 : Copter: Update PayLoadPlace code +July 22nd, 2023 : AP_TemperatureSensor: Add TSYS03 support + +June 27th, 2023 : Version: V4.3.7-Cal1S +June 27th, 2023 : Version: V4.3.7-Cal1 +May 31st, 2023 : Copter: version to 4.3.7 + +May 8th, 2023 : Version: V4.3.6-Cal3 +March 6th, 2023 : Copter: fix terrain offset reset +March 4th, 2023 : Copter: Add support for terrain altitude time constant +March 4th, 2023 : AC_WPNav: Provide terrain altitude for surface tracking +May 7th, 2023 : Callisto: Increase range finder minimum range + +April 24th, 2023 : Version: V4.3.6-Cal2 +April 24th, 2023 : Filter: Increase triple notch spacing +April 24th, 2023 : Callisto: Update Tune +April 5th, 2023 : Copter: version to 4.3.6 + +April 17th, 2023 : Version: V4.3.5-Cal1 +March 1st, 2023 : Copter: version to 4.3.5-rc1 + +April 15th, 2023 : Version: V4.3.3-Cal2 +March 15th, 2023 : AC_WPNav: Stop parameter changes overriding speeds +February 7th, 2023 : Update INS_HNTCH_BW +January 29th, 2023 : Update BAT Parameters +January 26th, 2023 : AC_WPNav: Fix Corner Acceleration Bug + +December 17th, 2022 : Version: V4.3.3-Cal1 +December 13th, 2022 : AP_Gripper: Add Neutral state after init +December 19th, 2022 : Copter: Use wp_start() in do_nav_wp() +December 19th, 2022 : Copter: Fix Auto Takeoff when complete_alt_cm is current altitude + +December 17th, 2022 : Version: V4.3.2-Cal1 +December 19th, 2022 : AC_WPNav: Add corner acceleration limit parameter +December 16th, 2022 : Copter: Tuning for position controller angle max +December 15th, 2022 : Copter: Update use of input_vel_accel_z +December 15th, 2022 : AC_AttitudeControl: AC_PosControl: Simplify and clarify use of vertical controllers +December 8th, 2022 : Copter: Reserve G2 63 for Ship Operations + +November 21st, 2022 : Callisto 4.3 parameter updates +November 21st, 2022 : Copter: Payload Place enhancements +November 13th, 2022 : Callisto 2.2: Version 4.3.0-Cal2.2S +November 13th, 2022 : AP_Math: Control Tools Enhancments +November 13th, 2022 : Copter: Add Ship Operations Mode +September 2nd, 2022 : AP_Math: Control: Add heading input shaping +November 13th, 2022 : Copter: Release Tie Down on take-off +November 13th, 2022 : Copter: Add Tie Down Release +November 13th, 2022 : Copter: Add Take-off vertical speed + +November 6th, 2022 : Callisto 2.2: Version 4.3.0-b1-Cal2.2 +October 31st, 2022: Copter: version to 4.3.0 official + +August 21st, 2022: Copter: V4.2.3-rc3-Cal2.2 +July 28th, 2022: Copter: Fix Payload Place Bug +July 22nd, 2022: Copter: Payload Place fix takeoff +August 20th, 2022: Copter: version to 4.2.3-rc3 + +November 18th, 2021: Copter: Version ArduCopter V4.0.7 Cal2.2" +November 18th, 2021: AP_Motors: Add CW X + +May 28th, 2021: Copter: version to 4.0.7 Cal21c +May 24th, 2021: Copter: Fix Loiter attitude error during Pre-Takeoff +May 28th, 2021: AC_AttitudeControl: Set yaw rate to zero during arming procedure + +April 15th, 2021: Copter: version to 4.0.7 Cal21b +April 15th, 2021: AP_GPS: fixed moving baseline Z check for attitude lag (for 4.0) + +March 30th, 2021: Copter: version to 4.0.7 Cal21a +August 15th, 2020: Copter: add gripper release FS_Option + +March 28th, 2021: Copter: Version Cal21 +February 9th, 2021: Callisto release +February 8th, 2021: Create LedFlash.lua script to flash arm LED's +February 8th, 2021: Create Callisto custom build + +January 29th, 2021: ArduCopter V4.0.6 ATUN1 +January 29th, 2021: AC_AutoTune: Tighten up Autotune level requirements \ No newline at end of file diff --git a/libraries/AP_HAL_ChibiOS/hwdef/Callisto/defaults.parm b/libraries/AP_HAL_ChibiOS/hwdef/Callisto/defaults.parm index 50afecebc7bd3..39d9a7120bf3b 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/Callisto/defaults.parm +++ b/libraries/AP_HAL_ChibiOS/hwdef/Callisto/defaults.parm @@ -14,6 +14,9 @@ ATC_ANG_RLL_P,7 ATC_ANG_YAW_P,5 ATC_ANGLE_BOOST,1 ATC_INPUT_TC,0.25 +ATC_LAND_P_MULT,0.5 +ATC_LAND_R_MULT,0.5 +ATC_LAND_Y_MULT,0.5 ATC_RAT_PIT_D,0.0055 ATC_RAT_PIT_FF,0 ATC_RAT_PIT_FLTD,10 @@ -313,7 +316,7 @@ SR2_RC_CHAN,2 SURFTRAK_MODE,0 TERRAIN_ENABLE,1 TERRAIN_MARGIN,0.05 -TERRAIN_OFS_MAX,15 +TERRAIN_OFS_MAX,0 TERRAIN_OPTIONS,0 TERRAIN_SPACING,100 THR_DZ,100 diff --git a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp index 08486b60e9b19..726974421227a 100644 --- a/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp +++ b/libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp @@ -211,6 +211,15 @@ void AP_Logger::handle_log_sending() { WITH_SEMAPHORE(_log_send_sem); + // we can spend a *long* time sending data on the main thread. We + // can spen more than 1.9 seconds doing this depending on what the + // SD card gets up to, moving us into the monitor thread's "you + // are going to watchdog" code. Until we move all these + // operations into a thread we will ask the timer thread to pat + // the watchdog and have the monitor thread chill out by being in + // an expected delay: + EXPECT_DELAY_MS(5000); + #if CONFIG_HAL_BOARD == HAL_BOARD_SITL // assume USB speeds in SITL for the purposes of log download const uint8_t num_sends = 40; diff --git a/libraries/AP_Scripting/examples/FSO/check_firmware_checksum.lua b/libraries/AP_Scripting/examples/FSO/check_firmware_checksum.lua new file mode 100644 index 0000000000000..13a0d7d00a546 --- /dev/null +++ b/libraries/AP_Scripting/examples/FSO/check_firmware_checksum.lua @@ -0,0 +1,31 @@ +--[[ + This script reads the firmware, calculates a checksum and compares it againsta a fixed known-good checksum. + + It does this 10 seconds after boot. And any time millis() wraps. +--]] + +local flash_file_path = "@SYS/flash.bin" + +function update() + -- wait until we've been booted 10 seconds before sending: + local now = millis() + if now < 10000 then + -- not time yet + return update, 100 + end + + local expected_checksum = 0x253F9506 + + local result = fs:crc32(flash_file_path):toint() + if result == nil then + gcs:send_text(1, string.format("checksum_firmware error: checksumming failed")) + elseif result - expected_checksum ~= 0 then + gcs:send_text(1, string.format("checksum_firmware: BAD: calculated=0x%x != expected=0x%x", result, expected_checksum)) + else + gcs:send_text(1, string.format("checksum_firmware: CORRECT: 0x%x", result)) + end + + return -- this script is one-shot +end + +return update() diff --git a/libraries/AP_Scripting/examples/FSO/config_profiles.lua b/libraries/AP_Scripting/examples/FSO/config_profiles.lua new file mode 100644 index 0000000000000..e6bf86c3b6fc1 --- /dev/null +++ b/libraries/AP_Scripting/examples/FSO/config_profiles.lua @@ -0,0 +1,1316 @@ +-- config_profiles.lua +-- Continuously watches CFG_* parameters and applies associated parameter sets + +-- all_params must contain default values for any parameter present in +-- the parameter lists. When switching between domain selections +-- these values are used if the domain doesn't have one. The same +-- parameter must not exist in multiple domains! + + +-- TODO: +-- far flung future - change parameters on peripherals too + + +gcs:send_text(6, string.format("CFG: config_profiles v0.3 starting")) + +local SEL_APPLY_DEFAULTS = 0 +local SEL_DO_NOTHING = -1 + +local must_be_set = "must be set" + +-- This is a marker for the start of the config_domains; it is used to swap these out for CI testing + +-- a table of param indexes to help avoid annoying conflicts: +local param_index = { + ["enable"] = 1, + ["pm_filter"] = 2, + + ["JOB"] = 10, + ["ARMS"] = 11, + ["BAT"] = 12, + ["PAY"] = 13, +} + +local config_domains = { + JOB = { + param_name = "JOB", + all_param_defaults = { + -- Flight Modes + ["FLTMODE_CH"] = must_be_set, + ["FLTMODE1"] = must_be_set, + ["FLTMODE2"] = must_be_set, + ["FLTMODE3"] = must_be_set, + ["FLTMODE4"] = must_be_set, + ["FLTMODE5"] = must_be_set, + ["FLTMODE6"] = must_be_set, + ["FLTMODE_GCSBLOCK"] = 12232576, + + -- Flight Behavior + ["ANGLE_MAX"] = 3000, + + -- Geofence + ["FENCE_ACTION"] = 1, + ["FENCE_ALT_MAX"] = 100, + ["FENCE_ALT_MIN"] = -10, + ["FENCE_ENABLE"] = 1, + ["FENCE_MARGIN"] = 2, + ["FENCE_RADIUS"] = 300, + ["FENCE_TOTAL"] = 0, + ["FENCE_TYPE"] = 7, + + -- Failsafes + ["FS_CRASH_CHECK"] = 1, + ["FS_DR_ENABLE"] = 2, + ["FS_DR_TIMEOUT"] = 30, + ["FS_EKF_ACTION"] = 1, + ["FS_EKF_FILT"] = 5, + ["FS_EKF_THRESH"] = 1, + ["FS_GCS_ENABLE"] = 1, + ["FS_GCS_TIMEOUT"] = 5, + ["FS_OPTIONS"] = 16, + ["FS_THR_ENABLE"] = 1, + ["FS_THR_VALUE"] = 975, + ["FS_VIBE_ENABLE"] = 1, + + -- Battery Failsafes + ["BATT_FS_CRT_ACT"] = 1, + ["BATT_FS_LOW_ACT"] = 2, + + -- IMU Logging + ["INS_FAST_SAMPLE"] = 7, + ["INS_LOG_BAT_CNT"] = 1024, + ["INS_LOG_BAT_MASK"] = 0, + ["INS_LOG_BAT_OPT"] = 0, + + -- Landing + ["LAND_ALT_LOW"] = 1000, + ["LAND_REPOSITION"] = 1, + ["LAND_SPEED"] = 50, + ["LAND_SPEED_HIGH"] = 0, + + -- Logging + ["LOG_BITMASK"] = 180223, + ["LOG_DISARMED"] = 0, + ["LOG_FILE_DSRMROT"] = 0, + + -- Loiter Mode + ["LOIT_ACC_MAX"] = 500, + ["LOIT_ANG_MAX"] = 25, + ["LOIT_BRK_ACCEL"] = 250, + ["LOIT_BRK_DELAY"] = 1, + ["LOIT_BRK_JERK"] = 500, + ["LOIT_SPEED"] = 1250, + + -- Pilot Input Tuning + ["PHLD_BRAKE_ANGLE"] = 3000, + ["PHLD_BRAKE_RATE"] = 8, + ["PILOT_ACCEL_Z"] = 250, + ["PILOT_SPEED_DN"] = 0, + ["PILOT_SPEED_UP"] = 250, + ["PILOT_THR_BHV"] = 3, + ["PILOT_THR_FILT"] = 0, + ["PILOT_TKOFF_ALT"] = 300, + ["PILOT_Y_EXPO"] = 0, + ["PILOT_Y_RATE"] = 60, + ["PILOT_Y_RATE_TC"] = 0.25, + + -- Trajectory Shaping + ["PSC_ANGLE_MAX"] = 45, + ["PSC_JERK_XY"] = 5, + ["PSC_JERK_Z"] = 10, + + -- RTL Behavior + ["RTL_ALT_FINAL"] = 0, + ["RTL_ALT_TYPE"] = 0, + ["RTL_CLIMB_MIN"] = 0, + ["RTL_CONE_SLOPE"] = 3, + ["RTL_LOIT_TIME"] = 1000, + ["RTL_OPTIONS"] = 0, + ["RTL_SPEED"] = 0, + + -- Takeoff + ["TKOFF_SLEW_TIME"] = 4, + + -- Terrain Following + ["TERRAIN_ENABLE"] = 1, + ["TERRAIN_MARGIN"] = 0.05, + ["TERRAIN_OFS_MAX"] = 15, + ["TERRAIN_OPTIONS"] = 0, + ["TERRAIN_SPACING"] = 100, + + -- Waypoint Navigation + ["WP_NAVALT_MIN"] = 0, + ["WP_YAW_BEHAVIOR"] = 1, + ["WPNAV_ACCEL"] = 250, + ["WPNAV_ACCEL_C"] = 0, + ["WPNAV_ACCEL_Z"] = 100, + ["WPNAV_JERK"] = 1, + ["WPNAV_RADIUS"] = 200, + ["WPNAV_RFND_USE"] = 0, + ["WPNAV_SPEED"] = 1500, + ["WPNAV_SPEED_DN"] = 150, + ["WPNAV_SPEED_UP"] = 250, + ["WPNAV_TER_MARGIN"] = 10, + }, + default_sel_value = 1, + profiles = { + [1] = { + name = "DeliveryDefault", + params = { + ["FLTMODE_CH"] = 5, + ["FLTMODE1"] = 2, + ["FLTMODE2"] = 2, + ["FLTMODE3"] = 5, + ["FLTMODE4"] = 5, + ["FLTMODE5"] = 6, + ["FLTMODE6"] = 6, + ["LOG_BITMASK"] = 180222, + } + }, + [100] = { + name = "Testing", + params = { + ["ANGLE_MAX"] = 4500, + ["FLTMODE_CH"] = 5, + ["FLTMODE1"] = 0, + ["FLTMODE2"] = 0, + ["FLTMODE3"] = 2, + ["FLTMODE4"] = 2, + ["FLTMODE5"] = 5, + ["FLTMODE6"] = 5, + }, + }, + [101] = { + name = "FSO", + params = { + ["PSC_ANGLE_MAX"] = 2500, + ["FLTMODE_CH"] = 5, + ["FLTMODE1"] = 5, + ["FLTMODE2"] = 5, + ["FLTMODE3"] = 3, + ["FLTMODE4"] = 3, + ["FLTMODE5"] = 6, + ["FLTMODE6"] = 6, + }, + }, + }, + }, + ARMS = { + param_name = "ARMS", + all_param_defaults = { -- all parameters present in the params for each option + ["ATC_ACCEL_P_MAX"] = 30000, + ["ATC_ACCEL_R_MAX"] = 30000, + ["ATC_ACCEL_Y_MAX"] = 6000, + ["ATC_ANGLE_BOOST"] = 1, + ["ATC_ANG_LIM_TC"] = 1, + ["ATC_ANG_PIT_P"] = 7, + ["ATC_ANG_RLL_P"] = 7, + ["ATC_ANG_YAW_P"] = 5, + ["ATC_INPUT_TC"] = 0.25, + ["ATC_LAND_P_MULT"] = 0.5, + ["ATC_LAND_R_MULT"] = 0.5, + ["ATC_LAND_Y_MULT"] = 0.5, + ["ATC_RATE_FF_ENAB"] = 1, + ["ATC_RATE_P_MAX"] = 150, + ["ATC_RATE_R_MAX"] = 150, + ["ATC_RATE_Y_MAX"] = 0, + ["ATC_RAT_PIT_D"] = 0.005, + ["ATC_RAT_PIT_FF"] = 0, + ["ATC_RAT_PIT_FLTD"] = must_be_set, + ["ATC_RAT_PIT_FLTE"] = 0, + ["ATC_RAT_PIT_FLTT"] = 20, + ["ATC_RAT_PIT_I"] = must_be_set, + ["ATC_RAT_PIT_IMAX"] = 0.5, + ["ATC_RAT_PIT_P"] = must_be_set, + ["ATC_RAT_PIT_SMAX"] = 50, + ["ATC_RAT_RLL_D"] = must_be_set, + ["ATC_RAT_RLL_FF"] = 0, + ["ATC_RAT_RLL_FLTD"] = must_be_set, + ["ATC_RAT_RLL_FLTE"] = 0, + ["ATC_RAT_RLL_FLTT"] = 20, + ["ATC_RAT_RLL_I"] = must_be_set, + ["ATC_RAT_RLL_IMAX"] = 0.5, + ["ATC_RAT_RLL_P"] = must_be_set, + ["ATC_RAT_RLL_SMAX"] = 50, + ["ATC_RAT_YAW_D"] = 0.02, + ["ATC_RAT_YAW_FF"] = 0, + ["ATC_RAT_YAW_FLTD"] = 4, + ["ATC_RAT_YAW_FLTE"] = 2, + ["ATC_RAT_YAW_FLTT"] = 20, + ["ATC_RAT_YAW_I"] = 0.05, + ["ATC_RAT_YAW_IMAX"] = 0.5, + ["ATC_RAT_YAW_P"] = 0.5, + ["ATC_RAT_YAW_SMAX"] = 50, + ["ATC_SLEW_YAW"] = 6000, + ["ATC_THR_MIX_MAN"] = 0.1, + ["ATC_THR_MIX_MAX"] = 2, + ["ATC_THR_MIX_MIN"] = 0.1, + ["AUTOTUNE_AGGR"] = 0.1, + ["AUTOTUNE_AXES"] = 3, + ["AUTOTUNE_MIN_D"] = 0.001, + ["INS_ACCEL_FILTER"] = 10, + ["INS_GYRO_FILTER"] = must_be_set, + ["INS_HNTC2_ATT"] = 80, + ["INS_HNTC2_BW"] = must_be_set, + ["INS_HNTC2_ENABLE"] = 1, + ["INS_HNTC2_FM_RAT"] = 1, + ["INS_HNTC2_FREQ"] = must_be_set, + ["INS_HNTC2_HMNCS"] = 1, + ["INS_HNTC2_MODE"] = 0, + ["INS_HNTC2_OPTS"] = 16, + ["INS_HNTC2_REF"] = 0, + ["INS_HNTCH_ATT"] = must_be_set, + ["INS_HNTCH_BW"] = must_be_set, + ["INS_HNTCH_ENABLE"] = 1, + ["INS_HNTCH_FM_RAT"] = must_be_set, + ["INS_HNTCH_FREQ"] = must_be_set, + ["INS_HNTCH_HMNCS"] = must_be_set, + ["INS_HNTCH_MODE"] = 1, + ["INS_HNTCH_OPTS"] = 16, + ["INS_HNTCH_REF"] = must_be_set, + ["MOT_BAT_CURR_MAX"] = must_be_set, + ["MOT_BAT_CURR_TC"] = 2, + ["MOT_BAT_IDX"] = 0, + ["MOT_BAT_VOLT_MAX"] = 50.4, + ["MOT_BAT_VOLT_MIN"] = 39.6, + ["MOT_BOOST_SCALE"] = 0, + ["MOT_HOVER_LEARN"] = 0, + ["MOT_PWM_MAX"] = 1900, + ["MOT_PWM_MIN"] = 1100, + ["MOT_PWM_TYPE"] = 0, + ["MOT_SAFE_DISARM"] = 0, + ["MOT_SLEW_DN_TIME"] = 0, + ["MOT_SLEW_UP_TIME"] = 0, + ["MOT_SPIN_ARM"] = 0.075, + ["MOT_SPIN_MAX"] = 1, + ["MOT_SPIN_MIN"] = 0.11, + ["MOT_SPOOL_TIME"] = 0.5, + ["MOT_THST_EXPO"] = 0.5, + ["MOT_THST_HOVER"] = must_be_set, + ["MOT_YAW_HEADROOM"] = 50, + ["PSC_ACCZ_D"] = 0, + ["PSC_ACCZ_FF"] = 0, + ["PSC_ACCZ_FLTD"] = 10, + ["PSC_ACCZ_FLTE"] = 0, + ["PSC_ACCZ_FLTT"] = 10, + ["PSC_ACCZ_I"] = 0.5, + ["PSC_ACCZ_IMAX"] = 800, + ["PSC_ACCZ_P"] = 0.25, + ["PSC_ACCZ_SMAX"] = 0, + ["PSC_POSXY_P"] = 1, + ["PSC_POSZ_P"] = 0.5, + ["PSC_VELXY_D"] = 0.25, + ["PSC_VELXY_FF"] = 0, + ["PSC_VELXY_FLTD"] = 5, + ["PSC_VELXY_FLTE"] = 5, + ["PSC_VELXY_I"] = 1, + ["PSC_VELXY_IMAX"] = 1000, + ["PSC_VELXY_P"] = 2, + ["PSC_VELZ_D"] = 0, + ["PSC_VELZ_FF"] = 0, + ["PSC_VELZ_FLTD"] = 5, + ["PSC_VELZ_FLTE"] = 5, + ["PSC_VELZ_I"] = 0, + ["PSC_VELZ_IMAX"] = 1000, + ["PSC_VELZ_P"] = 2.5, + }, + default_sel_value = 50, + profiles = { + [50] = { + name = "Callisto 50", + params = { + ["ATC_RAT_PIT_D"] = 0.0055, + ["ATC_RAT_PIT_FLTD"] = 10, + ["ATC_RAT_PIT_I"] = 0.14, + ["ATC_RAT_PIT_P"] = 0.14, + ["ATC_RAT_RLL_D"] = 0.0055, + ["ATC_RAT_RLL_FLTD"] = 10, + ["ATC_RAT_RLL_I"] = 0.14, + ["ATC_RAT_RLL_P"] = 0.14, + ["INS_GYRO_FILTER"] = 40, + ["INS_HNTC2_BW"] = 45, + ["INS_HNTC2_FREQ"] = 45, + ["INS_HNTCH_ATT"] = 80, + ["INS_HNTCH_BW"] = 15, + ["INS_HNTCH_FM_RAT"] = 0.9, + ["INS_HNTCH_FREQ"] = 30, + ["INS_HNTCH_HMNCS"] = 11, + ["INS_HNTCH_REF"] = 0.22, + ["MOT_BAT_CURR_MAX"] = 240, + ["MOT_THST_HOVER"] = 0.35, + }, + }, + [25] = { + name = "Callisto 25", + params = { + ["ATC_RAT_PIT_FLTD"] = 15, + ["ATC_RAT_PIT_I"] = 0.1, + ["ATC_RAT_PIT_P"] = 0.1, + ["ATC_RAT_RLL_D"] = 0.005, + ["ATC_RAT_RLL_FLTD"] = 15, + ["ATC_RAT_RLL_I"] = 0.1, + ["ATC_RAT_RLL_P"] = 0.1, + ["INS_GYRO_FILTER"] = 30, + ["INS_HNTC2_FREQ"] = 30, + ["INS_HNTC2_BW"] = 20, + ["INS_HNTCH_ATT"] = 40, + ["INS_HNTCH_BW"] = 33, + ["INS_HNTCH_FM_RAT"] = 0.75, + ["INS_HNTCH_FREQ"] = 49.5, + ["INS_HNTCH_HMNCS"] = 15, + ["INS_HNTCH_REF"] = 0.21, + ["MOT_BAT_CURR_MAX"] = 150, + ["MOT_THST_HOVER"] = 0.2053075, + }, + }, + [75] = { + name = "Callisto 75", + params = { + ["ATC_ANG_YAW_P"] = 4.0, + ["ATC_RAT_PIT_D"] = 0.004, + ["ATC_RAT_PIT_FLTD"] = 10.0, + ["ATC_RAT_PIT_I"] = 0.09, + ["ATC_RAT_PIT_P"] = 0.09, + ["ATC_RAT_RLL_D"] = 0.004, + ["ATC_RAT_RLL_FLTD"] = 10.0, + ["ATC_RAT_RLL_I"] = 0.09, + ["ATC_RAT_RLL_P"] = 0.09, + ["INS_GYRO_FILTER"] = 20.0, + ["INS_HNTC2_BW"] = 16.0, + ["INS_HNTC2_FREQ"] = 32.0, + ["INS_HNTC2_OPTS"] = 0.0, + ["INS_HNTCH_ATT"] = 80.0, + ["INS_HNTCH_BW"] = 11.0, + ["INS_HNTCH_FM_RAT"] = 0.9, + ["INS_HNTCH_FREQ"] = 22.0, + ["INS_HNTCH_HMNCS"] = 11.0, + ["INS_HNTCH_OPTS"] = 20.0, + ["INS_HNTCH_REF"] = 0.07, + ["MOT_BAT_CURR_MAX"] = 240.0, + ["MOT_PWM_MAX"] = 2000.0, + ["MOT_PWM_MIN"] = 1000.0, + ["MOT_SPIN_ARM"] = 0.15, + ["MOT_SPIN_MAX"] = 0.95, + ["MOT_SPIN_MIN"] = 0.2, + ["MOT_THST_EXPO"] = 0.55, + ["MOT_THST_HOVER"] = 0.2, + ["PSC_ACCZ_I"] = 0.25, + ["PSC_ACCZ_P"] = 0.125, + }, + }, + }, + }, + + BAT = { + param_name = "BAT", + all_param_defaults = { + ["BATT_CAPACITY"] = must_be_set, + ["BATT_CRT_MAH"] = must_be_set, + ["BATT_CRT_VOLT"] = must_be_set, + ["BATT_LOW_MAH"] = must_be_set, + ["BATT_LOW_VOLT"] = must_be_set, + }, + default_sel_value = 22, + profiles = { + [16] = { + name = "16Ah", + params = { + ["BATT_CAPACITY"] = 32000, + ["BATT_CRT_MAH"] = 6400, + ["BATT_CRT_VOLT"] = 44.5, + ["BATT_LOW_MAH"] = 9600, + ["BATT_LOW_VOLT"] = 45.0, + }, + }, + [22] = { + name = "22Ah", + params = { + ["BATT_CAPACITY"] = 44000, + ["BATT_CRT_MAH"] = 8800, + ["BATT_CRT_VOLT"] = 43.25, + ["BATT_LOW_MAH"] = 13200, + ["BATT_LOW_VOLT"] = 43.75, + }, + }, + [40] = { + name = "40Ah", + params = { + ["BATT_CAPACITY"] = 80000, + ["BATT_CRT_MAH"] = 16000, + ["BATT_CRT_VOLT"] = 43.75, + ["BATT_LOW_MAH"] = 24000, + ["BATT_LOW_VOLT"] = 44.25, + }, + }, + }, + }, + + PAY = { + param_name = "PAY", + all_param_defaults = { + ["GRIP_ENABLE"] = 0, + ["GRIP_GRAB"] = 1000, + ["GRIP_NEUTRAL"] = 1000, + ["GRIP_REGRAB"] = 0, + ["GRIP_RELEASE"] = 2000, + ["GRIP_TYPE"] = 1, + ["MNT1_DEFLT_MODE"] = 3, -- 3 = RC Targeting + ["MNT1_LEAD_PTCH"] = 0, + ["MNT1_LEAD_RLL"] = 0, + ["MNT1_NEUTRAL_X"] = 0, + ["MNT1_NEUTRAL_Y"] = 0, + ["MNT1_NEUTRAL_Z"] = 0, + ["MNT1_PITCH_MAX"] = 20, + ["MNT1_PITCH_MIN"] = -90, + ["MNT1_RC_RATE"] = 90, + ["MNT1_RETRACT_X"] = 0, + ["MNT1_RETRACT_Y"] = 0, + ["MNT1_RETRACT_Z"] = 0, + ["MNT1_ROLL_MAX"] = 30, + ["MNT1_ROLL_MIN"] = -30, + ["MNT1_TYPE"] = 0, + ["MNT1_YAW_MAX"] = 180, + ["MNT1_YAW_MIN"] = -180, + ["NET_ENABLE"] = 0, + ["NET_OPTIONS"] = 0, + ["NET_P1_IP0"] = 192, + ["NET_P1_IP1"] = 168, + ["NET_P1_IP2"] = 111, + ["NET_P1_IP3"] = 15, + ["NET_P1_PORT"] = 14550, + ["NET_P1_PROTOCOL"] = 2, + ["NET_P1_TYPE"] = 2, + ["NET_P2_TYPE"] = 0, + ["NET_P3_TYPE"] = 0, + ["NET_P4_TYPE"] = 0, + ["RC8_OPTION"] = 0, -- disabled + ["RC13_OPTION"] = 0, + ["RC13_REVERSED"] = 0, + ["RC14_OPTION"] = 0, + ["RC14_REVERSED"] = 0, + ["SERIAL5_BAUD"] = 115, -- 115 = 115200 baud + ["SERIAL5_OPTIONS"] = 0, + ["SERIAL5_PROTOCOL"] = 2, -- 2 = MAVLink2 + ["SERVO10_FUNCTION"] = -1, + ["SERVO10_MAX"] = 2000, + ["SERVO10_MIN"] = 1000, + ["SERVO10_REVERSED"] = 0, + ["SERVO10_TRIM"] = 1500, + ["SERVO11_FUNCTION"] = -1, + ["SERVO12_FUNCTION"] = -1, + ["SERVO13_FUNCTION"] = -1, + ["SERVO14_FUNCTION"] = -1, + ["SERVO9_FUNCTION"] = -1, + ["SERVO9_MAX"] = 2000, + ["SERVO9_MIN"] = 1000, + ["SERVO9_REVERSED"] = 0, + ["SERVO9_TRIM"] = 1500, + }, + default_sel_value = SEL_APPLY_DEFAULTS, + profiles = { + [1] = { + name = "HookFixed", + params = { + ["GRIP_ENABLE"] = 1, + ["RC8_OPTION"] = 19, -- 19 = Gripper (RC switch triggers grab/release) + ["SERVO9_FUNCTION"] = 28, -- 28 = gripper + } + }, + [2] = { + name = "HookSlung", + params = { + ["GRIP_ENABLE"] = 1, + ["RC8_OPTION"] = 19, -- 19 = Gripper (RC switch triggers grab/release) + ["SERVO9_FUNCTION"] = 146, -- 146 = k_rcin7_mapped?! + ["SERVO9_REVERSED"] = 1, + ["SERVO10_FUNCTION"] = 28, -- 28 = gripper + }, + }, + [3] = { + name = "Gimbal - AVT_CM62", + params = { + ["MNT1_TYPE"] = 6, -- 6 = Mount type MAVLinkv2 (or Custom) + ["RC13_OPTION"] = 214, -- 214 = Mount Yaw + ["RC14_OPTION"] = 213, -- 213 = Mount Pitch + }, + }, + [4] = { + name = "Gimbal - HookFixed", + params = { + ["GRIP_ENABLE"] = 1, + ["RC8_OPTION"] = 19, -- 19 = Gripper + + ["SERVO9_FUNCTION"] = 28, -- 28 = Mount Shutter (e.g. camera trigger) + ["SERVO10_FUNCTION"] = 64, -- 64 = Landing Gear + ["SERVO11_FUNCTION"] = 63, -- 63 = Mount Tilt + ["SERVO12_FUNCTION"] = 62, -- 62 = Mount Roll + ["SERVO13_FUNCTION"] = 58, -- 58 = Mount Zoom + ["SERVO14_FUNCTION"] = 57, -- 57 = Mount Focus + }, + }, + [5] = { + name = "Gimbal - Hook Slung", + params = { + ["GRIP_ENABLE"] = 1, + ["RC8_OPTION"] = 19, -- gripper + ["SERVO9_FUNCTION"] = 146, -- k_rcin7_mapped?! + ["SERVO9_REVERSED"] = 1, + ["SERVO10_FUNCTION"] = 28, -- gripper + ["SERVO10_TRIM"] = 2000, + ["SERVO11_FUNCTION"] = 64, -- rcin 14 + ["SERVO12_FUNCTION"] = 63, -- rcin 13 + ["SERVO13_FUNCTION"] = 61, -- rcin 11 + ["SERVO14_FUNCTION"] = 56, -- rcin 6 + } + }, + [10] = { + name = "Silvus", + params = { + ["SERIAL5_BAUD"] = 115, -- 115 = 115200 baud + ["SERIAL5_PROTOCOL"] = 2, -- 2 = MAVLink2 + } + }, + [11] = { + name = "Skylink", + params = { + ["NET_ENABLE"] = 1, + ["NET_OPTIONS"] = 0, + ["NET_P1_IP0"] = 192, + ["NET_P1_IP1"] = 168, + ["NET_P1_IP2"] = 111, + ["NET_P1_IP3"] = 15, + ["NET_P1_PORT"] = 14550, + ["NET_P1_PROTOCOL"] = 2, + ["NET_P1_TYPE"] = 2, + ["NET_P2_TYPE"] = 0, + ["NET_P3_TYPE"] = 0, + ["NET_P4_TYPE"] = 0, + } + }, + }, + }, +} +-- This is a marker for the end of the config_domains; it is used to swap these out for CI testing + +-- a list of parameters which can still be set even if we are +-- filtering which parameter values can bet set. We also permit the +-- configuration paramters to be dynamically set. +local parameters_which_can_be_set = { + -- Config / Parameter Management + ["CFG_FLT_PM_SET"] = true, + + -- Aircraft options + ["COMPASS_USE3"] = true, + ["LIGHTS_ON"] = true, + + -- Battery failsafes + ["BATT_ARM_MAH"] = true, + ["BATT_ARM_VOLT"] = true, + ["BATT_FS_CRT_ACT"] = true, + ["BATT_FS_LOW_ACT"] = true, + + -- Fence configuration + ["FENCE_ACTION"] = true, + ["FENCE_ALT_MAX"] = true, + ["FENCE_ENABLE"] = true, + ["FENCE_RADIUS"] = true, + ["FENCE_TOTAL"] = true, + ["FENCE_TYPE"] = true, + + -- Failsafes + ["FLTMODE_CH"] = true, + ["FLTMODE_GCSBLOCK"] = true, + ["FLTMODE1"] = true, + ["FLTMODE2"] = true, + ["FLTMODE3"] = true, + ["FLTMODE4"] = true, + ["FLTMODE5"] = true, + ["FLTMODE6"] = true, + + -- Failsafes + ["FS_DR_ENABLE"] = true, + ["FS_EKF_ACTION"] = true, + ["FS_GCS_ENABLE"] = true, + ["FS_GCS_TIMEOUT"] = true, + ["FS_OPTIONS"] = true, + ["FS_THR_ENABLE"] = true, + ["FS_VIBE_ENABLE"] = true, + + -- Follow mode + ["FOLL_DIST"] = true, + ["FOLL_ENABLE"] = true, + ["FOLL_HDG_MODE"] = true, + ["FOLL_OFS_X"] = true, + ["FOLL_OFS_Y"] = true, + ["FOLL_OFS_Z"] = true, + ["FOLL_SYSID"] = true, + + -- Guided mode timeout + ["GUID_OPTIONS"] = true, + ["GUID_TIMEOUT"] = true, + + -- IMU Logging + ["INS_LOG_BAT_CNT"] = true, + ["INS_LOG_BAT_MASK"] = true, + ["INS_LOG_BAT_OPT"] = true, + + -- Landing behavior + ["LAND_ALT_LOW"] = true, + ["LAND_REPOSITION"] = true, + ["LAND_SPEED"] = true, + ["LAND_SPEED_HIGH"] = true, + + -- Logging + ["LOG_BITMASK"] = true, + ["LOG_DISARMED"] = true, + ["LOG_FILE_DSRMROT"] = true, + + -- Loiter mode + ["LOIT_ACC_MAX"] = true, + ["LOIT_ANG_MAX"] = true, + ["LOIT_BRK_ACCEL"] = true, + ["LOIT_BRK_DELAY"] = true, + ["LOIT_BRK_JERK"] = true, + ["LOIT_SPEED"] = true, + + -- Payload: Mount/gimbal configuration + ["MNT1_TYPE"] = true, + ["MNT1_DEFLT_MODE"] = true, + ["MNT1_PITCH_MIN"] = true, + ["MNT1_PITCH_MAX"] = true, + ["MNT1_ROLL_MIN"] = true, + ["MNT1_ROLL_MAX"] = true, + ["MNT1_YAW_MIN"] = true, + ["MNT1_YAW_MAX"] = true, + ["MNT1_RC_RATE"] = true, + ["MNT1_LEAD_PTCH"] = true, + ["MNT1_LEAD_RLL"] = true, + ["MNT1_NEUTRAL_X"] = true, + ["MNT1_NEUTRAL_Y"] = true, + ["MNT1_NEUTRAL_Z"] = true, + ["MNT1_RETRACT_X"] = true, + ["MNT1_RETRACT_Y"] = true, + ["MNT1_RETRACT_Z"] = true, + + -- Pilot input shaping + ["PHLD_BRAKE_ANGLE"] = true, + ["PHLD_BRAKE_RATE"] = true, + ["PILOT_ACCEL_Z"] = true, + ["PILOT_SPEED_DN"] = true, + ["PILOT_SPEED_UP"] = true, + ["PILOT_TKOFF_ALT"] = true, + + -- Trajectory Shaping + ["PSC_ANGLE_MAX"] = true, + + -- RTL behavior + ["RTL_ALT"] = true, + ["RTL_ALT_FINAL"] = true, + ["RTL_ALT_TYPE"] = true, + ["RTL_CLIMB_MIN"] = true, + ["RTL_CONE_SLOPE"] = true, + ["RTL_LOIT_TIME"] = true, + ["RTL_OPTIONS"] = true, + ["RTL_SPEED"] = true, + + -- RC input channel configuration (RC5–RC16) + ["RC5_OPTION"] = true, + ["RC5_REVERSED"] = true, + ["RC5_TRIM"] = true, + ["RC5_MIN"] = true, + ["RC5_MAX"] = true, + ["RC5_DZ"] = true, + + ["RC6_OPTION"] = true, + ["RC6_REVERSED"] = true, + ["RC6_TRIM"] = true, + ["RC6_MIN"] = true, + ["RC6_MAX"] = true, + ["RC6_DZ"] = true, + + ["RC7_OPTION"] = true, + ["RC7_REVERSED"] = true, + ["RC7_TRIM"] = true, + ["RC7_MIN"] = true, + ["RC7_MAX"] = true, + ["RC7_DZ"] = true, + + ["RC8_OPTION"] = true, + ["RC8_REVERSED"] = true, + ["RC8_TRIM"] = true, + ["RC8_MIN"] = true, + ["RC8_MAX"] = true, + ["RC8_DZ"] = true, + + ["RC9_OPTION"] = true, + ["RC9_REVERSED"] = true, + ["RC9_TRIM"] = true, + ["RC9_MIN"] = true, + ["RC9_MAX"] = true, + ["RC9_DZ"] = true, + + ["RC10_OPTION"] = true, + ["RC10_REVERSED"] = true, + ["RC10_TRIM"] = true, + ["RC10_MIN"] = true, + ["RC10_MAX"] = true, + ["RC10_DZ"] = true, + + ["RC11_OPTION"] = true, + ["RC11_REVERSED"] = true, + ["RC11_TRIM"] = true, + ["RC11_MIN"] = true, + ["RC11_MAX"] = true, + ["RC11_DZ"] = true, + + ["RC12_OPTION"] = true, + ["RC12_REVERSED"] = true, + ["RC12_TRIM"] = true, + ["RC12_MIN"] = true, + ["RC12_MAX"] = true, + ["RC12_DZ"] = true, + + ["RC13_OPTION"] = true, + ["RC13_REVERSED"] = true, + ["RC13_TRIM"] = true, + ["RC13_MIN"] = true, + ["RC13_MAX"] = true, + ["RC13_DZ"] = true, + + ["RC14_OPTION"] = true, + ["RC14_REVERSED"] = true, + ["RC14_TRIM"] = true, + ["RC14_MIN"] = true, + ["RC14_MAX"] = true, + ["RC14_DZ"] = true, + + ["RC15_OPTION"] = true, + ["RC15_REVERSED"] = true, + ["RC15_TRIM"] = true, + ["RC15_MIN"] = true, + ["RC15_MAX"] = true, + ["RC15_DZ"] = true, + + ["RC16_OPTION"] = true, + ["RC16_REVERSED"] = true, + ["RC16_TRIM"] = true, + ["RC16_MIN"] = true, + ["RC16_MAX"] = true, + ["RC16_DZ"] = true, + + -- Payload: Servo outputs (e.g. gripper, zoom, tilt, shutter, etc.) + ["SERVO9_FUNCTION"] = true, + ["SERVO9_REVERSED"] = true, + ["SERVO9_MIN"] = true, + ["SERVO9_MAX"] = true, + ["SERVO9_TRIM"] = true, + + ["SERVO10_FUNCTION"] = true, + ["SERVO10_REVERSED"] = true, + ["SERVO10_MIN"] = true, + ["SERVO10_MAX"] = true, + ["SERVO10_TRIM"] = true, + + ["SERVO11_FUNCTION"] = true, + ["SERVO11_REVERSED"] = true, + ["SERVO11_MIN"] = true, + ["SERVO11_MAX"] = true, + ["SERVO11_TRIM"] = true, + + ["SERVO12_FUNCTION"] = true, + ["SERVO12_REVERSED"] = true, + ["SERVO12_MIN"] = true, + ["SERVO12_MAX"] = true, + ["SERVO12_TRIM"] = true, + + ["SERVO13_FUNCTION"] = true, + ["SERVO13_REVERSED"] = true, + ["SERVO13_MIN"] = true, + ["SERVO13_MAX"] = true, + ["SERVO13_TRIM"] = true, + + -- Terrain following + ["TERRAIN_ENABLE"] = true, + ["TERRAIN_MARGIN"] = true, + ["TERRAIN_OFS_MAX"] = true, + ["TERRAIN_OPTIONS"] = true, + ["TERRAIN_SPACING"] = true, + + -- Trajectory shaping + ["PSC_JERK_XY"] = true, + ["PSC_JERK_Z"] = true, + + -- Waypoint navigation + ["WP_NAVALT_MIN"] = true, + ["WP_YAW_BEHAVIOR"] = true, + ["WPNAV_ACCEL"] = true, + ["WPNAV_ACCEL_C"] = true, + ["WPNAV_ACCEL_Z"] = true, + ["WPNAV_JERK"] = true, + ["WPNAV_RADIUS"] = true, + ["WPNAV_RFND_USE"] = true, + ["WPNAV_SPEED"] = true, + ["WPNAV_SPEED_DN"] = true, + ["WPNAV_SPEED_UP"] = true, + ["WPNAV_TER_MARGIN"] = true, +} + + +local msg_pfx = "CFG: " + +-- set up for denying arming when problems occur: +local auth_id = arming:get_aux_auth_id() or 0 + +local function set_aux_auth_failed(reason) + arming:set_aux_auth_failed(auth_id, msg_pfx .. reason) +end + +local function set_aux_auth_passed() + arming:set_aux_auth_passed(auth_id) +end + +set_aux_auth_failed("Validation pending") + +-- initialize MAVLink rx with buffer depth and number of rx message IDs to register +mavlink:init(5, 1) + +-- register message id to receive +local PARAM_SET_ID = 23 +mavlink:register_rx_msgid(PARAM_SET_ID) + +-- initialise our knowledge of the GCS's allow-set-parameters state. +-- We do not want to fight over setting this GCS state via other +-- mechanisms (eg. an auxiliary function), so we keep this state +-- around to track what we last set: +local gcs_allow_set = gcs:get_allow_param_set() + +local function send_text(severity, message) + gcs:send_text(severity, msg_pfx .. message) +end + +local function validate_unique_parameters_across_domains() + -- ensure that no parameter exists in multiple domains: + local all_domain_parameters = {} + for _, domain in pairs(config_domains) do + for param_name, _ in pairs(domain.all_param_defaults) do + local first_domain_name = all_domain_parameters[param_name] + if first_domain_name ~= nil then + send_text(3, string.format("%s exists in two domains (%s and %s)", param_name, first_domain_name, domain.param_name)) + return false + end + all_domain_parameters[param_name] = domain.param_name + end + end + return true +end + +local function validate_params_in_param_defaults() + -- ensure that any parameter in the params tables are also in all_params. + -- Also ensure that it doesn't have the same value as the default + -- also ensure all defaults are over-ridden in one of the profiles + for _, domain in pairs(config_domains) do +-- local default_overridden = {} + for profile_num, profile in pairs(domain.profiles) do + for param_name, _ in pairs(profile.params) do + -- default_overridden[param_name] = true + local param_default = domain.all_param_defaults[param_name] + if param_default == nil then + send_text(3, string.format("%s exists in %s[%f](%s) but not in %s.all_param_defaults", param_name, domain.param_name, profile_num, profile.name, domain.param_name)) + return false + end +-- if param_default == param_value then +-- send_text(3, string.format("%s in %s[%f](%s) has the same value as %s.all_param_defaults", param_name, domain.param_name, profile_num, profile.name, domain.param_name)) +-- return false +-- end + end + end +-- turns out this is a pain as you need to find values to put into all_param_defaults which is annoying: +-- local failed_defaults_used = false +-- for param_name, _ in pairs(domain.all_param_defaults) do + --if default_overridden[param_name] == nil then +-- send_text(3, string.format("%s.all_param_defaults contains %s but no profile overrides it", domain.param_name, param_name)) +-- failed_defaults_used = true +-- end +-- end +-- if failed_defaults_used then +-- return false +-- end + end + return true +end + +local function validate_must_be_set_parameters() + -- ensure that if a parameter has a "must be set" value in the + -- defaults that every domain defines a value for that default + for _, domain in pairs(config_domains) do + for param_name, value in pairs(domain.all_param_defaults) do + if value ~= must_be_set then + goto vmbsp_next_param + end + local first_value = nil + for profile_num, profile in pairs(domain.profiles) do + param_value = profile.params[param_name] + if param_value == nil then + send_text(3, string.format("%s exists in %s.all_param_defaults with must-be-set-value but profile %s(%s) params does not have a value for it", param_name, domain.param_name, profile_num, profile.name)) + return false + end + if first_value == nil then + first_value = param_value + end + end + ::vmbsp_next_param:: + end + end + return true +end + +local function validate_domain_attributes() + -- check direct domain attributes are valid + for _, domain in pairs(config_domains) do + -- check default selection is valid + default_sel_value = domain.default_sel_value + if default_sel_value == nil then + goto good_sel_value + end + if default_sel_value == SEL_DO_NOTHING or default_sel_value == SEL_APPLY_DEFAULTS then + goto good_sel_value + end + + sel = domain.profiles[default_sel_value] + if sel ~= nil then + goto good_sel_value + end + + if true then -- needed to fool lua checker + send_text(3, string.format("%s.default_sel_value is invalid", domain.param_name)) + return false + end + + ::good_sel_value:: + end + return true +end + +-- this is a runtime check: +local function validate_apply_defaults_no_must_be_sets() + -- ensure that if a parameter is marked as must-be-supplied in the + -- defaults that the domain isn't in "apply defaults" mode + for _, domain in pairs(config_domains) do + local sel_value = domain.sel_param:get() + if sel_value == nil then + send_text(3, string.format("Bad sel parameter for %s", domain.param_name)) + return false + end + if sel_value ~= SEL_APPLY_DEFAULTS then + goto vadnn_next_domain + end + for param_name, value in pairs(domain.all_param_defaults) do + if value == must_be_set then + send_text(3, string.format("%s mode is set-values-from-defaults but %s is marked must-be-set in %s.all_param_defaults", domain.param_name, param_name, domain.param_name)) + return false + end + end + ::vadnn_next_domain:: + end + return true +end + +local function validate_param_profiles() + for _, domain in pairs(config_domains) do + for profile_num, profile in pairs(domain.profiles) do + if profile.name == nil then + send_text(3, string.format("%s profile is missing a name", domain.param_name)) + return false + end + if profile_num == 0 then + send_text(3, string.format("%s profile (%s) has zero index, which conflicts with the domain \"apply defaults\" option", domain.param_name, profile.name)) + return false + end + end + end + return true +end + +-- validate_config_domains - step through the domain configuration +-- structure and ensure that no mistake has been made in the setup of +-- the configuration sets +local function validate_config_domains() + validators = { + validate_unique_parameters_across_domains, + validate_params_in_param_defaults, + validate_must_be_set_parameters, + validate_apply_defaults_no_must_be_sets, + validate_domain_attributes, + validate_param_profiles, + } + for _, validator in pairs(validators) do + if validator == nil then + set_aux_auth_failed("Invalid validator") + return false + end + if not validator() then + set_aux_auth_failed("Domain configuration invalid") + return false + end + end + return true +end + +-- parameter setup +-- +local PARAM_TABLE_KEY = 10 +local PARAM_TABLE_PREFIX = "CFG_" +assert(param:add_table(PARAM_TABLE_KEY, PARAM_TABLE_PREFIX, 16), 'could not add param table') + +-- add a parameter and bind it to a variable +local function bind_add_param(name, idx, default_value) + assert(param:add_param(PARAM_TABLE_KEY, idx, name, default_value), string.format('could not add param %s', PARAM_TABLE_PREFIX .. name)) + return Parameter(PARAM_TABLE_PREFIX .. name) +end + +--[[ + // @Param: CFG_ENABLE + // @DisplayName: Enable configuration script + // @Description: Script immediately exits if this is zero + // @Values: 0:Disabled,1:Enabled + // @User: Standard +--]] +local CFG_ENABLE = bind_add_param("ENABLE", param_index["enable"], 1) + +--[[ + // @Param: CFG_FLTR_PM_SET + // @DisplayName: Filter parameter setting + // @Description: Disallows setting of parameters outside whitelist + // @Values: 0:Do not filter,1:Filter + // @User: Standard +--]] +local FLT_PM_SET = bind_add_param("FLT_PM_SET", param_index["pm_filter"], 0) + +local function add_param_for_domain(domain, index, name, default) + pname = domain.param_name .. "_" .. name + -- send_text(6, string.format("Registering %s at index=%s with default %s", pname, index, default)) + return bind_add_param(pname, index, default) +end + +for _, domain in pairs(config_domains) do + domain_sel_default = domain.default_sel_value + if domain_sel_default == nil then + domain_sel_default = 0 + end + domain.sel_param = add_param_for_domain(domain, param_index[domain.param_name], "SEL", domain_sel_default) +end + +local a_parameter_was_ever_set = false + + +-- utility: apply one profile +local function apply_parameters(domain, params) + for param_name, new_value in pairs(domain.all_param_defaults) do + local profile_param_value = params[param_name] + if profile_param_value ~= nil then + new_value = profile_param_value + end + if new_value == nil then + send_text(3, string.format("Validation code has failed as %s has nil value", param_name)) + return + end + if new_value == must_be_set then + send_text(3, string.format("Validation code has failed as %s is must_be_set", param_name)) + return + end + local old_value = param:get(param_name) + if old_value == nil then + send_text(3, string.format("Unable to fetch parameter %s", param_name)) + return + end + if old_value ~= new_value then + send_text(6, string.format("Set %s=%s (was %.3f)", param_name, new_value, old_value)) + param:set_and_save(param_name, new_value) + a_parameter_was_ever_set = true + end + end +end + +-- check each domain configuration parameter, act if any has changed +local last_active_domains_print_time = millis() +local function handle_show_domain_statuses() + local now = millis() + if now - last_active_domains_print_time < 30000 then + return + end + last_active_domains_print_time = now + for _, domain in pairs(config_domains) do + local sel_value = domain.sel_param:get() + if sel_value == SEL_DO_NOTHING then + send_text(6, string.format("%s: doing nothing", domain.param_name)) + goto hsds_next_domain + end + if sel_value == SEL_APPLY_DEFAULTS then + send_text(6, string.format("%s: applying defaults", domain.param_name)) + goto hsds_next_domain + end + local profile = domain.profiles[sel_value] + if profile == nil then + send_text(6, string.format("%s: Invalid profile selected", domain.param_name)) + goto hsds_next_domain + end + send_text(6, string.format("%s: applying profile %s", domain.param_name, profile.name)) + ::hsds_next_domain:: + end +end + +local selected_profile_was_nil = false +local function handle_domains() + local success = true + for _, domain in pairs(config_domains) do + local sel_value = domain.sel_param:get() + local sel_value_changed = false + if sel_value ~= domain.last_sel_value then + domain.last_sel_value = sel_value + sel_value_changed = true + end + + if sel_value == SEL_DO_NOTHING then + if sel_value_changed then + send_text(6, string.format("Doing nothing for %s", domain.param_name)) + end + goto cd_next_domain + end + + if sel_value == SEL_APPLY_DEFAULTS then + if sel_value_changed then + send_text(6, string.format("Applying %s defaults", domain.param_name)) + end + apply_parameters(domain, {}) + goto cd_next_domain + end + + local profile = domain.profiles[sel_value] + if profile == nil then + if not selected_profile_was_nil then + send_text(6, string.format("Invalid profile selected for %s", domain.param_name)) + selected_profile_was_nil = true + end + set_aux_auth_failed(string.format("Invalid profile selected for %s", domain.param_name)) + success = false + goto cd_next_domain + end + selected_profile_was_nil = false + + if domain.last_sel_value ~= sel_value or sel_value_changed then + domain.last_sel_value = sel_value + send_text(6, string.format("Applying %s profile: %s", domain.param_name, profile.name)) + end + apply_parameters(domain, profile.params) + goto cd_next_domain + + ::cd_next_domain:: + end + if a_parameter_was_ever_set then + set_aux_auth_failed("Reboot needed for config change") + return + end + if not success then + return + end + + set_aux_auth_passed() + + handle_show_domain_statuses() +end + +-- support for parameter-set-filtering: +local function should_set_parameter_id(param_id) + -- first check the static list: + if parameters_which_can_be_set[param_id] ~= nil then + return parameters_which_can_be_set[param_id] + end + + -- now check to see if this is one of the configuration selection + -- parameters: + for _, domain in pairs(config_domains) do + domain_sel_param_name = string.format("CFG_%s_SEL", domain.param_name) + if param_id == domain_sel_param_name then + return true + end + end + + return false +end + +local function handle_param_set(name, value) + -- we will not receive packets in here for the wrong system ID / + -- component ID; this is handled by ArduPilot's MAVLink routing + -- code + + -- check for this specific ID: + if not should_set_parameter_id(name) then + send_text(3, string.format("param set denied (%s)", name)) + return + end + + param:set_and_save(name, value) + gcs:send_text(3, string.format("param set applied")) +end + +local function handle_param_setting() + if gcs_allow_set and FLT_PM_SET:get() == 1 then + -- this script is filtering, disallow setting via normal means (once): + gcs:set_allow_param_set(false) + gcs_allow_set = false + elseif not gcs_allow_set and FLT_PM_SET:get() == 0 then + -- this script is not filtering, allow setting via normal means (once): + gcs:set_allow_param_set(true) + gcs_allow_set = true + end + + while true do + -- we consume all mavlink messages even when we won't set parameters + local msg, _ = mavlink:receive_chan() + if msg == nil then + break + end + + if gcs_allow_set == false then -- we are in change of param setting + local param_value, _, _, param_id, _ = string.unpack("